GC23005/Machine/FuXiaoWeiMachine/FXWChuantou_HeLian.c

#include "global.h"

#if  FU_XIAO_WEI_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 char CD_ORIGIN_FLG=0;
unsigned short XCCR_LENGTH;
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_CUDAI_Motor(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)
  {
     AxisEgmStop(X_AXIS);       
     AxisEgmStop(Y_AXIS); 
    //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;
  
  buff_pulse  = CTHL_PARAM_TDCYCLE_PULSE;
  buff_dist   =  CTHL_PARAM_CDCYCLE_LENGTH;   
  ZGearRatio  =  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);
  set_com_servo_param(Z_AXIS,CTHL_PARAM_TDCYCLE_PULSE,CTHL_PARAM_CDCYCLE_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,1);//X轴正常模式。
  ReadServoAlarm_Enable(X_AXIS,0);//X轴正常模式。
  ReadServoEncode_Enable(X_AXIS,0);
  
  SetServoComUse(Y_AXIS,1);//Y轴伺服扭矩通讯。
  ReadServoAlarm_Enable(Y_AXIS,1);
  ReadServoEncode_Enable(Y_AXIS,1);
  
  SetServoComUse(Z_AXIS,1);//Z轴伺服扭矩通讯。
  ReadServoAlarm_Enable(Z_AXIS,0);
  ReadServoEncode_Enable(Z_AXIS,0);
  
  
  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_LianLengthCheckFK_GL;//servo_z.vdi_value;//servo_z.init_step;//;//CTXM_DOWN_TIME;//dwYRealPos_com;//servo_y.init_step;//CTHL_ZhuangLiaoStep;
  user_datas[122] = CTHL_LianLengthCheckFK_DW;
  user_datas[123] = CTHL_AutoStep;
  user_datas[124] = CTHL_LianLengthCheck;//CTHL_ZhuangLiaoStep;
  user_datas[125] = CTHL_LianLengthCheckOld;//CTHL_TLiaoStep;
  user_datas[126] = CTHL_CD_MotorStep;//CTHL_FZ_MotorStep;//CTHL_TD_MotorStep;
  //    user_datas[127] = CTHL_cZipCnt;//CTHL_ZhuangLiaoStep;
  // user_datas[128] = CTHL_FZ_MotorStep;//CTHL_LianLengthAutolengthcheck;
  CTHL_bServo_Com_X=servo_x.IO_TO_COM;
  CTHL_bServo_Com_Y=servo_y.IO_TO_COM; 
  CTHL_bServo_Com_Z=servo_z.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_CUDAI_Motor();
}

void CTHL_AlarmProtect(void)
{ 

}
//自动动作
void CTHL_AutoStepAction(void)
{  
  unsigned short freq;
  static unsigned char hemoflag1=0,hemoflag2=0;
  //测试用
  if(dwTickCount >= CTHL_TestDelay)
  {
    CTHL_TestDelay = dwTickCount + 200;
    CTHL_TestLED = !CTHL_TestLED;   
    CTHL_Test = 0;
  }
  if(CTHL_bHandWork_MODE)CTHL_bHandWork_State=1;else CTHL_bHandWork_State=0;
  if(bRunning)
  {
    switch(CTHL_AutoStep)
    {
   case 1:
      if(dwTickCount >= CTHL_AutoDelay)
      {    
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
        {

          CTHL_YXC_VAVLE=1;
          CTHL_YFK_VAVLE=1;
          CTHL_TXM_VAVLE=0;          
          //设置反转电机的扭矩和速度的设置       
          freq=CTHL_PARAM_FZ_Speed+SERVO_PARAM_SPEED_DIFFER;
          if(CTHL_cZipCnt >=1)
          {
            set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW); 
            Set_Ctrlmode_trans(Z_AXIS,POS_MODE); 
          }
          else 
          {
            set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW);
            Set_Ctrlmode_trans(Z_AXIS,POS_MODE); 
          }
          Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
          set_servo_postotarr_limit(Z_AXIS,SERVO_PARAM_CUDAI_TRARRMAX,50000,50000,SERVO_TARR_CW);
          //
          set_servo_postotarr_limit(X_AXIS,80,20000,20000,SERVO_TARR_CCW);
          Set_Ctrlmode_trans(X_AXIS,POS_MODE);
          bCxcAuto=0;
          if(CTHL_cZipCnt > 1)  CTHL_AutoDelay = dwTickCount + 1;//拉头检测时间            
          else CTHL_AutoDelay = dwTickCount + 180;//拉头检测时间            
          if(CTHL_ZhuangLiaoStep == 0)
          { 
            CTHL_ZhuangLiaoStep = 1;
            CTHL_TryCnt = 0;
          } 
          CTHL_GF_Change = 0;
          CTHL_AutoStep = 2;          
        }
      }
      break;
    case 2:
      if(bZhuangLiaoOkFlg && (CTHL_ZhuangLiaoStep == 0)&&(CTHL_CTXMSSStep==0))
      {
        if(dwTickCount >= CTHL_AutoDelay )
        {
          if(CTHL_HSLiao_Origin_IN||CTHL_CTM_Minid_IN&&!CTHL_HSLiao_VAVLE)
          {            
            //有拉头，推斜码到位不能亮
            if(!CTHL_LTou_Check && !CTHL_TXM_Limit_IN)
            {
              if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
              {
                if(!CTHL_PARAM_XMZM_MODE) CTHL_QMDW_VAVLE = 1;   
                CTHL_FKDW_VAVLE = 0;
                CTHL_CTXM_VAVLE = 1; //下模上升 
                CTHL_JLTou_VAVLE = 0;  //接拉头
                CTHL_AutoStep = 3;
                CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
              }                                                   
            }
            else if(dwTickCount >= (CTHL_AutoDelay+3000))//无拉头告警，并按下启动，重新装拉头
            {
              CTHL_wltzi=1;
              user_datas[127]=1;
              CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);  // 没有拉头  
            }
          }
          else if(dwTickCount >= (CTHL_AutoDelay+4000))
          {
            CTHL_wltzi=1;
            CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning);//横送料原位异常                         
          }
        }
      }
      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(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,2000,-CTHL_PARAM_JMDW_LENGTH,1000,800,10,10,0); //前拖带松一点带 
          AxisContinueMoveAcc(Y_AXIS,1000,DIR_N,1000,610,20,20);
          
          //Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);
          CTHL_AutoStep = 5;
          CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
          CTHL_JXM_VAVLE = 1;  //夹斜码电磁阀 
        }
      }
      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_JXM_VAVLE = 1;  //夹斜码电磁阀 
          CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TXM_DELAY;
          CTHL_AutoStep = 6;                   
        }
        AxisEgmStop(X_AXIS);
        AxisEgmStop(Y_AXIS); 
        //set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW); 
        //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); //前拖带松一点带    
          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 )  //推斜码到位
      { 
        Set_Ctrlmode_trans(Z_AXIS,TARR_MODE);
        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))
      {
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
        {                  
          CTHL_QMDW_VAVLE=0;
          CTHL_FZ_MotorStep=1;      //反转穿入
          //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;
          set_servo_postotarr_limit(Z_AXIS,SERVO_PARAM_CUDAI_TRARRMIN,10000,10000,SERVO_TARR_CW);
          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_AutoStep = 12;
          CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
        }
      }
      break;
    case 12:
      if(CTHL_TXC_Limit_IN)  //摆臂到位
      {
        CTHL_DingLian_VAVLE=1;   //暂时不用
       
        AxisMovePosAccDec(Y_AXIS,2*CTHL_PARAM_XCCR_SPEED,CTHL_PARAM_FZCXC_LENGTH,1000,1000,15,15,0);//辅助小插插入  
        //AxisMovePosAccDec(X_AXIS,2*CTHL_PARAM_XCCR_SPEED,-CTHL_PARAM_FZCXC_LENGTH,1000,1000,15,15,0);//辅助小插插入  
        Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
        Set_Ctrlmode_trans(X_AXIS,POS_MODE); 
        CTHL_AutoStep = 13;  
        CTHL_AutoDelay = dwTickCount + CTHL_PARAM_XCIN_DELAY;//小插到位延时拖带   
      }
      else if(dwTickCount >= CTHL_AutoDelay)
      {
        bCxcAuto=1;
        CTHL_AutoStep = 100;
        CTHL_JCBI_VAVLE = 0;   //Y16小插摆臂电磁阀
        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_YXC_VAVLE=0;           
          CTHL_AutoDelay = dwTickCount + 0;//CTHL_PARAM_YDC_DELAY; 
          AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_XCCR_SPEED,CTHL_PARAM_XCCR_LENGTH,800,800,15,15,50); 
          //AxisMovePosAccDec(Y_AXIS,3*CTHL_PARAM_XCCR_SPEED,(CTHL_PARAM_XCCR_LENGTH+CTHL_PARAM_FZCXC_LENGTH),1000,1000,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_XCQM_VAVLE=0;
        //CTHL_TFK_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_SetAlarmCode(CTHL_XCDW_Limit_ALARM,bRunning);//小插定位针异常
        
        CTHL_AutoStep = 100; 
        CTHL_JCBI_VAVLE = 0;   //Y16小插摆臂电磁阀
        AxisMovePosAccDec(X_AXIS,800,-100,800,800,15,15,0);//前轮布放松一点        
      }
      break;  
    case 15:   
      if((dwXRealPos >= (CTHL_XSavePosBuff3 + 50)))
      {
        AxisEgmStop(X_AXIS);  
        AxisEgmStop(Y_AXIS); 
        if(dwTickCount >= CTHL_AutoDelay)
        {
          CTHL_TFK_VAVLE = 0;          
          if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
          {
            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 ; 
          CTHL_AutoStep = 17;           
          CTHL_JCBI_VAVLE=0;
        }
      }
      break;    
    case 100:  
      if(CTHL_START_IN_UP)
      {
        CTHL_AutoStep = 101;
      }
      break;
    case 101:  
      CTHL_AutoStep = 102;      
      break;        
    case 102:               
      if(!X_DRV&&!Y_DRV)
      {
        SetAlarmCode(CTHL_ALARM_ADDR,0);
        CTHL_YFK_VAVLE=1;
        CTHL_JCBI_VAVLE = 1;   //Y16小插摆臂电磁阀

        cBAOjin=1;
        CTHL_cZipCnt=0;
        bZhuangLiaoOkFlg=0;
        SetEn(X_AXIS, MOTOR_DISEN);
        SetEn(Y_AXIS, MOTOR_DISEN);

        CTHL_AutoStep = 103; 
        CTHL_AutoDelay = dwTickCount + 300 ; 
        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 = 13; 
        CTHL_AutoDelay = dwTickCount + 100 ;
        
      }  
      break;
    case 17:
      if(dwTickCount >= CTHL_AutoDelay)
      {
          if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
          {
            CTHL_TFK_VAVLE=0;
            if(!CTHL_bHandWork_MODE)CTHL_TD_MotorStep =10; 
            else
            {
              CTHL_YXC_VAVLE=0;
              CTHL_AutoDelay = dwTickCount + 1000 ;
            }
            CTHL_TD_MotorDelay = dwTickCount+ CTHL_PARAM_XCLD_DELAY;
            CTHL_AutoStep = 18;
          }        
      }
      break;  
    case 18:
      if(!CTHL_bHandWork_MODE)
      {
         CTHL_AutoStep = 19; 
      }
      else if(dwTickCount>=CTHL_AutoDelay)
      {
         CTHL_MGuo_VAVLE=0;
         CTHL_CTXM_VAVLE =0; 
         CTHL_CTXMSSStep=1;
         CTHL_AutoStep = 19; 
      }
      break;
    case 19:
      if((CTHL_TD_MotorStep == 0))
      { 
        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_bHandWork_MODE=0;
          } 
          CTHL_GF_Change = 0;
        }             
      }
      break;      
    }    
    
    //退下模后启动送拉头
    switch(CTHL_CTXMSSStep)
    { 
    case 1:
      if(!CTHL_CTM_Limit_IN)
      {
        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_CTXMSS_Delay = dwTickCount+ 1000;
      }
      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_CTXMSS_Delay) 
        {
          //穿头模和一次穿入回位异常
          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;         
          
        }
        else if(dwTickCount >= CTHL_CTXMSS_Delay) 
        {
          //穿头模和一次穿入回位异常
          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;
        CTHL_CTXMSSStep = 21;
        CTHL_CTXMSS_Delay = dwTickCount+ 1000;
      }
      break;
    case 21:
      if(!CTHL_LTou_Check && !CTHL_TXM_Limit_IN)//有拉头，推斜码到位不能亮
      {
        if(!bSingOneFlg)
        {
          CTHL_CTXM_VAVLE=1;
          CTHL_CTXMSSStep = 22;
        }
        else
        {
          CTHL_CTXMSSStep = 0;
        }
      }
      else if(dwTickCount >= CTHL_CTXMSS_Delay) 
      {
        CTHL_wltzi=1;                  
        CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);  // 没有拉头  
      }       
      break;
     case 22:
       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_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);
          SetClr(Z_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_bHandWork_Run ) && !CTHL_wltzi)
  {    
   
    if(!bRunning)
    {
      if(!CTHL_CTM_Origin_IN) CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning); 
      
      bRunning = 1;
      if( CTHL_bSingOne)
      {
        bSingOneFlg=1;
        
      }
      if(CTHL_bHandWork_Run)
      {
        CTHL_bHandWork_MODE=1;
        CTHL_bHandWork_Run=0;
      }
      bFZ_LengthOK = 0;
      SetEn(X_AXIS, MOTOR_DISEN);
      SetEn(Y_AXIS, MOTOR_DISEN);
      SetEn(Z_AXIS, MOTOR_DISEN);
      CTHL_FZ_MotorStep = 0;
      CTHL_CTXMSSStep=0;
      CTHL_CTXMSS_FLAG=0;
      CTHL_CD_MotorStep=0;
      
      bTuiLaTouOkFlg = 1;
      bZhuangLiaoOkFlg= 0;    
      CTHL_TryCnt = 0;
      CTHL_PARAM_ZDP_AUTO=0;
      bCxcAuto=0;
      bCL_EN_FLG = 0;
      CTHL_MOTOR_MODE=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_FKJC_VAVLE   =1;         
      CTHL_HL_VAVLE     =0;      
      CTHL_YFK_VAVLE    =0;      
      CTHL_YXC_VAVLE     =0;     
      CTHL_XCDW_VAVLE    =0;       
      CTHL_JCBI_VAVLE    =0; 
      CTHL_XCQM_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_ZhuangLiaoDelay = dwTickCount + 350;
      CTHL_ZhuangLiaoStep=1;
      CTHL_LianLengthAutoCheck = 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);
        SetClr(Z_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);
      SetEn(Z_AXIS, MOTOR_EN);
      //Set_VDI_Value(Z_AXIS,0);
      CTHL_CD_MotorStep=0;
      
      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=0;
      CTHL_cStopMode=0; 
      bSingOneFlg=0;
      bFZ_LengthOK = 0;
      cBAOjin=0;
      CTHL_wltzi=0;
      CTHL_MOTOR_MODE=0;
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);
      AxisEgmStop(Z_AXIS);
      Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
      SetEn(X_AXIS, MOTOR_EN);
      SetEn(Y_AXIS, MOTOR_EN);
      SetEn(Z_AXIS, MOTOR_EN);
      //Set_VDI_Value(Z_AXIS,0);
      CTHL_bHandWork_MODE=0;
      CTHL_bHandWork_Run=0;
      
      CTHL_AutoStep = 0; 
      CTHL_stop=0;
      CTHL_FZ_MotorStep =0;   
      CTHL_TD_MotorStep =0;
      CTHL_CD_MotorStep=0;
      CTHL_bResting_Z=0;
      //        CTHL_ZhuangLiaoStep=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_QMDW_VAVLE   =0;        
      CTHL_TFK_VAVLE    =0;        
      //  CTHL_FKJC_VAVLE   =0;         
      CTHL_HL_VAVLE     =0;      
      CTHL_YFK_VAVLE    =0;      
      CTHL_YXC_VAVLE     =0;     
      CTHL_XCDW_VAVLE=0;
      CTHL_JCBI_VAVLE    =0; 
      CTHL_XCQM_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);
        SetClr(Z_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);  
      
    }
    else if(!bRunning)
    {
      bRunning=0;
      CTHL_cStopMode=0;
      bSingOneFlg=0;
      CTHL_wltzi=0;
      bCxcAuto=0;
      cBAOjin=0;
      CTHL_stop=0;
      bFZ_LengthOK = 0;
      CTHL_MOTOR_MODE=0;
      CTHL_RunStepSave = 0;
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);
      Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
      SetEn(X_AXIS, MOTOR_EN);
      SetEn(Y_AXIS, MOTOR_EN);
      SetEn(Z_AXIS, MOTOR_EN);
      //Set_VDI_Value(Z_AXIS,0);
      SetAlarmCode(CTHL_ALARM_ADDR,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;  
      
      if(CTHL_PARAM_Test)
        CTHL_TXM_VAVLE    = 0 ;    
      else
        CTHL_TXM_VAVLE    = 1;   
      CTHL_QMDW_VAVLE   =0;        
      CTHL_TFK_VAVLE    =0;        
      CTHL_FKDW_VAVLE=0;            
      CTHL_HL_VAVLE     =0;      
      CTHL_YFK_VAVLE    =0;      
      CTHL_YXC_VAVLE     =0;     
      CTHL_XCDW_VAVLE    =0;       
      CTHL_JCBI_VAVLE    =0; 
      CTHL_XCQM_VAVLE   =0;   
      CTHL_AutoStep = 0;
      CTHL_ZhuangLiaoStep=0;
      CTHL_TLiaoStep=0;
      CTHL_TD_MotorStep=0;
      CTHL_SongLiaoStep   =0;     
      CTHL_FZ_MotorStep =0;
      CTHL_CTXMSSStep=0;  
      CTHL_bResting_Z=0;
      CTHL_CD_MotorStep=0;
      //CTHL_AutoStep1  =0;
      if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)
      {
        SetClr(X_AXIS, MOTOR_NOALARM);
        SetClr(Y_AXIS, MOTOR_NOALARM);
        SetClr(Z_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);  
      SetPos(X_AXIS, 0); 
      SetPos(Y_AXIS, 0); 
      SetPos(Z_AXIS, 0); 
      CTHL_TLiaoStep = 0; 
      CTHL_AutoStep = 0;
      CTHL_LianLengthCheck = 0;
      CTHL_LianLengthCheckOld = 0;
      
    }
    bStop=0;
  }
  
  if(bAlarmStop)
  {   
    bRunning   = 0;
    bAlarmStop=0;
    CTHL_cStopMode=0;
    CTHL_XCDW_VAVLE=0;
    AxisEgmStop(X_AXIS);
    AxisEgmStop(Y_AXIS);
    AxisEgmStop(Z_AXIS);
    Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
    Set_Ctrlmode_trans(Z_AXIS,POS_MODE);
    SetEn(X_AXIS, MOTOR_EN);
    SetEn(Y_AXIS, MOTOR_EN); 
    SetEn(Z_AXIS, MOTOR_EN);
    CTHL_AutoStep = 0; 
    CTHL_FZ_MotorStep =0;   
    CTHL_TD_MotorStep =0;   
  }
#endif    
} 
//无带处理
void CTHL_NoLaLian_Speed_Proc(void)
{
  if(CTHL_NoLaLian_IN_UP)
  { 
     CTHL_cZipCnt=0;
     AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_First_SPEED/2,DIR_P,1000,800,50,50);
     AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_First_SPEED/2,DIR_P,1000,800,50,50);  
  }  
}
//合链处理
void CTHL_HeLian_Proc(void)
{
    if((CTHL_cZipCnt < 2) || CTHL_NoLaLian_IN)
    {
      //松码钩后输出合链长度
      if(dwXRealPos >= ((CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1) + 600))
      {
        CTHL_XSavePosBuff2=dwXRealPos;
        CTHL_HL_VAVLE=1;
      }
    }
    else
    {                  
      //松码钩后输出合链长度
      if(dwXRealPos >= (CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1))
      {
        CTHL_XSavePosBuff2=dwXRealPos;
        CTHL_HL_VAVLE=1;
      }
      
    }    
    //   方块感应到了之后，最长15公分也要退
    if(((dwXRealPos+1700)>((CTHL_LianLengthAutoFZCRcheck+2700)-CTHL_PARAM_HELIAN_LENGTH)) || (bFKCheck_Flag && (dwXRealPos >= (CTHL_XSavePosBuff3 + 2500))))
    {
      CTHL_HL_VAVLE=0;
    }
}
void CTHL_TDLength_BH_Proc(unsigned short local)//拖带长度保护
{
  if(local==0)//为感应到开关前
  {
    if(CTHL_cZipCnt < 2)
    {
      if((dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length))
      {
        AxisEgmStop(X_AXIS);
        AxisEgmStop(Y_AXIS); 
        CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
      }
    }
    else
    {
      if((dwXRealPos >=(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheck)) )
      {
        AxisEgmStop(X_AXIS);
        AxisEgmStop(Y_AXIS);
        CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
      }            
    }
  }
  else//感应到开关后
  {
    if(CTHL_cZipCnt < 2)
    {
      if((dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length+CTHL_LianLengthAutoFZCRcheck))
      {
        AxisEgmStop(X_AXIS);
        AxisEgmStop(Y_AXIS); 
        CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
      }
    }
    else
    {
      if((dwXRealPos >=(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheck+CTHL_LianLengthCheckOld)) )
      {
        AxisEgmStop(X_AXIS);
        AxisEgmStop(Y_AXIS);
        CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
      }            
    }
  }
}
//无方块处理
void CTHL_TDWFK_Proc(void)
{
  //这里判断中间某条没有方块
  if(CTHL_cZipCnt > 1&&(dwXRealPos >= CTHL_LianLengthCheckFK_DW+200))
  {
    //说明下一条没有方块
      AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed/2,DIR_P,1000,800,50,50);
      AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed/2,DIR_P,1000,800,50,50);  
      CTHL_TD_MotorStep = 51;
  }
}
//拖带电机控制动作  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);
      SetEn(X_AXIS, MOTOR_EN);
      SetEn(Y_AXIS, MOTOR_EN);
      SetEn(Z_AXIS, MOTOR_EN);
      CTHL_SetAlarmCode(CTHL_SF_ALR_ALARM,bRunning);   //无拉链自动停机
      CTHL_JCBI_VAVLE = 0;
      CTHL_XCQM_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);
    SetEn(X_AXIS, MOTOR_EN);
    SetEn(Y_AXIS, MOTOR_EN);
    SetEn(Z_AXIS, MOTOR_EN);
    CTHL_AutoStep = 201; 
    CTHL_SetAlarmCode(CTHL_SF_ALR_ALARM,bRunning);   //无拉链自动停机
    CTHL_JCBI_VAVLE = 0;
    CTHL_XCQM_VAVLE=0;
    //          CTHL_TFK_VAVLE=0;
  }
#if 1
  switch(CTHL_TD_MotorStep)
  {
    //1步开始，纯定位, 没有穿入
  case 1:
    if(dwTickCount >= CTHL_TD_MotorDelay)  //给锁轴时间
    {
      SetPos(X_AXIS, 0); 
      CTHL_FKDW_VAVLE=0; 
      CD_ORIGIN_FLG=0;
      if(CTHL_PARAM_Test)
        CTHL_TXM_VAVLE    = 0 ;    
      else
        CTHL_TXM_VAVLE    = 1;            //因推斜码和固定方块一起，要避开

      CTHL_TD_MotorStep = 2;  
      AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1000,600,10,15);  
      AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1000,600,10,15); 
    }  
    break;
  case 2://检测到过链         
    if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length) 
    {
      AxisEgmStop(X_AXIS);
      CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
    }    
    if(CTHL_FK_Check_UP)
    {           
      CTHL_XSavePosBuff = dwXRealPos;
      AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH,
                        1000,1000,100,100,50); 
      AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed*4/5,CTHL_PARAM_XMCR_YJDW_LENGTH,
                        1000,1000,100,100,50);                   
      CTHL_TD_MotorStep = 3;  
    }    
    break;
  case 3:
    if(dwXRealPos>=CTHL_XSavePosBuff+CTHL_PARAM_XMCR_YJDW_LENGTH-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=0;
       CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常
    }
    if(CTHL_FKDW_IN)
    {
      CTHL_TD_MotorStep = 5; 
      CTHL_TD_MotorDelay = dwTickCount+ 0;
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);
    }    
    break;
  case 5: 
    AxisMovePosAccDec(X_AXIS,1000,-30,1000,1000,30,20,0); 
    CTHL_TD_MotorStep = 6; 
    break;
  case 6: 
    if(!X_DRV)
    {
      CTHL_TD_MotorStep = 0; 
    }
    break;
    //穿合拉头定位
  case 10:   
    if(dwTickCount >= CTHL_TD_MotorDelay)
    { 
      SetPos(X_AXIS, 0);  //启动位置设为0点
      CTHL_CD_MotorStep=1;
      Set_Ctrlmode_trans(Z_AXIS,POS_MODE);
      CTHL_CD_MOTORDelay=dwTickCount+10;
      CTHL_DingLian_VAVLE=0;
      CTHL_YXC_VAVLE=0;
      CTHL_TXM_VAVLE=0;
      CTHL_TD_MotorStep = 11; 
      if(CTHL_cZipCnt == 0)CTHL_LianLengthCheckOld = 0;    
      AxisMovePosAccDecNotStop(X_AXIS,(CTHL_PARAM_HLT_SPEED_Speed),CTHL_PARAM_HL_LENGTH, 
                               1000,CTHL_PARAM_HLT_SPEED_Speed,150,25,00);
      //在手动穿链模式下不用动电机同步
      if(!CTHL_bHandWork_MODE)
          AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_HLT_SPEED_Speed,CTHL_PARAM_HL_LENGTH ,1000,1000,50,50,00); 
      
      CTHL_XSavePosBuff = dwXRealPos;
      bFKCheck_Flag = 0;
      CTHL_XSavePosBuff3 = 0;
    }
    break;
  case 11:
    //最后拉链的处理
    CTHL_TDLength_BH_Proc(0);
    //第一方块信号检测
    if(CTHL_FK_Check_UP && !bFKCheck_Flag)
    {
      bFKCheck_Flag = 1; 
      CTHL_XSavePosBuff3= dwXRealPos;
    }  
    //慢速合链长度   如果已经到达下一条的方块也必须开模   ,CTHL_NoLaLian_IN最后几条慢速时感应才退下模
    if((dwXRealPos >= CTHL_PARAM_HL_LENGTH)|| (bFKCheck_Flag))
    {
      //码勾先打开15MS再退下模
      if(CTHL_MGuo_VAVLE)
      {
        CTHL_MGuo_VAVLE=0;
        CTHL_TD_MotorDelay = dwTickCount + CTHL_PARAM_TMG_TXM_DELAY;;
      }
      else if(dwTickCount >= CTHL_TD_MotorDelay)
      { 
          CTHL_XSavePosBuff1= dwXRealPos;              
          CTHL_MGuo_VAVLE=0;
          CTHL_CTXM_VAVLE=0;
          CTHL_CTXMSSStep=1;
          CTHL_TD_MotorStep = 12;
          CTHL_TD_MotorDelay = dwTickCount + CTHL_PARAM_TuiXiaMo_DELAY;
      }                           
    }         
    break;
  case 12: 
    CTHL_HeLian_Proc();
    CTHL_TDLength_BH_Proc(0);
    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;    
    }
    if(CTHL_CTM_Limit_IN)
    {
      CTHL_TD_MotorDelay = dwTickCount + CTHL_PARAM_TuiXiaMo_DELAY;
    }
    else if(dwTickCount >= CTHL_TD_MotorDelay)   //下模离开后延时加速
    { 
        bHeLaTouOK = 1;
        if((CTHL_cZipCnt > 1) && (!CTHL_NoLaLian_IN))
        {                    
          CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED;
          CTHL_LENTH=CTHL_LianLengthCheckFK_DW- dwXRealPos;
          AxisMovePosAccDecNotStop(X_AXIS,CTHL_DataBuff,CTHL_LENTH,1000,CTHL_PARAM_FKDW_Speed,150,250,CTHL_PARAM_TDDW_LENGTH); 
        }
        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,1000,800,15,15);           
        }        
        AxisContinueMoveAcc(Y_AXIS,CTHL_DataBuff,DIR_P,1000,800,15,15); 
      CTHL_TD_MotorStep = 13; 
    }
    break;   
  case 13: 
    CTHL_NoLaLian_Speed_Proc();
    CTHL_HeLian_Proc();
    CTHL_TDLength_BH_Proc(0);
    //此步为已经开模， 开始加速
    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; 
    }
    else if(bFKCheck_Flag)  //说明第一个方块已经检测到
    {
      CTHL_TD_MotorStep = 14; 
      if(CTHL_cZipCnt > 1)CTHL_CTXMSS_FLAG=1;
    } 
    break;   
  case 14:
    CTHL_NoLaLian_Speed_Proc();
    CTHL_HeLian_Proc();
    CTHL_TDLength_BH_Proc(1);
    if(!CTHL_FK_Check && (dwXRealPos >= (CTHL_XSavePosBuff3 + 800))) 
    {
      CTHL_TD_MotorStep = 15; 
    }              
    break;
  case 15:
    CTHL_NoLaLian_Speed_Proc();
    CTHL_HeLian_Proc();
    CTHL_TDLength_BH_Proc(1);
    CTHL_TDWFK_Proc();
    //第二次感应方块， 要做定位
    if(CTHL_FK_Check_UP && (dwXRealPos >= (CTHL_XSavePosBuff3 + 1800)))
    {
      //测量距离
       bFZ_LengthOK = 1; 
       CTHL_XSavePosBuff = dwXRealPos;
       if(CTHL_cZipCnt)CTHL_LianLengthCheckOld = CTHL_LianLengthCheck;
       CTHL_LianLengthCheck = dwXRealPos - CTHL_XSavePosBuff3; 
       CTHL_LianLengthCheckFK_GL=dwXRealPos;
       CTHL_LENTH=abs(CTHL_LianLengthCheckOld-CTHL_LianLengthCheck);
       if(CTHL_cZipCnt > 1&&!CTHL_NoLaLian_IN_UP)
       { 
        //公分节判断
        if(CTHL_LianLengthCheck<=CTHL_PARAM_GFJ_LENGTH)
        {
            CTHL_TD_MotorStep=50;
            AxisContinueMoveAcc(X_AXIS,2000,DIR_P,1000,600,15,15);
            AxisContinueMoveAcc(Y_AXIS,1600,DIR_P,1000,600,15,15);
            CTHL_TD_MotorDelay = dwTickCount + 200;
            bFZ_LengthOK= 0;
            CTHL_cZipCnt = 0;
        }
        else if(CTHL_LENTH>=1500)
        {
          AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH,
                            1000,1000,30,80,100); 
          AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH,
                            1000,1000,30,80,100);
          CTHL_TD_MotorStep=16;
          CTHL_TXM_VAVLE=0;
          if(CTHL_cZipCnt<2)CTHL_CTXMSS_FLAG=1;
          CTHL_cZipCnt=0;
        }
        else
        {
          CTHL_TD_MotorStep=16;
          if(CTHL_cZipCnt<2)CTHL_CTXMSS_FLAG=1;
        }
      }
      else
      { 
          CTHL_TD_MotorStep=16;
          CTHL_TXM_VAVLE=0;
          if(CTHL_cZipCnt<2)CTHL_CTXMSS_FLAG=1;
          AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH,
                            1000,1000,30,80,50); 
          AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH,
                            1000,1000,30,80,50);
      }
    }        
    break;          
  case 16:  
    if(CTHL_cZipCnt > 1&&!CTHL_NoLaLian_IN_UP)
    {
      if(dwXRealPos>=CTHL_XSavePosBuff+CTHL_PARAM_FKDW_OUT_DELAY)
      {
        CTHL_TXM_VAVLE=0;
        CTHL_FKDW_VAVLE=1;         
        CTHL_TD_MotorStep = 17; 
        CTHL_TD_MotorDelay = dwTickCount+ 1500;
      }
    }
    else
    {
       if(dwXRealPos>=CTHL_XSavePosBuff+CTHL_PARAM_XMCR_YJDW_LENGTH-200)
      {
        CTHL_FKDW_VAVLE=1; 
        CTHL_TXM_VAVLE=0;
        CTHL_TD_MotorStep = 17; 
        CTHL_TD_MotorDelay = dwTickCount+ 1500;
      } 
    }
    break;
  case 17:
     //定位感应亮时、位置到达时，或X轴停止时，停止X轴
    if(dwTickCount >= CTHL_TD_MotorDelay)//3S为检测到停机
    { 
       bCxcAuto=0;
       CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常
    }
    if(CTHL_FKDW_IN)
    {
      CTHL_TD_MotorStep = 18; 
      CTHL_TD_MotorDelay = dwTickCount+ 0;
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);
      AxisEgmStop(Z_AXIS);
      CTHL_CD_MotorStep=0;
      Set_Ctrlmode_trans(Z_AXIS,TARR_MODE);
      bFZ_LengthOK = 1;
      CTHL_TXM_VAVLE=0;
      CTHL_LianLengthCheckFK_DW=dwXRealPos;
    }    
    break;
  case 18:
    //AxisMovePosAccDec(X_AXIS,1000,-40,1000,1000,30,20,0); 
    CTHL_TD_MotorStep = 0; 
    
    break;
    //由公分节调转
  case 50: 
    if(CTHL_FK_Check_DW)  
    {              
      CTHL_cZipCnt = 0;
      CTHL_TD_MotorStep = 51;  
      CTHL_TD_MotorDelay = dwTickCount + 15;
      bFZ_LengthOK = 0; 
      CTHL_GF_Change = 1;
      
      SetPos(X_AXIS, 0);   
    }
    break;
  case 51:
    
    if(CTHL_FK_Check)
    {
      CTHL_TD_MotorDelay = dwTickCount + 50;
    }  
    else if(dwTickCount >= CTHL_TD_MotorDelay)
    {
      //    CTHL_MGuo_VAVLE=0;
      CTHL_CTXM_VAVLE=0;
      CTHL_TD_MotorStep = 52;  
    }  
    break;
  case 52:  
    if(dwXRealPos>=CTHL_PARAM_GFJ_LENGTH)
    {
      if(CTHL_FK_Check)
      {           
        CTHL_XSavePosBuff = dwXRealPos;   
        AxisContinueMoveChangeSpeed(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_FKDWMAN_Speed,20,5);
        AxisContinueMoveChangeSpeed(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed * 4/5,CTHL_PARAM_FKDWMAN_Speed* 4/5,20,5);     
        CTHL_TD_MotorStep = 53;  
      }
    }
    break;
    
  case 53:
    if(dwXRealPos >= (CTHL_PARAM_FKDW_OUT_DELAY + CTHL_XSavePosBuff + 100))
    {
      CTHL_TXM_VAVLE=0;
      CTHL_FKDW_VAVLE=1;
    }
    //定位感应亮时也要停止
    if(CTHL_FKDW_IN_UP || !X_DRV)
    {
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);  
      if(CTHL_FKDW_IN)
      {
        CTHL_TD_MotorStep = 54;  
        
        CTHL_TD_MotorDelay = dwTickCount + 50;
      }
      else
      {
        CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning); 
      }
    }
    break;     
  case 54:
    
    if(dwTickCount >= CTHL_TD_MotorDelay)//YWX_CTHL_bTiaoShiStep)
    {
      if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
      {
        bStart = 1; 
        CTHL_LianLengthCheck = 0;
        CTHL_LianLengthCheckOld = 0;
        //  bRunning = 0;
        CTHL_TD_MotorStep = 0;  
        CTHL_TD_MotorDelay = dwTickCount + 000;
        CTHL_TryCnt = 0;
        bCL_EN_FLG = 0;
      }               
    }
    break;
  } 
  /*if(!CD_ORIGIN_FLG&&CTHL_Z_ORIGN_IN&&CTHL_TD_MotorStep>=10)
  {
    CD_ORIGIN_FLG=1;
    Set_Ctrlmode_trans(Z_AXIS,POS_MODE); 
  }*/
#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;
      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN||CTHL_bHandWork_MODE)//前两次走
      {
        AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_FZFirst_SPEED,DIR_N,1000,1000,30,50); 
        AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_FZFirst_SPEED+100,DIR_N,1500,1000,30,50);
      }
      else 
      {
        AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_JXM_SPEED,-CTHL_PARAM_XMCR_SLOW_LENGTH,
                                 1000,CTHL_PARAM_JXM_SPEED,150,100,0);
        AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_JXM_SPEED+500,-CTHL_PARAM_XMCR_SLOW_LENGTH,
                                 1000,CTHL_PARAM_JXM_SPEED,50,100,0); 
      }  
      CTHL_FZ_MotorStep = 2; 
      CTHL_FZMOTOR_KADAI_Delay=dwTickCount+500;
    }
    break;
  case 2:
    if((abs(dwXRealPos) >=CTHL_PARAM_XMCR_SLOW_LENGTH-100)&&!CTHL_NoLaLian_IN&&!CTHL_bHandWork_MODE){//提前转换模式
      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||CTHL_bHandWork_MODE)
      {                    
        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,
                                 1000,CTHL_PARAM_FZDS_Speed,250,250,CTHL_PARAM_FZJS_LENGTH); 
        AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZ_Speed+120,-CTHL_LENTH,
                                 1000,CTHL_PARAM_FZDS_Speed,120,250,CTHL_PARAM_FZJS_LENGTH);
        
        //AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FZ_Speed,-CTHL_LENTH,
        //                         1000,CTHL_PARAM_FZDS_Speed,250,150,0); 
        //AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZ_Speed+120,-CTHL_LENTH,
        //                         1000,CTHL_PARAM_FZDS_Speed,120,150,0);
      }        
      CTHL_FZ_MotorStep++;      
    }
    if(dwTickCount>=CTHL_FZMOTOR_KADAI_Delay&&dwYRealPos_com<400)//穿入前码卡带
    {
      bCxcAuto=0;//停轴
      SetEn(Y_AXIS, MOTOR_EN);
      CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning);   //反转电机  
    }
    //电机速度差太大
    if((abs(dwXRealPos)>dwYRealPos_com+4500)&&!CTHL_bHandWork_MODE)
    {
       FZ_ALARM_FLG=1;
       CTHL_FZ_MotorStep=100;
       CTHL_XSavePosBuff1=dwYRealPos_com;
       AxisEgmStop(X_AXIS);
       AxisEgmStop(Y_AXIS);
      //bCxcAuto=0;//停轴
      //SetEn(Y_AXIS, MOTOR_DISEN);
      SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM);
      Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
      //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning);   //反转电机  
    }
    break;
  case 3:
    CTHL_FZ_MotorStep++; 
    /*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_bHandWork_MODE)
    {
      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,
                                 1000,1500,120,100,CTHL_PARAM_FZJSDS_LENGTH);
        AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZDS_Speed,-CTHL_PARAM_FZJS_LENGTH,
                                 1000,1500,120,100,CTHL_PARAM_FZJSDS_LENGTH);         
        CTHL_FZ_MotorStep++; 
      }
    }
    //长度超出报警
    if(CTHL_cZipCnt>=1&&!CTHL_bHandWork_MODE)
    {
      if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthAutoFZCRcheck)) //拉带报警长度
      {
        bCxcAuto=0;//停轴
        SetEn(Y_AXIS, MOTOR_DISEN);
        CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning);  //穿入方块超出预设长度            
      }
      //方块检测异常(光纤)
      if(abs(dwXRealPos) >= (CTHL_LianLengthAutoFZCRcheck + 300))
      {
        bCxcAuto=0;//停轴
        SetEn(Y_AXIS, MOTOR_DISEN);
        CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning); 
      }  
    }
    else
    {
      if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length&&!CTHL_bHandWork_MODE){  //位置大于第一条拉链长度    
        bCxcAuto=0;//停轴
        SetEn(Y_AXIS, MOTOR_DISEN);
        CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning);   //穿入方块超出预设长度                  
      }
    }
    //电机速度差太大
    if((abs(dwXRealPos)>dwYRealPos_com+4500)&&!CTHL_bHandWork_MODE)
    {
       FZ_ALARM_FLG=1;
       CTHL_FZ_MotorStep=100;
       CTHL_XSavePosBuff1=dwYRealPos_com;
       AxisEgmStop(X_AXIS);
       AxisEgmStop(Y_AXIS);
      //bCxcAuto=0;//停轴
      //SetEn(Y_AXIS, MOTOR_DISEN);
      SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM);
      Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
      //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) > 1500) && CTHL_FKGY_IN_UP)//位置大于100，且方块感应上升沿
    {               
      CTHL_XSavePosBuff1 = abs(dwXRealPos); 
      set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMIN,50000,50000,SERVO_TARR_CW);
      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN||CTHL_bHandWork_MODE){//记录前两条的方块感应长度；
        //停机距离调整
        CTHL_LENTH=CTHL_PARAM_FZSTOP_LENGTH;
        AxisMovePosAccDec(X_AXIS,2000,-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,2000,-CTHL_LENTH,
                          800,800,15,25,0);
        AxisMovePosAccDec(Y_AXIS,600,-CTHL_LENTH,
                          800,800,15,25,0);
      }
      CTHL_FZ_MotorStep++; 
      //小插前码定位
      //CTHL_XCQM_VAVLE=1;
    }
    else
    {
        //长度超出报警
        if(CTHL_cZipCnt>=1&&!CTHL_bHandWork_MODE)
        {
          if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthAutoFZCRcheck)) //拉带报警长度
          {
            bCxcAuto=0;//停轴
            SetEn(Y_AXIS, MOTOR_EN);
            CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning);  //穿入方块超出预设长度            
          }
          //方块检测异常(光纤)
          if(abs(dwXRealPos) >= (CTHL_LianLengthAutoFZCRcheck + 500))
          {
            bCxcAuto=1;//停轴
            CTHL_FZ_MotorStep=100;
            SetEn(Y_AXIS, MOTOR_EN);
            CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning); 
          }  
        }
        else
        {
          if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length&&!CTHL_bHandWork_MODE){  //位置大于第一条拉链长度    
            bCxcAuto=0;//停轴
            CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning);   //穿入方块超出预设长度                  
          }
        }
        if(dwTickCount>=CTHL_FZMOTOR_KADAI_Delay&&dwYRealPos_com<400)//穿入前码卡带
        {
          bCxcAuto=1;//停轴
          CTHL_FZ_MotorStep=100;
          SetEn(Y_AXIS, MOTOR_EN);   
          CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning);   //反转电机  
        }
        //电机速度差太大
        if((abs(dwXRealPos)>dwYRealPos_com+4500)&&!FZ_ALARM_FLG&&!CTHL_bHandWork_MODE)
        {
           FZ_ALARM_FLG=1;
           CTHL_FZ_MotorStep=100;
           CTHL_XSavePosBuff1=dwYRealPos_com;
           AxisEgmStop(X_AXIS);
           AxisEgmStop(Y_AXIS);
          //bCxcAuto=0;//停轴
          //SetEn(Y_AXIS, MOTOR_DISEN);
          SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM);
          Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
          //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);
       //前码扭矩
      //Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
      CTHL_LianLengthAutoFZcheck=abs(dwXRealPos);
      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:
     
     CTHL_FZ_MotorStep = 22;       
     AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_XCCR_SPEED,-CTHL_PARAM_CXDW_Length,1000,1000,20,20,0);   
     AxisMovePosAccDec(X_AXIS,CTHL_PARAM_XCCR_SPEED,-(CTHL_PARAM_CXDW_Length+100),1000,1000,20,20,0);              
     CTHL_FZ_MotorDelay=dwTickCount+10+CTHL_PARAM_XCDW_TIME;
     break;
  case 22: 
    if(!Y_DRV && !X_DRV  )
    {
      CTHL_FZ_MotorStep = 0; 
    }
    //前码扭矩
    /*if(!X_DRV && dwTickCount>=CTHL_FZ_MotorDelay&&GetTarr(Y_AXIS) )
    {
      CTHL_FZ_MotorStep = 23; 
      Set_Ctrlmode_trans(Y_AXIS,POS_MODE); 
      CTHL_FZ_MotorDelay=dwTickCount+10;
    } */        
    break;
  case 23: 
    if(dwTickCount>=CTHL_FZ_MotorDelay )
    {
       CTHL_FZ_MotorStep = 0; 
       Set_Ctrlmode_trans(X_AXIS,TARR_MODE);             
    }         
    break;
  case 100:
    if(CTHL_START_IN_UP)
    {
      CTHL_AutoStep = 101;
    }
  case 101:
    if(CTHL_FKGY_IN)
    {
      CTHL_FZ_MotorStep = 5; 
    }
    else
    {
      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN||CTHL_bHandWork_MODE)
      {
         AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_JXM_SPEED/2,DIR_N,1000,1000,30,50); 
         AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_JXM_SPEED,DIR_N,1000,1000,30,50);
      }
      else
      {
        CTHL_LENTH = CTHL_LianLengthAutoFZCRcheck-abs(dwXRealPos);
        if(CTHL_LENTH>0)
        {
          AxisMovePosAccDec(X_AXIS,1000,-CTHL_LENTH,800,800,15,25,0);
          AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_JXM_SPEED,DIR_N,1000,1000,30,50);
        }
      }
      CTHL_FZ_MotorDelay=dwTickCount+3000;
      CTHL_FZ_MotorStep = 102;
    }
    break;  
  case 102: 
    if(CTHL_FKGY_IN_UP)
    {
      CTHL_FZ_MotorStep = 5; 
    }
    else if(dwTickCount>CTHL_FZ_MotorDelay)
    {
       bCxcAuto=0;//停机  
       AxisEgmStop(X_AXIS);       
       AxisEgmStop(Y_AXIS);
       CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning);   //反转电机卡滞
    }    
    break; 
  }
#endif    
}
//储带电机
void CTHL_CUDAI_Motor(void)
{
#if 1
  switch(CTHL_CD_MotorStep)
  {
  case 1:
    if(dwTickCount>=CTHL_CD_MOTORDelay)
    {
      
      set_servo_postotarr_limit(Z_AXIS,SERVO_PARAM_CUDAI_TRARRMIN,50000,50000,SERVO_TARR_CW);  
      if((CTHL_cZipCnt >1))
      {
         // AxisMovePosAccDecNotStop(Z_AXIS,CTHL_LianLengthAutoFZCRcheck/2,CTHL_PARAM_HIGH_SPEED,
          //                         3000,CTHL_PARAM_SLOW_SPEED,50,50,300); 
          AxisContinueMoveAcc(Z_AXIS,CTHL_PARAM_HIGH_SPEED,DIR_P,5000,1000,30,50);
          CTHL_CD_MotorStep=3;
      }
      else
      {
          AxisContinueMoveAcc(Z_AXIS,CTHL_PARAM_FIRST_SPEED,DIR_P,3000,1000,30,50);
          CTHL_CD_MotorStep=3;
      }
    }
    break;
  case 2:
    if(dwXRealPos>=CTHL_LianLengthAutoFZCRcheck*3/4)
    {
      Set_VDI_Value(Z_AXIS,2);
      AxisContinueMoveChangeSpeed(Z_AXIS,CTHL_PARAM_SLOW_SPEED,1000,80,80);
      CTHL_CD_MotorStep=3;
    }
    else if(CTHL_Z_ORIGN_IN)
    {
      Set_VDI_Value(Z_AXIS,2);
      AxisEgmStop(Z_AXIS);
      CTHL_CD_MotorStep=0;
    }
    break;
  case 3:
    if(CTHL_Z_ORIGN_IN)
    {
      Set_Ctrlmode_trans(Z_AXIS,TARR_MODE);
      Set_VDI_Value(Z_AXIS,2);
      AxisEgmStop(Z_AXIS);
      CTHL_CD_MotorStep=0;
      CTHL_bResting_Z=0;
    }
    break;
  }
#endif   
}
//装拉头动作
void CTHL_ZLT_Step(void)
{
  
  switch(CTHL_ZhuangLiaoStep)
  {
  case 1:
    
    if(bZhuangLiaoOkFlg)
    {
      CTHL_ZhuangLiaoStep = 0;
    }
    else if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {
      
      if(((CTHL_MGuo_VAVLE && CTHL_LTou_Check) || (!CTHL_MGuo_VAVLE)) && !CTHL_JLiao_Limit_IN  )//说明没有拉头
      { 
         /*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_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  //已经有拉头直接跳到结束
      {
        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_JLTou_VAVLE = 0;
      CTHL_MGuo_VAVLE = 0;   
      if(!CTHL_JLiao_Limit_IN)
      {
        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_JLTou_VAVLE = 1;    //接拉头输出,不需要时间
      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 + QDCT_VAVLE_ERROR_TIME;
    }  
    break;
  case 5:    
    if(CTHL_JLiao_Limit_IN)
    {
      CTHL_ZhuangLiaoStep = 6;
      CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_MaGou_DELAY;
    }
    else if(dwTickCount >= CTHL_ZhuangLiaoDelay)  //接拉头(接料)到们信号异常
    {
      CTHL_SetAlarmCode(CTHL_JLiao_Origin_ALARM,bRunning);
      CTHL_HSLiao_VAVLE = 0; 
      CTHL_JLTou_VAVLE = 0; 
      CTHL_ZhuangLiaoStep = 0;
    }
    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 + QDCT_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;     //码勾打开
      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)
    {
      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;     //码勾打开
      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_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_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_Origin_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_Origin_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((CTHL_bDLP || bRunning) && CTHL_ZhenDongPian_OUT && CTHL_PARAM_DALIAOPIAN)
  {        
    if(CT_DLP_Time >= dwTickCount)
    {
      CT_ZDP_Delay = dwTickCount + CT_PARAM_DLP_TIME + 40;  
      CT_DaLaPian_VAVLE = ~CT_DaLaPian_VAVLE;
    }
  }
  else
    CT_DaLaPian_VAVLE = 0;   
#endif
  
#if 1  
  
  //    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    
}





//手动动作
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)  
  {   
    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 if(!CTHL_CTXM_VAVLE  && (!CTHL_bMG || (CTHL_bMG && CTHL_LTou_Check)))
      {
        if(!CTHL_TLiao_VAVLE)
        {
          if(!CTHL_JLTou_VAVLE)
          {
            if(CTHL_CTM_Origin_IN)
              CTHL_HSLiao_VAVLE = 1;
          }
          else
          {
            
            CTHL_SetAlarmCode(CTHL_JLiao_Origin_ALARM,0);
          }
        }
        else
        {
          
          CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,0);
        }
        
      }
      else
      {
        CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,0);
      }
    }  
    
    
    
    if(CTHL_bTLiao )  //手动推料(推拉头)
    {
      CTHL_bTLiao = 0;           
      if(CTHL_TLiao_VAVLE)
        CTHL_TLiao_VAVLE = 0;//CTHL_TLiao_VAVLE; 
      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_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_bCXDW)                   //插销定位
    {
      CTHL_bCXDW = 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_bXCQM)
    {
      CTHL_bXCQM=0;
      CTHL_XCQM_VAVLE=~CTHL_XCQM_VAVLE;
    }    
    if(CTHL_bCTXM)                  //下模
    {
      CTHL_bCTXM = 0;
      CTHL_TXM_VAVLE=0;
      if(CTHL_CTXM_VAVLE)
      {
        CTHL_CTXM_VAVLE = 0;
        CTHL_JLTou_VAVLE = 0;
      }    
      else if(!CTHL_HSLiao_VAVLE && CTHL_HSLiao_Origin_IN && (CTHL_JXM_VAVLE || !CTHL_TXM_VAVLE))                
      { 
        
        CTHL_CTXM_VAVLE = 1;
      }  
    } 
    //自动装料
    if(CTHL_bAutoZhuangLiao)
    {
      CTHL_bAutoZhuangLiao = 0;
      if(CTHL_ZhuangLiaoStep == 0)
      {
        bTuiLaTouOkFlg = 1;
        bZhuangLiaoOkFlg = 0;
        CTHL_ZhuangLiaoStep = 1;
        CTHL_MGuo_VAVLE = 1;   //判断是否已经有料
        CTHL_TryCnt = 0;
        CTHL_ZhuangLiaoDelay = dwTickCount + 150;
      }  
    }
    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_XCQM_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;
        
        SetEn(Z_AXIS, MOTOR_DISEN);
        CTHL_CD_MotorStep=1;
        CTHL_cZipCnt=0;
      }
      
      
    } 
    
    //X轴Y轴电机测试 
    if(CTHL_bCLMotor_N)
    { 
      SetEn(Y_AXIS, MOTOR_DISEN);
      SetEn(X_AXIS, MOTOR_DISEN);
      SetEn(Z_AXIS, MOTOR_DISEN);      
      //Set_VDI_Value(Z_AXIS,1);
      
      CTHL_FKDW_VAVLE=0;
      if(CTHL_FK_Check_UP)
      {
        SetPos(X_AXIS, 0); 
        SetPos(Y_AXIS, 0); 
      }
     
      if(!X_DRV && (!CTHL_QMDW_IN ||!CTHL_QMDW_VAVLE)&&(!CTHL_FKGY_IN||CTHL_FKGY_IN&&CTHL_YXC_VAVLE&&!CTHL_YFK_VAVLE))
      {
        //                Y轴       运行速度                    启动速度        加速度  减速度                 
        AxisContinueMoveAcc(X_AXIS,600 ,DIR_N,600,600,15,15);
        AxisContinueMoveAcc(Y_AXIS,580,DIR_N,580,580,15,15); 
      } 
      if(CTHL_QMDW_IN && CTHL_QMDW_VAVLE||CTHL_FKGY_IN&&CTHL_YXC_VAVLE&&CTHL_YFK_VAVLE)
      {
        AxisEgmStop(Y_AXIS);
        AxisEgmStop(X_AXIS);
      }
    }
    //在压方块电磁阀输出情况下，方块感应后电机要自动停止
    if((CTHL_XCDW_Limit_IN )&& CTHL_bCLMotor_P)
    {
      CTHL_bCLMotor_P=0;
      AxisEgmStop(Y_AXIS);
      AxisEgmStop(X_AXIS);
    }
    if(CTHL_bCLMotor_P)  //后退限位已经取消
    {
      SetEn(Y_AXIS, MOTOR_DISEN);
      SetEn(X_AXIS, MOTOR_DISEN);
      SetEn(Z_AXIS, MOTOR_DISEN);
      //Set_VDI_Value(Z_AXIS,1);
      CTHL_FKDW_VAVLE=0;
      if(CTHL_FK_Check_UP)
      {
        SetPos(X_AXIS, 0); 
        SetPos(Y_AXIS, 0); 
      }
      if(!CTHL_Z_ORIGN_IN)
          AxisContinueMoveAcc(Z_AXIS,630,DIR_P,630,630,15,15);
      //压方块电磁阀没有输出时，方块感应信号不限制
      if(!CTHL_XCDW_Limit_IN)
      {
        if(!X_DRV )
        {  
          //                Y轴       运行速度                    启动速度        加速度  减速度
          AxisContinueMoveAcc(Y_AXIS,630,DIR_P,630,630,15,15);
          AxisContinueMoveAcc(X_AXIS,600,DIR_P,600,600,15,15);
        } 
      } 
    }
    if(!CTHL_bCLMotor_P && !CTHL_bCLMotor_N && (CTHL_TD_MotorStep == 0))
    {
      AxisEgmStop(Y_AXIS);
      AxisEgmStop(X_AXIS);
      if(CTHL_CD_MotorStep==0)AxisEgmStop(Z_AXIS);
    }
    if(GetEn(Z_AXIS)==0&&CTHL_Z_ORIGN_IN)
    {
      SetEn(Z_AXIS, MOTOR_DISEN);      
      Set_Ctrlmode_trans(Z_AXIS,POS_MODE); 
    }
    if(CTHL_bRest_Z)
    {
      CTHL_bRest_Z=0;
      CTHL_bResting_Z=1;
      CTHL_CD_MotorStep=1;
      CTHL_cZipCnt=0;
      SetEn(Z_AXIS, MOTOR_DISEN);
      CTHL_CD_MOTORDelay = dwTickCount + 100; 
    }
  }        
}

#endif