GC23005/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_CTHeLian_TwoMotor(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(!cWULATOUCnt)
    {
      bAlarmStop = cStopFlag;
    }
 
}

//初始化动作
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;
    SetAlarmCode(YWX_CTHL_ALARM_ADDR,0); 
    
    YWX_CTHL_bZDP= 1;
    

    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[123] = YWX_CTHL_TD_MotorStep;
//    user_datas[124] = cDinWeiStep;
    user_datas[125] = YWX_CTHL_LianLengthCheckOld;
    
 //   user_datas[126] = YWX_CTHL_TD_MotorStep;//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();  
    YWX_CTHL_Test++;
}

void YWX_CTHL_AlarmProtect(void)
{ 
    

 
}



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

 
    

    if(bRunning)
    {
        switch(YWX_CTHL_AutoStep)
        {
            case 50:
                                             
                     YWX_CTHL_XKDW_VAVLE  =0;       
                     YWX_CTHL_DXK_VAVLE  =0;                
                                 
                     YWX_CTHL_FKGD_VAVLE =0;         
                                
                     YWX_CTHL_FWCX_VAVLE = 0;
                     YWX_CTHL_BAIBI_VAVLE = 0;   //摆臂  
                     YWX_CTHL_HELT_VAVLE = 0;
                     YWX_CTHL_LYLun_VAVLE= 0;     //Y15方块压轮
                     YWX_CTHL_RYLun_VAVLE = 0;       //Y16插销压轮
                     YWX_CTHL_CTXM_VAVLE = 0;
                if(YWX_CTHL_CTM_Origin_IN)
                {    
                    YWX_CTHL_AutoStep = 1;
                    YWX_CTHL_MGuo_VAVLE = 1;   //判断是否已经有料
                    YWX_CTHL_ZhuangLiaoDelay = dwTickCount + 150;  
                }  
                else if(dwTickCount >= YWX_CTHL_AutoDelay) 
                {
                  //穿头模和一次穿入回位异常
                      YWX_CTHL_SetAlarmCode(YWX_CTHL_CTM_Origin_ALARM,bRunning);                                   
                } 
                break;
                
            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) && (YWX_CTHL_TD_MotorStep == 0) && !YWX_CTHL_CTM_Limit_IN)
                {                  
                      //调试模式
                    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)      //X23方块上止到位异常
                {
                    YWX_CTHL_AutoStep = 8;
                    YWX_CTHL_AutoDelay = dwTickCount + 10;//方块上止定位输出后电机延时反转定位
                }
                else if(dwTickCount >= YWX_CTHL_AutoDelay)
                {
                    YWX_CTHL_SetAlarmCode(YWX_CTHL_FKSC_DW_ALARM,bRunning); //X23方块上止到位异常
                }
                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) && (YWX_CTHL_PARAM_NL_JY_SELECT == 0))
                    {
                        AxisMovePosAccDec(Y_AXIS,1000,YWX_CTHL_PARAM_FKSC_DW_DELAY,800,550,6,6,0); 
                        if(YWX_CTHL_PARAM_FKSC_DW_DELAY > 10)
                            AxisMovePosAccDec(X_AXIS,950,-(YWX_CTHL_PARAM_FKSC_DW_DELAY-YWX_CTHL_PARAM_FKSC_DW_DELAY/40),
                                              800,500,6,6,0); 
                    }
                  }      
                }
                break;         
            case 9: 
                if(YWX_CTHL_FKSC_TDW_IN)
                {
                    AxisEgmStop(X_AXIS);
                    AxisEgmStop(Y_AXIS);
                }
                if(!Y_DRV && !X_DRV)  //方块上止定位完成
                {              
                    if(YWX_CTHL_FKSC_TDW_IN || YWX_CTHL_PARAM_NL_JY_SELECT)
                    {
                        YWX_CTHL_AutoStep = 10;    
                        YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_PARAM_DXK_DELAY;
                    }
                    else   //方块斜码退到位异常
                    {
                        YWX_CTHL_SetAlarmCode(YWX_CTHL_FKSC_TDW_ALARM,bRunning);//X03斜码退到位异常
                    }                         
                }   
                
                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)  //顶斜口到位异常
                {
                    YWX_CTHL_SetAlarmCode(YWX_CTHL_XKSC_Limit_ALARM,bRunning);//X03斜口上止到位异常
                }   
                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)  //退方块上止离开异常
                {
                    YWX_CTHL_SetAlarmCode(YWX_CTHL_FKSC_DW_ALARM,bRunning); //X23方块上止到位异常
                }   
                break;
            case 13:  
                if(dwTickCount >= YWX_CTHL_AutoDelay)
                {  
                 //调试模式
                  if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                  {
                        YWX_CTHL_bTiaoShiStep = 0;
                        YWX_CTHL_AutoStep = 14;  

                        YWX_CTHL_FZ_MotorStep = 1;
                        cDinWeiStep = 0;
                        YWX_CTHL_bFirstFKOK = 0;
                        YWX_CTHL_bQDinWeiOK = 0;
                   }
                }   
                break;
               
            case 14:
              
                if(YWX_CTHL_FZ_MotorStep == 0)  //穿入定位完成
                {      
                    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;
//                        if(YWX_CTHL_GQ_IN)
                        {
                        
                   
                        YWX_CTHL_FKGD_VAVLE = 1;     //Y00方块固定 
                                  
                        YWX_CTHL_AutoStep = 16; 
                         
                        YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_VAVLE_ERROR_TIME;
                        }
 //                       else
 //                       {
                        
 //                       }   
                  }
                }  
                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方块固定到位异常
          {
            YWX_CTHL_SetAlarmCode(YWX_CTHL_XCFW_DW_ALARM,bRunning);//X20插销穿入异常
          }
          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_BAIBI_VAVLE = 1;       //摆臂  
                YWX_CTHL_AutoDelay = dwTickCount + YWX_CTHL_PARAM_BAIBI_TUI_YALUN_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_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 - 30;
          }
          break;
        case 21:
            if((dwXRealPos>= YWX_CTHL_XSavePosBuff) && ((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep))
            {
                    YWX_CTHL_FWCX_VAVLE = 1;
            }
            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;
                    YWX_CTHL_MGuo_VAVLE = 0;     //码勾退
                            
                }
            }  
            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)
            {
                YWX_CTHL_CTXM_VAVLE = 0;    //下模打开
              //调试模式
                if((!YWX_CTHL_bTiaoShiMode) || YWX_CTHL_bTiaoShiStep)
                {
                    
                    //信号离开后6MS穿入
                    if(dwTickCount >= YWX_CTHL_AutoDelay)
                    {
                        YWX_CTHL_bTiaoShiStep = 0;
                        YWX_CTHL_TD_MotorStep = 10;  //
                        YWX_CTHL_AutoStep = 24; 
                        YWX_CTHL_MGuo_VAVLE = 0;     //码勾退
                        YWX_CTHL_TD_MotorDelay = dwTickCount + 16;
                          
                        
                    }
                }
            }
            else
            {
                YWX_CTHL_AutoDelay = dwTickCount + 2;
                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 > 50) && X_DRV)    //穿入3MM后退码勾和摆臂
            {
                YWX_CTHL_AutoStep = 25; 
                YWX_CTHL_MGuo_VAVLE = 0;     //码勾退                
                YWX_CTHL_BAIBI_VAVLE = 0;
                YWX_CTHL_AutoDelay = dwTickCount + 6;  //延时穿下模                
                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);    
                AddToTal(YWX_CTHL_WORK_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;
            //退带时
 
        }  
        YWX_CTHL_RunStepSave = YWX_CTHL_AutoStep;
         
    }       
        //退下模后启动送拉头
        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;
                
                SetEn(Y_AXIS, MOTOR_EN);
                YWX_CTHL_FZ_MotorStep = 0;
                
                bTuiLaTouOkFlg = 1;
                bZhuangLiaoOkFlg= 0;
                
                YWX_CTHL_TryCnt = 0;
                bCL_EN_FLG = 0;
                YWX_CTHL_cZipCnt = 0;
                YWX_CTHL_LianLengthCheck = 0;
                YWX_CTHL_LianLengthCheckOld = 0;
                cStopInCnt = 0;
                YWX_CTHL_bFZ_LengthOK = 0;
                YWX_CTHL_bHDinWeiOk = 0;
                SetAlarmCode(YWX_CTHL_ALARM_ADDR,0); 

                YWX_CTHL_AutoDelay = dwTickCount + 0;
                YWX_CTHL_TD_MotorStep = 0;
                if(GetEn(X_AXIS) == MOTOR_EN)   //电机不能松掉
                {
                    if(cDinWeiStep == 1)  //前拖带定位
                    {
                        YWX_CTHL_XKDW_VAVLE  =0;       
                        YWX_CTHL_DXK_VAVLE  =0;                
                         
                        YWX_CTHL_FKGD_VAVLE =0;         
                        
                        YWX_CTHL_FWCX_VAVLE = 0;
                        YWX_CTHL_BAIBI_VAVLE = 0;   //摆臂  
                        YWX_CTHL_HELT_VAVLE = 0;
                        YWX_CTHL_YADAI_VAVLE = 1;
                        YWX_CTHL_LYLun_VAVLE= 0;     //Y15方块压轮
                        YWX_CTHL_RYLun_VAVLE = 0;       //Y16插销压轮
                         YWX_CTHL_MGuo_VAVLE = 1;   //判断是否已经有料
                        YWX_CTHL_ZhuangLiaoDelay = dwTickCount + 150;
                        YWX_CTHL_ZhuangLiaoStep=1;
                        if(YWX_CTHL_bQDinWeiOK)   //前拖带已经完成
                        {//开始输出下模, 斜码处穿入
                            YWX_CTHL_AutoStep = 5;  //对应的步                            
                        }  
                        else
                        {  
                           YWX_CTHL_CTXM_VAVLE =0;   
                            YWX_CTHL_AutoStep = 4;
                            if(!YWX_CTHL_bFirstFKOK)   //说明方块已经过了一个 ,直接只过一次就可以
                            {
                                YWX_CTHL_TD_MotorStep = 1;
                            }  
                        }
                    }
                    else //if(cDinWeiStep == 2)  //后拖带定位阶段
                    { 
                        
                        YWX_CTHL_CTXM_VAVLE = 0;
                        YWX_CTHL_MGuo_VAVLE = 0;
                        YWX_CTHL_AutoStep = 50;
                        SetEn(X_AXIS, MOTOR_EN); 
                        YWX_CTHL_AutoDelay = dwTickCount + 1500;
                    }
                    
                }
                else               
                {
                    SetEn(X_AXIS, MOTOR_EN);                  
                    YWX_CTHL_AutoStep = 1;
                      YWX_CTHL_MGuo_VAVLE = 1;   //判断是否已经有料
                        YWX_CTHL_ZhuangLiaoDelay = dwTickCount + 150;
                        YWX_CTHL_ZhuangLiaoStep=1;
                }                
             }     
             SetEn(X_AXIS, MOTOR_EN);
        }  
        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;
                if(YWX_CTHL_TD_MotorStep != 16)
                    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);
            cDinWeiStep = 0;
//            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;
            cDinWeiStep = 0;
            YWX_CTHL_bFirstFKOK = 0;
            YWX_CTHL_bQDinWeiOK = 0;
            YWX_CTHL_LianLengthCheck = 0;
            YWX_CTHL_LianLengthCheckOld = 0;
            
        }
        bStop=0;
    }
     
   if(bAlarmStop)
    {   
      bRunning   = 0;
      bAlarmStop=0;
      cStopMode=0;
      cXcxcrCnt=0;
      if(YWX_CTHL_TD_MotorStep != 16)
      {
            AxisEgmStop(X_AXIS);
            YWX_CTHL_TD_MotorStep = 0;
      }      
      AxisEgmStop(Y_AXIS);
      YWX_CTHL_AutoStep = 0; 
      YWX_CTHL_FZ_MotorStep =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);
                 AxisContinueMoveAcc(Y_AXIS,5000,DIR_N,1500,600,15,15);
                 if(bRunning)
                 {
                    cDinWeiStep = 1;
                 }
                 YWX_CTHL_bQDinWeiOK = 0;
            }  
            break;
        case 2://检测到过链
            if(dwXRealPos > 100)    //穿入2CM后退
            {
                YWX_CTHL_FWCX_VAVLE = 0;   //复位插销              
            } 
            
            if((dwXRealPos >= YWX_CTHL_PARAM_WLCDSHEZHI_LENTH) && bRunning && (YWX_CTHL_cZipCnt!= 0))
            {
                YWX_CTHL_SetAlarmCode(YWX_CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
                bAlarmStop = 1;
            }
            
            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 ,200,
                                          1000,YWX_CTHL_PARAM_FKDW_Speed,10,10,0);  
                
                AxisContinueMoveAcc(Y_AXIS,YWX_CTHL_PARAM_FKDW_Speed*2/3,DIR_N,1500,600,15,15);
            }
            break;
        case 3:               
             if(!YWX_CTHL_FKDW_IN)
            {              
                AxisMovePosAccDec(X_AXIS,YWX_CTHL_PARAM_FKDW_Speed,YWX_CTHL_PARAM_FKDW_Length +25,
                                  YWX_CTHL_PARAM_FKDW_Speed,YWX_CTHL_PARAM_FKDW_Speed,10,5,10); 
                YWX_CTHL_TD_MotorStep = 4;  
                YWX_CTHL_XSavePosBuff = dwXRealPos + YWX_CTHL_PARAM_FKDW_Length + 25;
            }
            if((dwXRealPos >= YWX_CTHL_PARAM_WLCDSHEZHI_LENTH)&& bRunning && (YWX_CTHL_cZipCnt!= 0))
            {
                YWX_CTHL_SetAlarmCode(YWX_CTHL_XKSC_Limit_ALARM,bRunning);   //X03斜口上止到位异常   
                bAlarmStop = 1;
            }
             break;
        case 4:
            if((!X_DRV) || (dwXRealPos >= YWX_CTHL_XSavePosBuff))
            {
                YWX_CTHL_TD_MotorStep = 0;  
                AxisEgmStop(X_AXIS);
                AxisEgmStop(Y_AXIS);
                if(bRunning)
                {
                    YWX_CTHL_bQDinWeiOK = 1;   
                    YWX_CTHL_bFirstFKOK = 0;
                }
            }
            break;
            //穿拉头定位
        case 10:   
            if(dwTickCount >= YWX_CTHL_TD_MotorDelay)
            {
                YWX_CTHL_TD_MotorStep = 11;
                SetPos(X_AXIS, 0);  //启动位置设为0点
                dwXRealPos = 0;
                AxisContinueMoveAcc(X_AXIS,3000,DIR_P,1000,1200,15,5); 
                AxisContinueMoveAcc(Y_AXIS,2800,DIR_N,800,1000,15,15);
                YWX_CTHL_CheckInX02PosBuff= 0;
                bCheckEN_FLG = 1;
                bHeLaTouOK = 1;
                YWX_CTHL_XSavePosBuff = dwXRealPos;
                
                 YWX_CTHL_FWCX_VAVLE = 0;   //复位插销  
                YWX_CTHL_TD_MotorDelay = dwTickCount + 4; 
                cDinWeiStep = 1;
                YWX_CTHL_bFirstFKOK = 0;
                YWX_CTHL_bQDinWeiOK = 0;
            }
            break;
        case 11:           
            if(YWX_CTHL_CTM_Limit_IN)
                YWX_CTHL_TD_MotorDelay = dwTickCount + 4; 
            if(dwXRealPos > 150)
                YWX_CTHL_HELT_VAVLE = 1;  
            if(dwXRealPos > 350) 
            {   
                YWX_CTHL_HELT_VAVLE = 1;  
                if(!YWX_CTHL_CTM_Limit_IN && !YWX_CTHL_CTXM_VAVLE)    //穿入2CM后退
                {
                    YWX_CTHL_CheckInX02PosBuff= 0;
                    YWX_CTHL_FWCX_VAVLE = 0;   //复位插销 (重复) 
                    if(dwTickCount >= YWX_CTHL_TD_MotorDelay)  
                    {
                        YWX_CTHL_TD_MotorStep = 12;
                        cFKCheckCnt = 0;
                        YWX_CTHL_bFKCheck_Time = dwTickCount;
                        bHeLaTouOK = 1;
                        if(YWX_CTHL_cZipCnt > 1)
                        {                    
                            if(YWX_CTHL_LianLengthCheckOld == 0)
                            {   
                              YWX_CTHL_DataBuff = 6000;
                              
                           //     AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_TD_RunSpeed*3/2,YWX_CTHL_LianLengthCheck - dwXRealPos,
                             //                                3000,YWX_CTHL_PARAM_TD_RunSpeed,45,20,0); //YWX_CTHL_PARAM_TD_RunSpeed 
                              AxisMovePosAccDecNotStop(X_AXIS,6000,YWX_CTHL_LianLengthCheck - dwXRealPos,
                                                             3000,YWX_CTHL_PARAM_TD_RunSpeed,45,20,0); //YWX_CTHL_PARAM_TD_RunSpeed 
                            }
                            else if((YWX_CTHL_LianLengthCheck < 2000) && (YWX_CTHL_LianLengthCheckOld  <2000))          
                            {
                                YWX_CTHL_DataBuff = YWX_CTHL_PARAM_TD_RunSpeed;
                                AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_TD_RunSpeed,YWX_CTHL_LianLengthCheck + YWX_CTHL_LianLengthCheckOld - 450  - dwXRealPos,
                                                             3000,YWX_CTHL_PARAM_FKDW_Speed* 4/5,45,15,0); 
                            }
                            else if(YWX_CTHL_LianLengthCheck >= (YWX_CTHL_LianLengthCheckOld + YWX_CTHL_LianLengthCheckOld/7))
                            {
                             //   AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_TD_RunSpeed*3/2,YWX_CTHL_LianLengthCheck/2 +YWX_CTHL_LianLengthCheckOld/2 - dwXRealPos,
                             //                                3000,YWX_CTHL_PARAM_TD_RunSpeed,45,20,0); //YWX_CTHL_PARAM_TD_RunSpeed 
                            
                              YWX_CTHL_DataBuff = YWX_CTHL_PARAM_TD_RunSpeed*3/2;
                             AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_TD_RunSpeed*3/2,YWX_CTHL_LianLengthCheck * 2 - 450  - dwXRealPos,
                                                             3000,YWX_CTHL_PARAM_FKDW_Speed,45,15,0); //YWX_CTHL_PARAM_TD_RunSpeed 
                            }  
                            else
                            {
                             //   AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_TD_RunSpeed*3/2,YWX_CTHL_LianLengthCheck/2 +YWX_CTHL_LianLengthCheckOld/2 - dwXRealPos,
                             //                                3000,YWX_CTHL_PARAM_TD_RunSpeed,45,20,0); //YWX_CTHL_PARAM_TD_RunSpeed 
                             YWX_CTHL_DataBuff = YWX_CTHL_PARAM_TD_RunSpeed*3/2;
                             AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_TD_RunSpeed*3/2,YWX_CTHL_LianLengthCheck + YWX_CTHL_LianLengthCheckOld - 450  - dwXRealPos,
                                                             3000,YWX_CTHL_PARAM_FKDW_Speed,45,15,0); //YWX_CTHL_PARAM_TD_RunSpeed 
                            }   
                        }
                        else
                        {  
                            YWX_CTHL_DataBuff = 6000;
                            AxisMovePosAccDecNotStop(X_AXIS,6000,YWX_CTHL_LianLengthCheck - dwXRealPos,
                                                         3000,YWX_CTHL_PARAM_TD_RunSpeed/2,25,20,0); //YWX_CTHL_PARAM_TD_RunSpeed 
                        }    
                        
                        AxisContinueMoveAcc(Y_AXIS,YWX_CTHL_DataBuff*4/5,DIR_N,1500,800,15,15);
                   }
                }        
            }
            //已经完成两条长度
            if(YWX_CTHL_LianLengthCheck && YWX_CTHL_LianLengthCheckOld)
            {                
              if(YWX_CTHL_LianLengthCheck >= YWX_CTHL_LianLengthCheckOld)                
                YWX_CTHL_LianLengthBuff = YWX_CTHL_LianLengthCheck * 2 + 5000;
              else 
                    YWX_CTHL_LianLengthBuff = YWX_CTHL_LianLengthCheck + YWX_CTHL_LianLengthCheckOld + 5000;
                //下模没法下去的情况下
                if(dwXRealPos>= (YWX_CTHL_LianLengthBuff))
                {
                    //没链自动
                    bRunning = 0;
                    bAlarmStop = 1;
                    YWX_CTHL_TD_MotorStep= 0;
                    //
                    SetAlarmCode(YWX_CTHL_ALARM_ADDR,YWX_CTHL_WLZDTJ_ALARM); 
                }
            }
            
            break;
        case 12://检测到过链         
 //                    
            if(YWX_CTHL_cZipCnt)
            { //保障到5公分后退合链
                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;     
                YWX_CTHL_bFirstFKOK = 1;
            }
           //已经完成两条长度
            if(YWX_CTHL_LianLengthCheck && YWX_CTHL_LianLengthCheckOld)
            {                
                if(YWX_CTHL_LianLengthCheck >= YWX_CTHL_LianLengthCheckOld)                
                    YWX_CTHL_LianLengthBuff = YWX_CTHL_LianLengthCheck * 2 + 5000;
                else 
                    YWX_CTHL_LianLengthBuff = YWX_CTHL_LianLengthCheck + YWX_CTHL_LianLengthCheckOld + 5000;
                //下模没法下去的情况下
                if(dwXRealPos>= (YWX_CTHL_LianLengthBuff))
                {
                    //没链自动
                    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 + 200)))
            {
                YWX_CTHL_TD_MotorStep = 14;
 //               AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_TD_RunSpeed ,500,3000,YWX_CTHL_PARAM_TD_RunSpeed,10,3,0);
            } 
            //已经完成两条长度
            if(YWX_CTHL_LianLengthCheck && YWX_CTHL_LianLengthCheckOld)
            {                
                if(YWX_CTHL_LianLengthCheck >= YWX_CTHL_LianLengthCheckOld)                
                    YWX_CTHL_LianLengthBuff = YWX_CTHL_LianLengthCheck * 2 + 5000;
                else 
                    YWX_CTHL_LianLengthBuff = YWX_CTHL_LianLengthCheck + YWX_CTHL_LianLengthCheckOld + 5000;
                //下模没法下去的情况下
                if(dwXRealPos>= (YWX_CTHL_LianLengthBuff))
                {   
                    //没链自动
                    bRunning = 0;
                    bAlarmStop = 1;
                    YWX_CTHL_TD_MotorStep= 0;
                    //
                    SetAlarmCode(YWX_CTHL_ALARM_ADDR,YWX_CTHL_WLZDTJ_ALARM); 
                }
            }
            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(YWX_CTHL_LianLengthCheckOld >= YWX_CTHL_LianLengthCheck)
            {
                //说明下一条没有方块
                if((dwXRealPos >= (YWX_CTHL_XSavePosBuff + YWX_CTHL_LianLengthCheckOld + 1500)) && (YWX_CTHL_LianLengthCheck >1500))
                {
                    YWX_CTHL_TD_MotorStep = 12; 
                }  
            } 
            else
            {  
                //说明下一条没有方块
                if((dwXRealPos >= (YWX_CTHL_XSavePosBuff + YWX_CTHL_LianLengthCheck + 1500)) && (YWX_CTHL_LianLengthCheck >1700))
                {
                    YWX_CTHL_TD_MotorStep = 12; 
                }  
            }
            if((!YWX_CTHL_FK_Check) && (dwXRealPos >= (YWX_CTHL_XSavePosBuff + 500))) //
            {              
                YWX_CTHL_TD_MotorStep = 15;  
                YWX_CTHL_XSavePosBuff1 = dwXRealPos + 200;  
                if(YWX_CTHL_cZipCnt > 2)
                {            
                    if((dwXRealPos + 1000 - YWX_CTHL_XSavePosBuff) < (YWX_CTHL_LianLengthCheck))
                    {
                        YWX_CTHL_cZipCnt = 0;
                        YWX_CTHL_TD_MotorStep = 50;   
                        YWX_CTHL_LianLengthCheck = 0;
                        YWX_CTHL_LianLengthCheckOld = 0;
                        AxisContinueMoveAcc(X_AXIS,2000,DIR_P,1000,600,15,15);
                        AxisContinueMoveAcc(Y_AXIS,1600,DIR_N,1000,600,15,15);
  //                      AxisEgmStop(X_AXIS);
 //                       AxisEgmStop(Y_AXIS);
                        YWX_CTHL_TD_MotorDelay = dwTickCount + 200;
                        YWX_CTHL_bFZ_LengthOK= 0;
                        return;
                        break;
                    }
                }  
        //        if(GetCurSpeed(X_AXIS) <= YWX_CTHL_PARAM_TD_RunSpeed)
                  //  AxisMovePosAccDecNotStop(X_AXIS,2500 ,250,3000,YWX_CTHL_PARAM_FKDW_Speed,10,3,0);
                  AxisContinueMoveAcc(Y_AXIS,YWX_CTHL_PARAM_FKDW_Speed,DIR_P,1500,YWX_CTHL_PARAM_FKDW_Speed,15,2);
        //        else
        //            AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_FKDW_Speed,5,1500,YWX_CTHL_PARAM_FKDW_Speed,10,0,0);
                AxisContinueMoveAcc(Y_AXIS,YWX_CTHL_PARAM_FKDW_Speed* 3/4,DIR_N,1500,800,15,5);
            }
 //已经完成两条长度
            if(YWX_CTHL_LianLengthCheck && YWX_CTHL_LianLengthCheckOld)
            {                
                if(YWX_CTHL_LianLengthCheck >= YWX_CTHL_LianLengthCheckOld)                
                    YWX_CTHL_LianLengthBuff = YWX_CTHL_LianLengthCheck * 2 + 5000;
                else 
                    YWX_CTHL_LianLengthBuff = YWX_CTHL_LianLengthCheck + YWX_CTHL_LianLengthCheckOld + 5000;
                //下模没法下去的情况下
                if(dwXRealPos>= (YWX_CTHL_LianLengthBuff))
                {
                    //没链自动
                    bRunning = 0;
                    bAlarmStop = 1;
                    YWX_CTHL_TD_MotorStep= 0;
                    //
                    SetAlarmCode(YWX_CTHL_ALARM_ADDR,YWX_CTHL_WLZDTJ_ALARM); 
                }
            }
            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;
               
               if(((dwXRealPos - YWX_CTHL_XSavePosBuff) > (YWX_CTHL_LianLengthCheck + YWX_CTHL_LianLengthCheck/7)) 
                  || ((dwXRealPos - YWX_CTHL_XSavePosBuff) < (YWX_CTHL_LianLengthCheck - YWX_CTHL_LianLengthCheck/7))
                    || (YWX_CTHL_LianLengthCheckOld == 0)
                 ||((dwXRealPos - YWX_CTHL_XSavePosBuff) > (YWX_CTHL_LianLengthCheckOld + YWX_CTHL_LianLengthCheckOld/7)) 
                  || ((dwXRealPos - YWX_CTHL_XSavePosBuff) < (YWX_CTHL_LianLengthCheckOld - YWX_CTHL_LianLengthCheckOld/7))
                         )
               {  
                   YWX_CTHL_LianLengthCheckOld = YWX_CTHL_LianLengthCheck;
                   YWX_CTHL_LianLengthCheck = dwXRealPos - YWX_CTHL_XSavePosBuff;                
                }
               YWX_CTHL_XSavePosBuff = dwXRealPos + YWX_CTHL_PARAM_FKDW_Length;
            }            
            //已经完成两条长度
            if(YWX_CTHL_LianLengthCheck && YWX_CTHL_LianLengthCheckOld)
            {                
                if(YWX_CTHL_LianLengthCheck >= YWX_CTHL_LianLengthCheckOld)                
                    YWX_CTHL_LianLengthBuff = YWX_CTHL_LianLengthCheck * 2 + 5000;
                else 
                    YWX_CTHL_LianLengthBuff = YWX_CTHL_LianLengthCheck + YWX_CTHL_LianLengthCheckOld + 5000;
                //下模没法下去的情况下
                if(dwXRealPos>= (YWX_CTHL_LianLengthBuff))
                {
                    //没链自动
                    bRunning = 0;
                    bAlarmStop = 1;
                    YWX_CTHL_TD_MotorStep= 0;
                    //
                    SetAlarmCode(YWX_CTHL_ALARM_ADDR,YWX_CTHL_WLZDTJ_ALARM); 
                }
            }
             break;
        case 16:
            if((!X_DRV) || (dwXRealPos >= YWX_CTHL_XSavePosBuff))
            {
              user_datas[124] = dwXRealPos;
                YWX_CTHL_TD_MotorStep = 0;  

                YWX_CTHL_bQDinWeiOK = 1;
                AxisEgmStop(X_AXIS);
                AxisEgmStop(Y_AXIS);
            }
            break;       
            
            
          case 50: 
            if(YWX_CTHL_FKDW_IN_UP)
            {
                     AxisEgmStop(X_AXIS);
                     AxisEgmStop(Y_AXIS);
                    YWX_CTHL_cZipCnt = 0;
                         
                        YWX_CTHL_LianLengthCheck = 0;
                        YWX_CTHL_LianLengthCheckOld = 0;
 
                      YWX_CTHL_TD_MotorStep = 51;  
                        YWX_CTHL_TD_MotorDelay = dwTickCount + 50;
                        YWX_CTHL_bFZ_LengthOK= 0;
                  
            }
            break;
        case 51:
            if(dwTickCount >= YWX_CTHL_TD_MotorDelay)//YWX_CTHL_bTiaoShiStep)
            {
              bStart = 1;
              bRunning = 0;
              
                YWX_CTHL_bTiaoShiStep = 1;
                YWX_CTHL_TD_MotorStep = 0;  
                YWX_CTHL_TD_MotorDelay = dwTickCount + 000;
                YWX_CTHL_TryCnt = 0;
                bCL_EN_FLG = 0;
                YWX_CTHL_cZipCnt = 0;
                YWX_CTHL_LianLengthCheck = 0;
                YWX_CTHL_LianLengthCheckOld = 0;
                YWX_CTHL_bFZ_LengthOK = 0;
                YWX_CTHL_bHDinWeiOk = 0;
            }
            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点
          //      SetDirReverse(X_AXIS, 0);
                if(YWX_CTHL_bFZ_LengthOK)// && (YWX_CTHL_LianLengthCheck && YWX_CTHL_LianLengthCheckOld))   //确定当条拉链长度已经明确
                {
        //            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);   //必须要保证拖带轮的线速度比后退轮的线速度大
                    if(YWX_CTHL_LianLengthCheck >= 280)
                    {   
                        if(YWX_CTHL_cZipCnt < 3)
                            YWX_CTHL_LENTH = YWX_CTHL_LianLengthCheck - 350;
                        else
                        {
                          if((YWX_CTHL_LianLengthCheck >= (YWX_CTHL_LianLengthCheckOld + 280)) && YWX_CTHL_LianLengthCheckOld)
                            YWX_CTHL_LENTH = YWX_CTHL_LianLengthCheckOld - 50;
                          else
                            YWX_CTHL_LENTH = YWX_CTHL_LianLengthCheck - 150;                         
                        }  
                    }    
                    else
                        YWX_CTHL_LENTH = YWX_CTHL_LianLengthCheck*2/3;
                   
                    AxisMovePosAccDecNotStop(Y_AXIS,(YWX_CTHL_PARAM_FZ_Speed + YWX_CTHL_PARAM_FZ_Speed*YWX_CTHL_PARAM_SDXZ/1000),YWX_CTHL_LENTH, 
                                                2000,920,15,15,00);
                    AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_FZ_Speed,-(YWX_CTHL_LENTH - YWX_CTHL_LENTH*YWX_CTHL_PARAM_SDXZ/1000),
                                                 1950,920-920*YWX_CTHL_PARAM_SDXZ/1000,15,15,50); 
                }
                else   //在未知长度情况下
                {
                    AxisMovePosAccDecNotStop(Y_AXIS,(YWX_CTHL_PARAM_FZ_Speed + YWX_CTHL_PARAM_FZ_Speed*YWX_CTHL_PARAM_SDXZ/1000),1000, 
                                                2000,1250,15,15,00);
                    AxisMovePosAccDecNotStop(X_AXIS,YWX_CTHL_PARAM_FZ_Speed,-980,
                                                1950,1250-1250*YWX_CTHL_PARAM_SDXZ/1000,15,15,00); 
                }   
                YWX_CTHL_FZ_MotorStep = 2; 

            }
            break;
        case 2:
             if(dwYRealPos > 350)
                YWX_CTHL_DXK_VAVLE = 0;      //Y01顶斜口电磁阀
             
             if(YWX_CTHL_bFZ_LengthOK)
             {
                if(((dwYRealPos + 800) > YWX_CTHL_LianLengthCheck) && YWX_CTHL_LianLengthCheck)
                {
                   YWX_CTHL_FZ_MotorStep = 3; 
                }
             }
             else if((dwYRealPos > 100) && YWX_CTHL_GQ_IN)
             {
                AxisEgmStop(Y_AXIS);
                AxisEgmStop(X_AXIS); 
                YWX_CTHL_FZ_MotorStep = 0; 
//                YWX_CTHL_LianLengthCheckOld = YWX_CTHL_LianLengthCheck;   
                    if(YWX_CTHL_cZipCnt < 2)
                        YWX_CTHL_LianLengthCheck = dwYRealPos;
                    cDinWeiStep = 2;    
              } 
             
             //方块检测异常(光纤)
             if((dwYRealPos >= (YWX_CTHL_LianLengthCheck + 800)) && YWX_CTHL_bFZ_LengthOK && YWX_CTHL_LianLengthCheck)
             {
                AxisEgmStop(X_AXIS);
                AxisEgmStop(Y_AXIS);                 
                //修正
                YWX_CTHL_SetAlarmCode(YWX_CTHL_GQ_IN_ALARM,1);
             }
            break; 
        case 3: 
             if(YWX_CTHL_GQ_IN)
             {  
                  AxisEgmStop(X_AXIS);
                  AxisEgmStop(Y_AXIS);
                  YWX_CTHL_FZ_MotorStep = 0; 
                  if(YWX_CTHL_cZipCnt < 2)
                    YWX_CTHL_LianLengthCheck = dwYRealPos;
                  cDinWeiStep = 2;   

            }
            //方块检测异常(光纤)
            if((dwYRealPos >= (YWX_CTHL_LianLengthCheck + 800)) && YWX_CTHL_bFZ_LengthOK && YWX_CTHL_LianLengthCheck)
            {
                AxisEgmStop(Y_AXIS);
                AxisEgmStop(X_AXIS); 
                //修正
                YWX_CTHL_SetAlarmCode(YWX_CTHL_GQ_IN_ALARM,1);
            }
            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,3080,-(YWX_CTHL_PARAM_CXDW_Length + 100),800,800,6,6,0); 
            }
            YWX_CTHL_FZ_MotorStep = 22; 
            break;
        case 22: 
            if(!Y_DRV && !X_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 + 10;
            }
            else if(dwTickCount >= YWX_CTHL_ZhuangLiaoDelay)
            {
                YWX_CTHL_SetAlarmCode(YWX_CTHL_HSLiao_Limit_ALARM,bRunning);
                YWX_CTHL_ZhuangLiaoStep = 0;
                    YWX_CTHL_HSLiao_VAVLE = 0; 
            }
            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);
                 YWX_CTHL_HSLiao_VAVLE = 0; 
                 YWX_CTHL_JLTou_VAVLE = 0; 
                 YWX_CTHL_ZhuangLiaoStep = 0;
            }
            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(YWX_CTHL_bZDP)
       {
            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;
            }
       }     
       else
       {
           YWX_CTHL_ZhenDongPian_OUT = 0; 
       }   
#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 0       
        //测试电机
        if(YWX_CTHL_bCLMotor_N)
        {           
            SetEn(Y_AXIS, MOTOR_EN);
            SetEn(X_AXIS, MOTOR_EN);
                if(!Y_DRV)
                {
                  //                Y轴       运行速度                    启动速度        加速度  减速度
                   AxisContinueMoveAcc(Y_AXIS,2000,DIR_N,1000,1000,15,15);    
                    AxisContinueMoveAcc(X_AXIS,2000,DIR_P,1000,1000,15,15);
                }
        }
        
        if(YWX_CTHL_bCLMotor_P)  //后退限位已经取消
        {
            SetEn(Y_AXIS, MOTOR_EN);
            SetEn(X_AXIS, MOTOR_EN);
           if(!Y_DRV)
           {  
              //                Y轴       运行速度                    启动速度        加速度  减速度
              AxisContinueMoveAcc(Y_AXIS,2000,DIR_P,1000,1000,15,15);
              AxisContinueMoveAcc(X_AXIS,2000,DIR_N,1000,1000,15,15);
           }  
        }
        
        if(!YWX_CTHL_bCLMotor_P && !YWX_CTHL_bCLMotor_N && (YWX_CTHL_FZ_MotorStep == 0))
        {
              AxisEgmStop(Y_AXIS);
              AxisEgmStop(X_AXIS);
        }
#endif        
        //合链电机返回原点
        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_FKGD_VAVLE  = 0; //Y00方块固定   
                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_HELT_VAVLE  = 0;       //Y05合拉头
                YWX_CTHL_YADAI_VAVLE  = 0;       //Y06压带
                YWX_CTHL_XKDW_VAVLE = 0;     //斜口定位
                YWX_CTHL_MGuo_VAVLE = 0;      //Y12码勾    
                YWX_CTHL_CTXM_VAVLE = 0;     //Y13穿头下模
                YWX_CTHL_LYLun_VAVLE= 0;     //Y15方块压轮
                YWX_CTHL_RYLun_VAVLE = 0;       //Y16插销压轮

                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