GC23005/Machine/HZFuChenMachine/HZFC_DengDaiDianJiaoJi.c

#include "global.h"

#if FU_CHEN_MACHINE == 1
//故障报警
void DDDJ_SetAlarmCode(unsigned alarm_code)
{
    SetAlarmCode(ALARM_ADDR,alarm_code);
    bAlarmStop=1;
}
//X轴回零
static int X_ReSetOrigin(void)
{
    switch(XRunResetOStep)
    {
    case 1:
      AxisDisableSoftwarelmt(X_AXIS);
      if(dwTickCount >= ReSetXDelay)
      {
        if(X_Reset_Flag)
         {            
            ReSetXOUT_TIME = dwTickCount + 15000;
            if(!DDDJ_X_ORIGIN_IN)
            {
                XRunResetOStep = 2;               
                AxisContinueMoveAcc(X_AXIS,PARAM_XREST_SPEED,DIR_N,
                                  1000,1000,20,20);
            }
            else
            {
                AxisContinueMoveAcc(X_AXIS,PARAM_XREST_SPEED/2,DIR_P,1000,1000,20,20);
                XRunResetOStep = 4;                
            }
         }
         else
         {
            if(0-dwXRealPos)
                AxisMovePosAccDec(X_AXIS,PARAM_XREST_SPEED,0-dwXRealPos,1000,1000,50,50,0); 
            XRunResetOStep = 10; 
            ReSetXOUT_TIME = dwTickCount + 10000;
         }
      }
        break;
    case 2:
        if(DDDJ_X_ORIGIN_IN)
        {
            AxisDecStop(X_AXIS);                
            XRunResetOStep = 3;
            ReSetXDelay = dwTickCount + 10;
            ReSetXOUT_TIME = dwTickCount + 15000;
        }
        break;
    case 3:
        if(!X_DRV&&dwTickCount >= ReSetXDelay)
        {
            AxisContinueMoveAcc(X_AXIS,1000,DIR_P,1000,1000,20,20);               
            XRunResetOStep = 4;                
        }
        break;
    case 4:
        if(!DDDJ_X_ORIGIN_IN)
        {                               
            XRunResetOStep = 5;
            ReSetXDelay = dwTickCount + 50;
            ReSetXOUT_TIME = dwTickCount + 15000;
        }
        break;
    case 5:
        if(!DDDJ_X_ORIGIN_IN&&dwTickCount >= ReSetXDelay)
        {                               
            AxisEgmStop(X_AXIS); 
            XRunResetOStep = 6;
            ReSetXDelay = dwTickCount + 10;
        }
        break;
    case 6:
        if(!X_DRV&&dwTickCount >= ReSetXDelay)
        {                               
            XRunResetOStep = 7;
            AxisContinueMoveAcc(X_AXIS,800,DIR_N,800,800,20,20);                 
        }
        break;
    case 7:
        if(DDDJ_X_ORIGIN_IN)
        {                               
            AxisEgmStop(X_AXIS); 
            XRunResetOStep = 9;
            ReSetXDelay = dwTickCount + 20;
        }
        break;
    case 9:
        if(!X_DRV&&dwTickCount >= ReSetXDelay)
        {                               
            XRunResetOStep = 10;
            AxisMovePosAccDec(X_AXIS,PARAM_XREST_SPEED,-10,1000,1000,20,20,0);                 
        }
        break;
    case 10:
        if(!X_DRV)
        {                
            SetPos(X_AXIS,0);
            AxisEnableSoftwarelmt(X_AXIS,PARAM_XMAX_LENGTH,0);
            XRunResetOStep = 11; 
            ReSetXDelay = dwTickCount + 50;
        }
        else if(dwTickCount >= ReSetXOUT_TIME)   //多长时间没回到位警告
        {            
            XRunResetOStep = 0;
            bXRsting=0;
            SetAlarmCode(ALARM_ADDR,X_ORIGIN_ALARM);//机械手回零故障
            return 0;
        }
        break;
     case 11:
      if(dwTickCount>=ReSetXDelay)
      {        
        XRunResetOStep = 12; 
      }
      break;  
    case 12:
      if(!X_DRV)
      {
        X_Reset_Flag=0; 
        XRunResetOStep = 0; 
        bXRsting=0;
      }
      break;  
    }
  return 0;
}
//Y轴回零
static int Y_ReSetOrigin(void)
{
    switch(YRunResetOStep)
    {
    case 1:
      AxisDisableSoftwarelmt(Y_AXIS);
      if(dwTickCount >= ReSetYDelay)
      {
        if(Y_Reset_Flag)
         {            
            ReSetYOUT_TIME = dwTickCount + 15000;
            if(!DDDJ_Y_ORIGIN_IN)
            {
                YRunResetOStep = 2;               
                AxisContinueMoveAcc(Y_AXIS,PARAM_YREST_SPEED,DIR_N,
                                  1000,1000,20,20);
            }
            else
            {
                AxisContinueMoveAcc(Y_AXIS,PARAM_YREST_SPEED/2,DIR_P,1000,1000,20,20);
                YRunResetOStep = 4;                
            }
         }
         else
         {
            if(0-dwYRealPos)
                AxisMovePosAccDec(Y_AXIS,PARAM_YREST_SPEED,0-dwYRealPos,1000,1000,50,50,0); 
            YRunResetOStep = 10; 
            ReSetYOUT_TIME = dwTickCount + 10000;
         }
      }
        break;
    case 2:
        if(DDDJ_Y_ORIGIN_IN)
        {
            AxisDecStop(Y_AXIS);                
            YRunResetOStep = 3;
            ReSetYDelay = dwTickCount + 10;
        }
        break;
    case 3:
        if(!Y_DRV&&dwTickCount >= ReSetYDelay)
        {
            AxisContinueMoveAcc(Y_AXIS,1000,DIR_P,1000,1000,20,20);               
            YRunResetOStep = 4;                
        }
        break;
    case 4:
        if(!DDDJ_Y_ORIGIN_IN)
        {                               
            YRunResetOStep = 5;
            ReSetYDelay = dwTickCount + 50;
        }
        break;
    case 5:
        if(!DDDJ_Y_ORIGIN_IN&&dwTickCount >= ReSetYDelay)
        {                               
            AxisEgmStop(Y_AXIS); 
            YRunResetOStep = 6;
            ReSetYDelay = dwTickCount + 10;
        }
        break;
    case 6:
        if(!Y_DRV&&dwTickCount >= ReSetYDelay)
        {                               
            YRunResetOStep = 7;
            AxisContinueMoveAcc(Y_AXIS,800,DIR_N,800,800,20,20);                 
        }
        break;
    case 7:
        if(DDDJ_Y_ORIGIN_IN)
        {                               
            AxisEgmStop(Y_AXIS); 
            YRunResetOStep = 9;
            ReSetYDelay = dwTickCount + 20;
        }
        break;
    case 9:
        if(!Y_DRV&&dwTickCount >= ReSetYDelay)
        {                               
            YRunResetOStep = 10;
            AxisMovePosAccDec(Y_AXIS,1000,-10,1000,1000,20,20,0);                 
        }
        break;
    case 10:
        if(!Y_DRV)
        {                
            SetPos(Y_AXIS,0);
            AxisEnableSoftwarelmt(Y_AXIS,PARAM_YMAX_LENGTH,0);
            YRunResetOStep = 11; 
            ReSetYDelay = dwTickCount + 50;
        }
        else if(dwTickCount >= ReSetYOUT_TIME)   //多长时间没回到位警告
        {            
            YRunResetOStep = 0;
            bXRsting=0;
            SetAlarmCode(ALARM_ADDR,Y_ORIGIN_ALARM);//机械手回零故障
            return 0;
        }
        break;
     case 11:
      if(dwTickCount>=ReSetYDelay)
      {        
        YRunResetOStep = 12; 
      }
      break;  
    case 12:
      if(!Y_DRV)
      {
        Y_Reset_Flag=0; 
        YRunResetOStep = 0; 
        bYRsting=0;
      }
      break;  
    }
  return 0;
}
//Z轴回零
static int Z_ReSetOrigin(void)
{
    switch(ZRunResetOStep)
    {
    case 1:
      AxisDisableSoftwarelmt(Z_AXIS);
      if(dwTickCount >= ReSetZDelay)
      {
        if(Z_Reset_Flag)
         {
            AxisDisableSoftwarelmt(Z_AXIS);
            ReSetZOUT_TIME = dwTickCount + 15000;
            if(!DDDJ_Z_ORIGIN_IN)
            {
                ZRunResetOStep = 2;               
                AxisContinueMoveAcc(Z_AXIS,PARAM_ZREST_SPEED,DIR_N,
                                  1000,1000,20,20);
            }
            else
            {
                AxisContinueMoveAcc(Z_AXIS,PARAM_ZREST_SPEED/2,DIR_P,1000,1000,20,20);
                ZRunResetOStep = 4;                
            }
         }
         else
         {
            if(0-dwZRealPos)
                AxisMovePosAccDec(Z_AXIS,PARAM_ZREST_SPEED,0-dwZRealPos,1000,1000,50,50,0); 
            ZRunResetOStep = 10; 
            ReSetZOUT_TIME = dwTickCount + 10000;
         }
      }
        break;
    case 2:
        if(DDDJ_Z_ORIGIN_IN)
        {
            AxisDecStop(Z_AXIS);                
            ZRunResetOStep = 3;
            ReSetZDelay = dwTickCount + 10;
        }
        break;
    case 3:
        if(!Z_DRV&&dwTickCount >= ReSetZDelay)
        {
            AxisContinueMoveAcc(Z_AXIS,1000,DIR_P,1000,1000,20,20);               
            ZRunResetOStep = 4;                
        }
        break;
    case 4:
        if(!DDDJ_Z_ORIGIN_IN)
        {                               
            ZRunResetOStep = 5;
            ReSetZDelay = dwTickCount + 50;
        }
        break;
    case 5:
        if(!DDDJ_Z_ORIGIN_IN&&dwTickCount >= ReSetZDelay)
        {                               
            AxisEgmStop(Z_AXIS); 
            ZRunResetOStep = 6;
            ReSetZDelay = dwTickCount + 10;
        }
        break;
    case 6:
        if(!Z_DRV&&dwTickCount >= ReSetZDelay)
        {                               
            ZRunResetOStep = 7;
            AxisContinueMoveAcc(Z_AXIS,800,DIR_N,800,800,20,20);                 
        }
        break;
    case 7:
        if(DDDJ_Z_ORIGIN_IN)
        {                               
            AxisEgmStop(Z_AXIS); 
            ZRunResetOStep = 9;
            ReSetZDelay = dwTickCount + 20;
        }
        break;
    case 9:
        if(!Z_DRV&&dwTickCount >= ReSetZDelay)
        {                               
            ZRunResetOStep = 10;
            AxisMovePosAccDec(Z_AXIS,1000,-10,1000,1000,20,20,0);                 
        }
        break;
    case 10:
        if(!Z_DRV)
        {                
            SetPos(Z_AXIS,0);
            AxisEnableSoftwarelmt(Z_AXIS,PARAM_ZMAX_LENGTH,0);
            ZRunResetOStep = 11; 
            ReSetZDelay = dwTickCount + 50;
        }
        else if(dwTickCount >= ReSetZOUT_TIME)   //多长时间没回到位警告
        {            
            ZRunResetOStep = 0;
            bZRsting=0;
            SetAlarmCode(ALARM_ADDR,Z_ORIGIN_ALARM);//机械手回零故障
            return 0;
        }
        break;
     case 11:
      if(dwTickCount>=ReSetZDelay)
      {        
        ZRunResetOStep = 12; 
      }
      break;  
    case 12:
      if(!Z_DRV)
      {
        Z_Reset_Flag=0; 
        ZRunResetOStep = 0; 
        bZRsting=0;
      }
      break;  
    }
  return 0;
}

//手动动作
void DDDJ_ManualAction(void)
{ 
    //清除订单产量
    if(bClearTotal)
    {
      bClearTotal=0;
      ClrcToTal(TOTAL_ADDR);
    }
    //清除当前匝数
    if(bClerNowTotal)
    {
      bClerNowTotal=0;
      ClrcToTal(NOWTOTAL_ADDR);
    }
    //清除总产量
    if(bClearAllTOTAL)
    {
      bClearAllTOTAL=0;
      ClrcToTal(ALL_TOTAL_ADDR);
    }
    //清除当班产量
    if(bClearDateTOTAL)
    {
      bClearDateTOTAL=0;
      ClrcToTal(DATE_TOTAL_ADDR);
    }
   
    if(bRunning == 0)  
    {

      //分料
      if(DDDJ_bM62)
      {
        DDDJ_bM62=0;
        DDDJ_TTP_VAVLE=~DDDJ_TTP_VAVLE;
      }
      //放料
      if(DDDJ_bM63)
      {
        DDDJ_bM63=0;
        DDDJ_JTP_VAVLE=~DDDJ_JTP_VAVLE;
      }
      //顶料
      if(DDDJ_bM64)
      {
        DDDJ_bM64=0;
        DDDJ_TPXZ_VAVLE=~DDDJ_TPXZ_VAVLE;
      }      
      //顶料
      if(DDDJ_bM65)
      {
        DDDJ_bM65=0;
        DDDJ_TPSJ_VAVLE=~DDDJ_TPSJ_VAVLE;
      }
            //顶料
      if(DDDJ_bM66)
      {
        DDDJ_bM66=0;
        DDDJ_DTP_VAVLE=~DDDJ_DTP_VAVLE;
      }      
      //顶料
      if(DDDJ_bM67)
      {
        DDDJ_bM67=0;
        DDDJ_ZJ_VAVLE=~DDDJ_ZJ_VAVLE;
      }
            //顶料
      if(DDDJ_bM68)
      {
        DDDJ_bM68=0;
        DDDJ_YDD_VAVLE=~DDDJ_YDD_VAVLE;
      }      
      //顶料
      if(DDDJ_bM69)
      {
        DDDJ_bM69=0;
        DDDJ_JDD_VAVLE=~DDDJ_JDD_VAVLE;
      }
      //插钩
      if(bXMotor_N)
      {  
        //DDDJ_bXMotor_N=0;
        //SetEn(X_AXIS, MOTOR_EN); 
        if(!X_DRV&&!X_SOFT_NLIMIT)
            AxisContinueMoveAcc(X_AXIS,2000,DIR_N,1000,1000,15,15); 
          //AxisMovePosAccDec(X_AXIS,1000,-200,1000,1000,120,120,10); 
      }
      if(bXMotor_P)
      {
          //bXMotor_P=0;
        //SetEn(X_AXIS, MOTOR_EN); 
        if(!X_DRV&&!X_SOFT_PLIMIT)
            AxisContinueMoveAcc(X_AXIS,2000,DIR_P,1000,1000,15,15);
            //AxisMovePosAccDec(X_AXIS,1000,2000,1000,1000,120,120,10); 
      }
      if(!bXMotor_N&&!bXMotor_P&&!bXRsting&&!DDDJ_All_RESTING)
      {
         AxisEgmStop(X_AXIS);
      }
      //下料
      if(bYMotor_N)
      {  
        //bYMotor_N=0;
        SetEn(Y_AXIS, MOTOR_EN); 
        if(!Y_DRV&&!Y_SOFT_NLIMIT)
            AxisContinueMoveAcc(Y_AXIS,2000,DIR_N,1000,1000,15,15); 
          //AxisMovePosAccDec(Y_AXIS,1000,-2000,1000,1000,120,120,10); 
      }
      if(bYMotor_P)
      {
        //bYMotor_P=0;
        SetEn(Y_AXIS, MOTOR_EN); 
        if(!Y_DRV&&!Y_SOFT_PLIMIT)
            AxisContinueMoveAcc(Y_AXIS,2000,DIR_P,1000,1000,15,15);
            //AxisMovePosAccDec(Y_AXIS,1000,2000,1000,1000,120,120,10); 
      }
      if(!bYMotor_N&&!bYMotor_P&&!bYRsting&&!DDDJ_All_RESTING)
      {
         AxisEgmStop(Y_AXIS);
      }
      //推料前后
      if(bZMotor_N)
      {           
        SetEn(Z_AXIS, MOTOR_EN); 
        if(!Z_DRV&&!Z_SOFT_NLIMIT)
            AxisContinueMoveAcc(Z_AXIS,2000,DIR_N,1000,1000,15,15);
        
      }
      if(bZMotor_P)
      {
        //DDDJ_bZMotor_P=0;
        SetEn(Z_AXIS, MOTOR_EN); 
        if(!Z_DRV&&!Z_SOFT_PLIMIT)
            AxisContinueMoveAcc(Z_AXIS,2000,DIR_P,1000,1000,15,15); 
            //AxisMovePosAccDec(Z_AXIS,1000,200,1000,1000,120,120,10); 
      }
      if(!bZMotor_N&&!bZMotor_P&&!bZRsting&&!DDDJ_All_RESTING)
      {
         AxisEgmStop(Z_AXIS);
      }
      //X复位
      if(bXREST)
      {
        bXREST=0;
        bXRsting=1;
        XRunResetOStep=1;
        X_Reset_Flag=1;
        ReSetXDelay = dwTickCount + 200;
      }
      //Y复位
      if(bYREST)
      {
        bYREST=0;
        bYRsting=1;
        YRunResetOStep=1;
        Y_Reset_Flag=1;
        ReSetYDelay = dwTickCount + 200;
      }
      //Z复位
      if(bZREST)
      {
        bZREST=0;
        bZRsting=1;
        ZRunResetOStep=1;
        Z_Reset_Flag=1;
        ReSetZDelay = dwTickCount + 200;
      }
      if(DDDJ_All_REST)
      {
        DDDJ_All_REST=0;
        DDDJ_All_RESTING=1;
        XRunResetOStep=1;
        YRunResetOStep=1;

      }
      if(DDDJ_All_RESTING)
      {
        if(XRunResetOStep==0&&YRunResetOStep==0&&ZRunResetOStep==0)
        {
          DDDJ_All_RESTING=0;
          ALL_Reset_Flag=0;
        }
      }

      //收线旋转
      if(DDDJ_bXZ_SWC)
      {
        DDDJ_bXZ_SWC=0;
        if(!DDDJ_bXZ_SWO)
        {
          DDDJ_bXZ_SWO=1;
          DDDJ_LSLSN_OUT=1;
          DDDJ_LSLQH_OUT=0;
        }
        else
        {
          DDDJ_bXZ_SWO=0;
          DDDJ_LSLSN_OUT=0;
          DDDJ_LSLQH_OUT=0;
        }
      }      
    }
}
//插钩动作
void DDDJ_cXMotor_Action(void)	 //
{ 
  
#if 1 
    long hand_local;
    switch(DDDJ_cXMOTORStep)
    {
    case 1://待机位置
      DDDJ_cXMOTORStep=2;
      DDDJ_WorkCnt++;
      hand_local=0;
      hand_local=hand_local-dwXRealPos;  
      //if(DDDJ_WorkCnt>=DDDJ_PARAM_CMP_SPACE)
      //{
     //   DDDJ_WorkCnt=0;
     //   hand_local=hand_local+;
      //} 
      AxisMovePosAccDec(X_AXIS,DDDJ_PARAM_QL_HSPEED,hand_local,1000,1000,120,120,50);       
      break;
    case 2:
      if(!X_DRV)
      {
        DDDJ_cXMOTORStep=3;
        DDDJ_cXMOTORDelay=dwTickCount+30;
      }
      break; 
    case 3:
      if(dwTickCount>=DDDJ_cXMOTORDelay)
      {
        DDDJ_cXMOTORStep=0;
      }
      break;   
      
    case 10://待机位置
      if(DDDJ_Fristwork==0)
      {
          AxisContinueMoveAcc(X_AXIS,DDDJ_PARAM_FRIST_HSPEED,DIR_P,1000,1000,50,50);
      }
      else
      {
        hand_local=DDDJ_CHECK_GY_LENGTH*2-DDDJ_PARAM_DW_LENGTH;
        //hand_local=hand_local-dwXRealPos;  
        AxisMovePosAccDecNotStop(X_AXIS,DDDJ_PARAM_QL_HSPEED,hand_local,1000,DDDJ_PARAM_FRIST_HSPEED,150,50,PosToPulse(X_AXIS,100)); 
      }
       DDDJ_cXMOTORStep=11;
      break;
    case 11:
      if(DDDJ_DT_GYING_IN_DW)
      {
        DDDJ_XSAVEPOS=dwXRealPos;
        DDDJ_cXMOTORStep=12;
        
        DDDJ_cXMOTORDelay=dwTickCount+50;
      } 
      break; 
    case 12:
      if(dwTickCount>=DDDJ_cXMOTORDelay)
      {
          if(DDDJ_DT_GYING_IN_DW)
          {   
            DDDJ_Fristwork=1;
            DDDJ_cXMOTORStep=13; 
            DDDJ_CHECK_GY_LENGTH=dwXRealPos-DDDJ_XSAVEPOS;
            hand_local=DDDJ_PARAM_DW_LENGTH;
            AxisMovePosAccDec(X_AXIS,DDDJ_PARAM_FRIST_HSPEED,hand_local,1000,1000,150,150,10); 
          } 
      }
      break;    
    case 13:
      if(!X_DRV)
      {
        //DDDJ_JS_VAVLE=1;
        DDDJ_cXMOTORStep=14;
        DDDJ_cXMOTORDelay=dwTickCount+DDDJ_PARAM_JS_H_TIME*10;
      } 
      break; 
    case 14://夹钩
      if(dwTickCount>=DDDJ_cXMOTORDelay)
      {                     
        DDDJ_cXMOTORStep=0;
      }
      break; 
    }
#endif
}
//取衣架放衣架动作
void DDDJ_cYMotor_Action(void)
{
    long hand_local;
    switch(DDDJ_cYMOTORStep)
    {
    case 1://判断是否松轴
      DDDJ_LSLSN_OUT=1;
      DDDJ_LSLQH_OUT=0;
      DDDJ_cYMOTORStep=2;
      hand_local=DDDJ_PARAM_START_LOCAL;
      hand_local=hand_local-dwYRealPos;  
      AxisMovePosAccDec(Y_AXIS,DDDJ_PARAM_JG_SPEED,hand_local,1000,1000,220,220,50);  
      break;
    case 2://取料位置
      if(!Y_DRV)
      {
        if(DDDJ_CW_TIME>0)
        {
           DDDJ_cYMOTORStep=3;
        }
        else if(DDDJ_CCW_TIME>0)
        {
           DDDJ_cYMOTORStep=19;
        }
        else
        {
          DDDJ_cYMOTORStep=6;
        }
      }
      break;
    case 3:
      if(DDDJ_CW_TIME>=DDDJ_PARAM_JG_NUM)
      {
          DDDJ_CW_TIME=0;
          DDDJ_cYMOTORStep=22;
      }
      else
      {
          DDDJ_cYMOTORStep=4;
      }
      break;  
    case 4:
      DDDJ_cYMOTORStep=5;
      hand_local=DDDJ_CW_TIME*DDDJ_PARAM_SXJG_LOCAL;
      AxisMovePosAccDec(Y_AXIS,DDDJ_PARAM_JG_SPEED,hand_local,1000,1000,220,220,50);       
      break; 
    case 5:
      if(!Y_DRV)
      {
        DDDJ_cYMOTORStep=6; 
      }
      break; 
     case 6:
      if(DDDJ_U_ORIGIN_IN_UP)
      {
          if(DDDJ_CW_TIME>=DDDJ_PARAM_JG_NUM)
          {
            DDDJ_CW_TIME=0;
            DDDJ_cYMOTORStep=22; 
          }
          else
          {
            DDDJ_cYMOTORStep=7;
            hand_local=DDDJ_PARAM_SXJG_LOCAL;
            AxisMovePosAccDec(Y_AXIS,DDDJ_PARAM_JG_SPEED,hand_local,2000,2000,250,250,100); 
          }
      }
      break; 
    case 7:
      if(!Y_DRV)
      {
         DDDJ_cYMOTORStep=8; 
         DDDJ_cYMOTORDelay=dwTickCount+500;
      }
      break;   
    case 8:
      if(dwTickCount>=DDDJ_cYMOTORDelay)
      {     
          DDDJ_CW_TIME++;
          DDDJ_cYMOTORStep=6; 
      }
      break;  

    case 19:
      if(DDDJ_CCW_TIME>=DDDJ_PARAM_JG_NUM)
      {
          DDDJ_CCW_TIME=0;
          DDDJ_cYMOTORStep=6;
      }
      else
      {
          DDDJ_cYMOTORStep=20;
      }
      break;          
    case 20:
      DDDJ_cYMOTORStep=21;
      hand_local=(DDDJ_PARAM_JG_NUM-DDDJ_CCW_TIME)*DDDJ_PARAM_SXJG_LOCAL;
      AxisMovePosAccDec(Y_AXIS,DDDJ_PARAM_JG_SPEED,hand_local,1000,1000,220,220,50);       
      break; 
    case 21:
      if(!Y_DRV)
      {
        DDDJ_cYMOTORStep=22; 
      }
      break; 
     case 22:
      if(DDDJ_U_ORIGIN_IN_UP)
      {
          if(DDDJ_CCW_TIME>=DDDJ_PARAM_JG_NUM)
          {
            DDDJ_CCW_TIME=0;
            DDDJ_cYMOTORStep=6; 
          }
          else
          {
            DDDJ_cYMOTORStep=23;
            hand_local=DDDJ_PARAM_SXJG_LOCAL;
            AxisMovePosAccDec(Y_AXIS,DDDJ_PARAM_JG_SPEED,-hand_local,2000,2000,250,250,100); 
          }
      }
      break; 
    case 23:
      if(!Y_DRV)
      {
         DDDJ_cYMOTORStep=24; 
         DDDJ_cYMOTORDelay=dwTickCount+500;
      }
      break;   
    case 24:
      if(dwTickCount>=DDDJ_cYMOTORDelay)
      {     
          DDDJ_CCW_TIME++;
          DDDJ_cYMOTORStep=22; 
      }
      break; 
    }
}

//自动动作
void DDDJ_AutoStepAction(void)
{ 

  dwXRealPos = GetPos(X_AXIS);
  dwYRealPos = GetPos(Y_AXIS);
  dwZRealPos = GetPos(Z_AXIS);

  X_SOFT_PLIMIT=GetSoftPLimit(X_AXIS);
  X_SOFT_NLIMIT=GetSoftNLimit(X_AXIS);
  Y_SOFT_PLIMIT=GetSoftPLimit(Y_AXIS);
  Y_SOFT_NLIMIT=GetSoftNLimit(Y_AXIS);
  Z_SOFT_PLIMIT=GetSoftPLimit(Z_AXIS);
  Z_SOFT_NLIMIT=GetSoftNLimit(Z_AXIS);
    
  user_datas[121] = DDDJ_AutoStep;
  user_datas[122] = DDDJ_cXMOTORStep;
  
  user_datas[123] = DDDJ_cYMOTORStep;
  user_datas[124] = DDDJ_CHECK_GY_LENGTH*2;//DDDJ_cZMOTORStep;
  user_datas[125] = DDDJ_WorkCnt;//DDDJ_LPDJStep;
  
  user_datas[126] = DDDJ_CW_TIME;
  user_datas[127] = DDDJ_CCW_TIME;//DDDJ_ZLTStep;
  user_datas[128] = DDDJ_PARAM_CMP_SPACE;
  
  *PARAM_X_LOCAL=dwXRealPos;
  *PARAM_Y_LOCAL=dwYRealPos;
  *PARAM_Z_LOCAL=dwZRealPos;
#if 1
  if(DDDJ_X_SALARM_IN)
  {
    DDDJ_SetAlarmCode(X_ORIGIN_ALARM);
  }
  else
  {
    if(GetAlarmCode(ALARM_ADDR)==X_ORIGIN_ALARM)SetAlarmCode(ALARM_ADDR,0);
  }
  switch(DDDJ_AutoStep)
  {
  case 1://注胶,夹手
    if(dwTickCount>=DDDJ_AutoDelay&&DDDJ_cXMOTORStep==0)       
    {
      if(bTestMode&&bStart||!bTestMode)
      {
        DDDJ_AutoStep=2;
        //DDDJ_JS_VAVLE=1;
        DDDJ_AutoDelay=dwTickCount+50;
      }
    }
    break;
  case 2://退档料，松压料
   if(dwTickCount>=DDDJ_AutoDelay)
    {
       //DDDJ_ZJ_VAVLE=0;
       if(bTestMode&&bStart||!bTestMode)
       {
          if(DDDJ_cXMOTORStep==0)
          {
            DDDJ_AutoStep=3;
            //DDDJ_DL_VAVLE=1;
            //DDDJ_YL_VAVLE=1;
            DDDJ_AutoDelay=dwTickCount+DDDJ_PARAM_YL_H_TIME*10;
          }
       }
    }
    break;
  case 3://拉带
    if(dwTickCount>=DDDJ_AutoDelay)
    {
      if(bTestMode&&bStart||!bTestMode)
      {
        DDDJ_AutoStep=4;
        DDDJ_cXMOTORStep=10;
      }
    }
    break;   
  case 4://压料
    if(DDDJ_cXMOTORStep==0)
    {
       if(bTestMode&&bStart||!bTestMode)
       {
          DDDJ_AutoStep=5;
          //DDDJ_DL_VAVLE=0;
          //DDDJ_YL_VAVLE=0;
          DDDJ_AutoDelay=dwTickCount+DDDJ_PARAM_YL_R_TIME*10;
       }
      
    }
    break;
  case 5://松夹手
    if(dwTickCount>=DDDJ_AutoDelay)
    {
      if(bTestMode&&bStart||!bTestMode)
      {
        DDDJ_AutoStep=6;
        //DDDJ_JS_VAVLE=0;
        DDDJ_ZJ_VAVLE=1;
        DDDJ_ZJDelay=dwTickCount+DDDJ_PARAM_ZJ_TIME*10;
        DDDJ_AutoDelay=dwTickCount+DDDJ_PARAM_JS_R_TIME*10;
      }
    }
    break;    
  case 6://伺服回味
    if(dwTickCount>=DDDJ_AutoDelay)
    {
       if(bTestMode&&bStart||!bTestMode)
       {
          if(DDDJ_cXMOTORStep==0)
          {
            DDDJ_AutoStep=7;
            DDDJ_cXMOTORStep=1;           
            CalProSP(SPEED_ADDR);//计算生产速度   
            AddToTal(TOTAL_ADDR);//生产总量加1并保存
            AddToTal(NOWTOTAL_ADDR);//生产总量加1并保存
            AddToTal(ALL_TOTAL_ADDR);//累计生产总量
            AddToTal(DATE_TOTAL_ADDR);//当班生产总量  
          }
       }
    }    
    break;   
  case 7://完成等待下一步
    if(START_IN_UP||DDDJ_bAuto_TEST)
    {
       DDDJ_AutoStep=1;
       DDDJ_AutoDelay=dwTickCount+30;
       if(DDDJ_bAuto_TEST)
         DDDJ_AutoDelay=dwTickCount+100;
    }
    break;
  }
  if(bRunning&&DDDJ_ZJ_VAVLE&&dwTickCount>=DDDJ_ZJDelay)
  {
    DDDJ_ZJ_VAVLE=0;
  }

#endif  
}
void DDDJ_CheckStart(void)
{
#if 1  

    //启动  
    if((START_IN_UP||bStart||bOnceStart)&&!bRunning&&!EMGSTOP_IN)
    {
      if(ALL_Reset_Flag)
      {            
          SetAlarmCode(ALARM_ADDR,DDDJ_NO_RESET_ALARM);           
      }
      else if(GetAlarmCode(ALARM_ADDR)==0)
      {
         //
         bRunning=1;
         DDDJ_AutoStep=1;
         //Y轴回待机位
         DDDJ_cXMOTORStep=1;
         DDDJ_cYMOTORStep=1;
         //IO清零
         //DDDJ_JS_VAVLE=1;
         //
         DDDJ_Fristwork=0;
         //
         DDDJ_WorkCnt=0;
         DDDJ_AutoDelay=dwTickCount+200;
         if(bOnceStart)SingOneFlg=1; 

      }
      bStart=0;
    }
    //停止
    if(STOP_IN_UP||bStop||EMGSTOP_IN)
    {
        //DDDJ_STOP_NUM++;
        //if(DDDJ_STOP_NUM>2)DDDJ_STOP_NUM=0;
        /*if(bStop&&bRunning)
        {
          SingOneFlg=1;
          bStop=0;
        }
        else*/
        {  
          DDDJ_LSLSN_OUT=1;
          DDDJ_LSLQH_OUT=1;
          //轴停止步
          bRunning=0;

          bStop=0;
          AxisEgmStop(X_AXIS);
          AxisEgmStop(Y_AXIS);
          AxisEgmStop(Z_AXIS);
          //自动
          DDDJ_AutoStep=0;
          DDDJ_cXMOTORStep=0;
          DDDJ_cYMOTORStep=0;
          DDDJ_cZMOTORStep=0;
          //
          DDDJ_LPDJStep=0;
          //         
          DDDJ_ZLTStep=0;
           //
          DDDJ_cGPJRStep=0;
          //IO
          //DDDJ_JS_VAVLE=0; 
          //DDDJ_DL_VAVLE=0;
          //DDDJ_YL_VAVLE=0;
          //DDDJ_ZJ_VAVLE=0;         
          //复位
          bXRsting=0;
          XRunResetOStep=0;
          bYRsting=0;
          YRunResetOStep=0;
          bZRsting=0;
          ZRunResetOStep=0;          
        }          
    }
  
    //故障处理
    if(bAlarmStop)
    {
        bAlarmStop=0;
        
        AxisEgmStop(X_AXIS);
        AxisEgmStop(Y_AXIS);
        AxisEgmStop(Z_AXIS);
        //自动
        bRunning=0;
        DDDJ_AutoStep=0;        
        //自动
        DDDJ_AutoStep=0;
        DDDJ_cXMOTORStep=0;
        DDDJ_cYMOTORStep=0;
        DDDJ_cZMOTORStep=0;
        //
        DDDJ_LPDJStep=0;
        //         
        DDDJ_ZLTStep=0;
         //
        DDDJ_cGPJRStep=0;
    }
    /*
    //伺服报警标志
    if(((GetAlarm(Y_AXIS)==1)) || GetAlarm(X_AXIS)==1))
    {
    
      DDDJ_bAlarmStop=1;
      DDDJ_SERVO_ALAEM_FLAG=1;
      DDDJ_SetAlarmCode(DDDJ_HAND_SERVO_ALARM);
    }
    //清除伺服告警
    if(dwTickCount>=DDDJ_CLR_ALARM_Pulse&&DDDJ_CLR_SERVOALAEM_FLAG)
    {
       DDDJ_CLR_SERVOALAEM_FLAG=0;
       SetClr(X_AXIS, 0);
       SetClr(Y_AXIS, 0);
    }*/
#endif    
}
//初始化动作
void DDDJ_InitAction(void)
{
    float buff_pulse,buff_dist;    
    buff_pulse  = PARAM_XCYCLE_PULSE;
    buff_dist   = PARAM_XCYCLE_LENGTH;
    XGearRatio  = buff_pulse/buff_dist;
    
    buff_pulse  = PARAM_YCYCLE_PULSE;
    buff_dist   = PARAM_YCYCLE_LENGTH;
    YGearRatio  = buff_pulse/buff_dist;
    
    buff_pulse  = PARAM_ZCYCLE_PULSE;
    buff_dist   = PARAM_ZCYCLE_LENGTH;
    ZGearRatio  = buff_pulse/buff_dist;
    
   
    //SetEnReverse(X_AXIS, 0);
    //SetEnReverse(Y_AXIS, 0);
    //SetEn(X_AXIS, DDDJ_MOTOR_DISEN);
    //SetEn(Y_AXIS, DDDJ_MOTOR_DISEN);

    SetAlarmCode(ALARM_ADDR,DDDJ_NO_ALARM);
    SetDirReverse(X_AXIS, 1);
    SetDirReverse(Y_AXIS, 0);
    SetDirReverse(Z_AXIS, 0);

    //伺服报警常闭输入、0为正常，1为告警，
    //SetAlarmReverse(X_AXIS,1);
    //SetAlarmReverse(Y_AXIS,1);
    X_Reset_Flag=1;
    
    Y_Reset_Flag=1;
    //Z_Reset_Flag=1;
    
    ALL_Reset_Flag=1;
    //X轴上电复位
    if(PARAM_XPOWERON_RST)
    {
      bXREST=1;
      bYREST=1;
      //bZREST=1;
    }
    DDDJ_CW_TIME=0;
    DDDJ_CCW_TIME=0;
}

void DDDJ_Action(void)
{
    DDDJ_CheckStart();
    DDDJ_ManualAction();
      //回原点处理
    Y_ReSetOrigin();
    X_ReSetOrigin();
    Z_ReSetOrigin();
    //插衣钩
    DDDJ_cXMotor_Action();
    //出料    
    DDDJ_cYMotor_Action();    
    //自动动作
    DDDJ_AutoStepAction(); 

}
#endif