#include "global.h"

#if FU_CHEN_MACHINE == 1
//故障报警
void YHNTJ_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(!YHNTJ_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(YHNTJ_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(!YHNTJ_X_ORIGIN_IN)
        {                               
            XRunResetOStep = 5;
            ReSetXDelay = dwTickCount + 50;
        }
        break;
    case 5:
        if(!YHNTJ_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(YHNTJ_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,-50,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(!YHNTJ_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(YHNTJ_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(!YHNTJ_Y_ORIGIN_IN)
        {                               
            YRunResetOStep = 5;
            ReSetYDelay = dwTickCount + 50;
        }
        break;
    case 5:
        if(!YHNTJ_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(YHNTJ_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(!YHNTJ_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(YHNTJ_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(!YHNTJ_Z_ORIGIN_IN)
        {                               
            ZRunResetOStep = 5;
            ReSetZDelay = dwTickCount + 50;
        }
        break;
    case 5:
        if(!YHNTJ_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(YHNTJ_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 YHNTJ_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(YHNTJ_bXDG)
      {
        YHNTJ_bXDG=0;
        YHNTJ_DL_VAVLE=~YHNTJ_DL_VAVLE;
      }
      //放料
      if(YHNTJ_bJG)
      {
        YHNTJ_bJG=0;
        YHNTJ_YL_VAVLE=~YHNTJ_YL_VAVLE;
      }
      //顶料
      if(YHNTJ_bDG)
      {
        YHNTJ_bDG=0;
        YHNTJ_JS_VAVLE=~YHNTJ_JS_VAVLE;
      }      
      //顶料
      if(YHNTJ_bQL)
      {
        YHNTJ_bQL=0;
        YHNTJ_ZJ_VAVLE=~YHNTJ_ZJ_VAVLE;
      }
         //插钩
      if(bXMotor_N)
      {  
        //YHNTJ_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&&!YHNTJ_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&&!YHNTJ_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)
      {
        //YHNTJ_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&&!YHNTJ_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(YHNTJ_All_REST)
      {
        YHNTJ_All_REST=0;
        YHNTJ_All_RESTING=1;
        XRunResetOStep=1;
        //YRunResetOStep=1;
        //YHNTJ_JS_VAVLE=0;

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

      //收线旋转
      if(YHNTJ_bXZ_SWC)
      {
        YHNTJ_bXZ_SWC=0;
        if(!YHNTJ_bXZ_SWO)
        {
          YHNTJ_bXZ_SWO=1;
          YHNTJ_LSLSN_OUT=1;
          YHNTJ_LSLQH_OUT=0;
        }
        else
        {
          YHNTJ_bXZ_SWO=0;
          YHNTJ_LSLSN_OUT=0;
          YHNTJ_LSLQH_OUT=0;
        }
      }      
    }
}
//插钩动作
void YHNTJ_cXMotor_Action(void)	 //
{ 
  
#if 1 
    long hand_local;
    switch(YHNTJ_cXMOTORStep)
    {
    case 1://待机位置
      YHNTJ_cXMOTORStep=2;
      YHNTJ_WorkCnt++;
      hand_local=0;
      hand_local=hand_local-dwXRealPos;  
      //if(YHNTJ_WorkCnt>=YHNTJ_PARAM_CMP_SPACE)
      //{
     //   YHNTJ_WorkCnt=0;
     //   hand_local=hand_local+;
      //} 
      AxisMovePosAccDec(X_AXIS,YHNTJ_PARAM_QL_HSPEED,hand_local,1000,1000,120,120,50);       
      break;
    case 2:
      if(!X_DRV)
      {
        YHNTJ_cXMOTORStep=3;
        YHNTJ_cXMOTORDelay=dwTickCount+30;
      }
      break; 
    case 3:
      if(dwTickCount>=YHNTJ_cXMOTORDelay)
      {
        YHNTJ_cXMOTORStep=0;
      }
      break;   
      
    case 10://待机位置
      if(YHNTJ_Fristwork==0)
      {
          AxisContinueMoveAcc(X_AXIS,YHNTJ_PARAM_FRIST_HSPEED,DIR_P,1000,1000,50,50);
      }
      else
      {
        hand_local=YHNTJ_CHECK_GY_LENGTH*2-YHNTJ_PARAM_DW_LENGTH;
        //hand_local=hand_local-dwXRealPos;  
        AxisMovePosAccDecNotStop(X_AXIS,YHNTJ_PARAM_QL_HSPEED,hand_local,1000,YHNTJ_PARAM_FRIST_HSPEED,150,50,PosToPulse(X_AXIS,100)); 
      }
       YHNTJ_cXMOTORStep=11;
      break;
    case 11:
      if(YHNTJ_DT_GYING_IN_DW)
      {
        YHNTJ_XSAVEPOS=dwXRealPos;
        YHNTJ_cXMOTORStep=12;
        
        YHNTJ_cXMOTORDelay=dwTickCount+50;
      } 
      break; 
    case 12:
      if(dwTickCount>=YHNTJ_cXMOTORDelay)
      {
          if(YHNTJ_DT_GYING_IN_DW)
          {   
            YHNTJ_Fristwork=1;
            YHNTJ_cXMOTORStep=13; 
            YHNTJ_CHECK_GY_LENGTH=dwXRealPos-YHNTJ_XSAVEPOS;
            hand_local=YHNTJ_PARAM_DW_LENGTH;
            AxisMovePosAccDec(X_AXIS,YHNTJ_PARAM_FRIST_HSPEED,hand_local,1000,1000,150,150,10); 
          } 
      }
      break;    
    case 13:
      if(!X_DRV)
      {
        YHNTJ_JS_VAVLE=1;
        YHNTJ_cXMOTORStep=14;
        YHNTJ_cXMOTORDelay=dwTickCount+YHNTJ_PARAM_JS_H_TIME*10;
      } 
      break; 
    case 14://夹钩
      if(dwTickCount>=YHNTJ_cXMOTORDelay)
      {                     
        YHNTJ_cXMOTORStep=0;
      }
      break; 
    }
#endif
}
//取衣架放衣架动作
void YHNTJ_cYMotor_Action(void)
{
    long hand_local;
    switch(YHNTJ_cYMOTORStep)
    {
    case 1://判断是否松轴
      YHNTJ_LSLSN_OUT=1;
      YHNTJ_LSLQH_OUT=0;
      YHNTJ_cYMOTORStep=2;
      hand_local=YHNTJ_PARAM_START_LOCAL;
      hand_local=hand_local-dwYRealPos;  
      AxisMovePosAccDec(Y_AXIS,YHNTJ_PARAM_JG_SPEED,hand_local,1000,1000,220,220,50);  
      break;
    case 2://取料位置
      if(!Y_DRV)
      {
        if(YHNTJ_CW_TIME>0)
        {
           YHNTJ_cYMOTORStep=3;
        }
        else if(YHNTJ_CCW_TIME>0)
        {
           YHNTJ_cYMOTORStep=19;
        }
        else
        {
          YHNTJ_cYMOTORStep=6;
        }
      }
      break;
    case 3:
      if(YHNTJ_CW_TIME>=YHNTJ_PARAM_JG_NUM)
      {
          YHNTJ_CW_TIME=0;
          YHNTJ_cYMOTORStep=22;
      }
      else
      {
          YHNTJ_cYMOTORStep=4;
      }
      break;  
    case 4:
      YHNTJ_cYMOTORStep=5;
      hand_local=YHNTJ_CW_TIME*YHNTJ_PARAM_SXJG_LOCAL;
      AxisMovePosAccDec(Y_AXIS,YHNTJ_PARAM_JG_SPEED,hand_local,1000,1000,220,220,50);       
      break; 
    case 5:
      if(!Y_DRV)
      {
        YHNTJ_cYMOTORStep=6; 
      }
      break; 
     case 6:
      if(YHNTJ_Z_ORIGIN_IN_UP)
      {
          if(YHNTJ_CW_TIME>=YHNTJ_PARAM_JG_NUM)
          {
            YHNTJ_CW_TIME=0;
            YHNTJ_cYMOTORStep=22; 
          }
          else
          {
            YHNTJ_cYMOTORStep=7;
            hand_local=YHNTJ_PARAM_SXJG_LOCAL;
            AxisMovePosAccDec(Y_AXIS,YHNTJ_PARAM_JG_SPEED,hand_local,2000,2000,250,250,100); 
          }
      }
      break; 
    case 7:
      if(!Y_DRV)
      {
         YHNTJ_cYMOTORStep=8; 
         YHNTJ_cYMOTORDelay=dwTickCount+500;
      }
      break;   
    case 8:
      if(dwTickCount>=YHNTJ_cYMOTORDelay)
      {     
          YHNTJ_CW_TIME++;
          YHNTJ_cYMOTORStep=6; 
      }
      break;  

    case 19:
      if(YHNTJ_CCW_TIME>=YHNTJ_PARAM_JG_NUM)
      {
          YHNTJ_CCW_TIME=0;
          YHNTJ_cYMOTORStep=6;
      }
      else
      {
          YHNTJ_cYMOTORStep=20;
      }
      break;          
    case 20:
      YHNTJ_cYMOTORStep=21;
      hand_local=(YHNTJ_PARAM_JG_NUM-YHNTJ_CCW_TIME)*YHNTJ_PARAM_SXJG_LOCAL;
      AxisMovePosAccDec(Y_AXIS,YHNTJ_PARAM_JG_SPEED,hand_local,1000,1000,220,220,50);       
      break; 
    case 21:
      if(!Y_DRV)
      {
        YHNTJ_cYMOTORStep=22; 
      }
      break; 
     case 22:
      if(YHNTJ_Z_ORIGIN_IN_UP)
      {
          if(YHNTJ_CCW_TIME>=YHNTJ_PARAM_JG_NUM)
          {
            YHNTJ_CCW_TIME=0;
            YHNTJ_cYMOTORStep=6; 
          }
          else
          {
            YHNTJ_cYMOTORStep=23;
            hand_local=YHNTJ_PARAM_SXJG_LOCAL;
            AxisMovePosAccDec(Y_AXIS,YHNTJ_PARAM_JG_SPEED,-hand_local,2000,2000,250,250,100); 
          }
      }
      break; 
    case 23:
      if(!Y_DRV)
      {
         YHNTJ_cYMOTORStep=24; 
         YHNTJ_cYMOTORDelay=dwTickCount+500;
      }
      break;   
    case 24:
      if(dwTickCount>=YHNTJ_cYMOTORDelay)
      {     
          YHNTJ_CCW_TIME++;
          YHNTJ_cYMOTORStep=22; 
      }
      break; 
    }
}

//自动动作
void YHNTJ_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] = YHNTJ_AutoStep;
  user_datas[122] = YHNTJ_cXMOTORStep;
  
  user_datas[123] = YHNTJ_cYMOTORStep;
  user_datas[124] = YHNTJ_CHECK_GY_LENGTH*2;//YHNTJ_cZMOTORStep;
  user_datas[125] = YHNTJ_WorkCnt;//YHNTJ_LPDJStep;
  
  user_datas[126] = YHNTJ_CW_TIME;
  user_datas[127] = YHNTJ_CCW_TIME;//YHNTJ_ZLTStep;
  user_datas[128] = YHNTJ_PARAM_CMP_SPACE;//YHNTJ_ZYGStep; 
  
  *PARAM_X_LOCAL=dwXRealPos;
  *PARAM_Y_LOCAL=dwYRealPos;
  *PARAM_Z_LOCAL=dwZRealPos;
#if 1
  if(bRunning&&dwTickCount>=YHNTJ_cYMOTORDelay)
  {
    YHNTJ_DL_VAVLE=~YHNTJ_DL_VAVLE;
    YHNTJ_cYMOTORDelay=dwTickCount+YHNTJ_PARAM_ZJ_TIME*10;
  }
  switch(YHNTJ_AutoStep)
  {
  case 1://取料位置，顶料
    if(dwTickCount>=YHNTJ_AutoDelay)       
    {
      if(bTestMode&&bStart||!bTestMode)
      {
        YHNTJ_AutoStep=2;
        YHNTJ_JS_VAVLE=1;
        YHNTJ_AutoDelay=dwTickCount+YHNTJ_PARAM_JS_H_TIME*10;
        hand_local=YHNTJ_PARAM_JSQW_LOCAL;
        hand_local=hand_local-dwXRealPos;  
        AxisMovePosAccDec(X_AXIS,YHNTJ_PARAM_QL_HSPEED,hand_local,1000,1000,220,220,50);  
        
      }
    }
    break;
  case 2://下降
   if(dwTickCount>=YHNTJ_AutoDelay&&!X_DRV)
    {
       if(bTestMode&&bStart||!bTestMode)
       {
            YHNTJ_AutoStep=3;
            YHNTJ_ZJ_VAVLE=1;
            YHNTJ_AutoDelay=dwTickCount+YHNTJ_PARAM_HD_H_TIME*10;          
       }
    }
    break;
  case 3://夹料
    if(dwTickCount>=YHNTJ_AutoDelay)
    {
      if(bTestMode&&bStart||!bTestMode)
      {
        YHNTJ_YL_VAVLE=1;       
        YHNTJ_AutoStep=4;
        YHNTJ_AutoDelay=dwTickCount+YHNTJ_PARAM_YL_H_TIME*10; 
      }
    }
    break;   
  case 4://上升
    if(dwTickCount>=YHNTJ_AutoDelay)
    {
       if(bTestMode&&bStart||!bTestMode)
       {
          YHNTJ_AutoStep=5;
          YHNTJ_ZJ_VAVLE=0;
          YHNTJ_JS_VAVLE=0;
          YHNTJ_AutoDelay=dwTickCount+YHNTJ_PARAM_HD_R_TIME*10;
       }
      
    }
    break;
  case 5://到放料位置
    if(dwTickCount>=YHNTJ_AutoDelay)
    {
      if(bTestMode&&bStart||!bTestMode)
      {
        YHNTJ_AutoStep=6;
        if(YHNTJ_WorkCnt>=YHNTJ_PARAM_FRIST_HSPEED)
        {
           YHNTJ_WorkCnt=0;
        }
        hand_local=YHNTJ_PARAM_JSHW_LOCAL+YHNTJ_WorkCnt*YHNTJ_PARAM_DW_LENGTH;
        hand_local=hand_local-dwXRealPos;  
        AxisMovePosAccDec(X_AXIS,YHNTJ_PARAM_QL_HSPEED,hand_local,1000,1000,220,220,50); 
        YHNTJ_WorkCnt++;
      }
    }
    break;    
  case 6://伺服回味
    if(!X_DRV)
    {
        YHNTJ_AutoStep=7;
        YHNTJ_AutoDelay=dwTickCount+50;
    }    
    break;   
  case 7://下降
    if(dwTickCount>=YHNTJ_AutoDelay)
    {
       if(bTestMode&&bStart||!bTestMode)
       {
          YHNTJ_AutoStep=8;
          YHNTJ_ZJ_VAVLE=1;
          YHNTJ_AutoDelay=dwTickCount+YHNTJ_PARAM_HD_H_TIME*10;          
       }
    }
    break;
   case 8://夹料
    if(dwTickCount>=YHNTJ_AutoDelay)
    {
      if(bTestMode&&bStart||!bTestMode)
      {
        YHNTJ_YL_VAVLE=0;
        YHNTJ_AutoStep=9;
        YHNTJ_AutoDelay=dwTickCount+YHNTJ_PARAM_YL_H_TIME*10; 
      }
    }
    break; 
  case 9://上升
    if(dwTickCount>=YHNTJ_AutoDelay)
    {
       if(bTestMode&&bStart||!bTestMode)
       {
          YHNTJ_AutoStep=1;
          YHNTJ_ZJ_VAVLE=0;
          YHNTJ_AutoDelay=dwTickCount+YHNTJ_PARAM_HD_R_TIME*10;
       }
      
    }
    break;    
  }

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

    //启动  
    if((START_IN_UP||bStart||bOnceStart)&&!bRunning&&!EMGSTOP_IN)
    {
      if(ALL_Reset_Flag)
      {            
          SetAlarmCode(ALARM_ADDR,YHNTJ_NO_RESET_ALARM);           
      }
      else if(GetAlarmCode(ALARM_ADDR)==0)
      {
         //
         bRunning=1;
         YHNTJ_AutoStep=1;
         //Y轴回待机位
         //IO清零
         YHNTJ_JS_VAVLE=0;
         YHNTJ_DL_VAVLE=0;
         YHNTJ_YL_VAVLE=0;
         YHNTJ_ZJ_VAVLE=0;
         //
         YHNTJ_Fristwork=0;
         //
         YHNTJ_WorkCnt=0;
         YHNTJ_AutoDelay=dwTickCount+200;
         YHNTJ_cYMOTORDelay=dwTickCount+YHNTJ_PARAM_ZJ_TIME*10;


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

          bStop=0;
          AxisEgmStop(X_AXIS);
          AxisEgmStop(Y_AXIS);
          AxisEgmStop(Z_AXIS);
          //自动
          YHNTJ_AutoStep=0;
          YHNTJ_cXMOTORStep=0;
          YHNTJ_cYMOTORStep=0;
          YHNTJ_cZMOTORStep=0;
          //
          YHNTJ_LPDJStep=0;
          //         
          YHNTJ_ZLTStep=0;
           //
          YHNTJ_cGPJRStep=0;
          //
          YHNTJ_ZYGStep=0;
          //IO
          YHNTJ_JS_VAVLE=0; 
          YHNTJ_DL_VAVLE=0;
          YHNTJ_YL_VAVLE=0;
          YHNTJ_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;
        YHNTJ_AutoStep=0;        
        //自动
        YHNTJ_AutoStep=0;
        YHNTJ_cXMOTORStep=0;
        YHNTJ_cYMOTORStep=0;
        YHNTJ_cZMOTORStep=0;
        //
        YHNTJ_LPDJStep=0;
        //         
        YHNTJ_ZLTStep=0;
         //
        YHNTJ_cGPJRStep=0;
        //
        YHNTJ_ZYGStep=0;
    }
    /*
    //伺服报警标志
    if(((GetAlarm(Y_AXIS)==1)) || GetAlarm(X_AXIS)==1))
    {
    
      YHNTJ_bAlarmStop=1;
      YHNTJ_SERVO_ALAEM_FLAG=1;
      YHNTJ_SetAlarmCode(YHNTJ_HAND_SERVO_ALARM);
    }
    //清除伺服告警
    if(dwTickCount>=YHNTJ_CLR_ALARM_Pulse&&YHNTJ_CLR_SERVOALAEM_FLAG)
    {
       YHNTJ_CLR_SERVOALAEM_FLAG=0;
       SetClr(X_AXIS, 0);
       SetClr(Y_AXIS, 0);
    }*/
#endif    
}
//初始化动作
void YHNTJ_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, YHNTJ_MOTOR_DISEN);
    //SetEn(Y_AXIS, YHNTJ_MOTOR_DISEN);

    SetAlarmCode(ALARM_ADDR,YHNTJ_NO_ALARM);
    SetDirReverse(X_AXIS, 0);
    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;
    }
    YHNTJ_CW_TIME=0;
    YHNTJ_CCW_TIME=0;
}

void YHNTJ_Action(void)
{
    YHNTJ_CheckStart();
    YHNTJ_ManualAction();
      //回原点处理
    Y_ReSetOrigin();
    X_ReSetOrigin();
    Z_ReSetOrigin();
    //插衣钩
    YHNTJ_cXMotor_Action();
    //出料    
    YHNTJ_cYMotor_Action();    
    //自动动作
    YHNTJ_AutoStepAction(); 

}
#endif