GC23005/Machine/JunYueMachine/JuYueChuantou_HeLian.c

#include "global.h"

#if  JUN_YUE_MACHINE ==  1

unsigned long CTXM_DOWN_TIME=0,PRV_CTXM_DOWN_TIME=0;
unsigned long HSLIAO_OUT_DELAY=0,PRV_HSLIAO_OUT_DELAY=0;
unsigned char FZ_ALARM_FLG=0;
unsigned short XCCR_LENGTH;
unsigned short CTHL_XCCR_Time;//小插传入次数
void CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag);
void CTHL_ManualAction(void);
void CTHL_AutoStepAction(void);
void CTHL_TD_Motor(void);
void CTHL_FZ_Motor(void);
void CTHL_CheckStart(void);
void CTHL_TLiao_Step(void);  //推料(推拉头)
void CTHL_AlarmProtect(void);
void CTHL_ZLT_Step(void);
void CTHL_ZhenDongAction(void);
void CTHL_CTXMSS_Proc(void);
void ExtiAcitionX02(void)
{
  
}

//故障报警
void CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag)
{
  SetAlarmCode(CTHL_ALARM_ADDR,alarm_code);
  if(!CTHL_wltzi && !bCxcAuto )
    bAlarmStop = 1;
  if(bCxcAuto)
  {
    SetEn(X_AXIS, MOTOR_EN);
    SetEn(Y_AXIS, MOTOR_EN);    
  }
  
}

//初始化动作
void CTHL_InitAction(void)
{
  float buff_pulse,buff_dist;
  
  buff_pulse  = CTHL_PARAM_TDCYCLE_PULSE;
  buff_dist   = CTHL_PARAM_TDCYCLE_LENGTH; 
  
  XGearRatio  =  buff_pulse/buff_dist; 
  
  buff_pulse  = CTHL_PARAM_TDCYCLE_PULSE;
  buff_dist   =  CTHL_PARAM_FZCYCLE_LENGTH;   
  YGearRatio  =  buff_pulse/buff_dist;
  
  
  //传递伺服齿轮比参数
  
  set_com_servo_param(X_AXIS,CTHL_PARAM_TDCYCLE_PULSE,CTHL_PARAM_TDCYCLE_LENGTH);
  set_com_servo_param(Y_AXIS,CTHL_PARAM_TDCYCLE_PULSE,CTHL_PARAM_FZCYCLE_LENGTH);
  
  SetAlarmCode(CTHL_ALARM_ADDR,0); 
  
  
  user_datas[121] = 0;
  user_datas[122] = 0;
  user_datas[123] = 0;
  
  SetEnReverse(X_AXIS, 0);
  SetEnReverse(Y_AXIS, 0);
  SetEnReverse(Z_AXIS, 0);
  
  SetDirReverse(X_AXIS, 0);
  SetDirReverse(Y_AXIS, 0);
  SetDirReverse(Z_AXIS, 0);
  
  //设置伺服告警极性,
  //伺服设置常开，则极性设置为0，  0为正常，1为告警，；
  //伺服设置为常闭，则极性设置为1，0，为正常，1位告警
  //SetAlarmReverse(X_AXIS,1);
  //SetAlarmReverse(Y_AXIS,0);
  
  SetServoComUse(X_AXIS,0);//Y轴正常模式。
  SetServoComUse(Y_AXIS,1);//X轴伺服扭矩通讯。
  
  SetEn(X_AXIS, MOTOR_EN);
  SetEn(Y_AXIS, MOTOR_EN);
  SetEn(Z_AXIS, MOTOR_EN);
  SetPos(X_AXIS, 0);  //启动位置设为0点    
  SetPos(Y_AXIS, 0);  //启动位置设为0点 
  SetPos(Z_AXIS, 0);  //启动位置设为0点 
  
  cBAOjin=1;
}

void CTHL_Action(void)
{ 

  dwXRealPos = GetPos(X_AXIS);
  dwYRealPos = GetPos(Y_AXIS);
  dwZRealPos = GetPos(Z_AXIS); 
  dwYRealPos_com=get_encode_value(Y_AXIS);
  
  user_datas[121] = CTHL_PARAM_XMZM_MODE;//XCCR_LENGTH;//CTXM_DOWN_TIME;//dwYRealPos_com;//servo_y.init_step;//CTHL_ZhuangLiaoStep;
  user_datas[122] = CTHL_CTXMSSStep;
  user_datas[123] = CTHL_TD_MotorStep;
  user_datas[124] = CTHL_LianLengthCheck;//CTHL_ZhuangLiaoStep;
  user_datas[125] = CTHL_LianLengthCheckOld;//CTHL_TLiaoStep;
  user_datas[126] = CTHL_AutoStep;//CTHL_FZ_MotorStep;//CTHL_TD_MotorStep;
  //    user_datas[127] = CTHL_cZipCnt;//CTHL_ZhuangLiaoStep;
  // user_datas[128] = CTHL_FZ_MotorStep;//CTHL_LianLengthAutolengthcheck;
  CTHL_SERVO_COM=servo_y.IO_TO_COM;
  
  CTHL_ZhenDongAction();
  CTHL_AlarmProtect();
  CTHL_CheckStart();    
  CTHL_ZLT_Step();
  CTHL_TLiao_Step();
  CTHL_TD_Motor();
  CTHL_FZ_Motor();
  CTHL_ManualAction();
  CTHL_AutoStepAction();  
  CTHL_Test++;
  servo_com_run();
  CTHL_CTXMSS_Proc();
}

void CTHL_AlarmProtect(void)
{ 

}
//自动动作
void CTHL_AutoStepAction(void)
{  
  unsigned short freq;  
  //测试用
  if(dwTickCount >= CTHL_TestDelay)
  {
    CTHL_TestDelay = dwTickCount + 200;
    CTHL_TestLED = !CTHL_TestLED;   
    CTHL_Test = 0;
  }  
  if(bRunning)
  {
    switch(CTHL_AutoStep)
    {
    case 1:      
      if(dwTickCount >= CTHL_AutoDelay)
      {    
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
        {
                   
          CTHL_TXM_VAVLE=0;
          CTHL_AutoStep = 2;
          //设置反转电机的扭矩和速度的设置       
          freq=CTHL_PARAM_FZ_Speed+SERVO_PARAM_SPEED_DIFFER;
          if(CTHL_cZipCnt > 1)
          {
            set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMIN,freq,freq,SERVO_TARR_CW); 
          }
          else 
          {
            set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW);
          }
          Set_Ctrlmode_trans(Y_AXIS,POS_MODE);          
          bCxcAuto=0;
          if(CTHL_cZipCnt > 1)  CTHL_AutoDelay = dwTickCount + 2;//拉头检测时间            
          else CTHL_AutoDelay = dwTickCount + 100;//拉头检测时间
            
          if(CTHL_ZhuangLiaoStep == 0)
          { 
            CTHL_ZhuangLiaoStep = 1;
            CTHL_TryCnt = 0;
          } 
          CTHL_GF_Change = 0;
        }
      }
      break;
    case 2:
      if(bZhuangLiaoOkFlg && (CTHL_ZhuangLiaoStep == 0)&&(CTHL_TD_MotorStep==0)&&(CTHL_CTXMSSStep==0))
      {
        if(dwTickCount >= CTHL_AutoDelay )
        {
            if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
            {
              if(!CTHL_PARAM_XMZM_MODE) {
                CTHL_QMDW_VAVLE = 1;//0为斜码，1为直码  
                //CTHL_FKDW_VAVLE = 0;          //方块定位退
              }              
              CTHL_YXC_VAVLE=1;
              CTHL_YFK_VAVLE=1;
              CTHL_JXM_VAVLE = 1;  //夹斜码电磁阀               
              if(CTHL_cZipCnt==0)CTHL_CTXM_VAVLE=1;   
              CTHL_JLTou_VAVLE = 0;  //接拉头
              CTHL_AutoStep = 3;
              CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
            }
        }
      }
      break;
    case 3:
      if(CTHL_CTM_Limit_IN) //下模上到位
      {       
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
        {          
          CTHL_AutoDelay = dwTickCount + CTHL_PARAM_XCDWFZ_DELAY;
          if(!CTHL_PARAM_XMZM_MODE)
            CTHL_AutoStep = 4;
          else
            CTHL_AutoStep = 5;
        }
      }
      else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE)
      {
        CTHL_SetAlarmCode(CTHL_CTM_Limit_ALARM,bRunning);//横送料原位异常  
      }
      break;
    case 4:
      if(CTHL_QMDW_IN&&dwTickCount >= CTHL_AutoDelay&&!X_DRV)
      {
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
        {
          //AxisContinueMoveAcc(X_AXIS,1000,DIR_N,1000,600,15,5);
          AxisMovePosAccDec(X_AXIS,1000,-CTHL_PARAM_JMDW_LENGTH,1000,800,10,10,0); //前拖带松一点带 
          AxisContinueMoveAcc(Y_AXIS,1000,DIR_N,1000,610,15,5);
          
          //Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);
          CTHL_AutoStep = 5;
          CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
          CTHL_JXM_VAVLE = 1;  //夹斜码电磁阀 
        }
      }
      else if((dwTickCount >= CTHL_AutoDelay+3000)
      {
        CTHL_SetAlarmCode(CTHL_QMDW_Limit_ALARM,bRunning);//前码定位感应异常
      }
      break;
    case 5: 
      if(!CTHL_QMDW_IN || CTHL_PARAM_XMZM_MODE)
      //if(!X_DRV|| CTHL_PARAM_XMZM_MODE)
      {
        //上升沿只能一次，不能做断点
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
        {
          CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TXM_DELAY;
          CTHL_AutoStep = 6;                   
        }
        AxisEgmStop(X_AXIS);
        AxisEgmStop(Y_AXIS); 
        //Set_Ctrlmode_trans(Y_AXIS,POS_MODE); 
      }
      else if((dwTickCount >= CTHL_AutoDelay) && (!CTHL_bHandWork_MODE))
      {
        CTHL_SetAlarmCode(CTHL_QMDW_Limit_ALARM,bRunning);//前码定位感应异常
      }
      break;
    case 6:
      if(dwTickCount >= CTHL_AutoDelay)   //顶斜码后拖反转
      {
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
        {
          AxisMovePosAccDec(X_AXIS,2000,-CTHL_PARAM_XMDD_LENGTH,2000,800,10,10,0); //前拖带松一点带    
          AxisMovePosAccDec(Y_AXIS,2000,20,2000,800,10,10,0);
          CTHL_TXM_VAVLE=1;     //Y06推斜码电磁阀 
          CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
          CTHL_AutoStep = 7;
        }        
      }
      break;
    case 7:
      if(CTHL_TXM_Limit_IN && CTHL_TXM_VAVLE )  //推斜码到位
      {           
        CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TQMDW_DELAY;               
        if(get_tarr_set(Y_AXIS)!=2)//伺服配置转矩限制失败
        {
          SetEn(Y_AXIS, MOTOR_DISEN);//设置扭矩做准备
          CTHL_wltzi=0;//停机 
          CTHL_SetAlarmCode(CTHL_SERVO_CONFIG_ALARM,bRunning);//伺服转矩配置失败
        }   
        CTHL_AutoStep = 8;
        
      }
      else if((dwTickCount >= CTHL_AutoDelay) && (!CTHL_bDANBU_MODE))
      {
        CTHL_SetAlarmCode(CTHL_TXM_Limit_ALARM,bRunning);//推斜码到异常  
      }                
      break;
    case 8:
      if(dwTickCount >= CTHL_AutoDelay&& !X_DRV&&!Y_DRV)
      {
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
        {                  
          CTHL_QMDW_VAVLE=0;
          CTHL_FZ_MotorStep=1;      //反转穿入
          //CTHL_FKDW_VAVLE = 0;
          //Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
          CTHL_FZ_MotorDelay = dwTickCount + CTHL_PARAM_CRTD_DELAY;//延时穿入拖带 
          CTHL_AutoStep = 9;
        }
      }
      break;
    case 9:
      if(CTHL_FZ_MotorStep==0)
      {
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
        {
          cBAOjin=0;
          CTHL_TFK_VAVLE=1;
          CTHL_YFK_VAVLE=0;
          CTHL_XCDW_VAVLE=1;                      
          CTHL_AutoStep = 10;
          CTHL_AutoDelay = dwTickCount + CTHL_PARAM_FKDW_FZSTOP_DELAY;//拖带定位
        }
      }
      break;
    case 10:
      if(dwTickCount >= CTHL_AutoDelay)
      {
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
        {
          CTHL_FZ_MotorStep=20;   
          CTHL_AutoStep = 11;
          CTHL_FZ_MotorDelay = dwTickCount + 0;          
        }
      }
      break;
    case 11:
      if(CTHL_FZ_MotorStep==0 && !X_DRV && !Y_DRV)
      {     
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
        {
          CTHL_JCBI_VAVLE = 1; 
          CTHL_SD = 1;         
          CTHL_AutoStep = 12;
          CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
        }
      }
      break;
    case 12:
      if(CTHL_TXC_Limit_IN)  //摆臂到位
      {
         //CTHL_DingDai_VAVLE=1;   //暂时不用
        CTHL_XCCR_Time=0;
         //if(CTHL_PARAM_FZCXC_LENGTH != 0)
         //   AxisMovePosAccDec(Y_AXIS,2*CTHL_PARAM_XCCR_SPEED,CTHL_PARAM_FZCXC_LENGTH,800,800,15,15,0);//辅助小插插入  
        CTHL_AutoStep = 13;  
        CTHL_AutoDelay = dwTickCount + CTHL_PARAM_XCIN_DELAY;//小插到位延时拖带      
        
      }
      else if(dwTickCount >= CTHL_AutoDelay)
      {
        bCxcAuto=1;
        //CTHL_AutoStep = 101; 
        //CTHL_JCBI_VAVLE = 0; 
        //AxisMovePosAccDec(X_AXIS,1000,-50,1000,1000,15,15,0);//前轮布放松一点        
        CTHL_SetAlarmCode(CTHL_XCBI_Limit_ALARM,bRunning);//小插摆臂到位异常
      }
      break;
    case 13:   
      if(dwTickCount >= CTHL_AutoDelay && !Y_DRV)
      {
         SetPos(X_AXIS, 0);
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
        {
          CTHL_AutoDelay = dwTickCount + 0;//CTHL_PARAM_YDC_DELAY; 
          AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_XCCR_SPEED,CTHL_PARAM_XCCR_LENGTH,800,800,15,15,50);  
          if(CTHL_SD)
          { 
            CTHL_SD = 0;
            AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_XCCR_SPEED+100,CTHL_PARAM_XCCR_LENGTH+CTHL_PARAM_FZCXC_LENGTH,800,800,15,15,0);
          }
          CTHL_AutoStep = 14; 
        }
        CTHL_XSavePosBuff3=dwXRealPos;
      }
      break;
    case 14:
      if(CTHL_XCDW_Limit_IN)//||(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_XCCRBH_LENGTH))
      {
        XCCR_LENGTH=dwXRealPos-CTHL_XSavePosBuff3;
        CTHL_XSavePosBuff3 = dwXRealPos;
        CTHL_AutoStep = 15; 
        
        CTHL_XCDW_VAVLE=0;       
        CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TXCDW_DELAY;
      }
      //else if((dwTickCount >= (CTHL_AutoDelay+1000)) && (!CTHL_bHandWork_MODE))
      else if((dwTickCount >= (CTHL_AutoDelay+1200)||(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_XCCRBH_LENGTH)) && (!CTHL_bHandWork_MODE))
      {
        bCxcAuto=1;
        CTHL_XSavePosBuff3 = dwXRealPos;
        AxisEgmStop(X_AXIS);  
        AxisEgmStop(Y_AXIS);
        CTHL_AutoStep = 100; 
        CTHL_JCBI_VAVLE = 0;   //Y16小插摆臂电磁阀
        //AxisMovePosAccDec(X_AXIS,800,-100,800,800,15,15,0);//前轮布放松一点
        CTHL_SetAlarmCode(CTHL_XCDW_Limit_ALARM,bRunning);//小插定位针异常
      }
      break;  
    case 15:   
      if((dwXRealPos >= (CTHL_XSavePosBuff3 + 50)))
      {
        AxisEgmStop(X_AXIS);  
        AxisEgmStop(Y_AXIS); 
        if(dwTickCount >= CTHL_AutoDelay)
        {                   
          CTHL_XCDW_VAVLE=0;
          CTHL_TFK_VAVLE = 0;          
          if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
          {
            cDELAYONE=0;
            bZhuangLiaoOkFlg=0;
            CTHL_AutoStep = 16;
            CTHL_AutoDelay = dwTickCount + 0;
          }
        }
      }  
      break;
    case 16:
      if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
      {
        if(dwTickCount >= CTHL_AutoDelay)
        {    
          CTHL_AutoDelay = dwTickCount + 0;//CTHL_PARAM_TXCDW_DELAY ; 
          bZhuangLiaoOkFlg=0;
          CTHL_AutoStep = 17;
          //AxisMovePosAccDec(X_AXIS,5000,-CTHL_PARAM_CRFZ_LENGTH,1500,800,10,10,0); //前拖带松一点带    
          CTHL_YXC_VAVLE=0;    //Y14压小插电磁阀
          CTHL_JCBI_VAVLE=0;
        }
      }
      break;    
    case 100:  
      if(CTHL_START_IN_UP)
      {
        SetEn(X_AXIS, MOTOR_DISEN);
        SetEn(Y_AXIS, MOTOR_DISEN);
        CTHL_AutoDelay = dwTickCount + 300 ; 
        CTHL_AutoStep = 101;
      }
      break;
    case 101:  
      if(dwTickCount >= CTHL_AutoDelay)
      { 
        CTHL_XCCR_Time++;
        if(CTHL_XCCR_Time==1)
        {
          AxisMovePosAccDec(X_AXIS,800,-CTHL_XSavePosBuff3,800,800,15,15,0);//前轮布放松
          AxisMovePosAccDec(Y_AXIS,800,-CTHL_XSavePosBuff3,800,800,15,15,0);//前轮布放松           
        }
        CTHL_AutoStep = 102;
      }
      break;        
    case 102:               
      if(!X_DRV&&!Y_DRV)
      {
        SetAlarmCode(CTHL_ALARM_ADDR,0); 
        //AxisMovePosAccDec(X_AXIS,800,-100,800,800,15,15,0);//前轮布放松
        CTHL_YFK_VAVLE=1;
        CTHL_JCBI_VAVLE = 1;   //Y16小插摆臂电磁阀
        //             CTHL_XCDW_VAVLE=0;
        cDELAYONE=1;
        cBAOjin=1;
        CTHL_cZipCnt=0;
        bZhuangLiaoOkFlg=0;
        SetEn(X_AXIS, MOTOR_DISEN);
        SetEn(Y_AXIS, MOTOR_DISEN);
        
        
        //            if(CTHL_FKGY_IN)
        //            {
        //               CTHL_AutoStep = 9;
        //               CTHL_FZ_MotorStep==0;
        //            }
        //            else
        //            {
        CTHL_AutoStep = 103; 
        //            }
        CTHL_AutoDelay = dwTickCount + 300 ; 
        //             if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)
        //            {
        SetClr(X_AXIS, MOTOR_NOALARM);
        SetClr(Y_AXIS, MOTOR_NOALARM);
        CTHL_bSFBBAOJIN_Delay= dwTickCount + 200;
        //            }
        if(CTHL_PARAM_DBSF_MODE)
        {
          if(!CTHL_SF_ALR_IN)
          {
            CTHL_SF_ALR_CLR=1;
            CTHL_bSFBBAOJIN_Delay= dwTickCount + 200;             
          }                   
        }
      }  
      break;  
    case 103:  
      if(dwTickCount >= CTHL_AutoDelay)
      {            
        CTHL_AutoStep = 104; 
        CTHL_AutoDelay = dwTickCount + 100 ;
        
      }  
      break;
     case 104:   
      if(dwTickCount >= CTHL_AutoDelay && !Y_DRV)
      {
          CTHL_AutoDelay = dwTickCount + 0;//CTHL_PARAM_YDC_DELAY; 
          AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_XCCR_SPEED,CTHL_PARAM_XCCR_LENGTH,800,800,15,15,50);  
          CTHL_AutoStep = 105;      
          CTHL_XSavePosBuff3=dwXRealPos;
      }
      break;
    case 105:
      if(CTHL_XCDW_Limit_IN)//||(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_XCCRBH_LENGTH))
      {
        XCCR_LENGTH=dwXRealPos-CTHL_XSavePosBuff3;
        CTHL_XSavePosBuff3 = dwXRealPos;
        CTHL_AutoStep = 15;         
        CTHL_XCDW_VAVLE=0;       
        CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TXCDW_DELAY;
      }
      else if((dwTickCount >= (CTHL_AutoDelay+1200)||(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_XCCRBH_LENGTH+300)) && (!CTHL_bHandWork_MODE))
      {
        
        CTHL_XSavePosBuff3 = dwXRealPos;
        AxisEgmStop(X_AXIS);  
        AxisEgmStop(Y_AXIS);
        if(CTHL_XCCR_Time>=2)
        {
          bCxcAuto=1;
          CTHL_SetAlarmCode(CTHL_XCDW_Limit_ALARM,bRunning);//小插定位针异常
        }
        else
        {
          CTHL_AutoStep = 100; 
          CTHL_JCBI_VAVLE = 0;   //Y16小插摆臂电磁阀
        }        
      }
      break;  
    case 17:
      if(dwTickCount >= CTHL_AutoDelay)
      {
        if(!X_DRV)
        {
          if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
          {
            cDELAYONE=0;
            CTHL_TFK_VAVLE=0;
            CTHL_TD_MotorStep =10;
            CTHL_TD_MotorDelay = dwTickCount+ CTHL_PARAM_XCLD_DELAY;
            CTHL_AutoStep = 18;
          }
        }
      }
      break;  
    case 18:
      //if(!CTHL_CTM_Limit_IN)
      {        
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
        {
          CTHL_JLTou_VAVLE = 0;
          //CTHL_CTXMSSStep=1;
          CTHL_AutoStep = 19;          
          
        }
      }
      break;
    case 19:
      if((CTHL_TD_MotorStep == 0))
      {   
        //               if(CTHL_START_IN)
        {  
          if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
          {
            CTHL_AutoStep = 1; 
            if(CTHL_cStopMode==1 || bSingOneFlg)
            {
              CTHL_cStopMode=2;
              CTHL_ZhouWantwo=1;
              CTHL_CTXM_VAVLE =0; 
            }
            AddToTal(CTHL_TOTAL_ADDR);    
            AddToTal(CTHL_TRUST_ALARM_ADDR);                 
            CalProSP(CTHL_SPEED_ADDR);
            //公分节那条也不加
            if((CTHL_cZipCnt<3) && !CTHL_GF_Change) 
              CTHL_cZipCnt++;
            if((CTHL_NoLaLian_IN))
              CTHL_cZipCnt = 0;
            if(bSingOneFlg || CTHL_bHandWork_MODE)
            {
              CTHL_AutoStep = 0;
              bRunning = 0;
              bSingOneFlg = 0;
            } 
            CTHL_GF_Change = 0;
          }
        }     
      }
      break;
      
    } 
  }
}
//下模上升处理
void CTHL_CTXMSS_Proc(void)
{
  static unsigned char hemoflag1=0,hemoflag2=0;
  if(bRunning)
  {
    switch(CTHL_CTXMSSStep)
    { 
    case 1:
      if(!CTHL_CTM_Limit_IN)
      {
        CTHL_JXM_VAVLE=0;
        PRV_CTXM_DOWN_TIME= dwTickCount;
        if(CTXM_DOWN_TIME)HSLIAO_OUT_DELAY=dwTickCount+CTXM_DOWN_TIME*7/8;
        hemoflag1=1;
        hemoflag2=1;
        CTHL_CTXMSSStep = 2; 
        CTHL_AutoDelay1 = dwTickCount+ 3000;
      }
      break;
    case 2:              //测量时间
      if(CTHL_CTM_Origin_IN&&hemoflag1)
      {
        hemoflag1=0;
        PRV_CTXM_DOWN_TIME=dwTickCount-PRV_CTXM_DOWN_TIME;
        CTXM_DOWN_TIME=PRV_CTXM_DOWN_TIME;
      }
       //
      /*if(CTXM_DOWN_TIME&&(CTHL_cZipCnt > 1))//测量时间后的动作
      {
        if(dwTickCount>=HSLIAO_OUT_DELAY&&hemoflag2)
        {
          hemoflag2=0;
          CTHL_ZhuangLiaoStep = 1;
          CTHL_TryCnt = 0;
          bZhuangLiaoOkFlg=0;
          CTHL_ZhuangLiaoDelay = dwTickCount;
        }
        if(CTHL_CTM_Origin_IN)
        {
          CTHL_CTXMSSStep = 0; 
          CTHL_MGuo_VAVLE = 0;
          CTHL_JLTou_VAVLE = 0;
         
        }
        else if(dwTickCount >= CTHL_AutoDelay1) 
        {
          //穿头模和一次穿入回位异常
          if(!CTHL_CTM_Origin_IN)
            CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning);                                   
        } 
      }
      else //测量时间前的动作*/
      {
        if(CTHL_CTM_Origin_IN)
        {          
          CTHL_ZhuangLiaoStep = 1;
          CTHL_TryCnt = 0;
          bZhuangLiaoOkFlg=0;
          CTHL_ZhuangLiaoDelay = dwTickCount;
          CTHL_CTXMSSStep = 20;
          CTHL_MGuo_VAVLE = 0;
          CTHL_JLTou_VAVLE = 0;
          CTHL_GDLT_VAVLE=1;
          
        }
        else if(dwTickCount >= CTHL_AutoDelay1) 
        {
          //穿头模和一次穿入回位异常
          if(!CTHL_CTM_Origin_IN)
            CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning);                                   
        } 
      }
      break; 
     case 20:
      if(bZhuangLiaoOkFlg && (CTHL_ZhuangLiaoStep == 0)&&CTHL_CTXMSS_FLAG)
      {  
        CTHL_CTXMSS_FLAG=0;
        if(!CTHL_LTou_Check && !CTHL_TXM_Limit_IN)//有拉头，推斜码到位不能亮
        {
          //if(!CTHL_ZhouWanone && !bSingOneFlg)
          {
            CTHL_CTXM_VAVLE=1;
            CTHL_CTXMSSStep = 21;
          }
          //else
          //{
          //  CTHL_CTXMSSStep = 0;
          //}
        }
        else 
        {
          CTHL_wltzi=1;                  
          CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);  // 没有拉头  
        } 
      }
      break;
     case 21:
       if(CTHL_CTM_Limit_IN)
       {
         CTHL_CTXMSSStep = 0;
       }
      break;
    }
  }
}
void CTHL_CheckStart(void)
{
  //if(!CTHL_SafeDoor_IN && bRunning)
  //{
  //  CTHL_SetAlarmCode(CTHL_SafeDoor_ALARM,0);  // 安全门关上才能启动
  //}
  if(CTHL_START_IN_UP || bStart)
  {
    if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)
    {
      SetClr(X_AXIS, MOTOR_NOALARM);
      SetClr(Y_AXIS, MOTOR_NOALARM);
      CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
    }
    if(CTHL_PARAM_DBSF_MODE)
    {
      if(!CTHL_SF_ALR_IN)
      {
        CTHL_SF_ALR_CLR=1;
        CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;        
      }
    }
    SetAlarmCode(CTHL_ALARM_ADDR,0); 
  }
  if(CTHL_wltzi)//无拉头启动
  {
    if(CTHL_START_IN_UP || bStart)
    {
      if(bRunning)
      {
        CTHL_wltzi=0;
        CTHL_AutoStep = 1;
        CTHL_ZhuangLiaoStep = 1;
        bTuiLaTouOkFlg = 1;
        bZhuangLiaoOkFlg= 0;
        CTHL_MGuo_VAVLE = 1;   //判断是否已经有料
        CTHL_GDLT_VAVLE=0;
        CTHL_ZhuangLiaoDelay = dwTickCount + 150;
        if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)
        {
          SetClr(X_AXIS, MOTOR_NOALARM);
          SetClr(Y_AXIS, MOTOR_NOALARM);
          CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
        }
        if(CTHL_PARAM_DBSF_MODE)
        {
          if(!CTHL_SF_ALR_IN)
          {
            CTHL_SF_ALR_CLR=1;
            CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
            
          }
        }
      }
      else
      {
        CTHL_wltzi=0;
      }
    }
  }
  
#if 1  
  //启动    自动启动 单一自动	 			
  if((CTHL_START_IN_UP || bStart || CTHL_bSingOne  ) && !CTHL_wltzi)
  {  
    if(!bRunning)
    {     
      bRunning = 1;
      if( CTHL_bSingOne)bSingOneFlg=1;
      SetEn(X_AXIS, MOTOR_DISEN);
      SetEn(Y_AXIS, MOTOR_DISEN);
      CTHL_FZ_MotorStep = 0;
      CTHL_CTXMSSStep=0;
      CTHL_CTXMSS_FLAG=0;
     
      CTHL_PARAM_ZDP_AUTO=0;//振动盘
       CTHL_TryCnt = 0;
      bCxcAuto=0;
      bCL_EN_FLG = 0;
      CTHL_stop=0;
      
      CTHL_TLiao_VAVLE =0;      
      CTHL_HSLiao_VAVLE   =0;      
      CTHL_JLTou_VAVLE  =0;      
      CTHL_CTXM_VAVLE  =0;       
      //  CTHL_MGuo_VAVLE  =0;        
      CTHL_JXM_VAVLE   =0;   
      CTHL_TXM_VAVLE    = 0 ;    
      CTHL_QMDW_VAVLE   =0;        
      CTHL_TFK_VAVLE    =0;        
         
      CTHL_HL_VAVLE     =0;      
      CTHL_YFK_VAVLE    =0;      
      CTHL_YXC_VAVLE     =0;     
      CTHL_XCDW_VAVLE    =0;       
      CTHL_JCBI_VAVLE    =0; 
      CTHL_DingDai_VAVLE=0;
      
      CTHL_cZipCnt = 0;        
      SetAlarmCode(CTHL_ALARM_ADDR,0); 
      CTHL_AutoDelay = dwTickCount + 0;
      CTHL_TD_MotorStep = 0;            
      CTHL_AutoStep = 1;
      CTHL_MGuo_VAVLE = 1;   //判断是否已经有料
      CTHL_GDLT_VAVLE=0;
      
      //方块定位
      CTHL_TD_MotorStep=1; 
      
      CTHL_ZhuangLiaoDelay = dwTickCount + 350;
      CTHL_ZhuangLiaoStep=1;
      bTuiLaTouOkFlg = 1;
      bZhuangLiaoOkFlg= 0; 
      
      bFZ_LengthOK = 0;
      CTHL_LianLengthCheck = 0; 
    }  
    bStart = 0;
    CTHL_bSingOne=0;
  }   
  if(CTHL_ZhouWantwo)//作为一条后停机
  {
    CTHL_ZhouWantwo=0;
    if(CTHL_ZhouWanone)
    {
      CTHL_ZhouWanone=0;
      bRunning=0;
      CTHL_cStopMode=0; 
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);      
      if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)
      {
        SetClr(X_AXIS, MOTOR_NOALARM);
        SetClr(Y_AXIS, MOTOR_NOALARM);
        CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
      }
      if(CTHL_PARAM_DBSF_MODE)
      {
        if(!CTHL_SF_ALR_IN)
        {
          CTHL_SF_ALR_CLR=1;
          CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;          
        }
      }
      SetEn(X_AXIS, MOTOR_EN);
      SetEn(Y_AXIS, MOTOR_EN);
      
      bSingOneFlg=0;
      CTHL_AutoStep = 0; 
      CTHL_FZ_MotorStep =0;   
      CTHL_TD_MotorStep =0;  
      CTHL_CTXM_VAVLE =0;       
    }
  }
  //停止
  if(CTHL_STOP_IN_UP || bStop)
  {
    if(CTHL_cStopMode < 2)CTHL_cStopMode++;      
    if(CTHL_cStopMode==1 && bRunning)CTHL_ZhouWanone=1; //按一次停止     
    if(bRunning && CTHL_cStopMode == 2||!bRunning)//按下2次停止
    {   
      
      CTHL_cStopMode=0; 
      bSingOneFlg=0;      
      cBAOjin=0;
      CTHL_wltzi=0;
      
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);
      Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
      SetEn(X_AXIS, MOTOR_EN);
      SetEn(Y_AXIS, MOTOR_EN);
      
      CTHL_AutoStep = 0; 
      CTHL_stop=0;
      CTHL_FZ_MotorStep =0;   
      CTHL_TD_MotorStep =0;
      if(!bRunning) 
      {
         CTHL_ZhuangLiaoStep=0;
         CTHL_TLiaoStep=0;
         CTHL_TLiao_VAVLE=0;             
         CTHL_HSLiao_VAVLE  =0;    
         CTHL_JLTou_VAVLE  =0;  
      }
      CTHL_TXM_VAVLE=0;
      CTHL_CTXM_VAVLE =0;           
      CTHL_MGuo_VAVLE = 0;
      CTHL_JXM_VAVLE   =0;                  
      CTHL_QMDW_VAVLE   =0;        
      CTHL_TFK_VAVLE    =0;        
    
      CTHL_HL_VAVLE     =0;      
      CTHL_YFK_VAVLE    =0;      
      CTHL_YXC_VAVLE     =0;     
      CTHL_XCDW_VAVLE=0;
      CTHL_JCBI_VAVLE    =0; 
      CTHL_DingDai_VAVLE=0;
      
      SetAlarmCode(CTHL_ALARM_ADDR,0); 
      if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)
      {
        SetClr(X_AXIS, MOTOR_NOALARM);
        SetClr(Y_AXIS, MOTOR_NOALARM);
        CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
      }
      if(CTHL_PARAM_DBSF_MODE)
      {
        if(!CTHL_SF_ALR_IN)
        {
          CTHL_SF_ALR_CLR=1;
          CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
        }
      }
      SetPos(X_AXIS, 0); 
      SetPos(Y_AXIS, 0); 
      SetPos(Z_AXIS, 0); 
      SetAlarmCode(CTHL_ALARM_ADDR,0);

      bFZ_LengthOK = 0;
      CTHL_LianLengthCheck = 0;
      CTHL_LianLengthCheckOld = 0;
      
      bRunning=0;
    }
    bStop=0;
  }  
  if(bAlarmStop)
  {   
    bRunning   = 0;
    bAlarmStop=0;
    CTHL_cStopMode=0;
    CTHL_XCDW_VAVLE=0;
    AxisEgmStop(X_AXIS);
    AxisEgmStop(Y_AXIS);
    Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
    //   AxisEgmStop(Z_AXIS);
    CTHL_AutoStep = 0; 
    CTHL_FZ_MotorStep =0;   
    CTHL_TD_MotorStep =0;   
  }
#endif    
}   
void CTHL_NoLaLian_Speed_Proc(void)
{
  if(CTHL_NoLaLian_IN_UP)
  {  
    AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,200,
                             1000,CTHL_PARAM_FKDWMAN_Speed/2,25,15,0); //YWX_CTHL_PARAM_TD_RunSpeed 
    AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,DIR_P,1500,800,15,15);  
  }  
}
void CTHL_TDLength_BH_Proc(unsigned short local)//拖带长度保护
{
  if(local==0)//为感应到开关前
  {
    if(CTHL_cZipCnt < 2)
    {
      if((dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length) && (!CTHL_bHandWork_MODE))
      {
        AxisEgmStop(X_AXIS);
        AxisEgmStop(Y_AXIS); 
        user_datas[128]=CTHL_TD_MotorStep;
        CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
      }
    }
    else
    {
      if((dwXRealPos >=(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheck)) && (!CTHL_bHandWork_MODE))
      {
        AxisEgmStop(X_AXIS);
        AxisEgmStop(Y_AXIS);
        user_datas[128]=CTHL_TD_MotorStep;
        CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
      }            
    }
  }
  else//感应到开关后
  {
    if(CTHL_cZipCnt < 2)
    {
      if((dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length+CTHL_LianLengthAutoFZCRcheck) && (!CTHL_bHandWork_MODE))
      {
        AxisEgmStop(X_AXIS);
        AxisEgmStop(Y_AXIS); 
        user_datas[128]=CTHL_TD_MotorStep;
        CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
      }
    }
    else
    {
      if((dwXRealPos >=(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheck+CTHL_LianLengthCheckOld)) && (!CTHL_bHandWork_MODE))
      {
        AxisEgmStop(X_AXIS);
        AxisEgmStop(Y_AXIS);
        user_datas[128]=CTHL_TD_MotorStep;
        CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
      }            
    }
  }
}
//拖带合链
void CTHL_TDHL_Proc(void)
{
  //合链              
  if(dwXRealPos >= (CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1)&&!CTHL_bHandWork_MODE)
  {
    CTHL_XSavePosBuff2=dwXRealPos;
    CTHL_HL_VAVLE=1;
  }    
  //合链结束
  if(CTHL_cZipCnt < 2)
  {
    if((dwXRealPos>(CTHL_LianLengthAutoFZCRcheck)))
    {
      CTHL_HL_VAVLE=0;
    }
  }
  else
  {
    if((dwXRealPos>(CTHL_PARAM_HLWZ_LENGTH+CTHL_LianLengthAutoFZCRcheck-CTHL_PARAM_HELIAN_LENGTH)))
    {
      CTHL_HL_VAVLE=0;
    }
  }
}
//无方块处理
void CTHL_TDWFK_Proc(void)
{
}
//拖带电机控制动作  X轴
void CTHL_TD_Motor(void)	 //
{   
  if(dwTickCount >= CTHL_bSFBBAOJIN_Delay)  
  {
    CTHL_SF_ALR_CLR=0;
    SetClr(X_AXIS,MOTOR_ALARM);
    SetClr(Y_AXIS,MOTOR_ALARM);
    
  }
  if(CTHL_PARAM_DBSF_MODE)
  {
    if(!CTHL_SF_ALR_IN)
    {
      bCxcAuto=1;
      CTHL_AutoStep = 201; 
      CTHL_FZ_MotorStep =0;
      CTHL_XCDW_VAVLE=0;
      CTHL_TD_MotorStep =0;   
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);
      CTHL_SetAlarmCode(CTHL_SF_ALR_ALARM,bRunning);   //无拉链自动停机
      CTHL_JCBI_VAVLE = 0;
    }     
  }  
  if((GetAlarm(Y_AXIS)==1 || GetAlarm(X_AXIS)==1) && !CTHL_PARAM_DBSF_MODE && !cBAOjin)
  {                       
    bCxcAuto=1;
    cBAOjin=1;
    AxisEgmStop(X_AXIS);
    AxisEgmStop(Y_AXIS);
    CTHL_AutoStep = 201; 
    CTHL_SetAlarmCode(CTHL_SF_ALR_ALARM,bRunning);   //无拉链自动停机
    CTHL_JCBI_VAVLE = 0;
    //          CTHL_TFK_VAVLE=0;
  }
#if 1
  switch(CTHL_TD_MotorStep)
  {
    //1步开始，纯定位, 没有穿入
  case 1:
    if(dwTickCount >= CTHL_TD_MotorDelay)  //给锁轴时间
    {
      //CTHL_FKDW_VAVLE=0;      
      if(CTHL_PARAM_Test)
        CTHL_TXM_VAVLE    = 0 ;    
      else
        CTHL_TXM_VAVLE    = 1;            //因推斜码和固定方块一起，要避开     
      CTHL_TD_MotorStep = 2;   
      SetPos(X_AXIS, 0);   
      AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1000,600,10,15);  
      AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_First_SPEED * 4/5,DIR_P,1000,600,10,15); 
    }  
    break;
  case 2://检测到过链         
    if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length) 
    {      
      CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
    }    
    if(CTHL_FK_Check_UP)
    {           
      CTHL_XSavePosBuff = dwXRealPos;
      AxisContinueMoveChangeSpeed(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,1000,20,20);
      AxisContinueMoveChangeSpeed(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,1000,20,20);
      //AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_TDDW_LENGTH,
      //                  1000,1000,30,100,50); 
      //AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed*4/5,CTHL_PARAM_TDDW_LENGTH,
      //                  1000,1000,30,100,50);                   
      CTHL_TD_MotorStep = 3;  
    }    
    break;
  case 3:
    if(dwXRealPos>=CTHL_XSavePosBuff+200)
    {
      //CTHL_FKDW_VAVLE=1; 
      CTHL_TXM_VAVLE=0;
      CTHL_TD_MotorStep = 4; 
      //CTHL_TD_MotorDelay = dwTickCount+ 1500;
    }  
    break;
  case 4:
     //定位感应亮时、位置到达时，或X轴停止时，停止X轴
    //if(dwTickCount >= CTHL_TD_MotorDelay)//3S为检测到停机
    //{ 
    //   bCxcAuto=1;
    //   CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常
    //}
    if(CTHL_FKDW_IN_UP)
    {
      CTHL_TD_MotorStep = 5; 
      CTHL_TD_MotorDelay = dwTickCount+ 0;
      AxisMovePosAccDec(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH,
                        1000,1000,30,100,50); 
      AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH,
                        1000,1000,30,100,50);      
    }    
    break;
  case 5: 
    if(!X_DRV&&!Y_DRV)
    {
      CTHL_TD_MotorStep = 0; 
    }
    break;
    //穿合拉头定位
  case 10:   
    if(dwTickCount >= CTHL_TD_MotorDelay)
    {
      CTHL_JCBI_VAVLE=0;
      CTHL_DingDai_VAVLE=0;      
      CTHL_YXC_VAVLE=0;
      CTHL_YFK_VAVLE=0;
      CTHL_TXM_VAVLE=0;      
      CTHL_TD_MotorStep = 11;       
      SetPos(X_AXIS, 0);  //启动位置设为0点      
      if(CTHL_cZipCnt == 0)CTHL_LianLengthCheckOld = 0; 
      
      AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_HLT_SPEED_Speed,CTHL_PARAM_HL_LENGTH, 
                               1000,1000,40,25,00);      
      if(!CTHL_bHandWork_MODE)//在手动穿链模式下不用动电机同步
          AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_HLT_SPEED_Speed,CTHL_PARAM_HL_LENGTH,
                              1000,1000,40,25,00);
      
      CTHL_XSavePosBuff = dwXRealPos;
      bFKCheck_Flag = 0;
      CTHL_XSavePosBuff3 = 0;
    }
    break;
  case 11:
    CTHL_NoLaLian_Speed_Proc(); //最后拉链慢速的处理 
    //慢速合链长度，移动方块检测，躲避方块
    if(dwXRealPos >= CTHL_PARAM_HL_LENGTH+200)  
    {
      if(CTHL_PARAM_Test)
        CTHL_TXM_VAVLE    = 0 ;    
      else
        CTHL_TXM_VAVLE    = 1; 
    }
    //第一方块信号检测
    if(CTHL_FK_Check_UP && !bFKCheck_Flag)
    {
      bFKCheck_Flag = 1; 
      CTHL_XSavePosBuff3= dwXRealPos;
    }  
    //慢速合链长度   如果已经到达下一条的方块也必须开模   ,CTHL_NoLaLian_IN最后几条慢速时感应才退下模
    if(((dwXRealPos >= CTHL_PARAM_HL_LENGTH) && (!CTHL_bHandWork_MODE) && !CTHL_NoLaLian_IN) || (bFKCheck_Flag))
    {
      //码勾先打开15MS再退下模
      if(CTHL_MGuo_VAVLE)
      {
        CTHL_MGuo_VAVLE=0;
        CTHL_GDLT_VAVLE=1;
        CTHL_TD_MotorDelay = dwTickCount + CTHL_PARAM_TMG_TXM_DELAY;;
        if(CTHL_NoLaLian_IN)
          CTHL_TD_MotorDelay = dwTickCount + 20;
      }
      else if(dwTickCount >= CTHL_TD_MotorDelay)
      {      
        CTHL_XSavePosBuff1= dwXRealPos;  
        CTHL_CTXM_VAVLE=0;//推下模
        CTHL_CTXMSSStep=1;
        CTHL_TD_MotorStep = 12;               
      }                           
    }         
    break;
  case 12://退下模加速
    CTHL_TDLength_BH_Proc(0);
    CTHL_NoLaLian_Speed_Proc();
    if(CTHL_FK_Check_UP && !bFKCheck_Flag)
    {
      bFKCheck_Flag = 1; 
      CTHL_XSavePosBuff3= dwXRealPos;
      cGFJAUTO=0;      
    }
    if(CTHL_CTM_Limit_IN)
    {
      CTHL_TD_MotorDelay = dwTickCount + CTHL_PARAM_TuiXiaMo_DELAY;
    }
    else if(dwTickCount >= CTHL_TD_MotorDelay)   //下模离开后延时加速
    {  
      if(!CTHL_bHandWork_MODE)
      {
        bHeLaTouOK = 1;
        if((CTHL_cZipCnt > 1) && (!CTHL_NoLaLian_IN))
        {                    
          if(CTHL_LianLengthCheckOld == 0)
          { 
            CTHL_DataBuff = 6000;
            CTHL_LENTH=CTHL_LianLengthCheck - CTHL_PARAM_HL_LENGTH;
          }
          else if((CTHL_LianLengthCheck < 4000) && (CTHL_LianLengthCheckOld  <4000))          
          {
            CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED*3/4;
            CTHL_LENTH=CTHL_LianLengthCheck + CTHL_LianLengthCheckOld- dwXRealPos;
             
          }
          else if((CTHL_LianLengthCheck < 5000) && (CTHL_LianLengthCheckOld <5000))          
          {
            CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED*4/5;
            CTHL_LENTH=CTHL_LianLengthCheck + CTHL_LianLengthCheckOld- dwXRealPos;           
          }
          else
          {
            CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED;
            CTHL_LENTH=CTHL_LianLengthCheck + CTHL_LianLengthCheckOld- dwXRealPos; 
          }
          AxisMovePosAccDecNotStop(X_AXIS,CTHL_DataBuff,CTHL_LENTH,
                                     1000,CTHL_PARAM_FKDW_Speed,80,50,200);          
        }
        else
        {  
          if(CTHL_NoLaLian_IN)
            CTHL_DataBuff = CTHL_PARAM_FKDWMAN_Speed/2;
          else
            CTHL_DataBuff = CTHL_PARAM_First_SPEED;
          AxisContinueMoveAcc(X_AXIS,CTHL_DataBuff,DIR_P,1500,800,15,15);
        }        
        AxisContinueMoveAcc(Y_AXIS,CTHL_DataBuff,DIR_P,1500,800,15,15); 
        CTHL_TD_MotorStep = 13;
      }
    }   
    break;        
  case 13:
    CTHL_TDLength_BH_Proc(0);
    CTHL_NoLaLian_Speed_Proc();
    CTHL_TDHL_Proc();
    //已经开完模具
    if(CTHL_FK_Check_UP&& !bFKCheck_Flag)
    {
      bFKCheck_Flag = 1; 
      CTHL_XSavePosBuff3= dwXRealPos; 
    }
    else if(bFKCheck_Flag)  //说明第一个方块已经检测到
    {
      CTHL_TD_MotorStep = 14;      
    }
    break; 
  case 14:
    CTHL_TDLength_BH_Proc(1);
    CTHL_NoLaLian_Speed_Proc();
    CTHL_TDHL_Proc(); 
    //无链运行15公分
    if(!CTHL_FK_Check && (dwXRealPos >= (CTHL_XSavePosBuff3 + 1500))) 
    {      
      CTHL_TD_MotorStep = 15;  
      if(CTHL_cZipCnt > 1)CTHL_CTXMSS_FLAG=1;
    }             
    break;
  case 15:
     CTHL_TDLength_BH_Proc(1);
     CTHL_NoLaLian_Speed_Proc();      
     CTHL_TDHL_Proc();
    
    //第二次感应方块， 要做定位
    if(CTHL_FK_Check_UP && (dwXRealPos >= (CTHL_XSavePosBuff3 + 1800)))
    {
      if(!CTHL_bHandWork_MODE)
      {
        if(!CTHL_NoLaLian_IN)
        {
          CTHL_XSavePosBuff = dwXRealPos;
          AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,300,
                            1000,CTHL_PARAM_FKDWMAN_Speed,30,80,50); 
          AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,300,
                            1000,CTHL_PARAM_FKDWMAN_Speed,30,80,50);  
        }
        //长度测量
        bFZ_LengthOK = 1;                               
        if(CTHL_cZipCnt)
        {
          CTHL_LianLengthCheckOld = CTHL_LianLengthCheck;
        }
        CTHL_LianLengthCheck = dwXRealPos - CTHL_XSavePosBuff3; 
        //公分节处理
        if((CTHL_LianLengthCheck <= CTHL_PARAM_GFJ_LENGTH) && (CTHL_cZipCnt>1))
        {
          CTHL_TD_MotorStep=50;
          AxisContinueMoveAcc(X_AXIS,2000,DIR_P,1000,600,15,15);
          AxisContinueMoveAcc(Y_AXIS,2000,DIR_P,1000,600,15,15);
          CTHL_TD_MotorDelay = dwTickCount + 200;
          bFZ_LengthOK= 0;
          CTHL_cZipCnt = 0;
        }
        else
        {          
          CTHL_TD_MotorStep = 16; 
          CTHL_AutoDelay=dwTickCount;
          if(CTHL_cZipCnt<2)CTHL_CTXMSS_FLAG=1;
        } 
      }  
      CTHL_XSavePosBuff = dwXRealPos; 
    }
    CTHL_TDWFK_Proc();
    break;          
  case 16:   
    if(dwXRealPos>=CTHL_XSavePosBuff+200)
    {
      //CTHL_FKDW_VAVLE=1; 
      CTHL_TD_MotorStep = 17; 
      //CTHL_TD_MotorDelay = dwTickCount+ 1500;
    }    
    break;
  case 17:
     //定位感应亮时、位置到达时，或X轴停止时，停止X轴
    //if(dwTickCount >= CTHL_TD_MotorDelay)//3S为检测到停机
    //{ 
    //   bCxcAuto=1;
    //   CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常
    //}
    if(CTHL_FKDW_IN)
    {
      if(CTHL_cZipCnt > 1&&!CTHL_NoLaLian_IN_UP)
      {
        AxisMovePosAccDec(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_TDDW_LENGTH,
                          1000,1000,30,100,50); 
        AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_TDDW_LENGTH,
                          1000,1000,30,100,50); 
      }
      else
      {
        AxisMovePosAccDec(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH,
                          1000,1000,30,100,50); 
        AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH,
                          1000,1000,30,100,50);  
      }
      CTHL_TD_MotorStep = 18; 
      CTHL_TD_MotorDelay = dwTickCount+ 0;  
      bFZ_LengthOK = 1;
    }    
    break;
  case 18:
    if(!X_DRV&&!Y_DRV)
    {
      CTHL_TD_MotorStep = 0; 
    }
    break;
    //由公分节调转
  case 50: 
    if(CTHL_FK_Check_DW)  
    { 
      CTHL_TD_MotorStep = 51;  
      CTHL_TD_MotorDelay = dwTickCount + 100;
      bFZ_LengthOK = 0; 
      CTHL_GF_Change = 1;      
      SetPos(X_AXIS, 0); 
      CTHL_CTXM_VAVLE=0;
      CTHL_XSavePosBuff3=0;
    }
    break;
  case 51: 
    if(dwTickCount >= CTHL_TD_MotorDelay&&!CTHL_FK_Check)
    {
      CTHL_TD_MotorStep = 52;
    }
    break;
  case 52:  
    if(dwXRealPos>=CTHL_PARAM_GFJ_LENGTH)
    {
       CTHL_TryCnt = 0;
       CTHL_LianLengthCheck = 0;
       CTHL_LianLengthCheckOld = 0;
       CTHL_TD_MotorStep = 15; 
    }
    break;
  }   
#endif    
}
//穿入电机动作，Y轴
void CTHL_FZ_Motor(void)	 //
{   
#if 1
  //1步开始,  穿链反转长度开始
  //20步开始  插销定位开始
  
  switch(CTHL_FZ_MotorStep)
  {
    //穿入长度位置
  case 1:           
    if(dwTickCount >= CTHL_FZ_MotorDelay)
    {           
      SetPos(X_AXIS, 0);  //启动位置设为0点
      SetPos(Y_AXIS, 0);  //启动位置设为0点
      FZ_ALARM_FLG=0;
      //Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);
      if(CTHL_cZipCnt<1)//前两次走
      {
        AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_JXM_SPEED,DIR_N,1000,1000,30,50); 
        AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_JXM_SPEED+500,DIR_N,1500,1000,30,50);
      }
      else 
      {
        AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_JXM_SPEED,-CTHL_PARAM_XMCR_SLOW_LENGTH,
                                 1200,CTHL_PARAM_JXM_SPEED,50,100,0);
        AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_JXM_SPEED+500,-CTHL_PARAM_XMCR_SLOW_LENGTH,
                                 1500,CTHL_PARAM_JXM_SPEED,50,100,0); 
      }  
      CTHL_FZ_MotorStep = 2;                              
    }
    break;
  case 2:
    if(abs(dwXRealPos) >=CTHL_PARAM_XMCR_SLOW_LENGTH-100){//提前转换模式
      Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);
    }
    if(abs(dwXRealPos) >= CTHL_PARAM_XMCR_SLOW_LENGTH)//穿链结束，开始走链加速
    {
      CTHL_FZ_MotorDelay = dwTickCount+0;
      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN)
      {                    
        AxisContinueMoveChangeSpeed(X_AXIS,CTHL_PARAM_FZFirst_SPEED,1000,20,100);
        AxisContinueMoveChangeSpeed(Y_AXIS,CTHL_PARAM_FZFirst_SPEED,1000,20,100);
      }
      else
      {
        //长度=反转长度-慢速长度-减速长度-修正长度;
        CTHL_LENTH = CTHL_LianLengthAutoFZCRcheck-abs(dwXRealPos)-CTHL_PARAM_FZJS_LENGTH;
        AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FZ_Speed,-CTHL_LENTH,
                                 1500,CTHL_PARAM_FZDS_Speed,120,100,0); 
        AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZ_Speed+120,-CTHL_LENTH,
                                 1500,CTHL_PARAM_FZDS_Speed,120,100,0);
      }        
      CTHL_FZ_MotorStep++;      
    }
    //电机速度差太大
    if(abs(dwXRealPos)>dwYRealPos_com+4500)
    {
      //CTHL_FZ_MotorStep=100;
      //bCxcAuto=0;//停轴
      //SetEn(Y_AXIS, MOTOR_DISEN);
      SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM);
      //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning);   //反转电机卡滞   
    }
    break;
  case 3:
    if(dwTickCount> CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10)CTHL_TXM_VAVLE=0;  //到达可推出侧推布带时
    //if((dwTickCount>CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10 ) && !CTHL_TXM_VAVLE && !CTHL_TXM_Limit_IN)CTHL_JXM_VAVLE=0;
    if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN)
    {
      CTHL_FZ_MotorStep++; 
    }
    else
    {
      if(abs(dwXRealPos)>=CTHL_LENTH+CTHL_PARAM_XMCR_SLOW_LENGTH)
      {
        AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FZDS_Speed,-CTHL_PARAM_FZJS_LENGTH,
                                 1500,1000,120,80,CTHL_PARAM_FZJSDS_LENGTH);
        AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZDS_Speed,-CTHL_PARAM_FZJS_LENGTH,
                                 1500,1000,120,80,CTHL_PARAM_FZJSDS_LENGTH);         
        CTHL_FZ_MotorStep++; 
      }
    }
    //长度超出报警
    if(CTHL_cZipCnt>=1)
    {
      if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthAutoFZCRcheck)) //拉带报警长度
      {
        bCxcAuto=1;//停轴
        SetEn(Y_AXIS, MOTOR_DISEN);
        CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning);  //穿入方块超出预设长度            
      }
      //方块检测异常(光纤)
      if(abs(dwXRealPos) >= (CTHL_LianLengthAutoFZCRcheck + 300))
      {
        bCxcAuto=1;//停轴
        SetEn(Y_AXIS, MOTOR_DISEN);
        CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning); 
      }  
    }
    else
    {
      if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length){  //位置大于第一条拉链长度    
        bCxcAuto=1;//停轴
        SetEn(Y_AXIS, MOTOR_DISEN);
        CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning);   //穿入方块超出预设长度                  
      }
    }
    //电机速度差太大
    if(abs(dwXRealPos)>dwYRealPos_com+4500)
    {
        //CTHL_FZ_MotorStep=100;
      //bCxcAuto=0;//停轴
      //SetEn(Y_AXIS, MOTOR_DISEN);
      SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM);
      //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning);   //反转电机   
    }
    break;
  case 4:
    if(dwTickCount> CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10)CTHL_TXM_VAVLE=0;  //到达可推出侧推布带时
    //if((dwTickCount>CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10) && !CTHL_TXM_VAVLE && !CTHL_TXM_Limit_IN)CTHL_JXM_VAVLE=0;
    if((abs(dwXRealPos) > 500) && CTHL_FKGY_IN_UP)//位置大于100，且方块感应上升沿
    {               
      CTHL_XSavePosBuff1 = abs(dwXRealPos);
      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN){//记录前两条的方块感应长度；
        //停机距离调整
        CTHL_LENTH=CTHL_PARAM_FZSTOP_LENGTH;
        AxisMovePosAccDec(X_AXIS,800,-CTHL_LENTH,
                          800,800,15,25,0);
        AxisMovePosAccDec(Y_AXIS,600,-CTHL_LENTH,
                          800,800,15,25,0);
      } 
      else {
        //停机距离调整        
        CTHL_LENTH=CTHL_PARAM_FZSTOP_LENGTH;       
        AxisMovePosAccDec(X_AXIS,1000,-CTHL_LENTH,
                          800,800,15,25,0);
        AxisMovePosAccDec(Y_AXIS,600,-CTHL_LENTH,
                          800,800,15,25,0);
      }
      CTHL_FZ_MotorStep++; 
    }
    //长度超出报警
    if(CTHL_cZipCnt>=1)
    {
      if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthAutoFZCRcheck)) //拉带报警长度
      {
        bCxcAuto=1;//停轴
        SetEn(Y_AXIS, MOTOR_DISEN);
        CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning);  //穿入方块超出预设长度            
      }
      //方块检测异常(光纤)
      if(abs(dwXRealPos) >= (CTHL_LianLengthAutoFZCRcheck + 300))
      {
        bCxcAuto=1;//停轴
        SetEn(Y_AXIS, MOTOR_DISEN);
        CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning); 
      }  
    }
    else
    {
      if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length){  //位置大于第一条拉链长度    
        bCxcAuto=1;//停轴
        CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning);   //穿入方块超出预设长度                  
      }
    }
    //电机速度差太大
    if((abs(dwXRealPos)>dwYRealPos_com+4500)&&!FZ_ALARM_FLG)
    {
       //CTHL_FZ_MotorStep=100;
      //bCxcAuto=0;//停轴
      //SetEn(Y_AXIS, MOTOR_DISEN);
      SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM);
      //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning);   //反转电机  
    }
    break;
  case 5:       
    if(!Y_DRV && !X_DRV)
    {
      if((CTHL_cZipCnt <= 1))
      {
        CTHL_LianLengthCheck = abs(dwXRealPos);   //为第一条长度给值
        CTHL_LianLengthAutoFZCRcheck = abs(dwXRealPos);      
        CTHL_LianLengthAutoFZcheck=abs(dwXRealPos);
      }
      Set_Ctrlmode_trans(Y_AXIS,POS_MODE);  
      CTHL_FZ_MotorStep = 0; 
    } 
    break;
    //20步开始小插定位
  case 20: //此处最好算好定位长度相减
    if(dwTickCount >= CTHL_FZ_MotorDelay)
    {
      CTHL_FZ_MotorStep = 21; 
      SetPos(Y_AXIS, 0);  //启动位置设为0点
      SetPos(X_AXIS, 0);  //启动位置设为0点
    }
    break;
  case 21:
    if(CTHL_PARAM_CXDW_Length)
    {            
      CTHL_FZ_MotorStep = 22;       
      AxisMovePosAccDec(Y_AXIS,2*CTHL_PARAM_XCCR_SPEED,-CTHL_PARAM_CXDW_Length,1000,1000,20,20,0);   
      AxisMovePosAccDec(X_AXIS,2*CTHL_PARAM_XCCR_SPEED,-CTHL_PARAM_CXDW_Length,1000,1000,20,20,0);              
      
    }
    break;
  case 22: 
    if(!Y_DRV && !X_AXIS )
    {
      CTHL_FZ_MotorStep = 0; 
      Set_Ctrlmode_trans(Y_AXIS,POS_MODE);                
    }         
    break;  
  case 100: 
    if(dwYRealPos_com>=CTHL_PARAM_XMCR_SLOW_LENGTH+100&&dwYRealPos_com<=CTHL_LianLengthAutoFZCRcheck-500)//穿过慢速长度
    {
       SetPos(Y_AXIS, 0);
       CTHL_FZ_MotorStep = 101; 
       CTHL_XSavePosBuff1=dwYRealPos_com;
    } 
    else
    {
      bCxcAuto=0;//停机       
      CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning);   //反转电机卡滞
    }
    break;  
  case 101: 
     CTHL_FZ_MotorStep = 102;                  
     Set_Ctrlmode_trans(Y_AXIS,POS_MODE); 
     CTHL_FZ_MotorDelay=dwTickCount+50;     
    break; 
  case 102: 
     CTHL_FZ_MotorStep = 103;                  
     AxisMovePosAccDec(Y_AXIS,1500,-500,1500,1500,20,20,0);        
    break; 
  case 103: 
    if(!Y_DRV)
    {
      if(dwYRealPos_com>=CTHL_XSavePosBuff1+500)
      {
        Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);
        SetAlarmCode(CTHL_ALARM_ADDR,0);
        CTHL_FZ_MotorStep = 4; 
        FZ_ALARM_FLG=1;
      }
      else
      {
        bCxcAuto=0;//停机  
        CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning);   //反转电机卡滞
      }
    }
     break;

  }
  
  
#endif    
}



//装拉头动作
void CTHL_ZLT_Step(void)
{
  
  switch(CTHL_ZhuangLiaoStep)
  {
  case 1:
    
    if(bZhuangLiaoOkFlg)
    {
      CTHL_ZhuangLiaoStep = 0;
    }
    else 
    {      
      if(((CTHL_MGuo_VAVLE && CTHL_LTou_Check) || (!CTHL_MGuo_VAVLE))&&CTHL_GDLT_VAVLE )//说明没有拉头
      { 
         /*if(bRunning&&CTXM_DOWN_TIME&&CTHL_cZipCnt>1) 
        {
          if(CTHL_TLiaoStep == 0)
          {
            CTHL_TLiaoStep = 1;           
            CTHL_ZhuangLiaoStep = 40; 
          }
        }
        else*/
        {
          //穿头模和一次穿入必须在原位,接料不能有输出
          if(CTHL_CTM_Origin_IN && !CTHL_CTXM_VAVLE)
          {  
            CTHL_JLTou_VAVLE = 0;
            CTHL_GDLT_VAVLE=1;
            CTHL_MGuo_VAVLE = 0;                     
            if(CTHL_TLiaoStep == 0)
            {
              CTHL_TLiaoStep = 1;        
              CTHL_ZhuangLiaoStep = 2; 
            }
          }        
          else if(dwTickCount >= CTHL_ZhuangLiaoDelay + 1000)  //警告横送拉必须保证穿头模和一次穿入在原位,接料不能有输出
          {
            CTHL_wltzi=1;
            CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning);
            
          } 
        }
      }
      else if(dwTickCount >= CTHL_ZhuangLiaoDelay)//已经有拉头直接跳到结束
      {
        CTHL_ZhuangLiaoDelay = dwTickCount + 40;
        CTHL_ZhuangLiaoStep = 9;
      }  
    }
    break; 
  /*  case 40:        
    if(CTHL_TLiaoStep == 0)  //推料已经完成
    {      
      CTHL_HSLiao_VAVLE = 1;        
      CTHL_ZhuangLiaoStep = 41;        
      CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
    }
    break;
  case 41:      
    if(!CTHL_HSLiao_Origin_IN&&!CTHL_CTM_Minid_IN)//离开原点
    {
      CTHL_ZhuangLiaoStep = 42;
      CTHL_ZhuangLiaoDelay=dwTickCount+HSLIAO_CHECK_TIME+1;//横送过程检测
    
    }    
    break;
  case 42:      
    if(dwTickCount>=CTHL_ZhuangLiaoDelay)//兼容没有做缓冲的情况
    {
        if(CTHL_CTM_Origin_IN )//穿头模和第一次合链在原位
        {
          CTHL_ZhuangLiaoStep = 3;
              
        }
        else
        {
          CTHL_ZhuangLiaoStep = 43;
          CTHL_HSLiao_VAVLE = 0;
          CTHL_ZhuangLiaoDelay = dwTickCount + 20;
       }
     }        
    break;
  case 43:        
    if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {
      if(CTHL_CTM_Origin_IN )//穿头模和第一次合链在原位
      {
        CTHL_ZhuangLiaoStep = 3;
        CTHL_HSLiao_VAVLE = 1;
        CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
      } 
    }
    break;*/
  case 2:        
    if(CTHL_TLiaoStep == 0)  //推料已经完成
    {
      CTHL_ZhuangLiaoDelay = dwTickCount+ 500;
      CTHL_GDLT_VAVLE = 1;
      CTHL_MGuo_VAVLE = 0;      
      if(!CTHL_JLTou_VAVLE)
      {
        CTHL_ZhuangLiaoStep = 3;
        CTHL_HSLiao_VAVLE = 1;
        PRV_HSLIAO_OUT_DELAY=dwTickCount;
      }
    }
    break;
  case 3:
    //横送到位
    if(CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_Origin_IN)
    {
      HSLIAO_OUT_DELAY=dwTickCount-PRV_HSLIAO_OUT_DELAY;
      CTHL_ZhuangLiaoStep = 4;     
      CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_H_JLiao_DELAY;
    }
    else if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {
      CTHL_ZhuangLiaoStep = 30;
      
    }
    break;
  case 4:
    if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {
      CTHL_ZhuangLiaoStep = 5;
      CTHL_JLTou_VAVLE = 1;    //接拉头输出,不需要时间
      CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_JLTOU_DELAY;
    }  
    break;
  case 5:    
    if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {
      CTHL_ZhuangLiaoStep = 6;
      CTHL_GDLT_VAVLE = 0;    //接拉头输出,不需要时间
      CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_GDLT_DELAY;
    }
    break;
  case 6:       
    if(dwTickCount >= CTHL_ZhuangLiaoDelay)  //延时码勾
    {
      CTHL_MGuo_VAVLE = 1; //码勾输出
      CTHL_ZhuangLiaoStep = 7;
      CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_HSLiaoOff_DELAY;
      bTuiLaTouOkFlg = 0;   //认为拉头没法带出
    }
    break;
  case 7:  
    if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {
      CTHL_HSLiao_VAVLE = 0; 
      CTHL_ZhuangLiaoStep = 8;
      CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
    }
    break;
  case 8:  
    if(!CTHL_HSLiao_Limit_IN)  //
    {
      CTHL_ZhuangLiaoStep = 9;
      //CTHL_TLiaoStep = 1; //推拉头
      CTHL_ZhuangLiaoDelay = dwTickCount +CTHL_PARAM_CheckLT_DELAY;   //也是给下模能上升的时间
    }          
    else if (CTHL_LTou_Check)  //时间内信号这说明没有拉头
    {
      //重送3次
      CTHL_TryCnt++;
      CTHL_JLTou_VAVLE = 0;    //接拉头关
      CTHL_MGuo_VAVLE = 0;     //码勾打开
      CTHL_GDLT_VAVLE = 1;
      if(CTHL_TryCnt == 1)//重新横送一次
      {
        CTHL_ZhuangLiaoStep = 21;    //重新送3次   
        CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
      }
      else if(CTHL_TryCnt == 2||CTHL_TryCnt == 3)//重新装料2次
      {
        CTHL_ZhuangLiaoStep = 20;    //重新送3次   
        CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
      }
      else if(CTHL_TryCnt == 4)
      {
        CTHL_TryCnt=0;
        bZhuangLiaoOkFlg = 0;
        CTHL_ZhuangLiaoStep = 0;
        CTHL_wltzi=1;
        user_datas[127]=5;
        CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);
        
      }   
    }
    else if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {
      CTHL_wltzi=1;
      CTHL_SetAlarmCode(CTHL_HSLiao_Limit_ALARM,bRunning);
      if(bRunning)
      {
        CTHL_ZhuangLiaoStep = 0 ;
      }  
    }  
    break;
  case 9:   
    if(CTHL_LTou_Check)  //时间内信号这说明没有拉头
    {
      CTHL_TryCnt++;
      CTHL_JLTou_VAVLE = 0;    //接拉头关
      CTHL_MGuo_VAVLE = 0;     //码勾打开
      CTHL_GDLT_VAVLE = 1;
      if(CTHL_TryCnt == 1)//重新横送一次
      {
        CTHL_ZhuangLiaoStep = 21;    //重新送3次   
        CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
      }
      else if(CTHL_TryCnt == 2||CTHL_TryCnt == 3)//重新装料2次
      {
        CTHL_ZhuangLiaoStep = 20;    //重新送3次   
        CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
      }
      else if(CTHL_TryCnt == 4)
      {
        CTHL_TryCnt=0;
        bZhuangLiaoOkFlg = 0;
        CTHL_ZhuangLiaoStep = 0;
        CTHL_wltzi=1;
        user_datas[127]=5;
        CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);
        
      } 
    }
    else if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {
      bZhuangLiaoOkFlg = 1;
      CTHL_TLiaoStep = 1; //推拉头
      CTHL_ZhuangLiaoStep = 0;
    }
    break;
    //装拉头2次
  case 20: 
    if(CTHL_HSLiao_Origin_IN)
    {
      CTHL_JLTou_VAVLE = 0;    //接拉头关
      CTHL_MGuo_VAVLE = 0;     //码勾打开
      CTHL_GDLT_VAVLE = 1;
      CTHL_ZhuangLiaoStep = 1;
      CTHL_ZhuangLiaoDelay = dwTickCount + 10;
    }
    else if(dwTickCount >=CTHL_ZhuangLiaoDelay)
    {
      CTHL_wltzi=1;
      CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning);
    }  
    break;
  case 21: //重新横送一次
    if(CTHL_HSLiao_Origin_IN)
    {
      CTHL_JLTou_VAVLE = 0;    //接拉头关
      CTHL_MGuo_VAVLE = 0;     //码勾打开
      CTHL_GDLT_VAVLE = 1;
      CTHL_ZhuangLiaoStep = 2;
      CTHL_ZhuangLiaoDelay = dwTickCount + 10;
    }
    else if(dwTickCount >=CTHL_ZhuangLiaoDelay)
    {
      CTHL_wltzi=1;
      CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning);
    }  
    break;    
  case 30:
    CTHL_HSLiao_VAVLE=0;
    CTHL_ZhuangLiaoStep = 31;
    CTHL_ZhuangLiaoDelay = dwTickCount + 200;
    break;
  case 31:
    if(dwTickCount >=CTHL_ZhuangLiaoDelay)
    {
      CTHL_HSLiao_VAVLE=1;
      CTHL_ZhuangLiaoStep = 32;
      CTHL_ZhuangLiaoDelay = dwTickCount + 3000;
      
    }
    break;
  case 32:
    if(CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_Origin_IN)
    {
      CTHL_ZhuangLiaoStep = 4;
      CTHL_ZhuangLiaoDelay = dwTickCount + 10;
    }
    
    else if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {
      
      CTHL_wltzi=1;
      CTHL_SetAlarmCode(CTHL_HSLiao_Limit_ALARM,bRunning);
      CTHL_ZhuangLiaoStep = 0;
      CTHL_HSLiao_VAVLE = 0; 
    }
  }  
}

void CTHL_TLiao_Step(void)
{
  switch(CTHL_TLiaoStep)
  {
  case 1:
    if(bTuiLaTouOkFlg)
    {
      CTHL_TLiaoStep = 0; 
    }
    else
    {
      CTHL_TLiao_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
      CTHL_TLiaoStep = 2;
    }
    break;
  case 2:    //横送料在原位
    if(CTHL_HSLiao_Origin_IN && !CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_VAVLE)
    {
      CTHL_TLiao_VAVLE = 1;    //推料(推拉头)输出
      CTHL_TLiao_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
      CTHL_TLiaoStep = 3;
    }
    else if(dwTickCount >= CTHL_TLiao_Delay) 
    {
      CTHL_wltzi=1;
      CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,1);   
      
    }
    break;
  case 3:  //推拉头到位
    if(CTHL_TLiao_Limit_IN)
    { 
      CTHL_TLiao_Delay = dwTickCount + CTHL_PARAM_TLiao_BACKDELAY;
      CTHL_TLiaoStep = 4;
    }
    else if(dwTickCount >= CTHL_TLiao_Delay) 
    {
      CTHL_wltzi=1;
      CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,1);  
      
    }
    break;
  case 4:
    if(dwTickCount >= CTHL_TLiao_Delay)
    {
      CTHL_TLiao_VAVLE       = 0;
      CTHL_TLiao_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME; 
      CTHL_TLiaoStep = 5;
    }
    break;
  case  5:
    if(!CTHL_TLiao_Limit_IN)
    {
      CTHL_TLiaoStep = 0;
      CTHL_TLiao_Delay = dwTickCount;
      bTuiLaTouOkFlg    = 1;
    }
    else if(dwTickCount >= CTHL_TLiao_Delay) 
    {
      CTHL_wltzi=1;
      CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,0);   
      
    }
    break;
  } 
  
}
//振动盘控制
void CTHL_ZhenDongAction(void)
{
 
#if 0  
  
  //    if(bRunning)
  //    {
  if(!CTHL_PARAM_ZDP_AUTO)
  {
    
    if(CTHL_ZhenDongPian_OUT == 0)
    {
      if(!CTHL_ZDP_IN)
      {
        if(dwTickCount >= CTHL_ZDP_Delay)
        {
          CTHL_ZhenDongPian_OUT = 1;
          CTHL_ZDP_Delay = dwTickCount + CTHL_PARAM_ZDP_Stop_TIME;//CTHL_PARAM_ZDP_Stop_TIME + 50;
        }
      }
      else
      {
        CTHL_ZDP_Delay = dwTickCount + CTHL_PARAM_ZDP_Start_TIME;//CTHL_PARAM_ZDP_Start_TIME;
      }   
    }
    else
    {  
      if(CTHL_ZDP_IN)
      {  
        if(dwTickCount >= CTHL_ZDP_Delay)
        {
          CTHL_ZhenDongPian_OUT = 0;  
          CTHL_ZDP_Delay = dwTickCount + CTHL_PARAM_ZDP_Start_TIME;//CTHL_PARAM_ZDP_Start_TIME
        }
      }
      else
        CTHL_ZDP_Delay = dwTickCount + CTHL_PARAM_ZDP_Stop_TIME;//CTHL_PARAM_ZDP_Stop_TIME+ 50;
    }
    
  }
  
  else if (CTHL_PARAM_ZDP_AUTO ==1 )
  {
    CTHL_ZhenDongPian_OUT=0;
    CTHL_ZDP_Delay = dwTickCount + 0;
  }
  else if(CTHL_PARAM_ZDP_AUTO == 2)
  {
    CTHL_ZhenDongPian_OUT=1;
    CTHL_ZDP_Delay = dwTickCount + 0;
    
  }
#endif    
}

unsigned char Manu_HSL_STEP;
unsigned long Manu_HSL_DELAY;
void CTHL_Manu_HSongLiao(void)
{
  switch(Manu_HSL_STEP)
  {
 case 1:
    if(dwTickCount>=Manu_HSL_DELAY)
    {
      CTHL_MGuo_VAVLE=1;
      Manu_HSL_STEP=2;
      Manu_HSL_DELAY=dwTickCount+1000;
    }
    break;
  case 2:
    if(CTHL_MGuo_VAVLE&&CTHL_LTou_Check)//无拉头
    {
      CTHL_GDLT_VAVLE=1;
      CTHL_MGuo_VAVLE=0;
      Manu_HSL_DELAY=dwTickCount+CTHL_VAVLE_ERROR_TIME;
      Manu_HSL_STEP=3;
    }
    else if(dwTickCount>=Manu_HSL_DELAY)
    {
      Manu_HSL_STEP=0;
      CTHL_SetAlarmCode(CTHL_YLT_CHECK_ALARM,0);//夹拉头异常   
    }    
    break;
  case 3:
      if(CTHL_CTM_Origin_IN &&!CTHL_TLiao_Limit_IN)//下模在原位
      {
        CTHL_HSLiao_VAVLE = 1;
        Manu_HSL_STEP=0;
      }
      else if(dwTickCount>=Manu_HSL_DELAY)
      {
        Manu_HSL_STEP=0;
        if(!CTHL_CTM_Origin_IN)CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,0); 
        if(CTHL_TLiao_Limit_IN)CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,0);
        
      }
    break;
  }
}
//手动动作
void CTHL_ManualAction(void)
{ 
  
  if(CTHL_bBaoJin) //报警清除加松轴
  {
    CTHL_bBaoJin=0;
    SetEn(Y_AXIS, MOTOR_EN);
    SetEn(X_AXIS, MOTOR_EN);
    SetAlarmCode(CTHL_ALARM_ADDR,0); 
    if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)
    {
      SetClr(X_AXIS, MOTOR_NOALARM);
      SetClr(Y_AXIS, MOTOR_NOALARM);
      CTHL_bSFBBAOJIN_Delay= dwTickCount + 500;
    }
    if(CTHL_PARAM_DBSF_MODE)
    {
      if(!CTHL_SF_ALR_IN)
      {
        CTHL_SF_ALR_CLR=1;
        CTHL_bSFBBAOJIN_Delay= dwTickCount + 500;
        
      }
    }
  }
  
  if(bRunning == 0)  
  { 
    CTHL_Manu_HSongLiao();
    if(bClearTotal) //切断计数清零
    {
      bClearTotal       = 0;
      ClrcToTal(QDCT_TOTAL_ADDR);
    }
    if(CTHL_bHSL)     //手动横送料   
    {
      CTHL_bHSL= 0;      
      if(CTHL_HSLiao_VAVLE)CTHL_HSLiao_VAVLE = 0;//~XY_CTHL_HSLiao_VAVLE;
      else 
      {       
        CTHL_CTXM_VAVLE=0;
        CTHL_TLiao_VAVLE=0;
        CTHL_JLTou_VAVLE=0;
        CTHL_GDLT_VAVLE=0;        
        if(Manu_HSL_STEP==0)Manu_HSL_STEP=1;
        Manu_HSL_DELAY=dwTickCount+20;
      }
    }  
   
    if(CTHL_bTLiao )  //手动推料(推拉头)
    {
      CTHL_bTLiao = 0;           
      if(CTHL_TLiao_VAVLE)CTHL_TLiao_VAVLE = 0;        
      else if(!CTHL_HSLiao_VAVLE && CTHL_HSLiao_Origin_IN)CTHL_TLiao_VAVLE = 1;
      else CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,0); // 横送料不能输出//条件警告
      
    }     
    if(CTHL_bJLT)//接拉头
    {
      CTHL_bJLT = 0;
      if(CTHL_JLTou_VAVLE)CTHL_JLTou_VAVLE = 0;        
      else if( !CTHL_CTXM_VAVLE && CTHL_CTM_Origin_IN)
      {
        CTHL_MGuo_VAVLE = 0;
        CTHL_JLTou_VAVLE = 1;
      } 
      else//条件警告
      {
        CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,0); 
      }
    }    
    if(CTHL_bMG)                   //码勾
    {
      CTHL_bMG = 0;
      CTHL_MGuo_VAVLE = !CTHL_MGuo_VAVLE;
    } 
    if(CTHL_bGDLT)//夹拉头
    {      
       CTHL_bGDLT = 0;
       if(!CTHL_GDLT_VAVLE)CTHL_MGuo_VAVLE = 0;       
       CTHL_GDLT_VAVLE = !CTHL_GDLT_VAVLE;
    }  
    
    if(CTHL_bJXM)                   //夹前码
    {
      CTHL_bJXM = 0;
      CTHL_JXM_VAVLE = !CTHL_JXM_VAVLE;
    } 
    
    if(CTHL_bTXM)     //推斜码
    {
      CTHL_bTXM = 0;
      if((CTHL_JXM_VAVLE || !CTHL_CTXM_VAVLE)  && !CTHL_TXM_VAVLE && !CTHL_TFK_VAVLE)
      {
        CTHL_TXM_VAVLE=1;
      }
      else if(CTHL_TXM_VAVLE)
      {
        CTHL_TXM_VAVLE=0;
      }
      else
        CTHL_SetAlarmCode(CTHL_JXM_Limit_ALARM,0); //夹前码要输出     
      
    } 
    if(CTHL_bQMDW)                   //前码定位
    {
      CTHL_bQMDW = 0;
      CTHL_QMDW_VAVLE = !CTHL_QMDW_VAVLE;
    } 
    
    
    if(CTHL_bTFK)                   //推方块
    {
      CTHL_bTFK = 0;
      if(!CTHL_TFK_VAVLE && !CTHL_TXM_VAVLE)
        CTHL_TFK_VAVLE = 1;
      else if(CTHL_TFK_VAVLE)
        CTHL_TFK_VAVLE = 0;
      
    } 
    if(CTHL_bFKDW)                   //方块检测
    {
      CTHL_bFKDW = 0;
      //CTHL_FKDW_VAVLE = !CTHL_FKDW_VAVLE;
    } 
    if(CTHL_bHL)                   //合链
    {
      CTHL_bHL = 0;
      CTHL_HL_VAVLE = !CTHL_HL_VAVLE;
    } 
    if(CTHL_bYFK)                   //压方块
    {
      CTHL_bYFK = 0;
      CTHL_YFK_VAVLE = !CTHL_YFK_VAVLE;
    } 
    if(CTHL_bYCX)                   //压小插
    {
      CTHL_bYCX = 0;
      CTHL_YXC_VAVLE = !CTHL_YXC_VAVLE;
    } 
    if(CTHL_bXCDW)                   //插销定位
    {
      CTHL_bXCDW = 0;
      CTHL_XCDW_VAVLE = !CTHL_XCDW_VAVLE;
    } 
    if(CTHL_bXCBI)                   //小插摆臂
    {
      SetEn(X_AXIS, MOTOR_EN);   
      SetEn(Y_AXIS, MOTOR_EN);  
      CTHL_bXCBI = 0;
      CTHL_JCBI_VAVLE = !CTHL_JCBI_VAVLE;
    } 
    
    if(CTHL_bCXDD)                   //小插摆臂
    {
      CTHL_bCXDD = 0;
      CTHL_DingDai_VAVLE = !CTHL_DingDai_VAVLE;
    } 
    
    if(CTHL_bCTXM)                  //下模
    {
      CTHL_bCTXM = 0;
      CTHL_TXM_VAVLE=0;
      if(CTHL_CTXM_VAVLE)
      {
        CTHL_CTXM_VAVLE = 0;
        CTHL_JLTou_VAVLE = 0;
        CTHL_JXM_VAVLE=0;
      }    
      else if(!CTHL_HSLiao_VAVLE && CTHL_HSLiao_Origin_IN && (CTHL_JXM_VAVLE || !CTHL_TXM_VAVLE))                
      { 
        
        CTHL_CTXM_VAVLE = 1;
        CTHL_JLTou_VAVLE=0;
        CTHL_JXM_VAVLE=1;
      }  
    } 
    //自动装料
    if(CTHL_bAutoZhuangLiao)
    {
      CTHL_bAutoZhuangLiao = 0;
      if(CTHL_ZhuangLiaoStep == 0)
      {
        bTuiLaTouOkFlg = 1;
        bZhuangLiaoOkFlg = 0;
        CTHL_ZhuangLiaoStep = 1;
        CTHL_MGuo_VAVLE = 1;   //判断是否已经有料  
        CTHL_GDLT_VAVLE=0;
        CTHL_TryCnt = 0;
        CTHL_ZhuangLiaoDelay = dwTickCount + 1000;
      }  
    }
    if(CTHL_bYJDW)  //一键定位
    {
      CTHL_bYJDW=0;
      //CTHL_FKDW_VAVLE = 0;
      CTHL_YFK_VAVLE=0;
      CTHL_YXC_VAVLE=0;
      CTHL_HL_VAVLE=0;
      CTHL_CTXM_VAVLE=0;
      CTHL_TXM_VAVLE=0;
      CTHL_JCBI_VAVLE=0;
      CTHL_QMDW_VAVLE=0;
      CTHL_TD_MotorDelay = dwTickCount + 100; 
      if(CTHL_TD_MotorStep==0)
      {
        CTHL_CTXM_VAVLE =0;
        if(GetEn(X_AXIS) == MOTOR_EN)
        {
          SetEn(X_AXIS, MOTOR_DISEN);   
          SetEn(Y_AXIS, MOTOR_DISEN);  
          CTHL_TD_MotorDelay = dwTickCount + 200;           
        } 
        else
          CTHL_TD_MotorDelay = dwTickCount + 100; 
          CTHL_TD_MotorStep=1;
      }
    } 
    
    //X轴Y轴电机测试 
    if(CTHL_bCLMotor_P)
    { 
      
      SetEn(Y_AXIS, MOTOR_DISEN);
      SetEn(X_AXIS, MOTOR_DISEN);
      //CTHL_FKDW_VAVLE=0;
      if(CTHL_FK_Check_UP)
      {
        SetPos(X_AXIS, 0); 
        SetPos(Y_AXIS, 0); 
      }
      
      if(!X_DRV)
      {
        //                Y轴       运行速度                    启动速度        加速度  减速度                 
        AxisContinueMoveAcc(X_AXIS,600 ,DIR_P,600,600,15,15);
        AxisContinueMoveAcc(Y_AXIS,580,DIR_P,580,580,15,15); 
      }      
    }
    
    //在压方块电磁阀输出情况下，方块感应后电机要自动停止
    if(CTHL_FKGY_IN && CTHL_YFK_VAVLE && CTHL_bCLMotor_P)
    {
      CTHL_bCLMotor_P=0;
      AxisEgmStop(Y_AXIS);
      AxisEgmStop(X_AXIS);
    }
    
    if(CTHL_bCLMotor_N)  //后退限位已经取消
    {
      SetEn(Y_AXIS, MOTOR_DISEN);
      SetEn(X_AXIS, MOTOR_DISEN);
      //CTHL_FKDW_VAVLE=0;
      if(CTHL_FK_Check_UP)
      {
        SetPos(X_AXIS, 0); 
        SetPos(Y_AXIS, 0); 
      }
      //压方块电磁阀没有输出时，方块感应信号不限制
      if(!CTHL_FKGY_IN || !CTHL_YFK_VAVLE)
      {
        if(!X_DRV && (!CTHL_QMDW_IN ||!CTHL_QMDW_VAVLE))
        {  
          //                Y轴       运行速度                    启动速度        加速度  减速度
          AxisContinueMoveAcc(Y_AXIS,630,DIR_N,630,630,15,15);
          AxisContinueMoveAcc(X_AXIS,600,DIR_N,600,600,15,15);
        }
        
        if(CTHL_FKGY_IN && CTHL_YFK_VAVLE)
        {
          CTHL_bCLMotor_P=0;
          AxisEgmStop(Y_AXIS);
          AxisEgmStop(X_AXIS);
        }
        
      }      
      if(CTHL_QMDW_IN && CTHL_QMDW_VAVLE)
      {
        AxisEgmStop(Y_AXIS);
        AxisEgmStop(X_AXIS);
        
      }      
    }    
    if(!CTHL_bCLMotor_P && !CTHL_bCLMotor_N && (CTHL_TD_MotorStep == 0))
    {
      AxisEgmStop(Y_AXIS);
      AxisEgmStop(X_AXIS);
    }
    
  }        
}

#endif