GC19038/Machine/YuWenXuanMachine/YWXChuantou_HeLian.c

#include "global.h"

#if YU_WEN_XUAN_MACHINE  ==  1

void YWX_CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag);
void YWX_CTHL_ManualAction(void);
void YWX_CTHL_AutoStepAction(void);
void YWX_CTHL_TableAction(void);
void YWX_CTHL_TD_Motor(void);
void YWX_CTHL_FZ_Motor(void);
void YWX_CTHL_CheckStart(void);
void YWX_CTHL_TLiao_Step(void);  //推料(推拉头)
void YWX_CTHL_AlarmProtect(void);
void YWX_CTHL_ZLT_Step(void);
void YWX_CTHL_ZhenDongAction(void);
void YWX_CTHL_bFKCXTDDWStep(void);
void YWX_CTHL_bCXCRStep(void);
void YWX_CTHL_bCLFirstStep(void);


void ExtiAcitionX02_Old(void)
{
    if(bCheckEN_FLG)
    {  
        bCheckEN_FLG = 0;
        YWX_CTHL_CheckInX02PosBuff = GetPos(X_AXIS);
    }    
}

//故障报警
void YWX_CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag)
{
    SetAlarmCode(YWX_CTHL_ALARM_ADDR,alarm_code);
  //  bAlarmStop = 0;
   // if(cStopFlag)
    if(!cWULATOUCnt)
    {
      bAlarmStop = 1;
    }
    else
    {
      cWULATOUCnt=0;
    }        
}

//初始化动作
void YWX_CTHL_InitAction(void)
{
    float buff_pulse,buff_dist;
    
    buff_pulse  = YWX_CTHL_PARAM_TDCYCLE_PULSE;
    buff_dist   = YWX_CTHL_PARAM_TDCYCLE_LENGTH; 
    
    XGearRatio  =  buff_pulse/buff_dist; 
    
    buff_pulse  = YWX_CTHL_PARAM_FZCYCLE_PULSE;
    buff_dist   = YWX_CTHL_PARAM_FZCYCLE_LENGTH;
    YGearRatio  =  buff_pulse/buff_dist;
    
   
    

    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, 1);
    SetDirReverse(Y_AXIS, 0);
    SetDirReverse(Z_AXIS, 0);
    
    SetEn(X_AXIS, MOTOR_DISEN);
    SetEn(Y_AXIS, MOTOR_DISEN);
    SetEn(Z_AXIS, MOTOR_DISEN);
    SetPos(X_AXIS, 0);  //启动位置设为0点    
}

void YWX_CTHL_Action(void)
{ 
//    user_datas[121] = YWX_CTHL_LianLengthCheck;
 //   user_datas[122] =axis_x->cur_speed;
 //  user_datas[123] = YWX_CTHL_bCXCR_Step;
    dwXRealPos = GetPos(X_AXIS);
    dwYRealPos = GetPos(Y_AXIS);
    dwZRealPos = GetPos(Z_AXIS);  
    user_datas[121] = dwXRealPos;
    user_datas[122] = dwYRealPos;
    
 //   user_datas[126] = YWX_CTHL_TD_MotorSte;//YWX_CTHL_FZ_MotorStep;
    user_datas[127] = YWX_CTHL_LianLengthCheck;
    user_datas[128] = YWX_CTHL_AutoStep;  
    
    YWX_CTHL_ZhenDongAction();
    YWX_CTHL_AlarmProtect();
    YWX_CTHL_CheckStart();    
    YWX_CTHL_ZLT_Step();
    YWX_CTHL_TLiao_Step();

    YWX_CTHL_TD_Motor();
    YWX_CTHL_FZ_Motor();
    YWX_CTHL_bFKCXTDDWStep();
    YWX_CTHL_bCXCRStep();
    YWX_CTHL_ManualAction();
    YWX_CTHL_AutoStepAction();  
}

void YWX_CTHL_AlarmProtect(void)
{ 
    

 
}



//自动动作
void YWX_CTHL_AutoStepAction(void)
{  

    //测试用
    if(dwTickCount >= YWX_CTHL_TestDelay)
    {
        YWX_CTHL_TestDelay = dwTickCount + 150;
        YWX_CTHL_TestLED = !YWX_CTHL_TestLED;        
    }

    if(bRunning)
    {
        switch(YWX_CTHL_AutoStep)
        {
            case 1:
        /* 
                YWX_CTHL_AutoStep = 10;       //测试拖带定位
               YWX_CTHL_AutoDelay = dwTickCount + 1000;
               break; 
        */
                if(dwTickCount >= YWX_CTHL_AutoDelay)
                {
                    YWX_CTHL_FKGD_VAVLE = 0;     //方块固定关
                    YWX_CTHL_YADAI_VAVLE = 1;     //过链压带
                    YWX_CTHL_LYLun_VAVLE = 0;     //Y15方块压轮
                    YWX_CTHL_RYLun_VAVLE = 0;     //Y16插销压轮
                    YWX_CTHL_DXK_VAVLE  = 0;       //Y01顶斜口电磁阀
                    YWX_CTHL_XKDW_VAVLE = 0;       //Y02斜口定位(定位上止)
                    YWX_CTHL_FWCX_VAVLE = 0;       //Y03复位插销
                    YWX_CTHL_BAIBI_VAVLE = 0;      //Y04摆臂
                    YWX_CTHL_AutoStep = 2;
                    YWX_CTHL_AutoDelay = dwTickCount + 120;
                    if(YWX_CTHL_ZhuangLiaoStep == 0)
                    { 
                        YWX_CTHL_ZhuangLiaoStep = 1;
                        YWX_CTHL_TryCnt = 0;
                    }   
                }
                break;
            case 2: 
                if(dwTickCount >= YWX_CTHL_AutoDelay)  //压带后才开始拖带
                {
                    YWX_CTHL_AutoStep = 3;
                    YWX_CTHL_AutoDelay = dwTickCount + 0;
                    
                }  
                break;
                
            case 3://斜口定位
                if(dwTickCount >= YWX_CTHL_AutoDelay)
                {                   
                    YWX_CTHL_AutoStep = 4;
                    YWX_CTHL_AutoDelay = dwTickCount + 0;//YWX_CTHL_PARAM_XKDW_TD_DELAY;
                }  
                break;
                
            case 4:
                if(dwTickCount >= YWX_CTHL_AutoDelay)
                {
                  
                                  //调试模式
                  if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                  {
                        YWX_CTHL_bTiaoShiStep = 0;
                    if(YWX_CTHL_cZipCnt == 0)
                    {
                        YWX_CTHL_TD_MotorStep = 1;          //合链定位    只感应一次      
                        YWX_CTHL_AutoStep = 5;
                    }
                    else
                   {
                        YWX_CTHL_TD_MotorStep = 10;       //数控穿入, 感应两次定位            
                        YWX_CTHL_AutoStep = 5;
                    } 
                  }
                }
                break;

            case 5:
                if(YWX_CTHL_TD_MotorStep == 0)   //方块定位完成，下模上去准备斜口拉头穿入
                {
                    //装料已经完成
                    if((YWX_CTHL_ZhuangLiaoStep == 0) && bZhuangLiaoOkFlg)
                    {   
                                      //调试模式
                  if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                  {
                        YWX_CTHL_bTiaoShiStep = 0;
                        YWX_CTHL_AutoStep = 6;
                        YWX_CTHL_CTXM_VAVLE = 1;    //下模合上
                        YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_VAVLE_ERROR_TIME;
                        YWX_CTHL_LYLun_VAVLE = 1;     //Y15方块压轮
                        YWX_CTHL_RYLun_VAVLE = 1;     //Y16插销压轮  
                        }
                    }
                    
                            
                } 
                break;
               
            case 6:
                if(!YWX_CTHL_CTM_Origin_IN)
                    YWX_CTHL_JLTou_VAVLE = 0;      //Y11接拉头关               
                if(YWX_CTHL_CTM_Limit_IN && !YWX_CTHL_CTM_Origin_IN)   //穿头模上升到位
                {
                 //调试模式
                  if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                  {
                        YWX_CTHL_bTiaoShiStep = 0;
                            YWX_CTHL_AutoStep = 7;
                            YWX_CTHL_XKDW_VAVLE = 1;    //Y02斜口定位(定位上止)
                            YWX_CTHL_YADAI_VAVLE = 0;     //过链压带
                            YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_VAVLE_ERROR_TIME;
                        }
                    
                }  
                else if(dwTickCount >= YWX_CTHL_AutoDelay)
                {
                    YWX_CTHL_SetAlarmCode(YWX_CTHL_CTM_Limit_ALARM,bRunning);  //穿头下模上升异常
                }
                break;
            case 7:
                if(YWX_CTHL_FKSC_DW_IN)      //Y02斜口定位(定位上止)
                {
                    YWX_CTHL_AutoStep = 8;
                    YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_PARAM_FKSC_DW_DELAY;//方块上止定位输出后电机延时反转定位
                }
                else if(dwTickCount >= YWX_CTHL_AutoDelay)
                {
                    YWX_CTHL_SetAlarmCode(YWX_CTHL_XKTD_Origin_ALARM,bRunning); //斜口推带回到异常
                }
                break;
                 
            
            case 8:  
                if(dwTickCount >= YWX_CTHL_AutoDelay)  //延时电机延时反转定位(也等压轮压到位)
                {                  
                //调试模式
                  if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                  {
                        YWX_CTHL_bTiaoShiStep = 0;
                    YWX_CTHL_AutoStep = 9;
                    if(YWX_CTHL_PARAM_FKSC_DW_DELAY)
                    {
                        AxisMovePosAccDec(Y_AXIS,2600,YWX_CTHL_PARAM_FKSC_DW_DELAY,800,500,6,6,0); 
                        if(YWX_CTHL_PARAM_FKSC_DW_DELAY > 10)
                            AxisMovePosAccDec(X_AXIS,2600,-(YWX_CTHL_PARAM_FKSC_DW_DELAY-10),800,500,6,6,0); 
                    }
                  }      
                }
                break;         
            case 9: 
                if(!Y_DRV && !X_DRV)  //方块上止定位完成
                {              
                    YWX_CTHL_AutoStep = 10;    
                    YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_PARAM_DXK_DELAY;
                }              
                break;                 
            case 10:                   
                if((dwTickCount >= YWX_CTHL_AutoDelay) && !X_DRV)
                {
                 //调试模式
                  if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                  {
                        YWX_CTHL_bTiaoShiStep = 0;
                    YWX_CTHL_DXK_VAVLE = 1; //Y01顶斜口电磁阀         
                    YWX_CTHL_AutoStep = 11;  
                    YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_VAVLE_ERROR_TIME;
                    }
                }
                break;
            case 11:    
                if(YWX_CTHL_XKTDW)
                {
                    YWX_CTHL_AutoStep = 12;  
                    YWX_CTHL_XKDW_VAVLE = 0;
                    YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_VAVLE_ERROR_TIME;
                   
                }
                else if(dwTickCount >= YWX_CTHL_AutoDelay)  //顶斜口到位异常
                {
                
                }   
                break;
            case 12:
                if(!YWX_CTHL_FKSC_DW_IN)
                {
                   YWX_CTHL_AutoStep = 13;
                   YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_PARAM_XK_CR_DELAY;//顶斜口后延时穿入
                }
                else if(dwTickCount >= YWX_CTHL_AutoDelay)  //退方块上止离开异常
                {
                
                }   
                break;
            case 13:  
                if(dwTickCount >= YWX_CTHL_AutoDelay)
                {  
                 //调试模式
                  if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                  {
                        YWX_CTHL_bTiaoShiStep = 0;
                    YWX_CTHL_AutoStep = 14;  
      //              if(YWX_CTHL_bFZ_LengthOK)
      //                  YWX_CTHL_FZ_MotorStep = 10;
      //              else
                        YWX_CTHL_FZ_MotorStep = 1;
                        }
                }   
                break;
               
            case 14:
              
                if(YWX_CTHL_FZ_MotorStep == 0)  //穿入定位完成
                {      
             /*   //测试拖带定位  
                    YWX_CTHL_AutoStep = 1;       
                    YWX_CTHL_AutoDelay = dwTickCount + 1000;
                     if(YWX_CTHL_cZipCnt<3)
                        YWX_CTHL_cZipCnt++;
                    break;  */
                         YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_PARAM_GDFK_DELAY;
                        YWX_CTHL_AutoStep = 15; 
                }
                break;
        case 15:
            if(dwTickCount >= YWX_CTHL_AutoDelay) 
            {
                
                
                 //调试模式
                  if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                  {
                        YWX_CTHL_bTiaoShiStep = 0;
                   
                   YWX_CTHL_FKGD_VAVLE = 1;     //Y00方块固定 
                 
                 
                 YWX_CTHL_AutoStep = 16; 
 //                 AxisMovePosAccDec(X_AXIS,2000,-(YWX_CTHL_PARAM_CXDW_Length + 250),1000,500,6,6,50);   
                         
                        YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_VAVLE_ERROR_TIME;
                  }
                }  
                break;
        case 16:
          if(YWX_CTHL_FKGD_DW_IN)
          {
                 //调试模式
                  if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                  {
                        YWX_CTHL_bTiaoShiStep = 0;
                YWX_CTHL_AutoStep = 17; 
                YWX_CTHL_LYLun_VAVLE = 0;    //Y15方块压轮
             YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_PARAM_DWCX_DELAY;
                  }
          }
          else if(dwTickCount >= YWX_CTHL_AutoDelay) //Y00方块固定到位异常
          {
          
          }
          break;
        case 17:
          if(dwTickCount >= YWX_CTHL_AutoDelay)   //延时定位插销
          {
            YWX_CTHL_AutoStep = 18; 
            YWX_CTHL_FZ_MotorStep = 20;   //定位插销启动
            
          }
          break;
        case 18:
          if(YWX_CTHL_FZ_MotorStep == 0)
          {
                YWX_CTHL_YADAI_VAVLE = 1;       //Y06压带
                YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_PARAM_BAIBI_DELAY;//延时摆臂
                //调试模式
                if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                {
                    YWX_CTHL_bTiaoShiStep = 0;
                    YWX_CTHL_AutoStep = 19;     
                }
            
          }
          break;
          case 19:
          if(dwTickCount >= YWX_CTHL_AutoDelay) 
          {
                YWX_CTHL_AutoStep = 20; 
                YWX_CTHL_BAIBI_VAVLE = 1;   //摆臂  
                YWX_CTHL_RYLun_VAVLE = 0;   //Y16插销压轮
                YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_PARAM_CXCR_DELAY;  //延时穿插销
          }
          break;
        case 20:
          if(dwTickCount >= YWX_CTHL_AutoDelay)   
          {
                //调试模式
                  if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                  {
                        YWX_CTHL_bTiaoShiStep = 0;
                    YWX_CTHL_AutoStep = 21; 
                        AxisMovePosAccDec(X_AXIS,3000,(YWX_CTHL_PARAM_XXCR_LENTH),1000,500,6,6,50); 
                }
//                YWX_CTHL_XSavePosBuff = dwXRealPos + YWX_CTHL_PARAM_CXDW_Length + 120;
          }
          break;
        case 21:
//            if((dwXRealPos+ 150)>= YWX_CTHL_XSavePosBuff) 
            {
                
            }
            if(!X_DRV)// && YWX_CTHL_FWCX_VAVLE)
            {
                //调试模式
                  if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                  {
                        YWX_CTHL_bTiaoShiStep = 0;
                    YWX_CTHL_AutoStep = 22; 
                    YWX_CTHL_FWCX_VAVLE = 1;
                    YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_PARAM_CXCR_O_Time;  //延时退方块固定
                     
                  }     
            }
            break;
        case 22:
            if(dwTickCount >= YWX_CTHL_AutoDelay)
            {    
                  //调试模式
                      if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                      {
                            YWX_CTHL_bTiaoShiStep = 0;
                            YWX_CTHL_AutoStep = 23; 
                            YWX_CTHL_FKGD_VAVLE = 0; //Y00方块固定 
                            YWX_CTHL_AutoDelay = dwTickCount + 15;
                      }
            }  
            if(YWX_CTHL_XCFW_DW_IN)
                 {  //插销穿入异常
                    YWX_CTHL_AutoStep = 0;
                    bRunning = 0;
                    bSingOneFlg = 0;
                    YWX_CTHL_SetAlarmCode(YWX_CTHL_XCFW_DW_ALARM,bRunning); 
                 }
            break;
        case 23:
            if(!YWX_CTHL_FKGD_DW_IN)
            {
              //调试模式
                if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                {
                    
                    if(dwTickCount >= YWX_CTHL_AutoDelay)
                    {
                        YWX_CTHL_bTiaoShiStep = 0;
                        YWX_CTHL_TD_MotorStep = 10;  //
                        YWX_CTHL_AutoStep = 24; 
                    }
                }
            }
            else
            {
                YWX_CTHL_AutoDelay = dwTickCount + 5;
                if(YWX_CTHL_XCFW_DW_IN)
                 {  //插销穿入异常
                    YWX_CTHL_AutoStep = 0;
                    bRunning = 0;
                    bSingOneFlg = 0;
                    YWX_CTHL_SetAlarmCode(YWX_CTHL_XCFW_DW_ALARM,bRunning); 
                 }
            }  
            
            break;
        case 24:
            if(dwXRealPos > 30)    //穿入2CM后退
            {
                YWX_CTHL_AutoStep = 25; 
                YWX_CTHL_MGuo_VAVLE = 0;     //码勾退
                
                YWX_CTHL_BAIBI_VAVLE = 0;
                YWX_CTHL_AutoDelay = dwTickCount + 8;  //延时穿插销
                
                bZhuangLiaoOkFlg = 0;
                
            }
            break;
        case 25:    
            if(dwTickCount >= YWX_CTHL_AutoDelay)
            {
                YWX_CTHL_AutoStep = 26; 
                YWX_CTHL_CTXM_VAVLE = 0;    //下模打开
                //退下出错警告
                YWX_CTHL_AutoDelay1 = dwTickCount + YWX_CTHL_VAVLE_ERROR_TIME;
                YWX_CTHL_AutoStep1 = 1;
            }
            break;
        case 26:
            if((YWX_CTHL_TD_MotorStep == 0) && (!YWX_CTHL_CTXM_VAVLE ))
            {
                YWX_CTHL_AutoStep = 5; 
                AddToTal(YWX_CTHL_TOTAL_ADDR);                    
                CalProSP(YWX_CTHL_SPEED_ADDR);
                if(YWX_CTHL_cZipCnt<3)
                   YWX_CTHL_cZipCnt++;
                if(bSingOneFlg)
                {
                    YWX_CTHL_AutoStep = 0;
                    bRunning = 0;
                    bSingOneFlg = 0;
                } 
            }
            break;
 
        }  
         
    }       
        //退下模后启动送拉头
        switch(YWX_CTHL_AutoStep1)
        {   
            case 1:
  /*            
              if(cStopMode==1)
              {
                cStopMode=2;
                 bStop=1;
              }
*/
                if(YWX_CTHL_CTM_Origin_IN)
                {
                    YWX_CTHL_ZhuangLiaoStep = 1;
                    YWX_CTHL_TryCnt = 0;
                    YWX_CTHL_AutoStep1 = 0;
                    YWX_CTHL_MGuo_VAVLE = 0;
                    YWX_CTHL_JLTou_VAVLE = 0;
                }
                else if(dwTickCount >= YWX_CTHL_AutoDelay1) 
                {
                  //穿头模和一次穿入回位异常
                    if(!YWX_CTHL_CTM_Origin_IN)
                      YWX_CTHL_SetAlarmCode(YWX_CTHL_CTM_Origin_ALARM,bRunning);                                   
                } 
                break;
                
        }  
        
}





void YWX_CTHL_CheckStart(void)
{
  
#if 1  
       //启动    自动启动 单一自动	 			
    if(YWX_CTHL_START_IN_UP || bStart )
    {       
        if(!bRunning)
        {
//            if(YWX_CTHL_SafeDoor_IN)
            {  
                bSingOneFlg= 0;
                cStopMode=0;
                bRunning = 1;
                cWULATOUCnt=0;
                SetDirReverse(X_AXIS, 1);
                SetEn(X_AXIS, MOTOR_EN);
                SetEn(Y_AXIS, MOTOR_EN);
                YWX_CTHL_FZ_MotorStep = 40;
                YWX_CTHL_AutoStep = 1;
                bTuiLaTouOkFlg = 1;
                bZhuangLiaoOkFlg= 0;
                YWX_CTHL_MGuo_VAVLE = 1;   //判断是否已经有料
                YWX_CTHL_ZhuangLiaoDelay = dwTickCount + 150;
                bCL_EN_FLG = 0;
                YWX_CTHL_cZipCnt = 0;
                YWX_CTHL_LianLengthCheck = 0;
                cStopInCnt = 0;
                YWX_CTHL_bFZ_LengthOK = 0;
                
             }         
        }  
        bStart = 0;
    }   
    
    //停止
    if(YWX_CTHL_STOP_IN_UP || bStop)
    {	

        if(cStopMode < 3) 
            cStopMode++;
        if(bRunning && !bSingOneFlg)// && cStopMode==2)
        {   
            
            if((cStopMode >1) || YWX_CTHL_STOP_IN_UP)
            {
                bRunning=0;
                cStopMode=0;
                cXcxcrCnt=0;
                cWULATOUCnt=0;
                AxisEgmStop(X_AXIS);
                AxisEgmStop(Y_AXIS);
                YWX_CTHL_AutoStep = 0; 
                YWX_CTHL_FZ_MotorStep =0;      
                //YWX_CTHL_ZhuangLiaoStep=0;      
                YWX_CTHL_XKDW_VAVLE =0;        
                SetAlarmCode(YWX_CTHL_ALARM_ADDR,0);  
            }
            else
                bSingOneFlg= 1;
        }
        else
        {
            bRunning=0;
            YWX_CTHL_RunStepSave = 0;
            cXcxcrCnt=0;
            cWULATOUCnt=0;
            AxisEgmStop(X_AXIS);
            AxisEgmStop(Y_AXIS);
            SetEn(X_AXIS, MOTOR_DISEN);
            SetEn(Y_AXIS, MOTOR_DISEN);
//            if(cStopMode >2)
            YWX_CTHL_TLiao_VAVLE=0;             
            YWX_CTHL_HSLiao_VAVLE  =0;    
            YWX_CTHL_JLTou_VAVLE  =0;           
            YWX_CTHL_XKDW_VAVLE  =0;       
            YWX_CTHL_DXK_VAVLE  =0;                
            YWX_CTHL_CTXM_VAVLE =0;           
            YWX_CTHL_FKGD_VAVLE =0;         
            YWX_CTHL_YADAI_VAVLE = 0;
            YWX_CTHL_FWCX_VAVLE = 0;
            YWX_CTHL_BAIBI_VAVLE = 0;   //摆臂  
            YWX_CTHL_LYLun_VAVLE = 0;   //
            YWX_CTHL_RYLun_VAVLE = 0;   //Y16插销压轮
            YWX_CTHL_HELT_VAVLE = 0;
        
                    

            YWX_CTHL_MGuo_VAVLE =0;
               
            YWX_CTHL_AutoStep = 0;
            YWX_CTHL_ZhuangLiaoStep=0;
            YWX_CTHL_TLiaoStep=0;
            YWX_CTHL_TD_MotorStep=0;
  
     
            YWX_CTHL_SongLiaoStep   =0;     
            YWX_CTHL_FZ_MotorStep =0;      
            YWX_CTHL_AutoStep1  =0;
            YWX_CTHL_bFKCXTDDW_Step=0;
            YWX_CTHL_bCXCR_Step=0;
            YWX_CTHL_bCLFirst_Step=0;        
            SetAlarmCode(YWX_CTHL_ALARM_ADDR,0);  
            SetPos(X_AXIS, 0); 

            YWX_CTHL_TLiaoStep = 0; 
            YWX_CTHL_AutoStep = 0;
        }
        bStop=0;
    }
     
   if(bAlarmStop)
    {   
      bRunning   = 0;
      bAlarmStop=0;
      cStopMode=0;
      cXcxcrCnt=0;
      AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);
      YWX_CTHL_AutoStep = 0; 
      YWX_CTHL_FZ_MotorStep =0;      
      YWX_CTHL_SongLiaoStep =0;
      YWX_CTHL_ZhuangLiaoStep=0;
     // YWX_CTHL_XKTD_VAVLE = 0; 
     // YWX_CTHL_XKDW_VAVLE =0;
      
 
    }
#endif    
}   

//拖带电机控制动作  X轴
void YWX_CTHL_TD_Motor(void)	 //
{   
#if 1
     switch(YWX_CTHL_TD_MotorStep)
	{
        //1步开始，纯定位, 没有穿入
        case 1:
            if(dwTickCount >= YWX_CTHL_TD_MotorDelay)  //给锁轴时间
            {
                bCheckEN_FLG = 1;
                YWX_CTHL_CheckInX02PosBuff= 0;
                 YWX_CTHL_TD_MotorStep = 2;   
                 SetPos(X_AXIS, 0);  //启动位置设为0点
                 AxisContinueMoveAcc(X_AXIS,6000,DIR_P,1500,600,15,15);
            }  
            break;
        case 2://检测到过链
            if(dwXRealPos > 300)    //穿入2CM后退
            {
                YWX_CTHL_FWCX_VAVLE = 0;   //复位插销              
            } 
            
//            if(dwXRealPos >= YWX_CTHL_PARAM_WLCDSHEZHI_LENTH)
            {
//              YWX_CTHL_SetAlarmCode(YWX_CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机              
            }
            
            if(YWX_CTHL_FK_Check)
            {
                YWX_CTHL_FWCX_VAVLE = 0;   //复位插销  
                YWX_CTHL_XSavePosBuff = dwXRealPos;
                YWX_CTHL_TD_MotorStep = 3;  
                
                
                AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_FKDW_Speed ,400,
                                                 3000,YWX_CTHL_PARAM_FKDW_Speed,10,10,0);
 /*               
                //说明有中断
                if((dwXRealPos >= YWX_CTHL_CheckInX02PosBuff) && YWX_CTHL_CheckInX02PosBuff)
                    AxisMovePosAccDec(X_AXIS,YWX_CTHL_PARAM_TD_RunSpeed,YWX_CTHL_PARAM_FKDW_Length - (dwXRealPos- YWX_CTHL_CheckInX02PosBuff),
                                  YWX_CTHL_PARAM_FKDW_Speed,YWX_CTHL_PARAM_FKDW_Speed,10,30,20); 
                else
                    AxisMovePosAccDec(X_AXIS,YWX_CTHL_PARAM_TD_RunSpeed,YWX_CTHL_PARAM_FKDW_Length,
                                  YWX_CTHL_PARAM_FKDW_Speed,YWX_CTHL_PARAM_FKDW_Speed,10,30,20); 
*/                
                
            }
            break;
        case 3:               
             if(YWX_CTHL_FKDW_IN )
            {
              
                AxisMovePosAccDec(X_AXIS,GetCurSpeed(X_AXIS),YWX_CTHL_PARAM_FKDW_Length,
                                  YWX_CTHL_PARAM_FKDW_Speed,YWX_CTHL_PARAM_FKDW_Speed,10,30,10); 
               YWX_CTHL_TD_MotorStep = 4;  
               YWX_CTHL_XSavePosBuff = dwXRealPos + YWX_CTHL_PARAM_FKDW_Length;
            }
             break;
        case 4:
            if((!X_DRV) || (dwXRealPos >= YWX_CTHL_XSavePosBuff))
            {
                YWX_CTHL_TD_MotorStep = 0;  
                AxisEgmStop(X_AXIS);
            }
            break;
            //穿拉头定位
        case 10:   
            if(dwTickCount >= YWX_CTHL_TD_MotorDelay)
            {
                YWX_CTHL_TD_MotorStep = 11;
                SetPos(X_AXIS, 0);  //启动位置设为0点
                dwXRealPos = 0;
                AxisContinueMoveAcc(X_AXIS,4000,DIR_P,2000,1200,5,5);   
                YWX_CTHL_CheckInX02PosBuff= 0;
                bCheckEN_FLG = 1;
                bHeLaTouOK = 1;
                YWX_CTHL_XSavePosBuff = dwXRealPos;
                YWX_CTHL_HELT_VAVLE = 1;
                 YWX_CTHL_FWCX_VAVLE = 0;   //复位插销  
            }
            break;
        case 11:   
            if((dwXRealPos > 150) && !YWX_CTHL_CTM_Limit_IN && !YWX_CTHL_CTXM_VAVLE)    //穿入2CM后退
            {
                YWX_CTHL_CheckInX02PosBuff= 0;
                YWX_CTHL_FWCX_VAVLE = 0;   //复位插销  
                YWX_CTHL_TD_MotorStep = 12;
                cFKCheckCnt = 0;
                YWX_CTHL_bFKCheck_Time = dwTickCount;
/*                
                if((YWX_CTHL_cZipCnt < 2))// || (YWX_CTHL_LianLengthCheck < 350)) 
                {
                    bHeLaTouOK = 1;
                    AxisContinueMoveAcc(X_AXIS,YWX_CTHL_PARAM_TD_RunSpeed,DIR_P,800,600,10,10); 
                }    
                else ////两段速度移动距离      
                { 
*/  
                bHeLaTouOK = 1;
   //                 if(YWX_CTHL_LianLengthCheck > 1700)  //太短直接下模合链
   //                 {   
                        //NotStop
   //                     AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_TD_RunSpeed*3/2 ,YWX_CTHL_LianLengthCheck * 2- YWX_CTHL_PARAM_FKDW_Length,
   //                                              3000,2000,10,10,0); //YWX_CTHL_PARAM_TD_RunSpeed
   //                 }
   //                 else
                      AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_TD_RunSpeed*9/5,YWX_CTHL_LianLengthCheck +150 - dwXRealPos,
                                                 3000,YWX_CTHL_PARAM_TD_RunSpeed,25,20,0); //YWX_CTHL_PARAM_TD_RunSpeed
//                }   
            }
            break;
        case 12://检测到过链
          
 //         
           
            if(YWX_CTHL_cZipCnt)
            { 
                if(((dwXRealPos + YWX_CTHL_PARAM_CL_Length) >= (YWX_CTHL_LianLengthCheck + 500)) && YWX_CTHL_LianLengthCheck)
                {
                    YWX_CTHL_HELT_VAVLE = 0;                    
                }               
            }   
            
            
            if((YWX_CTHL_FKDW_IN)) //
            {
                YWX_CTHL_TD_MotorStep = 13; 
                if(YWX_CTHL_cZipCnt == 0)
                    YWX_CTHL_HELT_VAVLE = 0;
                YWX_CTHL_XSavePosBuff = dwXRealPos;
                YWX_CTHL_bFKCheck_Time = dwTickCount + 5;
                YWX_CTHL_XSavePosBuff1 = 0;
                
            }
            if((dwXRealPos>= (4* YWX_CTHL_PARAM_WLCDSHEZHI_LENTH)) && YWX_CTHL_PARAM_WLCDSHEZHI_LENTH)
            {
                //没链自动
                bRunning = 0;
                bAlarmStop = 1;
                YWX_CTHL_TD_MotorStep= 0;
                //
                SetAlarmCode(YWX_CTHL_ALARM_ADDR,YWX_CTHL_WLZDTJ_ALARM); 
            }
            break;
        case 13:          
           if(YWX_CTHL_cZipCnt)
            { 
                  if(((dwXRealPos + YWX_CTHL_PARAM_CL_Length) >= (YWX_CTHL_LianLengthCheck + 500)) && YWX_CTHL_LianLengthCheck)
                {
                    YWX_CTHL_HELT_VAVLE = 0;                    
                }
            }   
            if((!YWX_CTHL_FKDW_IN) && (dwXRealPos >= (YWX_CTHL_XSavePosBuff + 500)))
            {
                YWX_CTHL_TD_MotorStep = 14;
            } 
            break;
    case 14:
            if(YWX_CTHL_cZipCnt)
            { 
                if(((dwXRealPos + YWX_CTHL_PARAM_CL_Length) >= (YWX_CTHL_LianLengthCheck + 500)) && YWX_CTHL_LianLengthCheck)
                {
                    YWX_CTHL_HELT_VAVLE = 0;                    
                }
            }   
            //说明下一条没有方块
            if((dwXRealPos >= (YWX_CTHL_XSavePosBuff + YWX_CTHL_LianLengthCheck + 100)) && (YWX_CTHL_LianLengthCheck >1700))
            {
                YWX_CTHL_TD_MotorStep = 12; 
            }  
            
            if((YWX_CTHL_FK_Check)) //
            {
              
                user_datas[123] = GetCurSpeed(X_AXIS);
                user_datas[124] = dwXRealPos;
                YWX_CTHL_TD_MotorStep = 15; 
                AxisMovePosAccDecNotStop(X_AXIS,2500 ,300,3000,YWX_CTHL_PARAM_FKDW_Speed,10,10,0);
            }
            break;
          case 15:   
             if(YWX_CTHL_FKDW_IN)
            {             
                user_datas[125] = GetCurSpeed(X_AXIS);
                user_datas[126] = dwXRealPos;
                
                AxisMovePosAccDec(X_AXIS,YWX_CTHL_PARAM_FKDW_Speed,YWX_CTHL_PARAM_FKDW_Length,
                                  YWX_CTHL_PARAM_FKDW_Speed,YWX_CTHL_PARAM_FKDW_Speed,2,5,0); 
               YWX_CTHL_TD_MotorStep = 16;  
               YWX_CTHL_bFZ_LengthOK = 1;
               YWX_CTHL_LianLengthCheck = dwXRealPos - YWX_CTHL_XSavePosBuff + YWX_CTHL_PARAM_FKDW_Length;
               YWX_CTHL_XSavePosBuff = dwXRealPos + YWX_CTHL_PARAM_FKDW_Length;
            }
             break;
        case 16:
            if((!X_DRV) || (dwXRealPos >= YWX_CTHL_XSavePosBuff))
            {
                YWX_CTHL_TD_MotorStep = 0;  
                AxisEgmStop(X_AXIS);
            }
            break;
 
          
	}   
#endif    
}
//穿入电机动作，Y轴
void YWX_CTHL_FZ_Motor(void)	 //
{   
#if 1
    //1步开始,  穿链反转长度开始
  //20步开始  插销定位开始

    switch(YWX_CTHL_FZ_MotorStep)
	{
         //穿入长度位置
         case 1:
            
            if(dwTickCount >= YWX_CTHL_FZ_MotorDelay)
            {   
                SetPos(Y_AXIS, 0);  //启动位置设为0点
                SetPos(X_AXIS, 0);  //启动位置设为0点
                if(YWX_CTHL_bFZ_LengthOK)   //确定当条拉链长度已经明确NotStop
                {
        //            AxisMovePosAccDec(Y_AXIS,YWX_CTHL_PARAM_FZ_Speed,(YWX_CTHL_LianLengthCheck-700),800,600,6,6,50); 
        //             AxisContinueMoveAcc(Y_AXIS,(YWX_CTHL_PARAM_FZ_Speed + YWX_CTHL_PARAM_FZ_Speed/30),DIR_P,2000,1000,10,10); 
        //            AxisContinueMoveAcc(X_AXIS,YWX_CTHL_PARAM_FZ_Speed,DIR_N,2000,1000,10,10);   //必须要保证拖带轮的线速度比后退轮的线速度大
                  YWX_CTHL_LENTH = YWX_CTHL_LianLengthCheck - 400;
                    AxisMovePosAccDecNotStop(Y_AXIS,(YWX_CTHL_PARAM_FZ_Speed + YWX_CTHL_PARAM_FZ_Speed/20),YWX_CTHL_LENTH, 
                                                2000,1050,10,15,00);
                    SetDirReverse(X_AXIS, 0);
                    AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_FZ_Speed,YWX_CTHL_LENTH - 150,
                                                 2000,850,10,15,00); 
                }
                else   //在未知长度情况下
                {
                    AxisContinueMoveAcc(Y_AXIS,1300+30,DIR_P,600,600,10,10); 
                    AxisContinueMoveAcc(X_AXIS,1300,DIR_N,600,600,10,10);   //必须要保证拖带轮的线速度比后退轮的线速度大
                    
                }   
                YWX_CTHL_FZ_MotorStep = 2; 
                user_datas[121] = 0;
            }
            break;
        case 2:
            if(dwYRealPos > 150)
              YWX_CTHL_DXK_VAVLE = 0;      //Y01顶斜口电磁阀
             if(YWX_CTHL_bFZ_LengthOK)
             {
                if((dwYRealPos + 800) > YWX_CTHL_LianLengthCheck)
                {
                   
 //                       AxisEgmStop(X_AXIS);
 //                       AxisEgmStop(Y_AXIS);
                   YWX_CTHL_FZ_MotorStep = 3; 
  //                  AxisContinueMoveAcc(Y_AXIS,830,DIR_P,600,600,10,10); 
  //                  AxisContinueMoveAcc(X_AXIS,800,DIR_N,600,600,10,10);   //必须要保证拖带轮的线速度比后退轮的线速度大
                }
             }
                 if((dwYRealPos > 800) && YWX_CTHL_GQ_IN)
                {

                     AxisEgmStop(Y_AXIS);
                     YWX_CTHL_FZ_MotorStep = 0; 
                     YWX_CTHL_LianLengthCheck = dwYRealPos;
                     AxisEgmStop(X_AXIS);
                   
                }      
 //           } 
 /*               
  //          else
            {  
    //           if((dwYRealPos > 700) && !YWX_CTHL_XKTDW && YWX_CTHL_GQ_IN)
               {
                    AxisEgmStop(Y_AXIS);
                    YWX_CTHL_LianLengthCheck = dwYRealPos;
                    YWX_CTHL_FZ_MotorStep = 0; 
                    AxisEgmStop(X_AXIS);
 //                   if(dwXRealPos > dwYRealPos)
                    {
       //                 AxisMovePosAccDec(X_AXIS,3000,-(40),600,600,10,10,30); 
 //                       AxisMovePosAccDec(Y_AXIS,1000,(20),600,600,10,10,0); 
                    }
                    
                }
            }
*/
            break; 
        case 3: 
             if(YWX_CTHL_GQ_IN)
                {
                  
                  AxisEgmStop(X_AXIS);
                     AxisEgmStop(Y_AXIS);
                     YWX_CTHL_FZ_MotorStep = 0; 
                     YWX_CTHL_LianLengthCheck = dwYRealPos;
                     
                     SetDirReverse(X_AXIS, 1);
                     user_datas[122] = dwXRealPos;     
                     user_datas[123] = dwYRealPos;//YWX_CTHL_PARAM_TD_RunSpeed/2
                }     
            break;
          //夹带插销到穿入位置  
        case 20: //此处最好算好定位长度相减
            YWX_CTHL_FZ_MotorStep = 21; 
            SetPos(Y_AXIS, 0);  //启动位置设为0点
            break;
        case 21:
            if(YWX_CTHL_PARAM_CXDW_Length)
            { 
                AxisMovePosAccDec(Y_AXIS,3080,YWX_CTHL_PARAM_CXDW_Length,800,800,6,6,0);   
                if(YWX_CTHL_PARAM_CXDW_Length> 20)
                    AxisMovePosAccDec(X_AXIS,3000,-(YWX_CTHL_PARAM_CXDW_Length- 20),800,800,6,6,0); 
            }
            YWX_CTHL_FZ_MotorStep = 22; 
            break;
        case 22: 
            if(!Y_DRV || (dwYRealPos >= YWX_CTHL_PARAM_CXDW_Length)) //插销定位已经结束
            {
                AxisEgmStop(Y_AXIS);
                AxisEgmStop(X_AXIS);
                YWX_CTHL_FZ_MotorStep = 0;                       
            } 
            break;                
	}
#endif    
}



//装拉头动作
void YWX_CTHL_ZLT_Step(void)
{

    switch(YWX_CTHL_ZhuangLiaoStep)
    {
        case 1:
          
             if(bZhuangLiaoOkFlg)
             {
                  YWX_CTHL_ZhuangLiaoStep = 0;
             }
             else if(dwTickCount >= YWX_CTHL_ZhuangLiaoDelay)
             {
                
                   if(((YWX_CTHL_MGuo_VAVLE && YWX_CTHL_LTou_Check) || (!YWX_CTHL_MGuo_VAVLE)) && !YWX_CTHL_JLiao_Limit_IN  )//说明没有拉头
                   {       
                          //穿头模和一次穿入必须在原位,接料不能有输出
                          if(YWX_CTHL_CTM_Origin_IN && !YWX_CTHL_CTXM_VAVLE)
                          {  
                               YWX_CTHL_JLTou_VAVLE = 0;
                              YWX_CTHL_MGuo_VAVLE = 0;                     
                              if(YWX_CTHL_TLiaoStep == 0)
                              {
                                 YWX_CTHL_TLiaoStep = 1;        
                               YWX_CTHL_ZhuangLiaoStep = 2; 
                              }
                           }
                      
                        else if(dwTickCount >= YWX_CTHL_ZhuangLiaoDelay + 500)  //警告横送拉必须保证穿头模和一次穿入在原位,接料不能有输出
                        {
                          YWX_CTHL_SetAlarmCode(YWX_CTHL_CTM_Origin_ALARM,bRunning);
                            
                        }  
                   }
                   else  //已经有拉头直接跳到结束
                   {
                         YWX_CTHL_ZhuangLiaoDelay = dwTickCount + 40;
                         YWX_CTHL_ZhuangLiaoStep = 9;
                   }  
             }
             break;            
        case 2:        
            if(YWX_CTHL_TLiaoStep == 0)  //推料已经完成
            {
                YWX_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
                YWX_CTHL_JLTou_VAVLE = 0;
                YWX_CTHL_MGuo_VAVLE = 0;   
                if(!YWX_CTHL_JLiao_Limit_IN)
                {
                    YWX_CTHL_ZhuangLiaoStep = 3;
                    YWX_CTHL_HSLiao_VAVLE = 1;   
                }
            }
            break;
        case 3:
            //横送到位
            if(YWX_CTHL_HSLiao_Limit_IN && !YWX_CTHL_HSLiao_Origin_IN)
            {
                YWX_CTHL_ZhuangLiaoStep = 4;
//                YWX_CTHL_JLTou_VAVLE = 1;    //接拉头输出,不需要时间
                YWX_CTHL_ZhuangLiaoDelay = dwTickCount + 5;
            }
            else if(dwTickCount >= YWX_CTHL_ZhuangLiaoDelay)
            {
                YWX_CTHL_SetAlarmCode(YWX_CTHL_HSLiao_Limit_ALARM,bRunning);
            }
            break;
        case 4:
            if(dwTickCount >= YWX_CTHL_ZhuangLiaoDelay)
            {
                YWX_CTHL_ZhuangLiaoStep = 5;
                YWX_CTHL_JLTou_VAVLE = 1;    //接拉头输出,不需要时间
                YWX_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
            }  
            break;
        case 5:    
            if(YWX_CTHL_JLiao_Limit_IN)
            {
                YWX_CTHL_ZhuangLiaoStep = 6;
                YWX_CTHL_ZhuangLiaoDelay = dwTickCount + YWX_CTHL_PARAM_MaGou_DELAY;
            }
            else if(dwTickCount >= YWX_CTHL_ZhuangLiaoDelay)  //接拉头(接料)到们信号异常
            {
                 YWX_CTHL_SetAlarmCode(YWX_CTHL_JLiao_Limit_ALARM,bRunning);
            }
            break;
        case 6:       
            if(dwTickCount >= YWX_CTHL_ZhuangLiaoDelay)  //延时码勾
            {
                YWX_CTHL_MGuo_VAVLE = 1; //码勾输出
                YWX_CTHL_ZhuangLiaoStep = 7;
                YWX_CTHL_ZhuangLiaoDelay = dwTickCount + YWX_CTHL_PARAM_HSLiaoOff_DELAY;
                bTuiLaTouOkFlg = 0;   //认为拉头没法带出
            }
            break;
        case 7:  
            if(dwTickCount >= YWX_CTHL_ZhuangLiaoDelay)
            {
                YWX_CTHL_HSLiao_VAVLE = 0; 
                YWX_CTHL_ZhuangLiaoStep = 8;
                YWX_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
            }
            break;
        case 8:  
            if(!YWX_CTHL_HSLiao_Limit_IN)  //
            {
                YWX_CTHL_ZhuangLiaoStep = 9;
                YWX_CTHL_TLiaoStep = 1; //推拉头
                YWX_CTHL_ZhuangLiaoDelay = dwTickCount + YWX_CTHL_PARAM_CheckLT_DELAY;   //也是给下模能上升的时间
            }          
            else if (YWX_CTHL_LTou_Check)  //时间内信号这说明没有拉头
            {
                //重送3次
               YWX_CTHL_TryCnt++;
               YWX_CTHL_JLTou_VAVLE = 0;    //接拉头关
                YWX_CTHL_MGuo_VAVLE = 0;     //码勾打开
                if(YWX_CTHL_TryCnt >= 3)
                {
                    YWX_CTHL_TryCnt = 0;
                    bZhuangLiaoOkFlg = 0;
                    YWX_CTHL_ZhuangLiaoStep = 0;
                    YWX_CTHL_SetAlarmCode(YWX_CTHL_ZLT_ALARM,bRunning);
                    cWULATOUCnt=1;
                }
                else
                {
                    YWX_CTHL_ZhuangLiaoStep = 20;    //重新送3次   
                    YWX_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
                }  
            }
            else if(dwTickCount >= YWX_CTHL_ZhuangLiaoDelay)
            {
                YWX_CTHL_SetAlarmCode(YWX_CTHL_HSLiao_Limit_ALARM,bRunning);
                if(bRunning)
                {
                    YWX_CTHL_ZhuangLiaoStep = 0 ;
                }  
            }  
            break;
        case 9:   
            if(YWX_CTHL_LTou_Check)  //时间内信号这说明没有拉头
            {
                YWX_CTHL_TryCnt++;
                YWX_CTHL_JLTou_VAVLE = 0;    //接拉头关
                YWX_CTHL_MGuo_VAVLE = 0;     //码勾打开
                if(YWX_CTHL_TryCnt >= 3)
                {
                  YWX_CTHL_TryCnt=0;
                    bZhuangLiaoOkFlg = 0;
                    YWX_CTHL_ZhuangLiaoStep = 0;
                    YWX_CTHL_SetAlarmCode(YWX_CTHL_ZLT_ALARM,bRunning);
                    cWULATOUCnt=1;
                }
                else
                {
                    YWX_CTHL_ZhuangLiaoStep = 20;    //重新送3次   
                    YWX_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
                }  
            }
            else if(dwTickCount >= YWX_CTHL_ZhuangLiaoDelay)
            {
                bZhuangLiaoOkFlg = 1;
                YWX_CTHL_ZhuangLiaoStep = 0;
            }
            break;
          //装拉头不成攻重新送拉头
        case 20: 
            if(YWX_CTHL_HSLiao_Origin_IN)
            {
                YWX_CTHL_JLTou_VAVLE = 0;    //接拉头关
                YWX_CTHL_MGuo_VAVLE = 0;     //码勾打开
                YWX_CTHL_ZhuangLiaoStep = 1;
                YWX_CTHL_ZhuangLiaoDelay = dwTickCount + 10;
            }
            else if(dwTickCount >= YWX_CTHL_ZhuangLiaoDelay)
            {
                YWX_CTHL_SetAlarmCode(YWX_CTHL_HSLiao_Origin_ALARM,bRunning);
            }  
            break;
    }  
}

void YWX_CTHL_TLiao_Step(void)
{
    switch(YWX_CTHL_TLiaoStep)
    {
        case 1:
            if(bTuiLaTouOkFlg)
            {
                YWX_CTHL_TLiaoStep = 0; 
            }
            else
            {
                YWX_CTHL_TLiao_Delay = dwTickCount + YWX_CTHL_VAVLE_ERROR_TIME;
                YWX_CTHL_TLiaoStep = 2;
            }
            break;
        case 2:    //横送料在原位
            if(YWX_CTHL_HSLiao_Origin_IN && !YWX_CTHL_HSLiao_Limit_IN && !YWX_CTHL_HSLiao_VAVLE)
            {
                YWX_CTHL_TLiao_VAVLE = 1;    //推料(推拉头)输出
                YWX_CTHL_TLiao_Delay = dwTickCount + YWX_CTHL_VAVLE_ERROR_TIME;
                YWX_CTHL_TLiaoStep = 3;
            }
            else if(dwTickCount >= YWX_CTHL_TLiao_Delay) 
                YWX_CTHL_SetAlarmCode(YWX_CTHL_HSLiao_Origin_ALARM,1);          
            break;
        case 3:  //推拉头到位
            if(YWX_CTHL_TLiao_Limit_IN)
            { 
                YWX_CTHL_TLiao_Delay = dwTickCount + YWX_CTHL_PARAM_TLiao_BACKDELAY;
                YWX_CTHL_TLiaoStep = 4;
            }
            else if(dwTickCount >= YWX_CTHL_TLiao_Delay) 
                YWX_CTHL_SetAlarmCode(YWX_CTHL_TLiao_Limit_ALARM,1);        
            break;
        case 4:
            if(dwTickCount >= YWX_CTHL_TLiao_Delay)
            {
                YWX_CTHL_TLiao_VAVLE       = 0;
                YWX_CTHL_TLiao_Delay = dwTickCount + YWX_CTHL_VAVLE_ERROR_TIME; 
                YWX_CTHL_TLiaoStep = 5;
            }
            break;
        case  5:
            if(!YWX_CTHL_TLiao_Limit_IN)
            {
                YWX_CTHL_TLiaoStep = 0;
                YWX_CTHL_TLiao_Delay = dwTickCount;
                bTuiLaTouOkFlg    = 1;
            }
            else if(dwTickCount >= YWX_CTHL_TLiao_Delay) 
                YWX_CTHL_SetAlarmCode(YWX_CTHL_TLiao_Limit_ALARM,0);    
            break;
    } 
   
}
//振动盘控制
void YWX_CTHL_ZhenDongAction(void)
{
 
#if 0  
    if((YWX_CTHL_bDLP || bRunning) && YWX_CTHL_ZhenDongPian_OUT && YWX_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(CT_PARAM_ZDP_MODE)
//       {
            if(YWX_CTHL_ZhenDongPian_OUT == 0)
            {
                if(!YWX_CTHL_ZDP_IN)
                {
                    if(dwTickCount >= YWX_CTHL_ZDP_Delay)
                    {
                        YWX_CTHL_ZhenDongPian_OUT = 1;
                        YWX_CTHL_ZDP_Delay = dwTickCount + YWX_CTHL_PARAM_ZDP_Stop_TIME;//YWX_CTHL_PARAM_ZDP_Stop_TIME + 50;
                    }
                }
                else
                {
                    YWX_CTHL_ZDP_Delay = dwTickCount + YWX_CTHL_PARAM_ZDP_Start_TIME;//YWX_CTHL_PARAM_ZDP_Start_TIME;
                }   
            }
            else
            {  
                if(YWX_CTHL_ZDP_IN)
                {  
                    if(dwTickCount >= YWX_CTHL_ZDP_Delay)
                    {
                         YWX_CTHL_ZhenDongPian_OUT = 0;  
                         YWX_CTHL_ZDP_Delay = dwTickCount + YWX_CTHL_PARAM_ZDP_Start_TIME;//YWX_CTHL_PARAM_ZDP_Start_TIME
                    }
                }
                else
                    YWX_CTHL_ZDP_Delay = dwTickCount + YWX_CTHL_PARAM_ZDP_Stop_TIME;//YWX_CTHL_PARAM_ZDP_Stop_TIME+ 50;
            }
#endif    
}
void YWX_CTHL_bFKCXTDDWStep(void)
{
#if 0  
    switch(YWX_CTHL_bFKCXTDDW_Step)
    {
        case 1:
                YWX_CTHL_FKJD_VAVLE = 1;   //方块夹带气缸输出
                YWX_CTHL_XKTD_VAVLE = 1;   //斜口推带气缸输出    
                YWX_CTHL_FKGD_VAVLE = 0;     //方块固定关
                YWX_CTHL_bFKCXTDDW_Step = 2;
                
                break;
            case 2: 
                if(!YWX_CTHL_XKTD_Origin_IN)  //斜口推带原位离开
                {
                    YWX_CTHL_bFKCXTDDW_Step = 3;
                    YWX_CTHL_bFKCXTDDW_Delay = dwTickCount + 20;
                }  
                break;
            case 3://斜口定位
                if(dwTickCount >= YWX_CTHL_bFKCXTDDW_Delay)
                {
                    YWX_CTHL_XKDW_VAVLE = 1;
                    YWX_CTHL_bFKCXTDDW_Step = 4;
                    YWX_CTHL_bFKCXTDDW_Delay = dwTickCount + YWX_CTHL_PARAM_XKDW_TD_DELAY;
                }  
                break;
            case 4:
                if(dwTickCount >= YWX_CTHL_bFKCXTDDW_Delay)
                {
                    AxisContinueMoveAcc(X_AXIS,1200,DIR_P,800,600,10,10);//拖带电机                       
                    YWX_CTHL_bFKCXTDDW_Step = 5;
                    YWX_CTHL_XSavePosBuff = dwXRealPos;
                    SetPos(X_AXIS, 0);  //启动位置设为0点
                    dwXRealPos = 0;
                }
                break;
            case 5:
                if(YWX_CTHL_XKSC_Limit_IN)   //方块到位
                {
                    AxisEgmStop(X_AXIS);
                    YWX_CTHL_bFKCXTDDW_Step = 0;
                    YWX_CTHL_XKTD_VAVLE = 0;   //斜口推带气缸关闭 
                    YWX_CTHL_bFKCXTDDW_Delay = dwTickCount + YWX_CTHL_VAVLE_ERROR_TIME;
                   
                }
                else if(dwXRealPos >= (YWX_CTHL_XSavePosBuff + 800))
                {
                     YWX_CTHL_SetAlarmCode(YWX_CTHL_FKCXDW_ALARM,bRunning);//方块插销定位异常警告
                    
                }  
                break;
    
    }  
#endif    
}

//插销穿入动作
void YWX_CTHL_bCXCRStep(void)
{
#if 0  
    switch(YWX_CTHL_bCXCR_Step)
    {
            case 11:
                YWX_CTHL_XKTD_VAVLE = 1;   //斜口推带气缸输出    
                YWX_CTHL_FKGD_VAVLE = 0;     //方块固定关
                YWX_CTHL_bCXCR_Step = 12;               
                break;
            case 12: 
                if(!YWX_CTHL_XKTD_Origin_IN)  //斜口推带原位离开
                {
                    YWX_CTHL_bCXCR_Step = 13;
                    YWX_CTHL_bCXCR_Delay = dwTickCount + 20;
                }  
                break;
            case 13: 
                if((YWX_CTHL_CL_MotorStep == 0) && (YWX_CTHL_SF_Origin_IN) && (dwTickCount >= YWX_CTHL_bCXCR_Delay))  //插销前夹带回到原位
                {
                    YWX_CTHL_bCXCR_Step = 14; 
                    YWX_CTHL_FKGD_VAVLE = 1;     //方块固定关
                    YWX_CTHL_CXQJD_VAVLE = 1;    //插销前夹带                    
                    YWX_CTHL_bCXCR_Delay = dwTickCount + YWX_CTHL_PARAM_XCQJD_XCDW_DELAY;
                }
                break;
            case 14:
                if(dwTickCount >= YWX_CTHL_bCXCR_Delay)
                {
                    YWX_CTHL_bCXCR_Step = 15; 
                    YWX_CTHL_FKGD_VAVLE = 1;     //方块固定
                    //Y轴启动插销
                    YWX_CTHL_CL_MotorStep = 20;  //插销定位                   
                }
                break;
            case 15:   
                if(YWX_CTHL_CL_MotorStep == 0)   //插销定位完成
                {
                    YWX_CTHL_bCXCR_Step = 16; 
                    YWX_CTHL_CXDWZ_VAVLE = 1;    //插销定位针(到位)电磁阀
                    YWX_CTHL_bCXCR_Delay = dwTickCount + 10;  //延时插销下移
                }  
                break;
            case 16: 
                if(dwTickCount >= YWX_CTHL_bCXCR_Delay)
                {
                    YWX_CTHL_bCXCR_Step = 17; 
                    YWX_CTHL_CXXY_VAVLE = 1;     //Y06 插销下移电磁阀
                    YWX_CTHL_bCXCR_Delay = dwTickCount + YWX_CTHL_VAVLE_ERROR_TIME;  //插销下移异常时间
                }
                break;
            case 17:   
                if(YWX_CTHL_CXXY_Limit_IN)
                {
               //   if(YWX_CTHL_START_IN_UP)
                  {
                    YWX_CTHL_bCXCR_Step = 18; 
                 if( cXcxcrCnt==0 && !X_DRV)  //拖带电机反转
                {
             //    SetDir(X_AXIS, DIR_N);
                 AxisMovePosAccDec(X_AXIS,10000,-YWX_CTHL_PARAM_TDFZ_LENTH,800,600,6,6,0);
                }
                    YWX_CTHL_CXQJD_VAVLE = 0;    //插销前夹带电磁阀
                    YWX_CTHL_CXSCDW_VAVLE = 0;   //Y21 插销上止定位关                    
                    YWX_CTHL_CXHJD_VAVLE = 1;         //Y14 插销后夹带输出
                    YWX_CTHL_bCXCR_Delay = dwTickCount + YWX_CTHL_PARAM_SCDWOff_CXDW_DELAY;

                  }
                }  
                else if(dwTickCount >= YWX_CTHL_bCXCR_Delay)  
                {
                    YWX_CTHL_SetAlarmCode(YWX_CTHL_CXXY_Limit_ALARM,bRunning);//插销下移异常
                    
                }
                break;
            case 18:   
  //              if(START_IN_UP)
                {
                if(dwTickCount >= YWX_CTHL_bCXCR_Delay) 
                {
                    YWX_CTHL_bCXCR_Step = 19; 
                    
                    YWX_CTHL_CXHDW_VAVLE = 1;     //插销后定位(插销穿入)
                    YWX_CTHL_bCXCR_Delay = dwTickCount + YWX_CTHL_VAVLE_ERROR_TIME +1000;  //插销穿入到位异常时间
                    YWX_CTHL_CL_MotorStep = 50;  //合链电机回原点
                }
                }
                break;
            case 19:  
                 if( cXcxcrCnt > 1)  
                {
                    YWX_CTHL_SetAlarmCode(YWX_CTHL_CX_Limit_ALARM,bRunning); //插销穿入到位异常  
                }
                
                if( YWX_CTHL_CX_Limit_IN  &&  cXcxcrCnt<2 )
                {
                    cXcxcrCnt=0;
                    bZhuangLiaoOkFlg = 0;
                    YWX_CTHL_bCXCR_Step = 0;
                    YWX_CTHL_CXDWZ_VAVLE = 0;    //插销定位针(到位)电磁阀  
              
                } 
                else if (dwTickCount >= YWX_CTHL_bCXCR_Delay)
                {
                  cXcxcrCnt++;
              
                   YWX_CTHL_CXHJD_VAVLE=0;
                   YWX_CTHL_CXHDW_VAVLE=0;
                   YWX_CTHL_CXDWZ_VAVLE = 0;
                   YWX_CTHL_CXXY_VAVLE=0;
                  YWX_CTHL_bCXCR_Step = 11;
                }
               
              break;
  }
#endif  
}


//手动动作
void YWX_CTHL_ManualAction(void)
{ 
      if(bRunning) // 运行灯输出
      {
        YWX_CTHL_Run_State=1;
        YWX_CTHL_Stop_State=0;
      }
      else 
      {
        YWX_CTHL_Stop_State=1;
        YWX_CTHL_Run_State=0;
      }
  
 
 
    if(bRunning == 0)  
    {   
        if(bClearTotal) //切断计数清零
        {
            bClearTotal       = 0;
            ClrcToTal(QDCT_TOTAL_ADDR);
        }
        if(YWX_CTHL_bDXK)                  //顶斜口
        {
            YWX_CTHL_bDXK = 0;
            if(YWX_CTHL_DXK_VAVLE)
                YWX_CTHL_DXK_VAVLE = 0;
             else
                YWX_CTHL_DXK_VAVLE = 1;
             
        }         
         if(YWX_CTHL_bXKDW)                  //斜口定位
        {
            YWX_CTHL_bXKDW = 0;
            YWX_CTHL_XKDW_VAVLE = ~YWX_CTHL_XKDW_VAVLE;
        } 
        if(YWX_CTHL_bFWCX)                 //复位插销
        {
            YWX_CTHL_bFWCX = 0;
            YWX_CTHL_FWCX_VAVLE = ~YWX_CTHL_FWCX_VAVLE;
        } 
        
        if(YWX_CTHL_bBB)                    //摆臂
        {
            YWX_CTHL_bBB = 0;
            YWX_CTHL_BAIBI_VAVLE = ~YWX_CTHL_BAIBI_VAVLE;
        } 
//退插销
 //夹带       
        
        if(YWX_CTHL_bTLiao )                 //手动推料(推拉头)
        {
            YWX_CTHL_bTLiao = 0;           
            if(YWX_CTHL_TLiao_VAVLE)
               YWX_CTHL_TLiao_VAVLE = 0;//YWX_CTHL_TLiao_VAVLE; 
            else if(!YWX_CTHL_HSLiao_VAVLE && YWX_CTHL_HSLiao_Origin_IN)
            {
                YWX_CTHL_TLiao_VAVLE = 1;
            } 
            else//条件警告
            {
              YWX_CTHL_SetAlarmCode(YWX_CTHL_HENSONGLIAO_ALARM,0); // 横送料不能输出
            }

        } 
        if(YWX_CTHL_bHSL)     //手动横送料   
        {
            YWX_CTHL_bHSL= 0;

            if(YWX_CTHL_HSLiao_VAVLE)
            {
                YWX_CTHL_HSLiao_VAVLE = 0;//~YWX_CTHL_HSLiao_VAVLE;
            }
            else if(!YWX_CTHL_TLiao_VAVLE && !YWX_CTHL_TLiao_Limit_IN && YWX_CTHL_CTM_Origin_IN && !YWX_CTHL_JLiao_Limit_IN &&
                    !YWX_CTHL_JLTou_VAVLE && !YWX_CTHL_CTXM_VAVLE && (!YWX_CTHL_bMG || (YWX_CTHL_bMG && YWX_CTHL_LTou_Check)))
            {
                YWX_CTHL_HSLiao_VAVLE = 1;
            }  
            else   //条件警告  最后一个括号条件为已经有拉头
            {
               YWX_CTHL_SetAlarmCode(YWX_CTHL_YCCT_ALARM,0); 
            }  

        }           
        if(YWX_CTHL_bJLT)                  //接拉头
        {
            YWX_CTHL_bJLT = 0;
            if(YWX_CTHL_JLTou_VAVLE)
                YWX_CTHL_JLTou_VAVLE = 0;
            else if( !YWX_CTHL_CTXM_VAVLE && YWX_CTHL_CTM_Origin_IN)
            {
                YWX_CTHL_MGuo_VAVLE = 0;
                YWX_CTHL_JLTou_VAVLE = 1;
            } 
            else//条件警告
            {
                 YWX_CTHL_SetAlarmCode(YWX_CTHL_YCXM_Origin_ALARM,0); 
            }
        } 
        if(YWX_CTHL_bMG)                   //码勾
        {
            YWX_CTHL_bMG = 0;
            YWX_CTHL_MGuo_VAVLE = !YWX_CTHL_MGuo_VAVLE;
        } 
      
       
        if(YWX_CTHL_bCTXM)                  //穿头下模
        {
            YWX_CTHL_bCTXM = 0;
            if(YWX_CTHL_CTXM_VAVLE)
            {
                YWX_CTHL_CTXM_VAVLE = 0;
                YWX_CTHL_JLTou_VAVLE = 0;
            }    
            else if(!YWX_CTHL_HSLiao_VAVLE && YWX_CTHL_HSLiao_Origin_IN)                
            { 
                YWX_CTHL_MGuo_VAVLE = 1;
                YWX_CTHL_CTXM_VAVLE = 1;
            }  
        } 
        if(YWX_CTHL_bLYLun)              //左压轮
        {
            YWX_CTHL_bLYLun = 0;
            YWX_CTHL_LYLun_VAVLE = ~YWX_CTHL_LYLun_VAVLE;
        }  
        if(YWX_CTHL_bRYLun)               //右压轮
        {
            YWX_CTHL_bRYLun= 0;
            YWX_CTHL_RYLun_VAVLE = ~YWX_CTHL_RYLun_VAVLE;
        }  
        
        
        if(YWX_CTHL_bFKGD)                 //方块固定
        {
            YWX_CTHL_bFKGD = 0;
            YWX_CTHL_FKGD_VAVLE = ~YWX_CTHL_FKGD_VAVLE;
        } 
        //合拉头
        if(YWX_CTHL_bHeLT)
        {
           YWX_CTHL_bHeLT = 0;
           YWX_CTHL_HELT_VAVLE = ~YWX_CTHL_HELT_VAVLE;
        }  
        //压带
        if(YWX_CTHL_bYD)
        {
            YWX_CTHL_bYD = 0;
            YWX_CTHL_YADAI_VAVLE =~YWX_CTHL_YADAI_VAVLE;
        }  
       
       

        //自动装料
        if(YWX_CTHL_bAutoZhuangLiao)
        {
            YWX_CTHL_bAutoZhuangLiao = 0;
            if(YWX_CTHL_ZhuangLiaoStep == 0)
            {
                bTuiLaTouOkFlg = 1;
                bZhuangLiaoOkFlg = 0;
                YWX_CTHL_ZhuangLiaoStep = 1;
                YWX_CTHL_MGuo_VAVLE = 1;   //判断是否已经有料
                YWX_CTHL_ZhuangLiaoDelay = dwTickCount + 150;
            }  
        }
        //测试电机
        if(YWX_CTHL_bCLMotor_N)
        {           
            SetEn(Y_AXIS, MOTOR_EN);
            
                if(!Y_DRV)
                  //                Y轴       运行速度                    启动速度        加速度  减速度
                   AxisContinueMoveAcc(Y_AXIS,2000,DIR_N,1000,1000,15,15);             
        }
        
        if(YWX_CTHL_bCLMotor_P)  //后退限位已经取消
        {
            SetEn(Y_AXIS, MOTOR_EN);
           if(!Y_DRV)
              //                Y轴       运行速度                    启动速度        加速度  减速度
              AxisContinueMoveAcc(Y_AXIS,2000,DIR_P,1000,1000,15,15);
        }
        
        if(!YWX_CTHL_bCLMotor_P && !YWX_CTHL_bCLMotor_N && (YWX_CTHL_FZ_MotorStep == 0))
        {
              AxisEgmStop(Y_AXIS);//AxisDecStop(Y_AXIS); 
        }
        //合链电机返回原点
        if(YWX_CTHL_bCLMotor_O)
        {
            YWX_CTHL_bCLMotor_O = 0;
            if(YWX_CTHL_FZ_MotorStep == 0)
                YWX_CTHL_FZ_MotorStep = 40;
        } 
        //拖带方块定位 ,因这步为前拖带定位, 很多电磁阀要关掉       
        if(YWX_CTHL_bFKTDDW)
        {
            YWX_CTHL_bFKTDDW = 0;
            if(YWX_CTHL_TD_MotorStep == 0)
            {
                YWX_CTHL_XKDW_VAVLE = 0;     //斜口定位
               
                if((GetEn(X_AXIS) == MOTOR_DISEN))
                {
                    SetEn(X_AXIS, MOTOR_EN);
                    YWX_CTHL_TD_MotorDelay = dwTickCount + 200;                    
                } 
                else
                    YWX_CTHL_TD_MotorDelay = dwTickCount + 50;
                YWX_CTHL_TD_MotorStep = 1;
            } 
        } 
        //拖带方块插销定位(斜口推带，斜口定位，顶斜口)
        if(YWX_CTHL_bFKCXTDDW)
        {
            YWX_CTHL_bFKCXTDDW = 0;
            if((YWX_CTHL_bFKCXTDDW_Step == 0) && (YWX_CTHL_FZ_MotorStep == 0) && 
               !YWX_CTHL_CTXM_VAVLE)
            {
                YWX_CTHL_bFKCXTDDW_Step = 1;
                SetEn(X_AXIS, MOTOR_EN);
                
            }  
        } 
        //插销穿入(包含电机前定位,插销下移,后夹,后定位)
        if(YWX_CTHL_bCXCR) 
        {
            YWX_CTHL_bCXCR = 0;
            //拉头模在上面到们并且码勾输出,判断有拉头
            if(YWX_CTHL_CTXM_VAVLE && YWX_CTHL_MGuo_VAVLE &&
               YWX_CTHL_CTM_Limit_IN && !YWX_CTHL_LTou_Check)
            {                
                if(YWX_CTHL_FZ_MotorStep == 0)
                {
                     YWX_CTHL_bCXCR_Step = 11; 
                     SetEn(Y_AXIS, MOTOR_EN);
                }
            }
            else   //警告不能自动穿插销
            {
                YWX_CTHL_SetAlarmCode(YWX_CTHL_WFZIDONGCX_ALARM,0);  
            }
        }
        //电机合链动作
        if(YWX_CTHL_bMotorHL)
        {
            YWX_CTHL_bMotorHL = 0;

        } 
        //一次穿链动作
        if(YWX_CTHL_bCL_First)
        {
            YWX_CTHL_bCL_First = 0;
            //拉头模在上面到们并且码勾输出,判断有拉头
            if(YWX_CTHL_CTXM_VAVLE && YWX_CTHL_MGuo_VAVLE &&
               YWX_CTHL_CTM_Limit_IN && !YWX_CTHL_LTou_Check && (YWX_CTHL_FZ_MotorStep == 0))
            {  
                YWX_CTHL_bCLFirst_Step = 19;

            }
        }  
        /*
        YWX_CTHL_LianLengthCheck = 6989;
        if(YWX_CTHL_bTiaoShiStep)
        {
            YWX_CTHL_bTiaoShiStep = 0;
            SetPos(X_AXIS,0);
            SetPos(Y_AXIS,0);
             dwXRealPos = 0;
            dwYRealPos = 0;
            AxisMovePosAccDec(Y_AXIS,YWX_CTHL_PARAM_FZ_Speed,-YWX_CTHL_LianLengthCheck, 
                                                 2000,1000,10,15,200); 
            AxisMovePosAccDec(X_AXIS,YWX_CTHL_PARAM_FZ_Speed,(YWX_CTHL_LianLengthCheck),
                                                 2000,1000,10,15,200); 
        }  
        
        if(YWX_CTHL_bTiaoShiMode)
        {
            YWX_CTHL_bTiaoShiMode = 0;
            SetPos(X_AXIS,0);
            SetPos(Y_AXIS,0);
            dwXRealPos = 0;
            dwYRealPos = 0;
            AxisMovePosAccDec(Y_AXIS,YWX_CTHL_PARAM_FZ_Speed,YWX_CTHL_LianLengthCheck, 
                                                 2000,1000,10,15,200); 
            AxisMovePosAccDec(X_AXIS,YWX_CTHL_PARAM_FZ_Speed,-(YWX_CTHL_LianLengthCheck),
                                                 2000,1000,10,15,200); 
        } 
*/           
    }        
}

#endif