GC23005/Machine/HZFuChenMachine/HZFC_YiJiaGouHanjie.c

#include "global.h"

#if FU_CHEN_MACHINE == 1
//故障报警
void YJHJ_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(!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(X_ORIGIN_IN)
        {
            AxisDecStop(X_AXIS);                
            XRunResetOStep = 3;
            ReSetXDelay = dwTickCount + 10;
        }
        break;
    case 3:
        if(!X_DRV&&dwTickCount >= ReSetXDelay)
        {
            AxisContinueMoveAcc(X_AXIS,1000,DIR_P,1000,1000,20,20);               
            XRunResetOStep = 4;                
        }
        break;
    case 4:
        if(!X_ORIGIN_IN)
        {                               
            XRunResetOStep = 5;
            ReSetXDelay = dwTickCount + 50;
        }
        break;
    case 5:
        if(!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(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(!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(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(!Y_ORIGIN_IN)
        {                               
            YRunResetOStep = 5;
            ReSetYDelay = dwTickCount + 50;
        }
        break;
    case 5:
        if(!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(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(!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(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(!Z_ORIGIN_IN)
        {                               
            ZRunResetOStep = 5;
            ReSetZDelay = dwTickCount + 50;
        }
        break;
    case 5:
        if(!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(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 YJHJ_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(YJHJ_bLPDJ)
      {
        YJHJ_bLPDJ=0;
        YJHJ_LPMOTOR_OUT=~YJHJ_LPMOTOR_OUT;
      }
      //加热开关
      if(YJHJ_bHeat)
      {
        YJHJ_bHeat=0;
        YJHJ_HEAT_OUT=~YJHJ_HEAT_OUT;
      }
      //分料
      if(YJHJ_bFENGL)
      {
        YJHJ_bFENGL=0;
        YJHJ_FENL_VAVLE=~YJHJ_FENL_VAVLE;
      }
      //放料
      if(YJHJ_bFANGL)
      {
        YJHJ_bFANGL=0;
        YJHJ_FANGL_VAVLE=~YJHJ_FANGL_VAVLE;
      }
      //顶料
      if(YJHJ_bDINGL)
      {
        YJHJ_bDINGL=0;
        YJHJ_DINGL_VAVLE=~YJHJ_DINGL_VAVLE;
      }
      //分钩
      if(YJHJ_bFENG)
      {
        YJHJ_bFENG=0;
        YJHJ_FENGG_VAVLE=~YJHJ_FENGG_VAVLE;
      }
      //上档钩
      if(YJHJ_bSDG)
      {
        YJHJ_bSDG=0;
        YJHJ_SDG_VAVLE=~YJHJ_SDG_VAVLE;
      }
      //下档钩
      if(YJHJ_bXDG)
      {
        YJHJ_bXDG=0;
        YJHJ_XDG_VAVLE=~YJHJ_XDG_VAVLE;
      }
      //夹钩气缸
      if(YJHJ_bJG)
      {
        YJHJ_bJG=0;
        YJHJ_JG_VAVLE=~YJHJ_JG_VAVLE;
      }
      //顶钩气缸
      if(YJHJ_bDG)
      {
        YJHJ_bDG=0;
        YJHJ_DG_VAVLE=~YJHJ_DG_VAVLE;
      }
      //取料
      if(YJHJ_bQL)
      {
        YJHJ_bQL=0;
        YJHJ_QL_VAVLE=~YJHJ_QL_VAVLE;

      }
      //压料
      if(YJHJ_bYL)
      {
        YJHJ_bYL=0;
        YJHJ_YL_VAVLE=~YJHJ_YL_VAVLE;
      }
      //夹子
      if(YJHJ_bJZ)
      {
        YJHJ_bJZ=0;
        YJHJ_JZ_VAVLE=~YJHJ_JZ_VAVLE;
      }
      //插钩
      if(YJHJ_bCYG)
      {
        YJHJ_bCYG=0;
        YJHJ_CYG_VAVLE=~YJHJ_CYG_VAVLE;
      }
      //插钩
      if(bXMotor_N)
      {  
        //YJHJ_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)
      {
        //YJHJ_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,200,1000,1000,120,120,10); 
      }
      if(!bXMotor_N&&!bXMotor_P&&!bXRsting&&!YJHJ_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&&!YJHJ_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)
      {
        //YJHJ_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&&!YJHJ_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(YJHJ_All_REST)
      {
        if(ALL_Reset_Flag)
        {
          if(SERVO_VAVLE==0)bXREST=1;
          bYREST=1;
          bZREST=1;
        }
        else
        {
            ZRunResetOStep=1;
            YRunResetOStep=1;        
            if(SERVO_VAVLE==0)XRunResetOStep=1;
        }
        YJHJ_All_REST=0;
        YJHJ_All_RESTING=1;
        YJHJ_FANGL_VAVLE=0;
        YJHJ_XDG_VAVLE=1;
        YJHJ_JG_VAVLE=0;  
        YJHJ_CYG_VAVLE=0;
      }
      if(YJHJ_All_RESTING)
      {
         if(ALL_Reset_Flag)
         {
             if(bXREST==0&&bYREST==0&&bZREST==0)
            {
              YJHJ_All_RESTING=0;
              ALL_Reset_Flag=0;
            }
         }
         else
         {
            if(XRunResetOStep==0&&YRunResetOStep==0&&ZRunResetOStep==0)
            {
              YJHJ_All_RESTING=0;
              ALL_Reset_Flag=0;
            }
         }
      }
      //料盘启动
      if(YJHJ_bLPStart)
      {
        YJHJ_bLPStart=0;
        if(YJHJ_bLPRunning)
        {
          YJHJ_bLPRunning=0;
          YJHJ_LPDJStep=0; 
          YJHJ_LPMOTOR_OUT=0;
        }
        else
        {
          YJHJ_bLPRunning=1;
          YJHJ_LPDJStep=1;          
        }
      }
      
    }

}
//装料头
void YJHJ_ZLT_Action(void)
{
  switch(YJHJ_ZLTStep)
  {
  case 1:
    if(dwTickCount>=YJHJ_ZLTDelay)
    {
      //YJHJ_ZYJ_EN_FLAG=0;
      YJHJ_ZLTStep=2;
      YJHJ_FANGL_VAVLE=0;
      YJHJ_ZLTDelay=dwTickCount+YJHJ_PARAM_FangL_R_TIME*10;
    }
    break;
  case 2:
    if(dwTickCount>=YJHJ_ZLTDelay)
    {
      YJHJ_ZLTStep=3;
      YJHJ_FENL_VAVLE=0;
      YJHJ_ZLTDelay=dwTickCount+YJHJ_PARAM_FenL_R_TIME*10;
    }
    break;
  case 3:
    if(dwTickCount>=YJHJ_ZLTDelay)
    {
       YJHJ_ZLTStep=4;
       YJHJ_FENL_VAVLE=1;       
       YJHJ_ZLTDelay=dwTickCount+YJHJ_PARAM_FenL_H_TIME*10;
    }
    break;
   case 4:
    if(dwTickCount>=YJHJ_ZLTDelay)
    {
      //YJHJ_ZYJ_YL_FLAG=1;
      YJHJ_ZLTStep=0;
    }
    break;   
  }
}
//推衣架动作
void YJHJ_cZMotor_Action(void)
{
  long hand_local;
  static unsigned char run_z_once=0;
  switch(YJHJ_cZMOTORStep)
  {
  case 1://判断是否复位
    if(Z_Reset_Flag!=0)
    {
        ZRunResetOStep=1;
        YJHJ_cZMOTORStep=2;
        YJHJ_cZMOTORDelay = dwTickCount + 50;
        ReSetZDelay=dwTickCount + 200;
    }
    else
    {
        YJHJ_cZMOTORStep=2;
        YJHJ_cZMOTORDelay = dwTickCount + 50;
    }
    break;
 case 2://移动到接料位置
    if(Z_Reset_Flag==0&&ZRunResetOStep==0&&dwTickCount>=YJHJ_cZMOTORDelay)
    {
        YJHJ_cZMOTORStep=3;
        hand_local=YJHJ_PARAM_TLJL_LOCAL;
        hand_local=hand_local-dwZRealPos;         
        AxisMovePosAccDec(Z_AXIS,YJHJ_PARAM_TLDJ_HSPEED,hand_local,1000,1000,150,150,50);  
    }
    break;
  case 3://
    if(!Z_DRV)
    {
        YJHJ_cZMOTORStep=0;
    }
    break; 
 // 回到待机点
  case 10://待机点
    YJHJ_cZMOTORStep=11;
    hand_local=YJHJ_PARAM_TLJL_LOCAL;
    hand_local=hand_local-dwZRealPos;         
    AxisMovePosAccDec(Z_AXIS,YJHJ_PARAM_TLDJ_HSPEED,hand_local,1000,1000,120,120,50);  
    break;
  case 11://
    if(!Z_DRV)
    {
        YJHJ_cZMOTORStep=12;
        YJHJ_cZMOTORDelay = dwTickCount + 10; 
    }
    break;    
   case 12://放料
    if(dwTickCount>=YJHJ_cZMOTORDelay)
    {
      if(YJHJ_ZLTStep==0)
      { 
        
        YJHJ_cZMOTORStep=13;
        if(YJHJ_ONCE_FLAG==0)
        {          
          YJHJ_FANGL_VAVLE=1;
        }        
        YJHJ_cZMOTORDelay = dwTickCount + YJHJ_PARAM_FangL_H_TIME*10;
      }
    }
    break;
   case 13://推料到工作位置,同时装料
    if(dwTickCount>=YJHJ_cZMOTORDelay)
    {        
        YJHJ_cZMOTORStep=0;
        YJHJ_ZLTStep=1;//装衣架使能
        YJHJ_ZLTDelay=dwTickCount+50;
    }
    break;  
   case 20://推料到工作位置,同时装料      
    YJHJ_cZMOTORStep=21;
    hand_local=YJHJ_PARAM_TLDW_LOCAL;
    hand_local=hand_local-dwZRealPos;         
    AxisMovePosAccDec(Z_AXIS,YJHJ_PARAM_TLDJ_LSPEED,hand_local,1000,1000,220,220,50); 
    break;  
  case 21://
    if(!Z_DRV)
    {        
        YJHJ_cZMOTORStep=22;
        YJHJ_cZMOTORDelay = dwTickCount +5; 
    }
    break; 
   case 22://
    if(dwTickCount>=YJHJ_cZMOTORDelay)
    {
        YJHJ_cZMOTORStep=0;
    }
    break;
  }
}
//料盘动作
void YJHJ_LPDJ_Action(void)
{  
  switch(YJHJ_LPDJStep)
  {
  case 1:
    if(!YJHJ_YG_GANYING_IN)
    {
      YJHJ_LPDJStep=2;
      YJHJ_LPMOTOR_OUT=1;      
    }
    break;
  case 2:
    if(YJHJ_YG_GANYING_IN)
    {
      YJHJ_LPDJStep=3;
      YJHJ_LPDJDelay=dwTickCount+YJHJ_PARAM_YG_CHECK_TIME*10;    
    }
    break;  
  case 3:
    if(dwTickCount>=YJHJ_LPDJDelay)
    {
      if(YJHJ_YG_GANYING_IN)
      {
        YJHJ_LPDJStep=4;
        YJHJ_LPDJDelay=dwTickCount+YJHJ_PARAM_LP_STOP_TIME*10;
      }
      else
      {
        YJHJ_LPDJStep=2;
      }
    }
    break; 
  case 4:
    if(dwTickCount>=YJHJ_LPDJDelay)
    {
      YJHJ_LPMOTOR_OUT=0;
      YJHJ_LPDJStep=5; 
    }
    break;  
  case 5:
    if(!YJHJ_YG_GANYING_IN)
    {
      YJHJ_LPDJStep=6;
      YJHJ_LPDJDelay=dwTickCount+200;  
    }
    break;   
  case 6:
    if(dwTickCount>=YJHJ_LPDJDelay)
    {
        if(YJHJ_YG_GANYING_IN)
        {
          YJHJ_LPDJStep=5;           
        }
        else
        {
          YJHJ_LPDJStep=1; 
        }
    }
    break;
  }
}
//装衣钩动作
void YJHJ_ZYG_Action(void)
{
  switch(YJHJ_ZYGStep)
  {
  case 1:
    //if(YJHJ_YG_GANYING_IN&&)
    if(YJHJ_GP_HEAT_FLAG)
    {
      YJHJ_ZYGStep=2;
      YJHJ_SDG_VAVLE=0;    
      YJHJ_ZYGDelay=dwTickCount+YJHJ_PARAM_SDG_R_TIME*10; 
    }
    break;
  case 2:
    if(dwTickCount>=YJHJ_ZYGDelay)
    {
      YJHJ_ZYGStep=3;
      YJHJ_FENGG_VAVLE=0;
      YJHJ_ZYGDelay=dwTickCount+YJHJ_PARAM_FG_R_TIME*10; 
    }
    break;
  case 3:
    if(dwTickCount>=YJHJ_ZYGDelay)
    {
      YJHJ_ZYGStep=4;
      YJHJ_FENGG_VAVLE=1;
      YJHJ_ZYGDelay=dwTickCount+YJHJ_PARAM_FG_H_TIME*10; 
    }
    break;
  case 4:
    if(dwTickCount>=YJHJ_ZYGDelay&&YJHJ_ZG_EN_FLAG)
    {
      YJHJ_ZG_EN_FLAG=0;
      YJHJ_ZYGStep=5;
      YJHJ_SDG_VAVLE=1;
      YJHJ_ZYGDelay=dwTickCount+YJHJ_PARAM_SDG_H_TIME*10; 
    }
    break;
  case 5:
    if(dwTickCount>=YJHJ_ZYGDelay)
    { 
      YJHJ_XDG_YL_FLAG=1;
      YJHJ_ZYGStep=1;
    }
    break; 
  
  }
}

//插钩动作
void YJHJ_cXMotor_Action(void)	 //
{ 
  
#if 1 
    long hand_local;
    switch(YJHJ_cXMOTORStep)
    {
    case 1://判断是否松轴
      if(X_Reset_Flag!=0)
      {       
        XRunResetOStep=1;
        YJHJ_cXMOTORStep=2;
        ReSetXDelay = dwTickCount + 200;
        YJHJ_cXMOTORDelay=dwTickCount+50;
      } 
      else
      {
        YJHJ_cXMOTORStep=2;
        YJHJ_cXMOTORDelay = dwTickCount + 50;
      }
      break;
    case 2://待机位置
      if(XRunResetOStep==0&&X_Reset_Flag==0&&dwTickCount>=YJHJ_cXMOTORDelay)
      {
          YJHJ_cXMOTORStep=3;
          hand_local=YJHJ_PARAM_CGJG_LOCAL;
          hand_local=hand_local-dwXRealPos;  
          AxisMovePosAccDec(X_AXIS,YJHJ_PARAM_CGDJ_HSPEED,hand_local,1000,1000,120,120,50); 
      }
      break;
    case 3:
      if(!X_DRV)
      {
        YJHJ_cXMOTORStep=0;
      }
      break; 
      
    case 10://待机位置
      YJHJ_cXMOTORStep=11;
      if(SERVO_VAVLE==0)
      {
          hand_local=YJHJ_PARAM_CGJG_LOCAL;
          hand_local=hand_local-dwXRealPos;  
          AxisMovePosAccDec(X_AXIS,YJHJ_PARAM_CGDJ_HSPEED,hand_local,1000,1000,150,150,10);      
      }
      else
      {
          YJHJ_CYG_VAVLE=0;
          YJHJ_cXMOTORDelay=dwTickCount+YJHJ_VAVLE_ERROR_TIME;
      }
      break; 
    case 11:
      if(SERVO_VAVLE==0)
      {
        if(!X_DRV)
        {
          YJHJ_DG_VAVLE=0;
          YJHJ_cXMOTORStep=12;
          YJHJ_cXMOTORDelay=dwTickCount+30;
        }
      }
      else
      {
        if(YJHJ_CG_ORIGIN_IN)
        {
          YJHJ_DG_VAVLE=0;
          YJHJ_cXMOTORStep=12;
          YJHJ_cXMOTORDelay=dwTickCount+YJHJ_VAVLE_ERROR_TIME;
        }
        else if(dwTickCount>=YJHJ_cXMOTORDelay)
        {           
          YJHJ_SetAlarmCode(YJHJ_CYG_ORIGIN_ALARM);
        }
      }
      break;    
    case 12://下档钩
      if(YJHJ_JG_ORIGIN_IN)//判断衣钩感
      {
        if(YJHJ_XDG_YL_FLAG)
        {
          YJHJ_cXMOTORStep=13;
          YJHJ_XDG_VAVLE=1;
          YJHJ_XDG_YL_FLAG=0;
          YJHJ_DG_VAVLE=0;
          YJHJ_cXMOTORDelay=dwTickCount+YJHJ_PARAM_XDG_H_TIME*10;
        }
      }
      else if(dwTickCount>=YJHJ_cXMOTORDelay)
      {
        if(!YJHJ_JG_ORIGIN_IN)
          YJHJ_SetAlarmCode(YJHJ_JYG_ORIGIN_ALARM);
      }
      break;  
    case 13://夹钩
      if(dwTickCount>=YJHJ_cXMOTORDelay)
      {                     
        //发送标志位
        YJHJ_XDG_VAVLE=0;  
        YJHJ_ZG_EN_FLAG=1;
        YJHJ_cXMOTORStep=14;
        YJHJ_ZYGDelay=dwTickCount+YJHJ_PARAM_XDG_R_TIME*10; 
        YJHJ_cXMOTORDelay=dwTickCount+YJHJ_PARAM_JiaoG_H_TIME*10; 
      }
      break; 
    case 14:
      if(dwTickCount>=YJHJ_cXMOTORDelay)//
      {
         YJHJ_cXMOTORStep=0;        
         YJHJ_JG_VAVLE=1;
          
      }
      break;    
    case 20:
        YJHJ_cXMOTORStep=21;        
        if(SERVO_VAVLE==0)
        {
          hand_local=YJHJ_PARAM_CGDW_LOCAL;
          hand_local=hand_local-dwXRealPos;  
          AxisMovePosAccDec(X_AXIS,YJHJ_PARAM_CGDJ_HSPEED,hand_local,1000,YJHJ_PARAM_CGDJ_LSPEED,150,250,
                          PosToPulse(X_AXIS,YJHJ_PARAM_CGDJ_LOCAL)); 
        }
        else
        {
          YJHJ_CYG_VAVLE=1;
          YJHJ_cXMOTORDelay=dwTickCount+YJHJ_PARAM_CG_H_TIME*10;
        }
      break; 
    case 21:
      if(SERVO_VAVLE==0)
      {
        if(!X_DRV)
        {
          YJHJ_cXMOTORStep=22;
          YJHJ_cXMOTORDelay=dwTickCount+30;
        }
      }
      else
      {
         YJHJ_cXMOTORStep=22;
      }
      break; 
    case 22:
      if(dwTickCount>=YJHJ_cXMOTORDelay)
      {
        YJHJ_cXMOTORStep=23;
        YJHJ_DG_VAVLE=1;
        YJHJ_JG_VAVLE=0;
        YJHJ_cXMOTORDelay=dwTickCount+YJHJ_PARAM_DingG_H_TIME*10;
      }
      break; 
    case 23:
      if(dwTickCount>=YJHJ_cXMOTORDelay)
      {
         YJHJ_cXMOTORStep=10;
         YJHJ_CG_WC_FLAG=1;
      }
    }
#endif
}
//取衣架放衣架动作
void YJHJ_cYMotor_Action(void)
{
    long hand_local;
    switch(YJHJ_cYMOTORStep)
    {
    case 1://判断是否松轴
      if(Y_Reset_Flag!=0)
      {       
        YRunResetOStep=1;
        YJHJ_cYMOTORStep=2;
        ReSetYDelay = dwTickCount + 200;
        YJHJ_cYMOTORDelay=dwTickCount+50;
      } 
      else
      {
        YJHJ_cYMOTORStep=2;
        YJHJ_cYMOTORDelay = dwTickCount + 100;
      }
      break;
    case 2://取料位置
      if(YRunResetOStep==0&&Y_Reset_Flag==0&&dwTickCount>=YJHJ_cYMOTORDelay)
      {
        if(YJHJ_XLJ_ORIGIN_IN)
        {
          YJHJ_cYMOTORStep=3;
          hand_local=YJHJ_PARAM_QYJ_LOCAL;
          hand_local=hand_local-dwYRealPos;  
          AxisMovePosAccDec(Y_AXIS,YJHJ_PARAM_XLDJ_HSPEED/2,hand_local,1000,1000,220,220,50); 
        }
        else
        {
            YJHJ_SetAlarmCode(YJHJ_XLJ_ORIGIN_ALARM);
        }
      }
      break;
    case 3:
      if(!Y_DRV)
      {
        YJHJ_cYMOTORStep=0;
      }
      break;     

    case 10:
      if(YJHJ_XLJ_ORIGIN_IN)
      {
        YJHJ_cYMOTORStep=11;
        hand_local=YJHJ_PARAM_FYJ_LOCAL;
        hand_local=hand_local-dwYRealPos;  
        AxisMovePosAccDec(Y_AXIS,YJHJ_PARAM_XLDJ_HSPEED,hand_local,2000,2000,250,250,100); 
      }
      else if(dwTickCount>=YJHJ_cYMOTORDelay)
      {
          YJHJ_SetAlarmCode(YJHJ_XLJ_ORIGIN_ALARM);
      }
      break;    
    case 11:
      if(!Y_DRV)
      {
        YJHJ_cYMOTORStep=12;
        YJHJ_cYMOTORDelay=dwTickCount+30;
      }
      break; 
   case 12:
      if(dwTickCount>=YJHJ_cYMOTORDelay)
      {     
        YJHJ_JZ_VAVLE=0;        
        YJHJ_cYMOTORStep=13;
        YJHJ_cYMOTORDelay=dwTickCount+YJHJ_PARAM_JZ_R_TIME*10;  
      }
      break;
    case 13://夹钩
      if(dwTickCount>=YJHJ_cYMOTORDelay)
      {
          YJHJ_cYMOTORStep=14;
          hand_local=YJHJ_PARAM_QYJ_LOCAL;
          hand_local=hand_local-dwYRealPos;  
          AxisMovePosAccDec(Y_AXIS,YJHJ_PARAM_XLDJ_HSPEED,hand_local,2000,2000,250,250,50); 
      }
      break; 
    case 14:
      if(!Y_DRV)
      {
        YJHJ_cYMOTORStep=15;
        YJHJ_cYMOTORDelay=dwTickCount+30;
      }
      break; 
    case 15:
      if(dwTickCount>=YJHJ_cYMOTORDelay)
      {     
        YJHJ_cYMOTORStep=0;
      }
      break;    
    //下取料，夹  
    case 20:
      YJHJ_QL_VAVLE=1;
      YJHJ_cYMOTORStep=21;
      YJHJ_cYMOTORDelay=dwTickCount+YJHJ_PARAM_QUL_H_TIME*10;      
      break;
    case 21:
      if(dwTickCount>=YJHJ_cYMOTORDelay)
      {
        YJHJ_JZ_VAVLE=1;
        YJHJ_cYMOTORStep=22;
        YJHJ_cYMOTORDelay=dwTickCount+YJHJ_PARAM_JZ_H_TIME*10;
      }      
      break; 
    case 22:
      if(dwTickCount>=YJHJ_cYMOTORDelay)
      {
        YJHJ_cYMOTORStep=0;
      }      
      break;    
    }
}
//高频加热动作
void YJHJ_GPJR_Action(void)
{
  switch(YJHJ_cGPJRStep)
  {
  case 1:
    if(YJHJ_WorkCnt==0)
    {
      //if(YJHJ_YG_GANYING_IN)
      {
        YJHJ_cGPJRStep=2;
        YJHJ_GP_HEAT_FLAG=0;
        YJHJ_HEAT_OUT=1;
        YJHJ_cGPJRDelay=dwTickCount+YJHJ_PARAM_GPJR_TIME*10;
      }
    }
    break;
  case 2:
    if(dwTickCount>=YJHJ_cGPJRDelay)
    {
      YJHJ_cGPJRStep=1;
      YJHJ_HEAT_OUT=0;
      YJHJ_GP_HEAT_FLAG=1;
      if(YJHJ_PARAM_GPJR_SPACE_MUN==0)YJHJ_PARAM_GPJR_SPACE_MUN=1;
      YJHJ_WorkCnt=YJHJ_PARAM_GPJR_SPACE_MUN;
    }
    break; 
  }
}
//自动动作
void YJHJ_AutoStepAction(void)
{ 
  long hand_local;
  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] = YJHJ_AutoStep;
  user_datas[122] = YJHJ_cXMOTORStep;
  
  user_datas[123] = YJHJ_cYMOTORStep;
  user_datas[124] = YJHJ_cZMOTORStep;
  user_datas[125] = YJHJ_LPDJStep;
  
  user_datas[126] = YJHJ_cGPJRStep;
  user_datas[127] = YJHJ_ZLTStep;
  user_datas[128] = YJHJ_PARAM_XLDJ_HSPEED;//YJHJ_ZYGStep; 
  
  *PARAM_X_LOCAL=dwXRealPos;
  *PARAM_Y_LOCAL=dwYRealPos;
  *PARAM_Z_LOCAL=dwZRealPos;
#if 1
  if(YJHJ_XL_SALARM_IN)
  {
    YJHJ_SetAlarmCode(Y_ORIGIN_ALARM);
  }
  else
  {
    if(GetAlarmCode(ALARM_ADDR)==Y_ORIGIN_ALARM)SetAlarmCode(ALARM_ADDR,0);
  }
  if(YJHJ_TL_SALARM_IN)
  {
    YJHJ_SetAlarmCode(Z_ORIGIN_ALARM);
  }
  else
  {
    if(GetAlarmCode(ALARM_ADDR)==Z_ORIGIN_ALARM)SetAlarmCode(ALARM_ADDR,0);
  }
 /* if(YJHJ_WLSTOP_FLAG&&dwTickCount>=YJHJ_cWLJCDelay)
  {
     YJHJ_WLSTOP_FLAG=0;
     YJHJ_SetAlarmCode(YJHJ_NO_WLSTOP_ALARM);
  }*/
  switch(YJHJ_AutoStep)
  {
  case 1://料盘启动，各种回待机位置
    if(dwTickCount>=YJHJ_AutoDelay&&YJHJ_cXMOTORStep==0&&
       YJHJ_cYMOTORStep==0&&YJHJ_cZMOTORStep==0)
    {
      if(bTestMode&&bStart||!bTestMode)
      {
        YJHJ_AutoStep=2;
        YJHJ_cGPJRStep=1;//加热   
        YJHJ_GP_HEAT_FLAG=0;
      }
    }
    break;
  case 2://判断衣钩感
    if(YJHJ_YG_GANYING_IN&&YJHJ_GP_HEAT_FLAG==1)
    {
      YJHJ_ZG_EN_FLAG=1;
      YJHJ_AutoStep=3;
    }
    break;
  case 3://加热完成
    if(YJHJ_GP_HEAT_FLAG==1)
    {
      if(bTestMode&&bStart||!bTestMode)
      {
        YJHJ_CG_WC_FLAG=0;
        YJHJ_cXMOTORStep=12;//推钩
        YJHJ_cXMOTORDelay=dwTickCount+YJHJ_VAVLE_ERROR_TIME;
        YJHJ_cZMOTORStep=12;//推衣架动作        
        YJHJ_AutoStep=40;
      }
    }
    break;
   case 40://衣架推到位，顶料
    if(YJHJ_cZMOTORStep==0)
    {      
       YJHJ_cZMOTORStep=20;
       YJHJ_AutoStep=4;
    }
    break;    
  case 4://衣架推到位，顶料
    if(YJHJ_cZMOTORStep==0)
    {
      if(bTestMode&&bStart||!bTestMode)
      {
          YJHJ_AutoStep=5;
          YJHJ_DINGL_VAVLE=1;
          YJHJ_AutoDelay=dwTickCount+YJHJ_PARAM_DINGL_H_TIME*10;
          
         if(YJHJ_YG_GANYING_IN)
         {
           YJHJ_WLSTOP_FLAG=1;
           YJHJ_cWLJCDelay=dwTickCount+YJHJ_PARAM_WL_STOP_TIME*10;
         }
      }
    }
    break; 
  case 5://压料
    if(dwTickCount>=YJHJ_AutoDelay)
    {
      if(bTestMode&&bStart||!bTestMode)
      {
        YJHJ_AutoStep=60;
        YJHJ_YL_VAVLE=1;
        YJHJ_cZMOTORStep =10; //回到待机位置
        if(YJHJ_ONCE_FLAG==1)
        {
            YJHJ_AutoDelay=dwTickCount+200;
        }
        else
           YJHJ_AutoDelay=dwTickCount+YJHJ_VAVLE_ERROR_TIME;
      }
    }
    break; 
  case 60://判断是否有料
      if(YJHJ_YL_LIMIT_IN)
      {
         YJHJ_AutoStep=6;
        YJHJ_AutoDelay=dwTickCount+30;
      }
      else if(YJHJ_ONCE_FLAG==1)
      {
        if(dwTickCount>=YJHJ_AutoDelay)
        {
            YJHJ_cZMOTORStep=12;//推衣架动作        
            YJHJ_AutoStep=40;
            YJHJ_ONCE_FLAG=0;
            YJHJ_YL_VAVLE=0;
            YJHJ_DINGL_VAVLE=0;
        }
      }
      else if(dwTickCount>=YJHJ_AutoDelay)
      {
          YJHJ_AutoStep=100;            
          SetAlarmCode(ALARM_ADDR,YJHJ_YL_ORIGIN_ALARM);
          YJHJ_AutoDelay=dwTickCount+500;
          //YJHJ_SetAlarmCode(YJHJ_YL_ORIGIN_ALARM);//无料告警停机

      }
    break; 
  case 6://判断是否有料
    if(dwTickCount>=YJHJ_AutoDelay)
    {
      if(YJHJ_YL_LIMIT_IN)//(dwTickCount>=YJHJ_AutoDelay)//
      {
        if(YJHJ_cXMOTORStep==0)  
        {
          YJHJ_ONCE_FLAG=0;
          YJHJ_AutoStep=7;
          YJHJ_cXMOTORStep=20; //插钩          
        }
      }
      else
      {
        YJHJ_AutoStep=60;
        YJHJ_AutoDelay=dwTickCount+YJHJ_VAVLE_ERROR_TIME;
      }
    }
    break;    
  case 7://有料，插钩，取料下
    if(bTestMode&&bStart||!bTestMode)
    {  
       if(YJHJ_cYMOTORStep==0)
       {
          YJHJ_cYMOTORStep=20; //取料下          
          YJHJ_AutoStep=8;
       }
    }    
    break;   
  case 8://插钩完成，取料下完，松开夹钩，顶料下；
    if(YJHJ_cYMOTORStep==0)
    {
      if(bTestMode&&bStart||!bTestMode)
      {
        if(YJHJ_CG_WC_FLAG)
        {
          YJHJ_CG_WC_FLAG=0;
          YJHJ_AutoStep=9;
          YJHJ_AutoDelay=dwTickCount+10;
        }          
      }
    }
    break;
  case 9://松开顶钩，松开压料，
    if(dwTickCount>=YJHJ_AutoDelay)
    {
         YJHJ_AutoStep=10;         
         YJHJ_YL_VAVLE=0;
         YJHJ_AutoDelay=dwTickCount+10;         
    }
    break;   
  case 10://
     if(dwTickCount>=YJHJ_AutoDelay)
    {
        YJHJ_AutoStep=11;
        YJHJ_QL_VAVLE=0;
        YJHJ_DINGL_VAVLE=0;
        YJHJ_cYMOTORStep=10;
        YJHJ_cYMOTORDelay=dwTickCount+2000;
        YJHJ_AutoDelay=dwTickCount+YJHJ_PARAM_DINGL_R_TIME*10;
    }  
    break; 
  case 11://
    if(dwTickCount>=YJHJ_AutoDelay)
    { 
       if(YJHJ_cZMOTORStep==0)
       {
          CalProSP(SPEED_ADDR);//计算生产速度   
          AddToTal(TOTAL_ADDR);//生产总量加1并保存
          AddToTal(NOWTOTAL_ADDR);//生产总量加1并保存
          AddToTal(ALL_TOTAL_ADDR);//累计生产总量
          AddToTal(DATE_TOTAL_ADDR);//当班生产总量  
          YJHJ_cZMOTORStep=20;
          YJHJ_AutoStep=12;
       }      
      
    }  
    break;    
  case 12://送线
      if((GetTotal(TOTAL_ADDR) >= GetTotal(SETTOTAL_ADDR)) && (GetTotal(SETTOTAL_ADDR) != 0))
      {            
          SetAlarmCode(ALARM_ADDR,YJHJ_TOTAL_ALARM); 
          //回到待机点            
          YJHJ_AutoStep=20;
      }
      else if(SingOneFlg)
      {
          SingOneFlg=0;
          YJHJ_AutoStep=20; 
      }
      else
      {
         YJHJ_AutoStep=4;
         if(YJHJ_WorkCnt>0)YJHJ_WorkCnt--;

      }
    break;
  case 20://回待机点
    if(YJHJ_cXMOTORStep==0&&YJHJ_cYMOTORStep==0&&YJHJ_cZMOTORStep==0)
    {    
        YJHJ_AutoStep=0;
        bRunning=0;
    }    
    break;
  case 100://回待机点 
    if(dwTickCount>=YJHJ_AutoDelay)
    {
      YJHJ_AutoStep=101;
      YJHJ_YL_VAVLE=0;
      YJHJ_DINGL_VAVLE=0;
      YJHJ_AutoDelay=dwTickCount+500;
    }
     break;
  case 101://回待机点 
    if(dwTickCount>=YJHJ_AutoDelay)
    {
        if(bStart)
        {
           bStart=0;
           YJHJ_ONCE_FLAG=1;
           YJHJ_AutoStep=5;
        }
    }
     break;   
  }

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

    //启动  
    if((START_IN_UP||bStart||bOnceStart)&&!bRunning&&!EMGSTOP_IN)
    {
      if(ALL_Reset_Flag)
      {            
          SetAlarmCode(ALARM_ADDR,YJHJ_NO_RESET_ALARM);           
      }
      else if(GetAlarmCode(ALARM_ADDR)==0)
      {
         //
         bRunning=1;
         YJHJ_AutoStep=1;
         //X轴回待机位      
         if(SERVO_VAVLE==0)YJHJ_cXMOTORStep=1;
         //Y轴回待机位
         YJHJ_cYMOTORStep=1;
         //Z轴回待机位
         YJHJ_cZMOTORStep=1;
         //装料，装钩
         //YJHJ_ZLTStep=1;
         //YJHJ_ZLTDelay=dwTickCount+20;
         YJHJ_FENL_VAVLE=1;
         //
         YJHJ_ZYGStep=1;
         //料盘启动
         YJHJ_LPDJStep=1;
         //IO清零
         YJHJ_DINGL_VAVLE=0;//顶料
         YJHJ_JG_VAVLE=0;
         YJHJ_DG_VAVLE=0;
         YJHJ_YL_VAVLE=0;
         YJHJ_QL_VAVLE=0;
         YJHJ_JZ_VAVLE=0;
         YJHJ_CYG_VAVLE=0;
         //
         //YJHJ_ZG_EN_FLAG=1;
         //YJHJ_ZYJ_EN_FLAG=1;
         //
         YJHJ_WLSTOP_FLAG=0;
         //
         YJHJ_WorkCnt=0;
         YJHJ_AutoDelay=dwTickCount+200;
         if(bOnceStart)SingOneFlg=1; 
         
         YJHJ_TYG_TIME=0;
         YJHJ_ONCE_FLAG=1;

      }
      bStart=0;
    }
    //停止
    if(STOP_IN_UP||bStop||EMGSTOP_IN)
    {
        //YJHJ_STOP_NUM++;
        //if(YJHJ_STOP_NUM>2)YJHJ_STOP_NUM=0;
        /*if(bStop&&bRunning)
        {
          SingOneFlg=1;
          bStop=0;
        }
        else*/
        {  
          //轴停止步
          bRunning=0;
          YJHJ_WLSTOP_FLAG=0;
          bStop=0;
          AxisEgmStop(X_AXIS);
          AxisEgmStop(Y_AXIS);
          AxisEgmStop(Z_AXIS);
          //自动
          YJHJ_AutoStep=0;
          YJHJ_cXMOTORStep=0;
          YJHJ_cYMOTORStep=0;
          YJHJ_cZMOTORStep=0;
          //
          YJHJ_LPDJStep=0;
          //         
          YJHJ_ZLTStep=0;
           //
          YJHJ_cGPJRStep=0;
          //
          YJHJ_ZYGStep=0;
          //IO
          YJHJ_FENL_VAVLE=0; 
          YJHJ_FANGL_VAVLE=0;
          YJHJ_DINGL_VAVLE=0;
          YJHJ_FENGG_VAVLE=0;
          YJHJ_SDG_VAVLE=0;  
          YJHJ_XDG_VAVLE=0;  
          YJHJ_JG_VAVLE=0;   
          YJHJ_DG_VAVLE=0;   
          YJHJ_QL_VAVLE=0;   
          YJHJ_YL_VAVLE=0;   
          YJHJ_JZ_VAVLE=0; 
          YJHJ_CYG_VAVLE=0;
          YJHJ_LPMOTOR_OUT=0;
          YJHJ_HEAT_OUT=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;
        YJHJ_AutoStep=0;        
        //自动
        YJHJ_AutoStep=0;
        YJHJ_cXMOTORStep=0;
        YJHJ_cYMOTORStep=0;
        YJHJ_cZMOTORStep=0;
        //
        YJHJ_LPDJStep=0;
        //         
        YJHJ_ZLTStep=0;
         //
        YJHJ_cGPJRStep=0;
        //
        YJHJ_ZYGStep=0;
    }
    /*
    //伺服报警标志
    if(((GetAlarm(Y_AXIS)==1)) || GetAlarm(X_AXIS)==1))
    {
    
      YJHJ_bAlarmStop=1;
      YJHJ_SERVO_ALAEM_FLAG=1;
      YJHJ_SetAlarmCode(YJHJ_HAND_SERVO_ALARM);
    }
    //清除伺服告警
    if(dwTickCount>=YJHJ_CLR_ALARM_Pulse&&YJHJ_CLR_SERVOALAEM_FLAG)
    {
       YJHJ_CLR_SERVOALAEM_FLAG=0;
       SetClr(X_AXIS, 0);
       SetClr(Y_AXIS, 0);
    }*/
#endif    
}
//初始化动作
void YJHJ_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, YJHJ_MOTOR_DISEN);
    //SetEn(Y_AXIS, YJHJ_MOTOR_DISEN);

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

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

void YJHJ_Action(void)
{
    YJHJ_CheckStart();
    YJHJ_ManualAction();
      //回原点处理
    Y_ReSetOrigin();
    X_ReSetOrigin();
    Z_ReSetOrigin();
    //料盘动作
    YJHJ_LPDJ_Action();
    //加热动作
    YJHJ_GPJR_Action();
    //装料动作
    YJHJ_ZLT_Action();
    //推料
    YJHJ_cZMotor_Action();
    //装衣钩
    YJHJ_ZYG_Action();
    //插衣钩
    YJHJ_cXMotor_Action();
    //出料    
    YJHJ_cYMotor_Action();    
    //自动动作
    YJHJ_AutoStepAction(); 

}
#endif