GC23005/Machine/ZhenYuMachine/ZhenYuChuantou_HeLian.c

#include "global.h"

#if  ZHEN_YU_MACHINE ==  1
static unsigned long cycletime,zlttime,cuanliantime,tuodaitime;
static unsigned char comflag=0,JLTFLG,XMFLG,ONCERUNFLG1;


void ExtiAcitionX02(void)//获取X，Y轴的实际位置
{
  
}

//故障报警
void CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag)
{
  SetAlarmCode(CTHL_ALARM_ADDR,alarm_code);
    if(!CTHL_wltzi && !bCxcAuto )
    bAlarmStop = 1;
    else if(bCxcAuto)//伺服告警松轴
    {
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);
      //SetEn(X_AXIS, MOTOR_DISEN);
      //SetEn(Y_AXIS, MOTOR_DISEN);      
    }
}


//运行状态
void CTHL_Running_state(void)
{ 
    
  user_datas[210] = CTHL_ZhuangLiaoStep;
  user_datas[211] = CTHL_FZ_MotorStep;
  user_datas[212] = CTHL_TD_MotorStep;
  user_datas[213] = CTHL_AutoStep;
  
  user_datas[214] = CTHL_LianLengthGLCheckFirst;  //第一条过链感应测量的长度//Y轴测量的长度反转
  user_datas[215] = CTHL_LianLengthGLCheck;//过链实时
  
  user_datas[216] = CTHL_LianLengthCheckFirst;//测量第一条长度
  user_datas[217] = CTHL_LianLengthCheck;//实时测量的长度  
  
  user_datas[218] = CTHL_LianLengthXFZCheckFirst;//X轴反转第一次
  user_datas[219] = CTHL_LianLengthXFZCheck;//X轴实时
  
  user_datas[220] = CTHL_LianLengthYFZCheckFirst;//Y轴第一次
  user_datas[221] = CTHL_LianLengthYFZCheck;       //Y轴实时  
  memcpy(&user_datas[222],&dwXRealPos,4);
  memcpy(&user_datas[224],&dwYRealPos_com,4);
  memcpy(&user_datas[226],&CTHL_FKGY_Length,4);//方块感应位置
  memcpy(&user_datas[228],&CTHL_FKGY_STOP_Length,4);//方块停长度
  memcpy(&user_datas[230],&CTHL_CXCD,4);//穿插销长度。CTHL_GLGY_FKDW_Length
  memcpy(&user_datas[232],&CTHL_GLGY_FKDW_Length,4);

  user_datas[234] = master1.send_count-master1.receive_count;//GetAlarm(X_AXIS);
  user_datas[235] = GetAlarm(Y_AXIS);
  user_datas[236] = servo_y.alarmcode;
  user_datas[237] = servo_y.IO_TO_COM;
}



void CTHL_AlarmProtect(void)
{ 
}


//自动动作
void CTHL_AutoStepAction(void)
{  
   int 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)
      { 
        cycletime=dwTickCount;
        bCxcAuto=0;
        CTHL_GF_Change = 0;//公分节转换
        CTHL_AutoStep++;
        SetEn(X_AXIS, MOTOR_EN);
         //设置反转电机的扭矩和速度的设置       
        if(CTHL_cZipCnt<1)
        {
          freq=CTHL_PARAM_FZ_Speed+20000;
          set_servo_postotarr_limit(Y_AXIS,400,freq,freq,SERVO_TARR_CW);
        }
        else 
        {
          freq=CTHL_PARAM_FZFirst_SPEED+SERVO_PARAM_SPEED_DIFFER;
          set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW);
        }
        Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
      }
      break;
    case 2://有拉头，方块并定位
      if(CTHL_ZhuangLiaoStep == 0&&bZhuangLiaoOkFlg&&CTHL_TD_MotorStep==0)//装拉头完成，方块定位
      {
         if(CTHL_FKDW_IN)//无方块定位信号返回，//人工处理信号，不停机
         {
            CTHL_wltzi=1;//不停机
            CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);// E17方块钩针定位信号异常
         }
         if(CTHL_LTou_Check)//无拉头信号返回，//人工处理信号，不停机
         {
            CTHL_wltzi=1;//不停机
            CTHL_SetAlarmCode(CTHL_XCDW_Limit_ALARM,bRunning);// E11拉头缺料感应异常
         }
        if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)//自动与单步，单步用启动按钮进行
        { 
           if(!CTHL_CTXM_Limit_IN){//下模未到位，下模动作
               CTHL_CTXM_VAVLE=1;
               CTHL_XCJC_VAVLE=1;//插销检测随着下模上升
               CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
               CTHL_AutoStep++;
           }
           else{//下模到位
               CTHL_TXM_VAVLE = 1; //侧推
               CTHL_FZYL_VAVLE=1;//压轮
               CTHL_AutoStep=4;
               CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
           }
        }          
      }
      break;
    case 3://下模到位
        if(CTHL_CTXM_VAVLE&&CTHL_CTXM_Limit_IN) {
           CTHL_TXM_VAVLE = 1; //侧推
           CTHL_FZYL_VAVLE=1;//压轮
           CTHL_AutoStep++;
           CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
        }
        else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE){//人工处理信号，不停机
          CTHL_wltzi=1;//不停机  
          CTHL_SetAlarmCode(CTHL_CTXM_Limit_ALARM,bRunning); //E08 下模到位异常
        }
        break;
    case 4://压轮到位，侧推布带到位，方块钩针升起
        if(CTHL_TXM_VAVLE&&CTHL_TXM_Limit_IN)//侧推布带到位
        { 
          if(CTHL_FZYL_VAVLE&&CTHL_FZYL_Limit_IN)//压轮到位
          {
              CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TXM_DELAY;//侧推延时时间              
              CTHL_AutoStep++;            
          }
          else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE)
          {
            CTHL_wltzi=1;//不停机  
            CTHL_SetAlarmCode(CTHL_FZYL_Limit_ALARM,bRunning);//E22 右压轮到位异常
          }
        }
        else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE)
        {
          CTHL_wltzi=1;//不停机  
          CTHL_SetAlarmCode(CTHL_TXM_Limit_ALARM,bRunning);//E13侧推布带到位异常
        }
        break;
     case 5://侧推到位后延时一小段时间后，退方块定位
        if(dwTickCount >= CTHL_AutoDelay)
        {
          if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP){ 
              CTHL_FKDW_VAVLE = 0;  //方块钩针
              if(servo_y.IO_TO_COM==0);//SetEn(X_AXIS, MOTOR_DISEN);//X轴松开使能
              else
              {
                 SetEn(Y_AXIS, MOTOR_DISEN);//设置扭矩做准备                 
                 if(get_tarr_set(Y_AXIS)!=2)//是否配置转矩限制失败
                 {
                   SetEn(Y_AXIS, MOTOR_DISEN);//设置扭矩做准备
                   CTHL_wltzi=0;//停机 
                   CTHL_SetAlarmCode(CTHL_SERVO_CONFIG_ALARM,bRunning);//伺服转矩配置失败
                 }                 
              } 
              CTHL_AutoStep++;
              CTHL_AutoDelay = dwTickCount + CTHL_PARAM_FKDW_OUT_DELAY;//方块钩针回退延时              
          }
        }
        break;        
      case 6://反转穿入
        if(dwTickCount >= CTHL_AutoDelay)
        {
          if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
          { 
            //定位完成后，位置清零。           
            SetPos(X_AXIS, 0);
            SetPos(Y_AXIS, 0); 
            CTHL_FZ_MotorStep=1;      //反转穿入，
            CTHL_FZ_MotorDelay = dwTickCount +1;//延时穿入拖带 
            CTHL_AutoStep = 9;
          }
        }
        break;
     case 9://穿方块完成
        if(CTHL_FZ_MotorStep==0)//等待穿入完成
        {
          if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
          {
            cBAOjin=0;
            CTHL_JCXBD_VAVLE=1;//插销夹布带，            
            CTHL_AutoStep++;
            CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
          }
        }
        break;        
      case 10://夹到位延时
        if(CTHL_JCXBD_VAVLE&&CTHL_JCXBD_Limit_IN&&!X_DRV){
            CTHL_AutoDelay = dwTickCount + CTHL_PARAM_JBD_DELAY+5;
            //布带先于气缸后退
            AxisMovePosAccDec(X_AXIS,CTHL_PARAM_HLT_SPEED_Speed,-CTHL_PARAM_FZCXC_LENGTH,
                  1200,1200,20,20,0);             
             if((CTHL_cZipCnt <1))//应该Y轴位置是准的，由于通讯的原因，二次读取。
            {                 
               CTHL_LianLengthYFZCheckFirst=dwYRealPos_com; //若这两个值取值比较准的话，可以作为后面的参考       
            }
            CTHL_LianLengthYFZCheck=dwYRealPos_com;
            CTHL_AutoStep++;
        }
        else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE)
        {
          CTHL_wltzi=1;//不停机  
          CTHL_SetAlarmCode(CTHL_JCXBD_Limit_ALARM,bRunning);//E23 插销布带夹到位异常  
        }
        break;
     case 11://插销夹布后退
       if(dwTickCount >= CTHL_AutoDelay){         
            if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP){  
            CTHL_CXHT_VAVLE=1;//插销布带后退动作                
            CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
            CTHL_AutoStep++;
            }
        }
        break;
      case 12://插销夹布后退到位延时
        if(CTHL_CXHT_VAVLE&&CTHL_CXHT_Limit_IN&&!X_DRV){
           CTHL_AutoDelay = dwTickCount + CTHL_PARAM_CXBB_DELAY;  
           CTHL_AutoStep++;
        }
        else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE)
        {
           CTHL_wltzi=1;//不停机 
          CTHL_SetAlarmCode(CTHL_CXHT_Limit_ALARM,bRunning);//插销回退到位异常  
        }
        break;
     case 13://插销摆臂动作
       if(dwTickCount >= CTHL_AutoDelay){         
            if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP){  
              CTHL_XCBB_VAVLE=1;              
              CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
              CTHL_AutoStep++; 
            }
        }
        break;
    case 14://插销摆臂到位延时
        if(CTHL_XCBB_VAVLE&&CTHL_XCBB_Limit_IN){
          
          CTHL_AutoDelay = dwTickCount + CTHL_PARAM_YSCX_DELAY;
          CTHL_AutoStep++;          
        }
        else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE)
        {
          CTHL_wltzi=1;//不停机 
          CTHL_SetAlarmCode(CTHL_CXHT_Limit_ALARM,bRunning);//插销回退到位异常  
        }
        break;
     case 15://
        if(dwTickCount >= CTHL_AutoDelay)
        {          
            CTHL_AutoDelay = dwTickCount +0; 
            CTHL_AutoStep++;         
        }
        break;
     case 16: // 
        if(dwTickCount >= CTHL_AutoDelay)
        {          
          if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP){
            CTHL_TD_MotorStep =10;
            CTHL_TD_MotorDelay = dwTickCount+ 1;            
            CTHL_AutoStep++;            
          }
        }
        break;
      case 17:
        if((CTHL_TD_MotorStep == 0))//FZ电机运行完成
        {   
            if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)
            {
              CTHL_PARAM_CYCLE_TIME=dwTickCount-cycletime;
              CTHL_AutoStep = 1; //开始循环
              if(cStopMode==1 || bSingOneFlg)
              {
                cStopMode=2;//停机后循环完成动作
                CTHL_ZhouWantwo=1;//做完两条
              }
              AddToTal(CTHL_TOTAL_ADDR);    
              AddToTal(CTHL_TRUST_ALARM_ADDR);                 
              CalProSP(CTHL_SPEED_ADDR);
              //公分节那条也不加 
              if((CTHL_cZipCnt<2) && !CTHL_GF_Change) CTHL_cZipCnt++;   
              if((CTHL_NoLaLian_IN))CTHL_cZipCnt = 0;                
              if(bSingOneFlg || CTHL_bHandWork_MODE)
              {
                CTHL_AutoStep = 0;
                bRunning = 0;
                bSingOneFlg = 0;
              }
              
            }
        }       
        break;    
      }    
  }
}

void CTHL_CheckStart(void)
{
  if(CTHL_START_IN_UP || bStart)
  {
    if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)//伺服告警DB9模式
    {
      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_ZhuangLiaoDelay = dwTickCount + 150;
      }
      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_EN);
      SetEn(Y_AXIS, MOTOR_EN);
      Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
      bFZ_LengthOK = 0;
      CTHL_FZ_MotorStep = 0;
         
      CTHL_PARAM_ZDP_AUTO=0;//振动盘控制
      bCxcAuto=0;//运行过程中伺服报警
      //
      CTHL_TLiao_VAVLE =0;      
      CTHL_HSLiao_VAVLE   =0;      
      //
      CTHL_XCBB_VAVLE=0;
      CTHL_JCXBD_VAVLE=0;      
      CTHL_CXHT_VAVLE=0;
      CTHL_FZYL_VAVLE=0;
      //
      CTHL_FKDW_VAVLE=0;
      CTHL_SMZ_VAVLE=0;
      CTHL_TXM_VAVLE=0;
      CTHL_HL_VAVLE=0;
      //装料拉头
      CTHL_ZhuangLiaoDelay = dwTickCount + 150;
      CTHL_ZhuangLiaoStep=1;        
      bTuiLaTouOkFlg = 1;      
      bZhuangLiaoOkFlg= 0; 
      bXiaoMOUpFlg=1;
      CTHL_TryCnt = 0;//重装拉头次数
      //方块定位
      CTHL_TD_MotorStep=1; 
      //自动运行
      CTHL_AutoDelay = dwTickCount + 10;        
      CTHL_AutoStep = 1;
      CTHL_cZipCnt = 0;//头两条检测
      //
    }  
    bStart = 0;
    CTHL_bSingOne=0;
  }   
  if(CTHL_ZhouWantwo)//按下停止后，该条完成
  {
    CTHL_ZhouWantwo=0;
    if(CTHL_ZhouWanone)
    {
      CTHL_ZhouWanone=0;
      bRunning=0;
      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;
      }
      else if(CTHL_PARAM_DBSF_MODE)
      {
        if(!CTHL_SF_ALR_IN)
        {
          CTHL_SF_ALR_CLR=1;
          CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;
          
        }   
      }
      SetEn(X_AXIS, MOTOR_DISEN);
      SetEn(Y_AXIS, MOTOR_DISEN);
      
      bSingOneFlg=0;
      CTHL_AutoStep = 0; 
      CTHL_FZ_MotorStep =0;   
      CTHL_TD_MotorStep =0; 
    }
  }
 
  //停止
  if(CTHL_STOP_IN_UP || bStop)
  {	
    if(cStopMode < 2) 
    {
      cStopMode++;//停止模式
    }
    if(cStopMode==1 && bRunning)CTHL_ZhouWanone=1;//按下停止后，记录标准
    else if(cStopMode==2 && bRunning)//按下停止两次后，停机。
    {   
      bRunning=0;
      cStopMode=0; 
      bSingOneFlg=0;
      bFZ_LengthOK = 0;
      cBAOjin=0;//伺服报警标志
      CTHL_wltzi=0;
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);
      SetEn(X_AXIS, MOTOR_DISEN);
      SetEn(Y_AXIS, MOTOR_DISEN);
      CTHL_AutoStep = 0; 
      CTHL_FZ_MotorStep =0;   
      CTHL_TD_MotorStep =0;    
      CTHL_ZhuangLiaoStep=0;  
      CTHL_TLiao_VAVLE=0;             
      CTHL_HSLiao_VAVLE  =0;       
      CTHL_XCJC_VAVLE=0;
      CTHL_XCBB_VAVLE=0;
      CTHL_JCXBD_VAVLE=0;
      CTHL_HL_VAVLE=0;
      
      if(CTHL_CTXM_VAVLE){
        CTHL_JLTou_VAVLE=1;
        CTHL_MGuo_VAVLE=0;
      }
      CTHL_CTXM_VAVLE =0;
      CTHL_TXM_VAVLE=0;
      CTHL_SMZ_VAVLE=0;
      CTHL_FKDW_VAVLE=0;
      CTHL_FZYL_VAVLE=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;
      }
      else 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;
      cStopMode=0;
      bSingOneFlg=0;
      CTHL_wltzi=0;
      bCxcAuto=0;
      cBAOjin=0;

      bFZ_LengthOK = 0;
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);
      
      SetEn(X_AXIS, MOTOR_DISEN);
      SetEn(Y_AXIS, MOTOR_DISEN);
      SetAlarmCode(CTHL_ALARM_ADDR,0); 
      CTHL_TLiao_VAVLE =0;      
      CTHL_HSLiao_VAVLE   =0;  
      CTHL_XCJC_VAVLE=0;
      CTHL_JCXBD_VAVLE=0;
      
      if(CTHL_CTXM_VAVLE){
        CTHL_JLTou_VAVLE=1;
        CTHL_MGuo_VAVLE=0;
      }      
      CTHL_CTXM_VAVLE  =0; 
      CTHL_FKDW_VAVLE=0; 
      CTHL_TXM_VAVLE=0;
      CTHL_SMZ_VAVLE=0;
      CTHL_FKDW_VAVLE=0;
      CTHL_FZYL_VAVLE=0;
      CTHL_XCBB_VAVLE=0;
      CTHL_HL_VAVLE=0;
     
      CTHL_AutoStep = 0;
      CTHL_ZhuangLiaoStep=0;
      CTHL_TLiaoStep=0;
      CTHL_TD_MotorStep=0;
      CTHL_SongLiaoStep   =0;     
      CTHL_FZ_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);
        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); 
      CTHL_TLiaoStep = 0; 
      CTHL_AutoStep = 0;
      //CTHL_LianLengthCheck = 0;
    }
    bStop=0;
  }
  
  if(bAlarmStop)
  {   
    bRunning   = 0;
    bAlarmStop=0;
    cStopMode=0;
    CTHL_TLiao_VAVLE =0;      
    CTHL_HSLiao_VAVLE   =0;      
      
    CTHL_FKDW_VAVLE=0; 
    CTHL_TXM_VAVLE=0;
    CTHL_FKDW_VAVLE=0;
    CTHL_FZYL_VAVLE=0;
    CTHL_XCBB_VAVLE=0;
    CTHL_JCXBD_VAVLE=0;
    CTHL_HL_VAVLE=0;
    AxisEgmStop(X_AXIS);
    AxisEgmStop(Y_AXIS);
    CTHL_AutoStep = 0; 
    CTHL_FZ_MotorStep =0;   
    CTHL_TD_MotorStep =0;   
  }
#endif    
}   
//拖带电机控制动作  X轴
void CTHL_TD_Motor(void)	 //
{ 

  if(dwTickCount >= CTHL_bSFBBAOJIN_Delay&&CTHL_SF_ALR_CLR) //伺服报警时间 
  {
    CTHL_SF_ALR_CLR=0;
    SetClr(X_AXIS,MOTOR_ALARM);    
    //SetClr(Y_AXIS,MOTOR_ALARM);
    
  }
 
  if((GetAlarm(Y_AXIS)==1 || GetAlarm(X_AXIS)==1) && !CTHL_PARAM_DBSF_MODE &&!cBAOjin)
  {
             
    bCxcAuto=1;
    cBAOjin=1;
    CTHL_SF_ALR_IN=0;
    AxisEgmStop(X_AXIS);
    AxisEgmStop(Y_AXIS);
    CTHL_AutoStep = 101; 
    SetEn(X_AXIS, MOTOR_DISEN);
    SetEn(Y_AXIS, MOTOR_DISEN); 
    CTHL_SetAlarmCode(CTHL_SF_ALR_ALARM,bRunning);   //无拉链自动
  }
  else
  {
    CTHL_SF_ALR_IN=1;
  }
  memcpy(&user_datas[420],&dwXRealPos,4);
#if 1
  switch(CTHL_TD_MotorStep)
  {
    //1步开始，纯定位, 没有穿入
  case 1://判断是否有方块感应
    if(dwTickCount >= CTHL_TD_MotorDelay)
    {
      if(!CTHL_FKDW_IN)CTHL_TD_MotorStep = 0; 
      else  {
       SetEn(X_AXIS, MOTOR_EN);
       SetEn(Y_AXIS, MOTOR_EN);//松轴
       CTHL_FKDW_VAVLE=0;
       CTHL_SMZ_VAVLE=0;
       CTHL_TXM_VAVLE=0;
       CTHL_XCBB_VAVLE=0;
       CTHL_FZYL_VAVLE=0;//压轮松开
       CTHL_TD_MotorDelay=dwTickCount+50;
       CTHL_TD_MotorStep++;
      }
    }
    break;
  case 2:
  if(dwTickCount >= CTHL_TD_MotorDelay){  //给锁轴时间
      SetPos(X_AXIS, 0);   
      AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1500,600,30,15);  
      AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1500,600,30,15); 
      CTHL_TD_MotorStep++; 
    }  
    break;
  case 3://检测到过链 
    if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块
      bCxcAuto=1;
      CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
    }    
    if(CTHL_FK_Check_UP)//有方块信号；
    {      
      CTHL_XSavePosBuff = dwXRealPos;//方块位置      
      CTHL_TD_MotorStep++;  
    }    
    break;    
  case 4:
    if(dwXRealPos>=CTHL_XSavePosBuff+CTHL_PARAM_FKDW_DW_LENGTH)
    {
       CTHL_FKDW_VAVLE=1;
       
    }
    if(dwXRealPos>=CTHL_XSavePosBuff+CTHL_PARAM_TDDW_LENGTH)
    {
       AxisContinueMoveChangeSpeed(X_AXIS,600,600,20,10);
       AxisContinueMoveChangeSpeed(Y_AXIS,600,600,20,10); 
       CTHL_TD_MotorDelay = dwTickCount + 1500;
       CTHL_TD_MotorStep++; 
    }
    break;
  case 5:
    //定位感应亮时、位置到达时，或X轴停止时，停止X轴
    if(!CTHL_FKDW_IN)
    {
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);
      CTHL_TD_MotorDelay = dwTickCount + 50;
      CTHL_TD_MotorStep++; 
    }    
    if(dwTickCount >= CTHL_TD_MotorDelay)//1.5S为检测到停机
    { 
       bCxcAuto=1;;
       CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常
    }
    if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块
      bCxcAuto=1;
      CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
    }  
    break;
   case 6://压轮动作
    if(dwTickCount >= CTHL_TD_MotorDelay&&!X_DRV&&!Y_DRV){
      CTHL_FZYL_VAVLE=1;
      AxisMovePosAccDec(X_AXIS,1200,-10,1200,1200,20,20,0);
      CTHL_TD_MotorDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
      CTHL_TD_MotorStep++;  
    }
    break;  
   case 7://压轮到位
     if(CTHL_FZYL_VAVLE&&CTHL_FZYL_Limit_IN&&!X_DRV){
       CTHL_TD_MotorStep = 0; 
       CTHL_GLGY_FKDW_Length=dwXRealPos-CTHL_XSavePosBuff;//第一次获取过链感应与定位间的长度
     }
    else if(dwTickCount >= CTHL_TD_MotorDelay){
       CTHL_SetAlarmCode(CTHL_FZYL_Limit_ALARM,bRunning);//压轮到位异常
    }
    break;
  case 10://开始穿小插
    if(dwTickCount >= CTHL_TD_MotorDelay&&!X_DRV&&!Y_DRV)
    { 
        tuodaitime=dwTickCount;
        CTHL_XSavePosBuff3=dwXRealPos;
        CTHL_CXHT_VAVLE=0;        
        AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_HLT_SPEED_Speed,CTHL_PARAM_CRFZ_LENGTH+180,
              1000,1000,20,20,CTHL_PARAM_XCCR_LENGTH);
        CTHL_TD_MotorStep++;  
        CTHL_TD_MotorDelay = dwTickCount+ 1000;
        ONCERUNFLG1=1;
    }
    break;        
  case 11: 
    if(abs(dwXRealPos-CTHL_XSavePosBuff3)>=120)CTHL_JCXBD_VAVLE=0;//夹插销布带松开
    if(abs(dwXRealPos-CTHL_XSavePosBuff3)>=50&&ONCERUNFLG1)
    {
      ONCERUNFLG1=0;
      AxisMovePosAccDec(Y_AXIS,1500,250,1500,1000,20,20,0);//Y轴送带
    }
    if(CTHL_CXGY_IN)
    {
      CTHL_XCJC_VAVLE=0;//插销检测下降     
      CTHL_HL_VAVLE=1;
      CTHL_TD_MotorStep++;
      CTHL_TD_MotorDelay = dwTickCount+ CTHL_PARAM_XCIN_DELAY;
      
    }
    else if(dwTickCount >= CTHL_TD_MotorDelay)  
    {
      bCxcAuto=1;//不停机 
      CTHL_SetAlarmCode(CTHL_CXGY_Check_ALARM,bRunning);//E28 插销感应到位检测异常
    }      
    break;
  case 12: // 
    if(dwTickCount >= CTHL_TD_MotorDelay) //延时停X轴
    { 
         CTHL_TD_MotorStep=40;         
         AxisEgmStop(X_AXIS);   
    }
    break;
  case 40: // 
    if(!X_DRV&&!Y_DRV)
    {
        CTHL_SMZ_VAVLE=0;
        CTHL_MGuo_VAVLE=0;        
        CTHL_TD_MotorStep=41;
        CTHL_CXCD=abs(dwXRealPos-CTHL_XSavePosBuff3);
        CTHL_XSavePosBuff3 = dwXRealPos;
        CTHL_XSavePosBuff =  dwXRealPos;        
    }
    break; 
  case 41: // 
    if(CTHL_SML_Limit_IN)
    {
       CTHL_TD_MotorDelay = dwTickCount+ CTHL_PARAM_YSXC_DELAY;//延时合练
    }
    else if(dwTickCount >= CTHL_TD_MotorDelay&&!X_DRV) //开始提速
    {
      if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)//自动与单步，单步用启动按钮进行
      {
         CTHL_TD_MotorStep=42;         
         CTHL_FZYL_VAVLE=0; 
         CTHL_XSavePosBuff3=dwXRealPos;
      }
    }
    break; 
  case 42: // 
      //if(dwTickCount >= CTHL_TD_MotorDelay)   
      {        
         CTHL_TD_MotorStep=13; 
      }     
    break; 
  case 13: //退推方块延时到  ，合练
    //if(dwTickCount >= CTHL_TD_MotorDelay&&CTHL_SML_Limit_IN)//走合练长度
    {    
      
       CTHL_HL_LENTH=abs(dwXRealPos-CTHL_XSavePosBuff3);
      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN_UP)
      {
         //前两条用反转长度定位合练长度
        if(CTHL_LianLengthXFZCheckFirst>=CTHL_PARAM_HELIAN_LENGTH)
        {
          CTHL_LENTH=CTHL_LianLengthXFZCheckFirst+CTHL_PARAM_HLDW_LENGTH+200-CTHL_PARAM_HELIAN_LENGTH-CTHL_HL_LENTH;
        }
        else//剩余合练设置错误
        {
          bCxcAuto=1;
          CTHL_SetAlarmCode(CTHL_SYHL_ERRO_ALARM,bRunning);
        }
        //用第一次速度
        AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_First_SPEED,CTHL_LENTH, 
                       1000,CTHL_PARAM_First_SPEED,30,200,0);//快速合链
        
      }
      else
      {                 //前两条用反转长度定位合练长度
         if(CTHL_LianLengthCheckFirst>=CTHL_PARAM_HELIAN_LENGTH)
        {
          CTHL_LENTH=CTHL_LianLengthCheckFirst+CTHL_PARAM_HLDW_LENGTH-CTHL_FKGY_FKDW_Length-CTHL_PARAM_HELIAN_LENGTH-CTHL_HL_LENTH;
        }
        else//剩余合练设置错误
        {
          bCxcAuto=1;
          CTHL_SetAlarmCode(CTHL_SYHL_ERRO_ALARM,bRunning);
        }
        //用合练速度
        AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_TD_RunSpeed,CTHL_LENTH, 
                       1200,CTHL_PARAM_TD_RunSpeed,50,100,100);//快速合链
      }
      CTHL_XSavePosBuff3 = dwXRealPos;
      bFKCheck_Flag = 0; 
      cDELAYONE=0;
      CTHL_TD_MotorStep++;
      JLTFLG=1;
      XMFLG=1;
    }
    break;
  case 14: 
    if(abs(dwXRealPos-CTHL_XSavePosBuff3)>=80&&JLTFLG)
    {
      JLTFLG=0;
      CTHL_JLTou_VAVLE=1;
    }
    if(abs(dwXRealPos-CTHL_XSavePosBuff3)>=200&&XMFLG)
    {
      XMFLG=0;
      CTHL_CTXM_VAVLE=0;
      if(CTHL_ZhuangLiaoStep==0)//装拉头
      {
        CTHL_ZhuangLiaoDelay = dwTickCount + 20;
        CTHL_ZhuangLiaoStep=1;
        bZhuangLiaoOkFlg= 0;
        bXiaoMOUpFlg=0;
      } 
    }
    if(abs(dwXRealPos-CTHL_XSavePosBuff3)>=30)
    {
       AxisMovePosAccDecNotStop(Y_AXIS,2000,100, 
                 1000,1000,30,150,0);//Y轴转动
    }
    if((CTHL_FK_Check_UP)&& !bFKCheck_Flag){//第一方块信号检测
      bFKCheck_Flag = 1; //置方块检测为1
      cDELAYONE=1;
      CTHL_XSavePosBuff1= dwXRealPos;//第一个方块检测位置
    }
    if(CTHL_cZipCnt<1)
    {        
        //长度限制  
        if(abs(dwXRealPos-CTHL_XSavePosBuff) >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块
          bCxcAuto=1;
          CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
        }
    }
    else
    {
       //长度限制  
      if((abs(dwXRealPos-CTHL_XSavePosBuff) >=(long)(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheckFirst)))
      {
        bCxcAuto=1;  
        CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
      }
    }    
    if(abs(dwXRealPos-CTHL_XSavePosBuff3) >=CTHL_LENTH-20){      
      CTHL_XCJC_VAVLE=0;
      CTHL_XCBB_VAVLE=0;
      CTHL_HL_VAVLE=0;
    }
    if(abs(dwXRealPos-CTHL_XSavePosBuff3) >=CTHL_LENTH+50)
    {
      CTHL_TD_MotorStep++;//
    }
    break;
  case 15: //加速
    if(CTHL_FK_Check_UP && !bFKCheck_Flag)//为检测到第一方块继续检测
    {
      bFKCheck_Flag = 1;
      cDELAYONE=2;
      CTHL_XSavePosBuff1= dwXRealPos; 
      CTHL_TD_MotorDelay = dwTickCount+ 200;
    }
    if((!CTHL_NoLaLian_IN))
    {
      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN_UP)
      {
         AxisContinueMoveChangeSpeed(X_AXIS,CTHL_PARAM_First_SPEED,1000,20,50);
          //长度限制  
         if(abs(dwXRealPos-CTHL_XSavePosBuff) >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块
            bCxcAuto=1;
            CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
          }
      }
      else
      {
        if((CTHL_LianLengthCheckFirst < 4000) && (CTHL_LianLengthCheck  <4000)){ // 40公分    
           CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED*2/3;//长链拖带快速速度，小于4000的链条     
         }
        else if((CTHL_LianLengthCheckFirst < 5000) && (CTHL_LianLengthCheck <5000)) { 
          CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED*4/5;//长链拖带快速速度，小于5000的链条
        }
        else{
          CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED;//长链拖带快速速度
        } 
        CTHL_LENTH=CTHL_LianLengthCheckFirst-CTHL_GLGY_FKDW_Length-dwXRealPos;
        AxisMovePosAccDecNotStop(X_AXIS,CTHL_DataBuff,CTHL_LENTH,
                        1000,CTHL_PARAM_FKDW_Speed,50,100,CTHL_PARAM_GJSY_LENGTH);
         //长度限制  
        if((abs(dwXRealPos-CTHL_XSavePosBuff) >=(long)(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheckFirst)))
        {
          bCxcAuto=1;
          CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
        }
      }
    }
    //CTHL_TD_MotorStep = 17;
    if(bFKCheck_Flag) CTHL_TD_MotorStep = 17;  //说明第一个方块已经检测到
    else CTHL_TD_MotorStep++; //第一个方块没检测到时先运动的第一个再检测定位
    break;   
  case 16: 
    //检测方块
    if(CTHL_FK_Check_UP&& !bFKCheck_Flag)//第一次检测到方块
    {   
      bFKCheck_Flag = 1; 
      cDELAYONE=3;
      CTHL_XSavePosBuff1= dwXRealPos;
      CTHL_TD_MotorDelay = dwTickCount+ 200;
      CTHL_TD_MotorStep++; 
    }
    if(CTHL_cZipCnt<1)
    {
       //长度限制  
       if(abs(dwXRealPos-CTHL_XSavePosBuff) >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块
          bCxcAuto=1;
          CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
        }
    }
    else
    {
         //长度限制  
        if((abs(dwXRealPos-CTHL_XSavePosBuff) >=(long)(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheckFirst)))
        {
          bCxcAuto=1;
          CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
        }
    } 
    break;   
  case 17://需判断中间无方块的情况，或者是短链的情况
    //在链上行走8公分
        //检测方块    
    /*if(CTHL_FK_Check_UP&& !bFKCheck_Flag)//第一次检测到方块
    {   
      bFKCheck_Flag = 1; 
      cDELAYONE=3;
      CTHL_XSavePosBuff1= dwXRealPos;
    }*/
    if(!CTHL_FK_Check && (dwXRealPos >= 1000))
    {  
      CTHL_TD_MotorStep++;       
    }   
    break;
  case 18:
        //在链上行走8公分
    if(!CTHL_FK_Check && dwXRealPos>400 &&(abs(dwXRealPos-CTHL_XSavePosBuff1) >= (CTHL_PARAM_XMUP_LENGTH)))//上摸位置
    {   
        CTHL_XCBB_VAVLE=0; 
        bXiaoMOUpFlg=1; 
    }
    //第二次感应方块， 要做定位，//感应到第一个方块插销位置
    if(CTHL_FK_Check_UP &&dwTickCount >= CTHL_TD_MotorDelay&& (dwXRealPos >=  1600))
    {
      CTHL_XSavePosBuff3= dwXRealPos;
      if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN_UP)
      {          
          
          CTHL_LianLengthGLCheckFirst= dwXRealPos-CTHL_XSavePosBuff1;
          CTHL_LianLengthCheckFirst= dwXRealPos-CTHL_XSavePosBuff1;
      }
      else
      {
        //感应到方块后，走段距离减速
        CTHL_LENTH=CTHL_PARAM_TDDW_LENGTH;
        AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDW_Speed,CTHL_LENTH,
                      1000,800,30,30,100);        
      }
       CTHL_LianLengthGLCheck= dwXRealPos-CTHL_XSavePosBuff1;
       CTHL_TD_MotorStep++;      
       //公分节
      /*if((CTHL_LianLengthGLCheck <= CTHL_GLGY_FKDW_Length) && (CTHL_cZipCnt>1))//感应到方块后定位距离
      {
        CTHL_TD_MotorStep=50;
        AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1500,600,30,15);  
        AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1500,600,30,15); 
        bFZ_LengthOK= 0;
        CTHL_cZipCnt = 0;
      }
      else
      {
        CTHL_TD_MotorStep++;
      }*/  
    }
    if(CTHL_cZipCnt<1)
    {
       //长度限制  
       if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块
          bCxcAuto=1;
          CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
        }
    }
    else
    {
       //长度限制  
      if((dwXRealPos >=(long)(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheckFirst)))
      {
        bCxcAuto=1;
        CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
      }
    }       
    break; 
   case 19:
    //方块位置+方块钩针下降位置
    if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN_UP){
      if(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_FKDW_DW_LENGTH){CTHL_FKDW_VAVLE=1;}
        if(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_TDDW_LENGTH)
        {
          CTHL_TD_MotorStep++; 
          CTHL_TD_MotorDelay = dwTickCount+ 1500;
          AxisContinueMoveChangeSpeed(X_AXIS,800,800,20,10);
        }
               
    }
    else
    {
       if(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_FKDW_DW_LENGTH)
       {
         CTHL_FKDW_VAVLE=1;         
         CTHL_TD_MotorStep++; 
         CTHL_TD_MotorDelay = dwTickCount+ 1500;
       }      
    }
    break;
   case 20:
    //定位感应亮时、位置到达时，或X轴停止时，停止X轴
    if(dwTickCount >= CTHL_TD_MotorDelay)//3S为检测到停机
    { 
       bCxcAuto=1;
       CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常
    }
    if(!CTHL_FKDW_IN)
    {
      CTHL_TD_MotorStep++; 
      CTHL_TD_MotorDelay = dwTickCount+ 0;
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);
    }
    if(CTHL_cZipCnt<1)
    {
        //长度限制  
       if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块
          bCxcAuto=1;
          CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
        }
    }
    else
    {
      //长度限制  
      if((dwXRealPos >=(long)(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheckFirst)))
      {
        bCxcAuto=1;
        CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
      }
    }
    break;
   case 21://压轮动作，做一个反转动作，松一下拉链
    if(dwTickCount >= CTHL_TD_MotorDelay&&!X_DRV)
    {
       CTHL_FZYL_VAVLE=1;//压轮
       CTHL_TD_MotorStep++; 
       AxisMovePosAccDec(X_AXIS,1500,-30,1000,1000,20,20,0);
       CTHL_TD_MotorDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
    }
    break;  
   case 22://到位
    //if(CTHL_FZYL_VAVLE&&CTHL_FZYL_Limit_IN)
    {
        CTHL_TD_MotorStep = 0;
        CTHL_PARAM_TD_TIME=dwTickCount-tuodaitime;
        if(CTHL_cZipCnt<1)
        {
          //测量过链感应与方块定位的距离
          // CTHL_LianLengthCheckFirst=dwXRealPos;
           CTHL_GLGY_FKDW_Length=dwXRealPos-CTHL_XSavePosBuff3;
           //CTHL_PARAM_TDDW_LENGTH=CTHL_GLGY_FKDW_Length;
           //测量方块感应与方块定位的距离
           CTHL_FKGY_FKDW_Length=CTHL_LianLengthCheckFirst-CTHL_LianLengthXFZCheckFirst;           
        }
        else
        {           
            bFZ_LengthOK = 1;
        }
        CTHL_LianLengthCheck = dwXRealPos ;
        CTHL_TD_MotorDelay = dwTickCount + 0;      
     }
      //尺寸变化较大，重新测量
      if(CTHL_cZipCnt>2)
      {
        if(abs(CTHL_LianLengthCheck-CTHL_LianLengthCheckFirst)>2000)
        {
            CTHL_cZipCnt=0;
        }
      }    
    break;
    //由公分节调转
  case 50: 
    
    if(CTHL_FK_Check_DW)  
    {          
      CTHL_TD_MotorStep = 51;  
      CTHL_GF_Change = 1;      
      SetPos(X_AXIS, 0);   
    }
    break;
  case 51:  
    if(CTHL_FK_Check)
    {
      CTHL_TD_MotorDelay = dwTickCount + 100;
    }  
    else if(dwTickCount >= CTHL_TD_MotorDelay)
    {
      CTHL_TD_MotorStep = 52;      
    }  
    break;    
  case 52:  
    if(dwXRealPos>CTHL_GLGY_FKDW_Length+500)
    {
      if(CTHL_FK_Check_UP)
      {           
        CTHL_XSavePosBuff = dwXRealPos;//方块位置      
        CTHL_TD_MotorStep=4;  
      }
    }
    break;   
    
  }   
#endif    
}
//穿入电机动作，Y轴
void CTHL_FZ_Motor(void)	 //
{ 
   short *revise_buff;
#if 1
  //1步开始,  穿链反转长度开始，
  //20步开始  插销定位开始
  switch(CTHL_FZ_MotorStep)
  {
    //穿入长度位置
  case 1:           
    if(dwTickCount >= CTHL_FZ_MotorDelay)
    { 
      cuanliantime=dwTickCount;
      //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,DIR_N,1000,1000,30,50);
      }
      else 
      {
         AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_JXM_SPEED+80,-CTHL_PARAM_XMCR_SLOW_LENGTH,
                         1000,CTHL_PARAM_JXM_SPEED,50,100,0);
         AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_JXM_SPEED,-CTHL_PARAM_XMCR_SLOW_LENGTH,
                         1000,CTHL_PARAM_JXM_SPEED,50,100,0); 
      }
      CTHL_FZ_MotorStep ++;                              
    }
    break;
  case 2:
    //if(dwYRealPos_com > CTHL_PARAM_TJXM_LENGTH)CTHL_TXM_VAVLE=0;  //到达可推出侧推布带时
    //if(dwYRealPos_com > CTHL_PARAM_SMXJ_LENGTH)CTHL_SMZ_VAVLE=1; //到达上模位置上摸下降
    if(abs(dwXRealPos) >=CTHL_PARAM_XMCR_SLOW_LENGTH-50){//提前转换模式
      Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);
    }
    if(abs(dwXRealPos) >= CTHL_PARAM_XMCR_SLOW_LENGTH)//穿链结束，开始走链加速
    {
      CTHL_FZ_MotorDelay = dwTickCount+0;
      if(CTHL_cZipCnt<1)
      {
         //Set_Ctrlmode_trans(Y_AXIS,TARR_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_LianLengthXFZCheckFirst-CTHL_PARAM_XMCR_SLOW_LENGTH+CTHL_PARAM_FZXZ_LENGTH-CTHL_PARAM_FZJS_LENGTH;
         revise_buff=(short *)(&CTHL_PARAM_FZXZ_LENGTH);    
         CTHL_LENTH = CTHL_LianLengthAutoFZCRcheck-abs(dwXRealPos)+(*revise_buff)-CTHL_PARAM_FZJS_LENGTH;
         AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FZ_Speed,-CTHL_LENTH,
                                   1500,CTHL_PARAM_FZDS_Speed,100,120,0); 
         AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZ_Speed+100,-CTHL_LENTH,
                                   1500,CTHL_PARAM_FZDS_Speed,100,120,0);
      }        
      CTHL_FZ_MotorStep++;      
    }
    //电机速度差太大
    if(abs(dwXRealPos)>dwYRealPos_com+4500)
    {
        bCxcAuto=1;//停轴
        SetEn(Y_AXIS, MOTOR_DISEN);
        CTHL_SetAlarmCode(CTHL_FZMOTOR_ALARM,bRunning);   //反转电机卡滞   
    }
    comflag=1;
    break;
  case 3:
    //if(dwYRealPos_com> CTHL_PARAM_TJXM_LENGTH)CTHL_TXM_VAVLE=0;  //到达可推出侧推布带时
    //if(dwYRealPos_com > CTHL_PARAM_SMXJ_LENGTH)CTHL_SMZ_VAVLE=1; //到达上模位置上摸下降
    if(dwTickCount> CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH)CTHL_TXM_VAVLE=0;  //到达可推出侧推布带时
    if(dwTickCount > CTHL_FZ_MotorDelay+CTHL_PARAM_SMXJ_LENGTH)CTHL_SMZ_VAVLE=1; //到达上模位置上摸
    if(CTHL_cZipCnt<1)
    {
      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,900,120,80,CTHL_PARAM_FZJSDS_LENGTH);
         AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZDS_Speed,-CTHL_PARAM_FZJS_LENGTH,
                                     1500,900,120,80,CTHL_PARAM_FZJSDS_LENGTH);         
         CTHL_FZ_MotorStep++; 
      }
    }
         //长度超出报警
    if(CTHL_cZipCnt>=1)
    {
        if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthXFZCheckFirst)) //拉带报警长度
        {
          bCxcAuto=1;//停轴
          SetEn(Y_AXIS, MOTOR_DISEN);
          CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning);  //穿入方块超出预设长度            
        }
         //方块检测异常(光纤)
        if(abs(dwXRealPos) >= (CTHL_LianLengthXFZCheckFirst + 500))
        {
          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)
    {
        bCxcAuto=1;//停轴
        SetEn(Y_AXIS, MOTOR_DISEN);
        //CTHL_FZYL_VAVLE=0;
        CTHL_SetAlarmCode(CTHL_FZMOTOR_ALARM,bRunning);   //穿入方块超出预设长度    
    }
    break;
  case 4:
    //if(dwYRealPos_com > CTHL_PARAM_TJXM_LENGTH)CTHL_TXM_VAVLE=0;  //到达可推出侧推布带时
    //if(dwYRealPos_com > CTHL_PARAM_SMXJ_LENGTH)CTHL_SMZ_VAVLE=1; //到达上模位置上摸下降
    if(dwTickCount> CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH)CTHL_TXM_VAVLE=0;  //到达可推出侧推布带时
    if(dwTickCount > CTHL_FZ_MotorDelay+CTHL_PARAM_SMXJ_LENGTH)CTHL_SMZ_VAVLE=1; //到达上模位置上摸
    if((abs(dwXRealPos) > 500) && CTHL_FKGY_IN_UP)//位置大于100，且方块感应上升沿
    {
       //Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);
       CTHL_XSavePosBuff1 = abs(dwXRealPos);
       if(CTHL_cZipCnt<1){//记录前两条的方块感应长度；
       
        //CTHL_FKGY_Length=abs(dwXRealPos);        
        //停机距离调整
        CTHL_LENTH=0;
        AxisMovePosAccDecNotStop(X_AXIS,500,-CTHL_LENTH,
                              800,800,15,25,0);
        AxisMovePosAccDecNotStop(Y_AXIS,600,-CTHL_LENTH,
                               800,800,15,25,0);
      } 
      else {
        //停机距离调整        
        CTHL_LENTH=CTHL_PARAM_FZSTOP_LENGTH;       
        AxisMovePosAccDecNotStop(X_AXIS,500,-CTHL_LENTH,
                               800,800,15,25,0);
        AxisMovePosAccDecNotStop(Y_AXIS,600,-CTHL_LENTH,
                              800,800,15,25,0);
      }
      CTHL_FZ_MotorStep++; 
      //启动定时器
      CTHL_FZ_MotorDelay = dwTickCount +2000;//定时2S定位完成
    }
     //长度超出报警
    if(CTHL_cZipCnt>=1)
    {
        if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthXFZCheckFirst)) //拉带报警长度
        {
          bCxcAuto=1;//停轴
          SetEn(Y_AXIS, MOTOR_DISEN);
          CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning);  //穿入方块超出预设长度            
        }
         //方块检测异常(光纤)
        if(abs(dwXRealPos) >= (CTHL_LianLengthXFZCheckFirst + 500))
        {
          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)
    {
        bCxcAuto=1;//停轴
        SetEn(Y_AXIS, MOTOR_DISEN);
        CTHL_FZYL_VAVLE=0;
        CTHL_SetAlarmCode(CTHL_FZMOTOR_ALARM,bRunning);   //穿入方块超出预设长度    
    }
    break;
  case 5: //方块有亮到灭 ，完成    
    if(CTHL_cZipCnt<1)
    {
       if(!CTHL_FKGY_IN)
      { 
        CTHL_FZ_MotorStep++; 
        AxisDecStop(X_AXIS);
        AxisEgmStop(Y_AXIS);
        CTHL_FZ_MotorDelay = dwTickCount +1; 
        Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
      } 
      else if(dwTickCount >= CTHL_FZ_MotorDelay){//方块检测异常
        bCxcAuto=1;//停轴
        SetEn(Y_AXIS, MOTOR_DISEN);
        CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning);   
      }
    }
    else
    {
       if(!CTHL_FKGY_IN&&abs(dwXRealPos)>=CTHL_XSavePosBuff1+30)
      { 
        CTHL_FZ_MotorStep++; 
        AxisDecStop(X_AXIS);
        AxisEgmStop(Y_AXIS);
        CTHL_FZ_MotorDelay = dwTickCount +1; 
        Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
      } 
      else if(dwTickCount >= CTHL_FZ_MotorDelay){//方块检测异常
        bCxcAuto=1;//停轴
        SetEn(Y_AXIS, MOTOR_DISEN);
        CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning);   
      }
    }
    break;
  case 6: //定位到位后,X轴进行正转拖紧一下带；
    if(!Y_DRV && !X_DRV&&dwTickCount >= CTHL_FZ_MotorDelay )
    {
      if(CTHL_cZipCnt <1)
      {        
        CTHL_LianLengthXFZCheckFirst =abs(dwXRealPos); 
        AxisMovePosAccDec(Y_AXIS,1500,CTHL_PARAM_FZSJ_LENGTH,1500,600,20,20,0);
      } 
      else
      {
        if(CTHL_cZipCnt <2)
        {
          AxisMovePosAccDec(Y_AXIS,1500,CTHL_PARAM_FZSJ_LENGTH,1500,600,20,20,0);
        }
        else
        {
          AxisMovePosAccDec(Y_AXIS,1500,CTHL_PARAM_FZSJ_LENGTH,1500,600,20,20,0);
        }
      }
      CTHL_FKGY_STOP_Length=abs(dwXRealPos)-CTHL_XSavePosBuff1;
      CTHL_LianLengthXFZCheck=abs(dwXRealPos);      
      CTHL_FZ_MotorStep++;      
    } 
    break;
  case 7: //定位到位后,X轴进行正转拖紧一下带；
      CTHL_FZ_MotorStep=0; 
      CTHL_PARAM_CL_TIME=dwTickCount-cuanliantime;
    break;
  }  
#endif    
}

//装拉头动作
void CTHL_ZLT_Step(void)
{
   switch(CTHL_ZhuangLiaoStep)
  {
  case 1://判断是否有拉头
    
    if(bZhuangLiaoOkFlg){//装拉头完成，则等待
      CTHL_ZhuangLiaoStep = 0;
    }
    else if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {
      zlttime=dwTickCount;
      if(!CTHL_JLTou_VAVLE&&!CTHL_JLiao_Limit_IN && CTHL_LTou_Check||CTHL_JLTou_VAVLE ){ //夹拉头、无拉头到位信号、缺拉头有信号；说明没有拉头
        
        if(CTHL_CTXM_Origin_IN && !CTHL_CTXM_VAVLE){ //判断下模是否在原位 
          CTHL_ZhuangLiaoDelay = dwTickCount + 5;
          if(CTHL_TLiaoStep == 0){//执行给拉头步骤动作
            CTHL_TLiaoStep = 1; 
          }
          CTHL_ZhuangLiaoStep++; 
        }                      
        else if(dwTickCount >= CTHL_ZhuangLiaoDelay + QDCT_VAVLE_ERROR_TIME){  
          CTHL_wltzi=1;//装拉头故障标志
          CTHL_SetAlarmCode(CTHL_CTXM_Origin_ALARM,bRunning);//警告下模原位异常     
        }  
      }
      else if(!CTHL_JLTou_VAVLE&&CTHL_JLiao_Limit_IN && !CTHL_LTou_Check){//夹拉头、有拉头到位信号、缺拉头无信号；说明拉头
        CTHL_ZhuangLiaoDelay = dwTickCount + 40;
        CTHL_MGuo_VAVLE = 1;
        CTHL_ZhuangLiaoStep = 12;
      }
      else{//由于上电已经置CTHL_JLTou_VAVLE=0，所有剩下的情况时拉头到位有信号、缺料到位有信号的情况，报警
        CTHL_wltzi=1;
        CTHL_SetAlarmCode(CTHL_JLT_MZ_ALARM,bRunning);//夹拉头模组异常
      }
    }
    break;  
  case 2://延时松开夹，和码钩
    if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {      
        CTHL_JLTou_VAVLE = 1; //夹拉头松开
        CTHL_MGuo_VAVLE = 0; //码钩松开  
        CTHL_ZhuangLiaoDelay = dwTickCount + 10;   
        CTHL_ZhuangLiaoStep++; 
    }
    break;  
  case 3: // 给拉头动作步骤完成   
    if(CTHL_TLiaoStep == 0){  //推料已经完成
      if(dwTickCount >= CTHL_ZhuangLiaoDelay){        
        if(!CTHL_JLiao_Limit_IN&&CTHL_JLTou_VAVLE)//夹拉头不在限位，
        {
          if(CTHL_XCJC_Origin_IN&&!CTHL_XCJC_VAVLE){//插销检测在原位
            CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_HSLiao_FORWARDDELAY;
            CTHL_ZhuangLiaoStep++; 
          }
          else if(dwTickCount >= CTHL_ZhuangLiaoDelay + QDCT_VAVLE_ERROR_TIME){
            CTHL_wltzi=1;
            CTHL_SetAlarmCode(CTHL_XCJC_Origin_ALARM,bRunning);//插销钩针原位异常
          }
        }
        else if(dwTickCount >= CTHL_ZhuangLiaoDelay + QDCT_VAVLE_ERROR_TIME){
          CTHL_wltzi=1;
          CTHL_SetAlarmCode(CTHL_JLT_Limit_ALARM,bRunning);//夹拉头到位异常
        }
      }
    }
    break;
  case 4://延时横送
    if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {      
        CTHL_HSLiao_VAVLE = 1;   //横送阀动作
        CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
        CTHL_ZhuangLiaoStep++; 
    }
    break; 
  case 5://横推拉头到位后延时
    if(CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_Origin_IN){//横送拉头在到位          
      CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_HSLiao_BACKDELAY;//停顿10ms，再接拉头
      CTHL_ZhuangLiaoStep ++;  
    }
    else if(dwTickCount >= CTHL_ZhuangLiaoDelay){//横送拉头超时，重新横送一次
      CTHL_ZhuangLiaoStep = 30;     
    }
    break;
  case 6://接拉头动作
    if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {      
      CTHL_JLTou_VAVLE = 0;      
      CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
      CTHL_ZhuangLiaoStep ++;
    }  
    break;
  case 7:  //判断接拉头情况  
    if(CTHL_JLiao_Limit_IN&&!CTHL_LTou_Check)//接拉头到位,且缺拉头无信号，有拉头
    {
      CTHL_ZhuangLiaoStep ++;
      CTHL_ZhuangLiaoDelay=dwTickCount+CTHL_PARAM_JLiaoCH_DELAY;//延时检测
    }
    else if(!CTHL_JLiao_Limit_IN&&CTHL_LTou_Check)//接拉头到位无信号,且缺拉头有信号，无拉头
    {
       //重新装拉头3次
      CTHL_TryCnt++;
      CTHL_JLTou_VAVLE = 1;   //接拉头关
       CTHL_HSLiao_VAVLE = 0; //横送回退
       bTuiLaTouOkFlg = 0;
      if(CTHL_TryCnt >= 3)
      {
        CTHL_TryCnt = 0;
        bZhuangLiaoOkFlg = 0;//装拉头失败
        CTHL_ZhuangLiaoStep = 0;//重新开始
        CTHL_wltzi=1;
        //user_datas[127]=4;//测试用
        CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);//装拉头异常
        
      }
      else
      {
        CTHL_ZhuangLiaoStep = 19;    //重新送3次   
        CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
      } 
    }
    else if(dwTickCount >= CTHL_ZhuangLiaoDelay+QDCT_VAVLE_ERROR_TIME)  //接拉头(接料)到们信号异常
    {
      CTHL_SetAlarmCode(CTHL_JLT_MZ_ALARM,bRunning);//接料到位异常
      CTHL_HSLiao_VAVLE = 0; //横送会回位
      CTHL_JLTou_VAVLE = 1;  //接拉头松开
      CTHL_ZhuangLiaoStep = 0;//本次装料结束
    }    
    break;
  case 8:  //二次检测
    if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {
      if(CTHL_JLiao_Limit_IN&&!CTHL_LTou_Check)//接拉头到位,且缺拉头无信号，有拉头
      {
        CTHL_ZhuangLiaoStep ++;
        CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_MaGou_DELAY;//延时码钩
      }
      else //接拉头到位无信号,且缺拉头有信号，无拉头
      {
         //重新装拉头3次
         CTHL_TryCnt++;
         CTHL_JLTou_VAVLE = 1;   //接拉头关
         CTHL_HSLiao_VAVLE = 0; //横送回退
         bTuiLaTouOkFlg = 0;
        if(CTHL_TryCnt >= 3)
        {
          CTHL_TryCnt = 0;
          bZhuangLiaoOkFlg = 0;//装拉头失败
          CTHL_ZhuangLiaoStep = 0;//重新开始
          CTHL_wltzi=1;
          //user_datas[127]=4;//测试用
          CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);//装拉头异常
          
        }
        else
        {
          CTHL_ZhuangLiaoStep = 19;    //重新送3次   
          CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
        } 
      }
    }    
    break;
  case 9: //码钩动作      
    if(dwTickCount >= CTHL_ZhuangLiaoDelay)  //延时码勾
    {
      CTHL_MGuo_VAVLE = 1; //码勾输出
      CTHL_ZhuangLiaoStep++;
      CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_HSLiao_BACKDELAY;//横送回退延时
      bTuiLaTouOkFlg = 0;   
    }
    break;
  case 10:  //横送回退
    if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {
      CTHL_HSLiao_VAVLE = 0; 
      CTHL_ZhuangLiaoStep++;
      CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
    }
    break;
  case 11: //横送原位，给拉头，并判断是否存在着拉头。
    if(!CTHL_HSLiao_Limit_IN&&CTHL_HSLiao_Origin_IN) //横送原位
    {
      CTHL_ZhuangLiaoStep++;
      CTHL_TLiaoStep = 1; //推拉头，步骤动作
      CTHL_ZhuangLiaoDelay = dwTickCount+5 ; //再次检测拉头
    }          
    else if(dwTickCount >= CTHL_ZhuangLiaoDelay)//横送原位位异常
    {
      CTHL_wltzi=1;
      CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning);
      if(bRunning)//异常后自动暂停
      {
        CTHL_ZhuangLiaoStep = 0 ;//重新开始
      }  
    }  
    break;
  case 12: //再次检测是否存在拉头
    if(dwTickCount >= CTHL_ZhuangLiaoDelay)
    {
      if(CTHL_JLiao_Limit_IN&&!CTHL_LTou_Check)//接拉头到位,且缺拉头无信号，有拉头
      {
         //bZhuangLiaoOkFlg = 1;//装拉头完成
         CTHL_ZhuangLiaoStep++;
      }
      else 
      {
         //重新装拉头3次
        CTHL_TryCnt++;
        CTHL_JLTou_VAVLE = 1;    //接拉头关
        CTHL_MGuo_VAVLE = 0;
        if(CTHL_TryCnt >= 3)
        {
          CTHL_TryCnt = 0;
          bZhuangLiaoOkFlg = 0;//装拉头失败
          CTHL_ZhuangLiaoStep = 0;//重新开始
          CTHL_wltzi=1;
          //user_datas[127]=4;//测试用
          CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);//装拉头异常
          
        }
        else
        {
          CTHL_ZhuangLiaoStep = 20;    //重新送3次   
          CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
        } 
      }
    }
    break;
  case 13: //检测是否有下模上升的条件
      if(!CTHL_TXM_VAVLE && !CTHL_TXM_Limit_IN&&!CTHL_SMZ_VAVLE&&!CTHL_SML_Limit_IN&&bXiaoMOUpFlg)//侧推在原位、上摸在原位，下模上升
      {
          CTHL_CTXM_VAVLE = 1; //下模上升 
          CTHL_XCJC_VAVLE=1;//插销检测随着下模上升
          CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME;
          CTHL_ZhuangLiaoStep++;
                                                           
      }
    break;
  case 14: //下模上升到位，插销钩针到位
    if(CTHL_CTXM_VAVLE&&CTHL_CTXM_Limit_IN)//下模上到位
    { 
       bZhuangLiaoOkFlg = 1;//装拉头完成
       bXiaoMOUpFlg=0;
       CTHL_ZhuangLiaoStep = 0;
       CTHL_PARAM_ZLT_TIME=dwTickCount-zlttime;
    }
    else if((dwTickCount >= CTHL_ZhuangLiaoDelay))
    {
      CTHL_SetAlarmCode(CTHL_CTXM_Limit_ALARM,bRunning);//横送料原位异常  
    }
    break;    
  case 19: //横送原位，给拉头，并判断是否存在着拉头。
    if(!CTHL_HSLiao_Limit_IN&&CTHL_HSLiao_Origin_IN) //横送原位
    {
      CTHL_ZhuangLiaoStep = 20;
    }          
    else if(dwTickCount >= CTHL_ZhuangLiaoDelay)//横送原位位异常
    {
      CTHL_wltzi=1;
      CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning);
      if(bRunning)CTHL_ZhuangLiaoStep = 0 ;//重新开始，结束本次装拉头
    }  
    break;    
  case 20: //重装拉头
    if(CTHL_JLTou_VAVLE&&!CTHL_JLiao_Limit_IN && !CTHL_LTou_Check )//夹拉头松开
    {
      CTHL_ZhuangLiaoStep = 1;
      CTHL_ZhuangLiaoDelay = dwTickCount + 10;
    }
    else if(dwTickCount >=CTHL_ZhuangLiaoDelay)
    {
      CTHL_wltzi=1;
      CTHL_SetAlarmCode(CTHL_JLT_MZ_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 = 5;
      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++;
    }
    break;
  case 2: //信号到位后，稳定一会给拉头
    if(CTHL_HSLiao_Origin_IN && !CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_VAVLE)//横送料在原位，横送料不在限位，横送气缸不动作
    {
      CTHL_TLiao_Delay = dwTickCount + CTHL_PARAM_TLiao_FORWARDDELAY;//信号到位后，稳定一段时间动作
      CTHL_TLiaoStep++;
    }
    else if(dwTickCount >= CTHL_TLiao_Delay) 
    {
      CTHL_wltzi=1;
      CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning); // //横送料原位异常     
    }
    break;
  case 3: //给拉头
    if(dwTickCount >= CTHL_TLiao_Delay) 
    {
      CTHL_TLiao_VAVLE = 1;    //推料(推拉头)输出
      CTHL_TLiao_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME;//气缸电磁阀不动作判断时间
      CTHL_TLiaoStep++;
    }
    break;
  case 4:  //给拉头到位后，延时一段时间
    if(CTHL_TLiao_Limit_IN)//给拉头到位
    { 
      CTHL_TLiao_Delay = dwTickCount + CTHL_PARAM_TLiao_BACKDELAY;//推拉头到位后停留时间
      CTHL_TLiaoStep++;
    }
    else if(dwTickCount >= CTHL_TLiao_Delay) 
    {
      CTHL_wltzi=1;
      CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,bRunning);  //推料到位异常
      
    }
    break;
  case 5://推料到位，推料气缸回退
    if(dwTickCount >= CTHL_TLiao_Delay)
    {
      CTHL_TLiao_VAVLE       = 0;//推料回退
      CTHL_TLiao_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME; //气缸电磁阀不动作判断时间
      CTHL_TLiaoStep++;
    }
    break;
  case  6://推料气缸回退到位
    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((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_DISEN);
    SetEn(X_AXIS, MOTOR_DISEN);
    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;
        
      }      
      
    }   
    
  }
  
  
  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_CTXM_Origin_IN ) && (!CTHL_bMG || (CTHL_bMG && CTHL_LTou_Check)))//下模在原位
      {
        if(!CTHL_TLiao_VAVLE&&!CTHL_TLiao_Limit_IN)//推拉头在原位
        {
          if(!CTHL_XCJC_VAVLE&& CTHL_XCJC_Origin_IN)//插销钩针在原点
          {
              if(CTHL_JLTou_VAVLE&&!CTHL_LTou_Check&&!CTHL_JLiao_Limit_IN)
              {
                CTHL_HSLiao_VAVLE = 1;
              }
              else
              {
                 CTHL_SetAlarmCode(CTHL_JLT_Limit_ALARM,0);//夹拉头异常
              }
                
          }
          else
          {
            
            CTHL_SetAlarmCode(CTHL_XCJC_Origin_ALARM,0);//插销钩针原位异常
          }
        }
        else
        {
          
          CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,0);
        }
        
      }
      else
      {
        CTHL_SetAlarmCode(CTHL_CTXM_Origin_ALARM,0);
      }
    }  
    if(CTHL_bJLT)//接拉头
    {      
       CTHL_bJLT = 0;
       CTHL_MGuo_VAVLE = 0;
       CTHL_JLTou_VAVLE = !CTHL_JLTou_VAVLE;

    }   
    
    
    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_bMG)                   //码勾
    {
      CTHL_bMG = 0;
      CTHL_MGuo_VAVLE = !CTHL_MGuo_VAVLE;
    } 
    if(CTHL_bFKDW)                 //方块检测
    {
      CTHL_bFKDW = 0;
      CTHL_FKDW_VAVLE = !CTHL_FKDW_VAVLE;
    }
    
    if(CTHL_bCTXM)                  //下模
    {
      CTHL_bCTXM = 0;
      if(CTHL_CTXM_VAVLE)
      {
        CTHL_CTXM_VAVLE = 0;
      }    
      else if(!CTHL_HSLiao_VAVLE && CTHL_HSLiao_Origin_IN ) //横推在原点              
      { 
         CTHL_CTXM_VAVLE=1;
      }
      else//条件警告
      {
        CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,0);//横送料原位异常
      }
    }
    //侧推布带
    if(CTHL_bTXM)
    {
      CTHL_bTXM=0;
      if(CTHL_TXM_VAVLE)
      {
        CTHL_TXM_VAVLE = 0;
      }
      else if(!CTHL_SMZ_VAVLE&&!CTHL_SML_Limit_IN)
      {
        CTHL_TXM_VAVLE = 1;
      }
      else//条件警告
      {
        CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,0);//上模原位异常
      }
    }
    if(CTHL_bSM)//上模
    {
      CTHL_bSM=0;
      if(CTHL_SMZ_VAVLE)
      {
        CTHL_SMZ_VAVLE = 0;
      }
      else if(!CTHL_TXM_VAVLE && !CTHL_TXM_Limit_IN ) //横推在原点              
      { 
         CTHL_SMZ_VAVLE=1;
      }
      else//条件警告
      {
        CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,0);//横送料原位异常
      }
    }
    
    if(CTHL_bCXDW)                   //插销钩针
    {
      CTHL_bCXDW = 0;
      CTHL_XCJC_VAVLE = !CTHL_XCJC_VAVLE;
    } 
    
    if(CTHL_bJCXBD)//插销布带夹
    {
      CTHL_bJCXBD=0;
      CTHL_JCXBD_VAVLE = !CTHL_JCXBD_VAVLE;
    }    
   if( CTHL_bCXHT)//插销回退
   {
     CTHL_bCXHT=0;
     CTHL_CXHT_VAVLE = !CTHL_CXHT_VAVLE;
    }
    if(CTHL_bCXBB)//插销摆动
    {
      CTHL_bCXBB=0;
      CTHL_XCBB_VAVLE = !CTHL_XCBB_VAVLE;
    }
    if(CTHL_bFZYL)//右拖压轮
    {
      CTHL_bFZYL=0;
      CTHL_FZYL_VAVLE = !CTHL_FZYL_VAVLE;
    }
    if(CTHL_bGLBG)//过链摆杆
    {
      CTHL_bGLBG=0;
      CTHL_HL_VAVLE=~CTHL_HL_VAVLE;
    }

    //自动装料
    if(CTHL_bAutoZhuangLiao)
    {
      CTHL_bAutoZhuangLiao = 0;
      if(CTHL_ZhuangLiaoStep == 0)
      {
        bTuiLaTouOkFlg = 1;
        bZhuangLiaoOkFlg = 0;
        bXiaoMOUpFlg=1;
        CTHL_ZhuangLiaoStep = 1;
        CTHL_MGuo_VAVLE = 0;  
        CTHL_ZhuangLiaoDelay = dwTickCount + 150;
      }  
    }
     //一键定位
    if(CTHL_bYJDW)
    {
      CTHL_bYJDW=0;
      CTHL_FKDW_VAVLE = 0;//方块定位气缸
      CTHL_TXM_VAVLE=0;   //侧推布带气缸
      CTHL_SMZ_VAVLE=0;   //上模气缸
      CTHL_XCBB_VAVLE=0;    //插销摆臂
      CTHL_JCXBD_VAVLE=0;   //夹插销布带
      CTHL_CXHT_VAVLE=0;   //插销后退
      CTHL_FZYL_VAVLE=0;   //右拖带压轮
      CTHL_TD_MotorDelay = dwTickCount + 100;
      if(CTHL_TD_MotorStep==0){ 
        if(GetEn(X_AXIS) == MOTOR_DISEN){//未使能
          SetEn(X_AXIS, MOTOR_EN);//X使能   
          CTHL_TD_MotorDelay = dwTickCount + 200;      
        } 
        else{
          CTHL_TD_MotorDelay = dwTickCount + 100;
        }
        CTHL_TD_MotorStep=1;
      }
    } 
    //X轴Y轴电机测试 
    if(CTHL_bCLMotor_N)
    { 
      
      SetEn(Y_AXIS, MOTOR_EN);//使能X轴
      SetEn(X_AXIS, MOTOR_EN);//使能Y轴

   
      if(CTHL_FK_Check_UP){//检测到过链感应信号后，设置原位。
        SetPos(X_AXIS, 0); 
        SetPos(Y_AXIS, 0); 
      }      
      if(!X_DRV){
        // Y轴— 运行速度—方向—启动速度—停止速度— 加速时间— 减速时间  
        //AxisMovePosAccDec(X_AXIS,3000,2953,600,600,100,100,900);
        //AxisMovePosAccDec(Y_AXIS,3000,2953,600,600,100,100,900);
        AxisContinueMoveAcc(X_AXIS,600 ,DIR_P,600,600,15,15);
        if(servo_y.IO_TO_COM==0)AxisContinueMoveAcc(Y_AXIS,580,DIR_P,580,580,15,15);      
      }      
    }

    if(CTHL_bCLMotor_P)  
    {
      SetEn(Y_AXIS, MOTOR_EN);//使能X轴
      SetEn(X_AXIS, MOTOR_EN);//使能Y轴
   
      if(CTHL_FK_Check_UP){//检测到过链感应信号后，设置原位。
        SetPos(X_AXIS, 0); 
        SetPos(Y_AXIS, 0); 
        CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,0);//方块定位有信号
      }
      if(!X_DRV ){
         // Y轴— 运行速度—方向—启动速度—停止速度— 加速时间— 减速时间 速度
        AxisContinueMoveAcc(X_AXIS,600,DIR_N,600,600,15,15);
        if(servo_y.IO_TO_COM==0)AxisContinueMoveAcc(Y_AXIS,630,DIR_N,630,630,15,15);
      }

    }    
    if(!CTHL_bCLMotor_P && !CTHL_bCLMotor_N && (CTHL_TD_MotorStep == 0))
    {
      AxisEgmStop(X_AXIS);
      //if(servo_y.IO_TO_COM==0)AxisEgmStop(Y_AXIS);else  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;
  //传递伺服齿轮比参数
#if  X_USERING_TARR==1
  set_com_servo_param(X_AXIS,CTHL_PARAM_TDCYCLE_PULSE,CTHL_PARAM_TDCYCLE_LENGTH);
#endif
#if  Y_USERING_TARR==1
  set_com_servo_param(Y_AXIS,CTHL_PARAM_TDCYCLE_PULSE,CTHL_PARAM_FZCYCLE_LENGTH);
#endif
 
  SetAlarmCode(CTHL_ALARM_ADDR,0); 
  
  
  SetEnReverse(X_AXIS, 0);
  SetEnReverse(Y_AXIS, 0);
  
  SetDirReverse(X_AXIS, 0);
  SetDirReverse(Y_AXIS, 0);
  //设置伺服告警极性,
  //伺服设置常开，则极性设置为0，  0为正常，1为告警，；
  //伺服设置为常闭，则极性设置为1，0，为正常，1位告警
  SetAlarmReverse(X_AXIS,0);
  SetAlarmReverse(Y_AXIS,0);
  
  SetEn(X_AXIS, MOTOR_DISEN);
  SetEn(Y_AXIS, MOTOR_DISEN);
  
  SetPos(X_AXIS, 0);  //启动位置设为0点    
  SetPos(Y_AXIS, 0);  //启动位置设为0点 

}
//运行
void CTHL_Action(void)
{ 
  dwXRealPos = GetPos(X_AXIS);
  dwYRealPos = GetPos(Y_AXIS);
  dwZRealPos = GetPos(Z_AXIS); 
  dwYRealPos_com=get_encode_value(Y_AXIS);
  
  CTHL_ZhenDongAction();
  CTHL_AlarmProtect();
  CTHL_CheckStart();    
  CTHL_ZLT_Step();//装拉头
  CTHL_TLiao_Step();//推拉头
  CTHL_TD_Motor();//拖带电机X轴
  CTHL_FZ_Motor();//穿拉头电机Y轴
  CTHL_ManualAction();
  CTHL_AutoStepAction();  
  CTHL_Test++;
  servo_com_run();
  CTHL_Running_state();
}
#endif