GC23005/Machine/YingXingMachine/YXTWOChuantou_HeLian.c

#include "global.h"

#if  FJ_YING_XING_MACHINE  ==  1

void YX_CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag);
void YX_CTHL_ManualAction(void);
void YX_CTHL_AutoStepAction(void);
void YX_CTHL_TD_Motor(void);
void YX_CTHL_FZ_Motor(void);
void YX_CTHL_CheckStart(void);
void YX_CTHL_TLiao_Step(void);  //推料(推拉头)
void YX_CTHL_AlarmProtect(void);
void YX_CTHL_ZLT_Step(void);
void YX_CTHL_ZhenDongAction(void);
void YX_CTHL_Color_Action(void);
/*
void YXExtiAcitionX02(void)
{
    if(bCheckEN_FLG)
    {  
        bCheckEN_FLG = 0;
        YX_CTHL_CheckInX02PosBuff = GetPos(X_AXIS);
    }    
}
*/


void YX_CTHL_Color_Action(void)
{
  if(bRunning && YX_CTHL_Colorcode_IN_UP)
  {
    YX_CTHL_ColorStep=1; 
  }
   if(bRunning && YX_CTHL_XIAOCHA_IN_DW)
  {
     YX_CTHL_SetAlarmCode(YX_CTHL_XIAOCHA_ALARM,bRunning);
  }
  if(YX_CTHL_PARAM_COLOR_CODE)
   {
   switch(YX_CTHL_ColorStep)
   {
     
     case 1:
       
       {
             bRunning=0;
                cStopMode=0; 
                bSingOneFlg=0;
                YX_CTHL_wltzi=0;
                YX_CTHL_MOTOR_MODE=0;
                AxisEgmStop(X_AXIS);
                AxisEgmStop(Y_AXIS);
     
      //        SetEn(X_AXIS, MOTOR_DISEN);
      //        SetEn(Y_AXIS, MOTOR_DISEN);
  
                YX_CTHL_AutoStep = 0; 
                YX_CTHL_stop=0;
                YX_CTHL_FZ_MotorStep =0;   
                YX_CTHL_TD_MotorStep =0;    
                YX_CTHL_ZhuangLiaoStep=0;  
                YX_CTHL_TLiao_VAVLE=0;             
              YX_CTHL_HSLiao_VAVLE  =0;    
              YX_CTHL_JLTou_VAVLE  =0;                             
              YX_CTHL_CTXM_VAVLE =0;           
              YX_CTHL_FKGD_VAVLE =0;         
            YX_CTHL_YADAI_VAVLE = 0;     
              YX_CTHL_LYLun_VAVLE = 0;   //
              YX_CTHL_RYLun_VAVLE = 0;   //Y16插销压轮
              YX_CTHL_MGuo_VAVLE = 0;
              YX_CTHL_CXC_VAVLE = 0;   
              YX_CTHL_GDQM_VAVLE =0;
              YX_CTHL_TQM_VAVLE=0;
              SetPos(X_AXIS, 0); 
              SetPos(Y_AXIS, 0); 
              SetEn(X_AXIS, MOTOR_EN);
              SetEn(Y_AXIS, MOTOR_EN);
     
        }
          YX_CTHL_ColorStep=2; 
         YX_CTHL_bColor_Delay=dwTickCount+500;
              
       break;
     case 2:
       if(dwTickCount >=  YX_CTHL_bColor_Delay)
       {
         if(YX_CTHL_AUTO_run_LENGTH)
         {
           AxisMovePosAccDec(X_AXIS,3000,YX_CTHL_AUTO_run_LENGTH,800,600,20,20,0);  
           YX_CTHL_ColorStep=3;
            
         }
         else
         {
          YX_CTHL_SetAlarmCode(YX_CTHL_COLOR_IN_ALARM,bRunning);
         }
       
       }
       break;
     case 3:
      
       if(!X_DRV)
       {
         YX_CTHL_bYJDW=1;
         YX_CTHL_ColorStep=4;
         YX_CTHL_bColor_Delay=dwTickCount+20;
       
       } 

       break;
     case 4:
       if(dwTickCount >=  YX_CTHL_bColor_Delay)
       {
         if(YX_CTHL_TD_MotorStep==0)
         {
           bStart=1;
           YX_CTHL_ColorStep=0;
         }
       }
       break;

 }

 }
 else
 {
 YX_CTHL_ColorStep=0;
 }

}
//故障报警
void YX_CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag)
{
  if(!YX_CTHL_cONERROR)
  {
    SetAlarmCode(YX_CTHL_ALARM_ADDR,alarm_code);
    YX_CTHL_cONERROR=1;
}
    if(!YX_CTHL_wltzi)
      bAlarmStop = 1;
    else
    {
    
       YX_CTHL_AutoStep = 0; 
    }
      
 
}

//初始化动作
void YX_CTHL_InitAction(void)
{
    float buff_pulse,buff_dist;
    
    buff_pulse  = YX_CTHL_PARAM_TDCYCLE_PULSE;
    buff_dist   = YX_CTHL_PARAM_TDCYCLE_LENGTH; 
    
    XGearRatio  =  buff_pulse/buff_dist; 
    
    buff_pulse  = YX_CTHL_PARAM_TDCYCLE_PULSE;
    buff_dist   =  YX_CTHL_PARAM_FZCYCLE_LENGTH;   
    YGearRatio  =  buff_pulse/buff_dist;
    
    buff_pulse  = YX_CTHL_PARAM_TDCYCLE_PULSE;
    buff_dist   = YX_CTHL_PARAM_FZLDCYCLE_LENGTH;
    ZGearRatio  =  buff_pulse/buff_dist;
    
    SetAlarmCode(YX_CTHL_ALARM_ADDR,0); 
   

    user_datas[121] = 0;
    user_datas[122] = 0;
    user_datas[123] = 0;
    
    SetEnReverse(X_AXIS, 0);
    SetEnReverse(Y_AXIS, 0);
    SetEnReverse(Z_AXIS, 0);
    
    SetDirReverse(X_AXIS, 0);
    SetDirReverse(Y_AXIS, 0);
    SetDirReverse(Z_AXIS, 0);
    
    SetEn(X_AXIS, MOTOR_DISEN);
    SetEn(Y_AXIS, MOTOR_DISEN);
    SetEn(Z_AXIS, MOTOR_DISEN);
    SetPos(X_AXIS, 0);  //启动位置设为0点    
    SetPos(Y_AXIS, 0);  //启动位置设为0点 
    SetPos(Z_AXIS, 0);  //启动位置设为0点 
}

void YX_CTHL_Action(void)
{ 

    dwXRealPos = GetPos(X_AXIS);
    dwYRealPos = GetPos(Y_AXIS);
    dwZRealPos = GetPos(Z_AXIS);  
    user_datas[121] = YX_CTHL_ColorStep;
    user_datas[122] = dwYRealPos;
    user_datas[123] = dwXRealPos;
    user_datas[124] = YX_CTHL_TLiaoStep;
    user_datas[125] = YX_CTHL_ZhuangLiaoStep;
    user_datas[126] = YX_CTHL_FZ_MotorStep;
    user_datas[127] = YX_CTHL_TD_MotorStep;
    user_datas[128] = YX_CTHL_AutoStep;  
    
    YX_CTHL_ZhenDongAction();
    YX_CTHL_AlarmProtect();
    YX_CTHL_CheckStart();    
    YX_CTHL_ZLT_Step();
    YX_CTHL_TLiao_Step();
     YX_CTHL_Color_Action();
    YX_CTHL_TD_Motor();
    YX_CTHL_FZ_Motor();
    YX_CTHL_ManualAction();
    YX_CTHL_AutoStepAction();  
    YX_CTHL_Test++;
}

void YX_CTHL_AlarmProtect(void)
{ 
    

 
}



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

    //测试用
    if(dwTickCount >= YX_CTHL_TestDelay)
    {
        YX_CTHL_TestDelay = dwTickCount + 200;
        YX_CTHL_TestLED = !YX_CTHL_TestLED;   
        YX_CTHL_Test = 0;
    }

    if(bRunning)
    {
        switch(YX_CTHL_AutoStep)
        {
         
                
            case 1:

                if(dwTickCount >= YX_CTHL_AutoDelay && YX_CTHL_TD_MotorStep==0 )
                {        
                   if(!YX_CTHL_bDanBu_MODE || YX_CTHL_START_IN_UP)
                   {
                    YX_CTHL_LYLun_VAVLE = 0;     
                     YX_CTHL_RYLun_VAVLE = 0;            
                     YX_CTHL_CXC_VAVLE=0;
                     YX_CTHL_GDQM_VAVLE=1;
                     YX_CTHL_TQM_VAVLE=0;
                    YX_CTHL_AutoStep = 2;
                   YX_CTHL_AutoDelay = dwTickCount + 10;
                    if(YX_CTHL_ZhuangLiaoStep == 0)
                    { 
                        YX_CTHL_ZhuangLiaoStep = 1;
                        YX_CTHL_TryCnt = 0;
                    }   
                   }
                }
                break;
            case 2: 
                if(bZhuangLiaoOkFlg && (YX_CTHL_ZhuangLiaoStep == 0))
                    {
                     if(dwTickCount >= YX_CTHL_AutoDelay && YX_CTHL_HSLiao_Origin_IN)
                     {
                       if(!YX_CTHL_LTou_Check  )
                       {
                          if(!YX_CTHL_bDanBu_MODE || YX_CTHL_START_IN_UP)
                          {
                          YX_CTHL_CTXM_VAVLE=1;
                          YX_CTHL_AutoStep = 3;
                       //    YX_CTHL_AutoDelay = dwTickCount + YX_CTHL_PARAM_GDQM_DELAY;  
                          }
                                                    
                       }
                      else
                      {
                      YX_CTHL_wltzi=1;
                        YX_CTHL_SetAlarmCode(YX_CTHL_ZLT_ALARM,bRunning);  // 没有拉头  
                      }
                     }
                    }
                 break;
             case 3:    
              //  if(dwTickCount >= YX_CTHL_AutoDelay)
                {
                  if(!YX_CTHL_bDanBu_MODE || YX_CTHL_START_IN_UP)
                  {
                 
                      YX_CTHL_AutoStep = 4;
                     YX_CTHL_AutoDelay = dwTickCount + YX_CTHL_VAVLE_ERROR_TIME;
                  }
                
                }
                
                break;
            case 4:
              if(YX_CTHL_CTM_Limit_IN)
              {
                 if(!YX_CTHL_bDanBu_MODE || YX_CTHL_START_IN_UP)
                 {
                      YX_CTHL_GDQM_VAVLE=1;
                      YX_CTHL_JLTou_VAVLE=0;
                 if(!YX_CTHL_PARAM_QM_MODE)
                 {
                      YX_CTHL_AutoStep = 5;
                        YX_CTHL_AutoDelay = dwTickCount + YX_CTHL_PARAM_QMFZ_DELAY;  
                 }
                 else
                 {
                    YX_CTHL_AutoStep = 6;
                      YX_CTHL_AutoDelay = dwTickCount + YX_CTHL_PARAM_TQM_DELAY;   
                 }
                 }
  
              }
              else if(dwTickCount >= YX_CTHL_AutoDelay)
                {                   
                    YX_CTHL_SetAlarmCode(YX_CTHL_CTM_Limit_ALARM,bRunning);  // 没有拉头  
                }  
                break;
                
        case 5:
          if(dwTickCount >= YX_CTHL_AutoDelay)
          {
                if(!YX_CTHL_bDanBu_MODE || YX_CTHL_START_IN_UP)
                {
                  AxisMovePosAccDec(X_AXIS,5000,-YX_CTHL_PARAM_DWFZ_LENGTH,800,800,20,20,0);
                 YX_CTHL_AutoStep = 6;
                  YX_CTHL_AutoDelay = dwTickCount + YX_CTHL_PARAM_TQM_DELAY;   
                }
          } 
         break;
            case 6:
               if(dwTickCount >= YX_CTHL_AutoDelay && !X_DRV)
                {
                   if(!YX_CTHL_bDanBu_MODE || YX_CTHL_START_IN_UP)
                   {
                    YX_CTHL_LYLun_VAVLE=1;
                    YX_CTHL_RYLun_VAVLE=1;
                    YX_CTHL_TQM_VAVLE=1;
                    YX_CTHL_AutoDelay = dwTickCount + YX_CTHL_VAVLE_ERROR_TIME;  //延时输出压轮
                    YX_CTHL_AutoStep = 7;
                   }
                }
              
                break;
          case 7:  
            if(YX_CTHL_TQM_Limit_IN)
            {
               if(!YX_CTHL_bDanBu_MODE || YX_CTHL_START_IN_UP)
               {
                if(!YX_CTHL_PARAM_QM_MODE)
                  YX_CTHL_GDQM_VAVLE=0;
                
                  YX_CTHL_FZ_MotorStep=1;
                  YX_CTHL_FZ_MotorDelay = dwTickCount + YX_CTHL_PARAM_GDQMTD_BACK_DELAY;  //延时反转
                  YX_CTHL_AutoStep =8;
               }
            }
            else if(dwTickCount >= YX_CTHL_AutoDelay)
            {
             YX_CTHL_SetAlarmCode(YX_CTHL_TQM_Limit_ALARM,bRunning);  // 没有拉头
            }
              
                  break;
          case 8:
             if(YX_CTHL_FZ_MotorStep == 0) 
             {
              if(!YX_CTHL_bDanBu_MODE || YX_CTHL_START_IN_UP)
              {
               YX_CTHL_RYLun_VAVLE=0;//大插压轮
                YX_CTHL_FZ_MotorStep=20;
                YX_CTHL_AutoStep =9;
               YX_CTHL_FZ_MotorDelay = dwTickCount + YX_CTHL_PARAM_XCDWTD_BACK_DELAY;  //小插定位延时拖带
              }
      
             }      
             break;
        case 9:

           if(YX_CTHL_FZ_MotorStep == 0) 
           {
              if(!YX_CTHL_bDanBu_MODE || YX_CTHL_START_IN_UP)
              {
                bZhuangLiaoOkFlg=0;
                 YX_CTHL_CXC_VAVLE=1;
                 YX_CTHL_LYLun_VAVLE=0;
                YX_CTHL_AutoDelay = dwTickCount + YX_CTHL_PARAM_TXCYL_BACK_DELAY; //延时退小插压轮
                 YX_CTHL_AutoStep =10;
              }
           }
          break;
    case 10:
      if(dwTickCount >= YX_CTHL_AutoDelay)
         {
           if(!YX_CTHL_bDanBu_MODE || YX_CTHL_START_IN_UP)
           {
            YX_CTHL_TD_MotorStep=10;
            YX_CTHL_TD_MotorDelay = dwTickCount + YX_CTHL_PARAM_YSHL_DELAY;  //延时合链 
            YX_CTHL_AutoStep =11;
            YX_CTHL_AutoDelay1 = dwTickCount + YX_CTHL_MOTOR_ALARM_TIME;
            YX_CTHL_AutoStep1=1;
           }
         }
         break;
      /*
      
        case 10:
          if(YX_CTHL_FK_Check_DW)
          {
           YX_CTHL_AutoStep =11;
            YX_CTHL_AutoDelay = dwTickCount + YX_CTHL_PARAM_GYH_DELAY; 
          }
          break;*/

        case 11:

       /*    if(dwTickCount >= YX_CTHL_AutoDelay)
           {
            YX_CTHL_FKGD_VAVLE=0; 
            YX_CTHL_CXC_VAVLE=0;
           }*/
 
            if(YX_CTHL_TD_MotorStep==0 && (!YX_CTHL_CTXM_VAVLE ))
            {   
                YX_CTHL_AutoStep = 2; 

                AddToTal(YX_CTHL_TOTAL_ADDR);    
               AddToTal(YX_CTHL_TRUST_ALARM_ADDR);  
               AddToTal( YX_CTHL_TEST_ALARM_ADDR);               
                CalProSP(YX_CTHL_SPEED_ADDR);
                YX_CTHL_cONERROR=0;
                if(YX_CTHL_cZipCnt<3)
                   YX_CTHL_cZipCnt++;
            

            }
            break;
         
    }       

        
}
        //退下模后启动送拉头
        switch(YX_CTHL_AutoStep1)
        {   
            case 1:
         
                if(YX_CTHL_CTM_Origin_IN)
                {
                    YX_CTHL_ZhuangLiaoStep = 1;
                    YX_CTHL_TryCnt = 0;
                    bZhuangLiaoOkFlg=0;
                    YX_CTHL_AutoStep1 = 0;
                    YX_CTHL_MGuo_VAVLE = 0;
                
                }
                /*
                else if(dwTickCount >= YX_CTHL_AutoDelay1) 
                {
                  //穿头模和一次穿入回位异常
                    if(!YX_CTHL_CTM_Origin_IN)
                      YX_CTHL_SetAlarmCode(YX_CTHL_CTM_Origin_ALARM,bRunning);                                   
                } */
                break;
                
        }  

}



void YX_CTHL_CheckStart(void)
{
  
  
/*
 if(YX_CTHL_SGY_START_IN)
 {
   YX_CTHL_MOTOR_MODE=1;
      SetEn(Z_AXIS, MOTOR_EN); 
     // SetPos(Z_AXIS, 0);  //启动位置设为0点 
     YX_CTHL_AutoDelay = dwTickCount + 100;
      if(!Z_DRV)
      {
        //                Y轴       运行速度                    启动速度        加速度  减速度
         AxisContinueMoveAcc(Z_AXIS,6000,DIR_P,1000,1000,15,15);    

     }
 }
 if(YX_CTHL_XGY_STOP_IN || (dwTickCount >= YX_CTHL_AutoDelay && YX_CTHL_MOTOR_MODE) )
 {
   YX_CTHL_MOTOR_MODE=0;
    AxisEgmStop(Z_AXIS);
 }
 */

#if 1  
  if(YX_CTHL_wltzi)
  {
  
   
     if(YX_CTHL_START_IN_UP || bStart || YX_CTHL_bSingOne )
       {
           YX_CTHL_wltzi=0;
     //      if(!YX_CTHL_CTM_Origin_IN) YX_CTHL_SetAlarmCode(YX_CTHL_CTM_Origin_ALARM,bRunning); 
       if(!YX_CTHL_FKJC)    
       {
           if(bRunning)
        {
          YX_CTHL_cONERROR=0;
             YX_CTHL_CTXM_VAVLE =0; 
                YX_CTHL_AutoStep = 1;
                YX_CTHL_MGuo_VAVLE = 1;   //判断是否已经有料
                YX_CTHL_ZhuangLiaoDelay = dwTickCount + 150;
                YX_CTHL_ZhuangLiaoStep=1;
          
        }
           
           }
       }
  
  
  
  }
       //启动    自动启动 单一自动	 			
    if((YX_CTHL_START_IN_UP || bStart || YX_CTHL_bSingOne ) && !YX_CTHL_wltzi)
    {       
   //   if(!YX_CTHL_CTM_Origin_IN) YX_CTHL_SetAlarmCode(YX_CTHL_CTM_Origin_ALARM,bRunning); 

        if(!bRunning)
        {

               bRunning = 1;
              if( YX_CTHL_bSingOne)
              {
                 bSingOneFlg=1;
              
              }
                SetEn(X_AXIS, MOTOR_EN);
                SetEn(Y_AXIS, MOTOR_EN);
                 SetEn(Z_AXIS, MOTOR_EN);
                YX_CTHL_FZ_MotorStep = 0;
                YX_CTHL_cONERROR=0;
                bTuiLaTouOkFlg = 1;
                bZhuangLiaoOkFlg= 0;    
                YX_CTHL_TryCnt = 0;
                bCL_EN_FLG = 0;
                YX_CTHL_MOTOR_MODE=0;
                YX_CTHL_stop=0;
                YX_CTHL_cZipCnt = 0;        
                SetAlarmCode(YX_CTHL_ALARM_ADDR,0); 
                YX_CTHL_AutoDelay = dwTickCount + 0;
                  YX_CTHL_CTXM_VAVLE =0;           
                  YX_CTHL_FKGD_VAVLE =0;         
                  YX_CTHL_YADAI_VAVLE = 0;     
                  YX_CTHL_LYLun_VAVLE = 0;   //
                  YX_CTHL_RYLun_VAVLE = 0;   //Y16插销压轮
                  YX_CTHL_CXC_VAVLE = 0;   
                //  YX_CTHL_GDQM_VAVLE =0;
                  YX_CTHL_TQM_VAVLE=0;   
              if(!YX_CTHL_GDQM_VAVLE)
              {
                YX_CTHL_TD_MotorStep = 1; 
                 YX_CTHL_TD_MotorDelay = dwTickCount + 300;
              }
              else
                YX_CTHL_TD_MotorStep=0;
              
                YX_CTHL_AutoStep = 1;
                YX_CTHL_MGuo_VAVLE = 1;   //判断是否已经有料
                YX_CTHL_ZhuangLiaoDelay = dwTickCount + 150;
                YX_CTHL_ZhuangLiaoStep=1;
                         
                  
           
        }  
        bStart = 0;
        YX_CTHL_bSingOne=0;
    }   
     if(YX_CTHL_ZhouWantwo)
    {
      YX_CTHL_ZhouWantwo=0;
        if(YX_CTHL_ZhouWanone)
        {
          YX_CTHL_ZhouWanone=0;
           bRunning=0;
          cStopMode=0; 
          AxisEgmStop(X_AXIS);
          AxisEgmStop(Y_AXIS);
         
        SetEn(X_AXIS, MOTOR_DISEN);
        SetEn(Y_AXIS, MOTOR_DISEN);
      
        bSingOneFlg=0;
          YX_CTHL_AutoStep = 0; 
          YX_CTHL_FZ_MotorStep =0;   
          YX_CTHL_TD_MotorStep =0;    
         
        }
    }
    
    //停止
    if(YX_CTHL_STOP_IN_UP || bStop)
    {	

        if(cStopMode < 2) 
            cStopMode++;
       if(cStopMode==1 && bRunning)
          YX_CTHL_ZhouWanone=1;
        if(bRunning && cStopMode == 2)
        {   
                bRunning=0;
                cStopMode=0; 
                bSingOneFlg=0;
                YX_CTHL_wltzi=0;
                YX_CTHL_MOTOR_MODE=0;
                AxisEgmStop(X_AXIS);
                AxisEgmStop(Y_AXIS);
                AxisEgmStop(Z_AXIS);
              SetEn(X_AXIS, MOTOR_DISEN);
              SetEn(Y_AXIS, MOTOR_DISEN);
              SetEn(Z_AXIS, MOTOR_DISEN);
                YX_CTHL_AutoStep = 0; 
                YX_CTHL_stop=0;
                YX_CTHL_FZ_MotorStep =0;   
                YX_CTHL_TD_MotorStep =0;    
                YX_CTHL_ZhuangLiaoStep=0;  
                YX_CTHL_TLiao_VAVLE=0;             
              YX_CTHL_HSLiao_VAVLE  =0;    
              YX_CTHL_JLTou_VAVLE  =0;                             
              YX_CTHL_CTXM_VAVLE =0;           
              YX_CTHL_FKGD_VAVLE =0;         
            YX_CTHL_YADAI_VAVLE = 0;     
              YX_CTHL_LYLun_VAVLE = 0;   //
              YX_CTHL_RYLun_VAVLE = 0;   //Y16插销压轮
              YX_CTHL_MGuo_VAVLE = 0;
              YX_CTHL_CXC_VAVLE = 0;   
              YX_CTHL_GDQM_VAVLE =0;
              YX_CTHL_TQM_VAVLE=0;
              SetPos(X_AXIS, 0); 
             SetPos(Y_AXIS, 0); 
            SetPos(Z_AXIS, 0); 
              SetAlarmCode(YX_CTHL_ALARM_ADDR,0);  
    
        }
        else if(!bRunning)
        {
            bRunning=0;
            cStopMode=0;
            bSingOneFlg=0;
             YX_CTHL_wltzi=0;
            YX_CTHL_stop=0;
            YX_CTHL_MOTOR_MODE=0;
            YX_CTHL_RunStepSave = 0;
            AxisEgmStop(X_AXIS);
            AxisEgmStop(Y_AXIS);
             AxisEgmStop(Z_AXIS);
            SetEn(X_AXIS, MOTOR_DISEN);
            SetEn(Y_AXIS, MOTOR_DISEN);
            SetEn(Z_AXIS, MOTOR_DISEN);
            YX_CTHL_TLiao_VAVLE=0;             
            YX_CTHL_HSLiao_VAVLE  =0;    
            YX_CTHL_JLTou_VAVLE  =0;                             
            YX_CTHL_CTXM_VAVLE =0;           
            YX_CTHL_FKGD_VAVLE =0;         
           YX_CTHL_YADAI_VAVLE = 0;     
            YX_CTHL_LYLun_VAVLE = 0;   //
            YX_CTHL_RYLun_VAVLE = 0;   //Y16插销压轮
            YX_CTHL_MGuo_VAVLE = 0;
            YX_CTHL_CXC_VAVLE = 0;   
            YX_CTHL_GDQM_VAVLE =0;
            YX_CTHL_TQM_VAVLE=0;
            
            YX_CTHL_AutoStep = 0;
            YX_CTHL_ZhuangLiaoStep=0;
            YX_CTHL_TLiaoStep=0;
            YX_CTHL_TD_MotorStep=0;
  
     
            YX_CTHL_SongLiaoStep   =0;     
            YX_CTHL_FZ_MotorStep =0;      
            YX_CTHL_AutoStep1  =0;
             
            SetAlarmCode(YX_CTHL_ALARM_ADDR,0);  
            SetPos(X_AXIS, 0); 
             SetPos(Y_AXIS, 0); 
            SetPos(Z_AXIS, 0); 
            YX_CTHL_TLiaoStep = 0; 
            YX_CTHL_AutoStep = 0;
         
       
            YX_CTHL_bQDinWeiOK = 0;
            YX_CTHL_LianLengthCheck = 0;
            YX_CTHL_LianLengthCheckOld = 0;
            
        }
        bStop=0;
    }
     
   if(bAlarmStop)
    {   
      bRunning   = 0;
      bAlarmStop=0;
      cStopMode=0;
      cXcxcrCnt=0;
       AxisEgmStop(X_AXIS);
      AxisEgmStop(Y_AXIS);
       AxisEgmStop(Z_AXIS);
       YX_CTHL_AutoStep = 0; 
       YX_CTHL_FZ_MotorStep =0;   
          YX_CTHL_TD_MotorStep =0;   
    }
#endif    
}   

//拖带电机控制动作  X轴
void YX_CTHL_TD_Motor(void)	 //
{   
#if 1
     switch(YX_CTHL_TD_MotorStep)
	{
        //1步开始，纯定位, 没有穿入
        case 1:
            if(dwTickCount >= YX_CTHL_TD_MotorDelay)  //给锁轴时间
            {
              
              bCheckEN_FLG = 1;
              YX_CTHL_CheckInX02PosBuff=0;
             YX_CTHL_TD_MotorStep = 2;   
             
             SetPos(X_AXIS, 0);  //启动位置设为0点   
      //       SetPos(Y_AXIS, 0);  //启动位置设为0点
             dwXRealPos=0;
             AxisContinueMoveAcc(X_AXIS,YX_CTHL_PARAM_TD_RunSpeed * 66/100,DIR_P,1500,600,25,20);
              AxisContinueMoveAcc(Y_AXIS,YX_CTHL_PARAM_TD_RunSpeed * 1/2,DIR_N,1500,600,25,20);
       //      AxisContinueMoveAcc(Y_AXIS,YX_CTHL_PARAM_TD_RunSpeed * 2/3,DIR_N,1500,600,15,15);
             YX_CTHL_bQDinWeiOK = 0;
       
            }  
            break;

        case 2:  
          if(dwXRealPos > 100)
          {
            if(YX_CTHL_FK_Check_UP )
            {
               AxisContinueMoveAcc(X_AXIS,YX_CTHL_PARAM_BinLin_SPEED,DIR_P,3000,600,20,25); 
                AxisContinueMoveAcc(Y_AXIS,YX_CTHL_PARAM_BinLin_SPEED,DIR_N,3000,600,20,25); 
              SetStartSpeed(X_AXIS,YX_CTHL_PARAM_BinLin_SPEED);
               SetStartSpeed(Y_AXIS,YX_CTHL_PARAM_BinLin_SPEED);
              AxisChangeSpeed(X_AXIS,YX_CTHL_PARAM_BinLin_SPEED);
              AxisChangeSpeed(Y_AXIS,YX_CTHL_PARAM_BinLin_SPEED);
              YX_CTHL_TD_MotorStep = 3;   
               YX_CTHL_YADAI_VAVLE=1;
    
             
            }
          }
       
          if(dwXRealPos >= YX_CTHL_PARAM_first_CL_MAX_Length) 
          {
              YX_CTHL_SetAlarmCode(YX_CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
          }
            break;
        case 3://检测到过链         
            if(dwXRealPos >= YX_CTHL_PARAM_first_CL_MAX_Length) 
            {
                YX_CTHL_SetAlarmCode(YX_CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
            }
           
            
            if(YX_CTHL_FKDWE_IN_UP)
            {
             
                YX_CTHL_TD_MotorDelay = dwTickCount + YX_CTHL_PARAM_GYH_DELAY;
                YX_CTHL_XSavePosBuff = dwXRealPos;
              AxisMovePosAccDec(X_AXIS,YX_CTHL_PARAM_FKDW_Speed,YX_CTHL_PARAM_FKDW_Length,
                                 1000,1000,20,130,50);
                AxisMovePosAccDec(Y_AXIS,YX_CTHL_PARAM_FKDW_Speed*1/2,-YX_CTHL_PARAM_FKDW_Length,
                                 1000,1000,20,130,50);
            
             //   AxisMovePosAccDec(Y_AXIS,YX_CTHL_PARAM_TD_RunSpeed * 2/3,-YX_CTHL_PARAM_FKDW_Length,
                  //              YX_CTHL_PARAM_FKDW_Speed,XY_CTHL_PARAM_FKDW_Speed,10,50,150); 
            YX_CTHL_TD_MotorStep = 4;  
            }
            break;

        case 4:
          if(dwTickCount >= YX_CTHL_TD_MotorDelay)
          {
            YX_CTHL_GDQM_VAVLE=1;
          }
            if(!X_DRV || (dwXRealPos >= (YX_CTHL_XSavePosBuff + YX_CTHL_PARAM_FKDW_Length)))
            {
           
            
                 YX_CTHL_TD_MotorStep = 0; 
                AxisEgmStop(X_AXIS);
                AxisEgmStop(Y_AXIS);
            }
            break;
   
          
            
            //穿拉头定位
        case 10:   
            if(dwTickCount >= YX_CTHL_TD_MotorDelay)
            {
            
               YX_CTHL_TD_MotorDelay = dwTickCount + YX_CTHL_PARAM_TCXC_BACK_DELAY;
                YX_CTHL_TD_MotorStep = 50;
                SetPos(X_AXIS, 0);  //启动位置设为0点
                AxisContinueMoveAcc(X_AXIS,3000,DIR_P,1500,600,20,20);
                  AxisContinueMoveAcc(Y_AXIS,1500,DIR_N,1500,600,20,20);
                YX_CTHL_XSavePosBuff = dwXRealPos;
                bCheckEN_FLG = 1;
           
           
            }
            break;
        case 50:
         if(dwTickCount >= YX_CTHL_TD_MotorDelay)
           {
            YX_CTHL_FKGD_VAVLE=0; 
            YX_CTHL_CXC_VAVLE=0;
           }
          if(!YX_CTHL_FKGD_VAVLE)
          {
           YX_CTHL_TD_MotorDelay = dwTickCount + 20;
            YX_CTHL_TD_MotorStep = 11;
          }
         break;
          
        case 11:
                      
          if(dwTickCount >= YX_CTHL_TD_MotorDelay)
          {       
            YX_CTHL_XSavePosBuff3=dwXRealPos;
              if((YX_CTHL_cZipCnt < 2) || (YX_CTHL_LianLengthAutoCRCDcheck < 350)) 
              {
               AxisContinueMoveAcc(X_AXIS,YX_CTHL_PARAM_HLT_SPEED_Speed * 66/100,DIR_P,3000,600,20,20); 
                 AxisContinueMoveAcc(Y_AXIS,YX_CTHL_PARAM_HLT_SPEED_Speed * 1/2,DIR_N,3000,600,20,20); 
              }
              else
              {
                  AxisMovePosAccDecNotStop(X_AXIS,YX_CTHL_PARAM_HLT_SPEED_Speed,YX_CTHL_LianLengthAutoCRCDcheck-YX_CTHL_XSavePosBuff3,1000,YX_CTHL_PARAM_FKDW_Speed,30,300,250); 
                AxisMovePosAccDecNotStop(Y_AXIS,YX_CTHL_PARAM_HLT_SPEED_Speed*1/2,-(YX_CTHL_LianLengthAutoCRCDcheck-YX_CTHL_XSavePosBuff3),1000,YX_CTHL_PARAM_FKDW_Speed,30,300,250); 
        
              }     
            YX_CTHL_TD_MotorStep = 12;
            YX_CTHL_FKJC=0;

          }
           if(YX_CTHL_cZipCnt < 2)
            {
              if(dwXRealPos >= YX_CTHL_PARAM_first_CL_MAX_Length) 
              {
                  YX_CTHL_SetAlarmCode(YX_CTHL_FKYC_ALARM,bRunning);   //无拉链自动停机   
              }
            }
            else
            {
              if((YX_CTHL_LianLengthAutoFZcheck<YX_CTHL_LianLengthAutoCRCDcheck*0.7) && (YX_CTHL_LianLengthAutoFZcheck<1500))
              { 
              if(dwXRealPos >= 300)
              {
              YX_CTHL_LYLun_VAVLE=1;
              YX_CTHL_RYLun_VAVLE=1;
               YX_CTHL_MGuo_VAVLE=0;
               YX_CTHL_CTXM_VAVLE=0;
                  AxisEgmStop(X_AXIS);
                AxisEgmStop(Y_AXIS);
               YX_CTHL_TD_MotorStep = 13;  
              }
              }
              if(dwXRealPos >= (100+YX_CTHL_LianLengthAutoCRCDcheck))
              {
                 YX_CTHL_MGuo_VAVLE=0;
                 YX_CTHL_CTXM_VAVLE=0;
             
          
              }
               if(dwXRealPos >=(YX_CTHL_PARAM_WLCDSHEZHI_LENTH+ YX_CTHL_LianLengthAutoCRCDcheck))
              {
                
                  YX_CTHL_SetAlarmCode(YX_CTHL_FKYC_ALARM,bRunning);   //无拉链自动停机   
              }
            }
          break;
        case 12: 
            if(YX_CTHL_cZipCnt < 2)
            {
              if(dwXRealPos >= YX_CTHL_PARAM_first_CL_MAX_Length) 
              {
                  YX_CTHL_SetAlarmCode(YX_CTHL_FKYC_ALARM,bRunning);   //无拉链自动停机   
              }
            }
            else
            {
              if((YX_CTHL_LianLengthAutoFZcheck<YX_CTHL_LianLengthAutoCRCDcheck*0.7) && (YX_CTHL_LianLengthAutoFZcheck<1500))
              { 
              if(dwXRealPos >= 300)
              {
              YX_CTHL_LYLun_VAVLE=1;
              YX_CTHL_RYLun_VAVLE=1;
               YX_CTHL_MGuo_VAVLE=0;
               YX_CTHL_CTXM_VAVLE=0;
                  AxisEgmStop(X_AXIS);
                AxisEgmStop(Y_AXIS);
               YX_CTHL_TD_MotorStep = 13;  
                 if(YX_CTHL_cZipCnt ==1 )
                   YX_CTHL_LianLengthAutoCRCDcheck=dwXRealPos;
                 if(YX_CTHL_cZipCnt >1 && dwXRealPos < YX_CTHL_LianLengthAutoCRCDcheck*0.8)
                 {
                   YX_CTHL_cZipCnt=0;
                 }
              }
              }
    
              if(dwXRealPos >= (100+YX_CTHL_LianLengthAutoCRCDcheck))
              {
                 YX_CTHL_MGuo_VAVLE=0;
                 YX_CTHL_CTXM_VAVLE=0;
             
          
              }
               if(dwXRealPos >=(YX_CTHL_PARAM_WLCDSHEZHI_LENTH+ YX_CTHL_LianLengthAutoCRCDcheck))
              {
                
                  YX_CTHL_SetAlarmCode(YX_CTHL_FKYC_ALARM,bRunning);   //无拉链自动停机   
              }
            }
            
              
              if(YX_CTHL_FK_Check_UP)
              {
                 
              YX_CTHL_LYLun_VAVLE=1;
              YX_CTHL_RYLun_VAVLE=1;
                AxisEgmStop(X_AXIS);
                AxisEgmStop(Y_AXIS);
  
                YX_CTHL_TD_MotorDelay= dwTickCount+YX_CTHL_PARAM_TJSJ_TIME;
         
                if(YX_CTHL_cZipCnt ==1 )
                   YX_CTHL_LianLengthAutoCRCDcheck=dwXRealPos;
                 if(YX_CTHL_cZipCnt >1 && dwXRealPos < YX_CTHL_LianLengthAutoCRCDcheck*0.8)
                 {
                   YX_CTHL_cZipCnt=0;
                 }
                 YX_CTHL_MGuo_VAVLE=0;
                 YX_CTHL_CTXM_VAVLE=0;
                  YX_CTHL_XSavePosBuff = dwXRealPos;
                     
                if(cStopMode==1 || bSingOneFlg)
                  {
                  //  cStopMode=2;
                    YX_CTHL_ZhouWantwo=1;
                  }
                else
                {
                  YX_CTHL_TD_MotorStep = 13;  
                }
                if(bSingOneFlg)
                {
                    YX_CTHL_AutoStep = 0;
                    bRunning = 0;
                    YX_CTHL_TD_MotorStep = 0;  
                    bSingOneFlg = 0;
                } 
                 

              }
             break;
         case 13:
             if(dwXRealPos > 350)
          {
                 
            YX_CTHL_CheckInX02PosBuff=0;
          }

         if(YX_CTHL_CTM_Origin_IN)//YX_CTHL_FKDWE_IN_UP)
           {  
             YX_CTHL_LianLengthAutoSECONDcheck=dwXRealPos;
             YX_CTHL_TD_MotorStep = 14;
             if((YX_CTHL_cZipCnt < 2) || (YX_CTHL_LianLengthAutoSECONDONEcheck < 350)) 
              {
               AxisContinueMoveAcc(X_AXIS,YX_CTHL_PARAM_TD_RunSpeed * 66/100,DIR_P,3000,600,20,25); 
               AxisContinueMoveAcc(Y_AXIS,YX_CTHL_PARAM_TD_RunSpeed * 66/100,DIR_N,3000,600,20,25);
              }
             else
             {
              AxisMovePosAccDecNotStop(X_AXIS,YX_CTHL_PARAM_TD_RunSpeed,YX_CTHL_LianLengthAutoSECONDONEcheck,1500,YX_CTHL_PARAM_FKDW_Speed,25,300,250);       
              AxisMovePosAccDecNotStop(Y_AXIS,YX_CTHL_PARAM_TD_RunSpeed,-YX_CTHL_LianLengthAutoSECONDONEcheck,1500,YX_CTHL_PARAM_FKDW_Speed,25,300,250);       
              
             }
         
           }    
          if(dwXRealPos >= YX_CTHL_PARAM_first_CL_MAX_Length) 
          {
              YX_CTHL_SetAlarmCode(YX_CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
          }

             break;
             
        case 14:
          if(dwXRealPos>(YX_CTHL_LianLengthAutoSECONDcheck+800))
          {
           if(YX_CTHL_FK_Check_UP)
            {
                 AxisContinueMoveAcc(X_AXIS,YX_CTHL_PARAM_BinLin_SPEED,DIR_P,3000,600,20,25); 
                  AxisContinueMoveAcc(Y_AXIS,YX_CTHL_PARAM_BinLin_SPEED,DIR_N,3000,600,20,25); 
              SetStartSpeed(X_AXIS,YX_CTHL_PARAM_BinLin_SPEED);
                SetStartSpeed(Y_AXIS,YX_CTHL_PARAM_BinLin_SPEED);
              AxisChangeSpeed(X_AXIS,YX_CTHL_PARAM_BinLin_SPEED);
              AxisChangeSpeed(Y_AXIS,YX_CTHL_PARAM_BinLin_SPEED);
               YX_CTHL_YADAI_VAVLE=1;
               YX_CTHL_TD_MotorStep = 15; 
            }
          }
            if(YX_CTHL_cZipCnt > 1)
                {
                  if(dwXRealPos >= (YX_CTHL_PARAM_WLCDSHEZHI_LENTH+YX_CTHL_LianLengthAutoCheck)) 
                  {
                   
                      YX_CTHL_SetAlarmCode(YX_CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机                  
                  }
                if(dwXRealPos>(YX_CTHL_LianLengthAutoCheck- YX_CTHL_PARAM_YAdai_lenth))
                {
                   YX_CTHL_YADAI_VAVLE=1;
                }
                }
                else
                {
                   if(dwXRealPos >= YX_CTHL_PARAM_first_CL_MAX_Length+ YX_CTHL_LianLengthAutoFZcheck) 
                  {
                   
                      YX_CTHL_SetAlarmCode(YX_CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机                  
                  }
                }
            
          
          
          break;
  
        case 15://检测到过链    

          if(YX_CTHL_cZipCnt > 1)
          {
            if(dwXRealPos >= (YX_CTHL_PARAM_WLCDSHEZHI_LENTH+YX_CTHL_LianLengthAutoCheck)) 
            {
              YX_CTHL_AUTO_start=2;
                YX_CTHL_SetAlarmCode(YX_CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机                  
            }
          }
          else
          {
          
               if(dwXRealPos >= YX_CTHL_PARAM_first_CL_MAX_Length+ YX_CTHL_LianLengthAutoFZcheck) 
            {
            
                YX_CTHL_SetAlarmCode(YX_CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机                  
            }
            if(dwXRealPos>(YX_CTHL_LianLengthAutoCheck- YX_CTHL_PARAM_YAdai_lenth))
            {
               YX_CTHL_YADAI_VAVLE=1;
            }
          
          }
        if(YX_CTHL_FKDWE_IN_UP)
        {
          YX_CTHL_YADAI_VAVLE=1;
          YX_CTHL_TD_MotorDelay = dwTickCount + YX_CTHL_PARAM_GYH_DELAY;
          YX_CTHL_XSavePosBuff = dwXRealPos;
          if(YX_CTHL_cZipCnt ==1)
                    YX_CTHL_LianLengthAutoSECONDONEcheck= dwXRealPos-YX_CTHL_LianLengthAutoSECONDcheck;
         AxisMovePosAccDec(X_AXIS,YX_CTHL_PARAM_FKDW_Speed,YX_CTHL_PARAM_FKDW_Length,
                             1000,1000,20,250,50); 
             AxisMovePosAccDec(Y_AXIS,YX_CTHL_PARAM_FKDW_Speed,-YX_CTHL_PARAM_FKDW_Length,
                             1000,1000,20,250,50); 
          YX_CTHL_TD_MotorStep = 16;  
        }
  
            break;
        case 16:   
          if(dwTickCount >= YX_CTHL_TD_MotorDelay)
          {
            YX_CTHL_GDQM_VAVLE=1;
          }          

            if(!X_DRV || (dwXRealPos >= (YX_CTHL_XSavePosBuff + YX_CTHL_PARAM_FKDW_Length)))
            {
             
                 YX_CTHL_LYLun_VAVLE=0;
                    YX_CTHL_RYLun_VAVLE=0;
                 YX_CTHL_TD_MotorStep = 0;  
                AxisEgmStop(X_AXIS);
                AxisEgmStop(Y_AXIS);
              YX_CTHL_LianLengthAutoTHREEcheck= dwXRealPos;
               if(YX_CTHL_cZipCnt ==1)
                    YX_CTHL_LianLengthAutoCheck = dwXRealPos;
              
            }
     
            break;
     
   

	}   
#endif    
}
//穿入电机动作，Y轴
void YX_CTHL_FZ_Motor(void)	 //
{   
#if 1
    //1步开始,  穿链反转长度开始
  //20步开始  插销定位开始

    switch(YX_CTHL_FZ_MotorStep)
	{
         //穿入长度位置
         case 1:
            
            if(dwTickCount >= YX_CTHL_FZ_MotorDelay)
            {   
        
                SetPos(Y_AXIS, 0);  //启动位置设为0点
                  AxisContinueMoveAcc(Y_AXIS,YX_CTHL_PARAM_CRSD_SPEED,DIR_P,1000,1200,15,20); 
                  AxisContinueMoveAcc(X_AXIS,YX_CTHL_PARAM_CRSD_SPEED*YX_CTHL_PARAM_FMOTOR_BILI,DIR_N,1000,1200,15,20); 
                  YX_CTHL_FZ_MotorStep=2;
                  YX_CTHL_YADAI_VAVLE=0;
                  
             }
            break;
            case 2:

              if(YX_CTHL_cZipCnt < 2)
              {
                  if(dwYRealPos >= YX_CTHL_PARAM_first_CL_MAX_Length) 
                {
              
                    YX_CTHL_SetAlarmCode(YX_CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机                  
                }
              }
              else
              {
               if(dwYRealPos >= (YX_CTHL_PARAM_WLCDSHEZHI_LENTH+ YX_CTHL_LianLengthAutoFZcheck)) 
                {
                 
                  YX_CTHL_SetAlarmCode(YX_CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
       
              }
              }
               if(dwYRealPos>YX_CTHL_PARAM_XKCR_LENGTH)
               {
                 YX_CTHL_TQM_VAVLE=0;
                 YX_CTHL_GDQM_VAVLE=0;
                if(YX_CTHL_cZipCnt<2)
                {
                 AxisContinueMoveAcc(X_AXIS,YX_CTHL_PARAM_FZ_Speed * 2/3,DIR_N,3000,600,25,30); 
                  AxisContinueMoveAcc(Y_AXIS,(YX_CTHL_PARAM_FZ_Speed * 2/3),DIR_P,3000,600,25,30); 
                }
                else
                {
                      AxisMovePosAccDecNotStop(X_AXIS,YX_CTHL_PARAM_FZ_Speed,-(YX_CTHL_LianLengthAutoFZcheck-YX_CTHL_PARAM_XKCR_LENGTH),1500*YX_CTHL_PARAM_FMOTOR_BILI,YX_CTHL_PARAM_FKDW_Speed,20,300,250); 
                      AxisMovePosAccDecNotStop(Y_AXIS,YX_CTHL_PARAM_FZ_Speed,YX_CTHL_LianLengthAutoFZcheck-YX_CTHL_PARAM_XKCR_LENGTH,1500,YX_CTHL_PARAM_FKDW_Speed,20,300,250); 
                }
                 YX_CTHL_FZ_MotorStep=3;
               }
              break;
        case 3:
          if(YX_CTHL_cZipCnt < 2)
              {
                  if(dwYRealPos >= YX_CTHL_PARAM_first_CL_MAX_Length) 
                {
                 
                    YX_CTHL_SetAlarmCode(YX_CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机                  
                }
              }
              else
              {
               if(dwYRealPos >= (YX_CTHL_PARAM_WLCDSHEZHI_LENTH+ YX_CTHL_LianLengthAutoFZcheck)) 
                {
                
                  YX_CTHL_SetAlarmCode(YX_CTHL_WLZDTJ_ALARM,bRunning);   //无拉链自动停机   
       
              }
              }

          if(YX_CTHL_FKQL_IN_UP)
          {
        
            AxisEgmStop(X_AXIS);        
            AxisEgmStop(Y_AXIS);
            YX_CTHL_FKGD_VAVLE=1;
           YX_CTHL_FZ_MotorStep = 0; 
           if(YX_CTHL_cZipCnt ==1 || dwYRealPos<(YX_CTHL_LianLengthAutoFZcheck*0.7))
             YX_CTHL_LianLengthAutoFZcheck = dwYRealPos;
        
          }
          break;
       case 20: //此处最好算好定位长度相减
            if(dwTickCount >= YX_CTHL_FZ_MotorDelay)
            {
                  YX_CTHL_FZ_MotorStep = 21; 
                  SetPos(Y_AXIS, 0);  //启动位置设为0点
            }
            break;
        case 21:
            if(YX_CTHL_PARAM_CXDW_Length)
            { 
          //    YX_CTHL_FZ_MotorDelay=dwTickCount+ YX_CTHL_PARAM_CXCSC_DELAY;
               YX_CTHL_FZ_MotorStep = 22; 
                AxisMovePosAccDec(Y_AXIS,5000,YX_CTHL_PARAM_CXDW_Length,800,800,25,25,0);   
                if(YX_CTHL_PARAM_CXDW_Length> 20)
                    AxisMovePosAccDec(X_AXIS,5000,-(YX_CTHL_PARAM_CXDW_Length- YX_CTHL_PARAM_CXDW_Length/30),800,800,25,25,0); 
            }
            break;
        case 22: 
            if(!Y_DRV || (dwYRealPos >= YX_CTHL_PARAM_CXDW_Length)) //插销定位已经结束
            {
                AxisEgmStop(Y_AXIS);
                AxisEgmStop(X_AXIS);
                YX_CTHL_FZ_MotorStep = 0;   
                
            } 
           
            
            break;              



            

	}
    
#endif    
}



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

    switch(YX_CTHL_ZhuangLiaoStep)
    {
        case 1:
          
             if(bZhuangLiaoOkFlg)
             {
                  YX_CTHL_ZhuangLiaoStep = 0;
             }
             else if(dwTickCount >= YX_CTHL_ZhuangLiaoDelay)
             {
                 
                   if(((YX_CTHL_MGuo_VAVLE && YX_CTHL_LTou_Check) || (!YX_CTHL_MGuo_VAVLE)) && !YX_CTHL_JLiao_Limit_IN  )//说明没有拉头
                   {       
                          //穿头模和一次穿入必须在原位,接料不能有输出
                          if(YX_CTHL_CTM_Origin_IN && !YX_CTHL_CTXM_VAVLE)
                          {  
                              YX_CTHL_JLTou_VAVLE = 0;
                              YX_CTHL_MGuo_VAVLE = 0;                     
                              if(YX_CTHL_TLiaoStep == 0)
                              {
                                 YX_CTHL_TLiaoStep = 1;        
                               YX_CTHL_ZhuangLiaoStep = 2; 
                              }
                           }
                      
                        else if(dwTickCount >= YX_CTHL_ZhuangLiaoDelay + 500)  //警告横送拉必须保证穿头模和一次穿入在原位,接料不能有输出
                        {
                          YX_CTHL_wltzi=1;
                          YX_CTHL_SetAlarmCode(YX_CTHL_CTM_Origin_ALARM,bRunning);
                            
                        }  
                   }
                   else  //已经有拉头直接跳到结束
                   {
                         YX_CTHL_ZhuangLiaoDelay = dwTickCount + 40;
                         YX_CTHL_ZhuangLiaoStep = 9;
                   }  
             }
             break;            
        case 2:        
            if(YX_CTHL_TLiaoStep == 0)  //推料已经完成
            {
                YX_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
                YX_CTHL_JLTou_VAVLE = 0;
                YX_CTHL_MGuo_VAVLE = 0;   
                if(!YX_CTHL_JLiao_Limit_IN)
                {
                    YX_CTHL_ZhuangLiaoStep = 3;
                    YX_CTHL_HSLiao_VAVLE = 1;   
                }
            }
            break;
        case 3:
            //横送到位
            if(YX_CTHL_HSLiao_Limit_IN && !YX_CTHL_HSLiao_Origin_IN)
            {
                YX_CTHL_ZhuangLiaoStep = 4;
//                YX_CTHL_JLTou_VAVLE = 1;    //接拉头输出,不需要时间
                YX_CTHL_ZhuangLiaoDelay = dwTickCount + 10;
            }
            else if(dwTickCount >= YX_CTHL_ZhuangLiaoDelay)
            {
               YX_CTHL_wltzi=1;
                YX_CTHL_SetAlarmCode(YX_CTHL_HSLiao_Limit_ALARM,bRunning);
                YX_CTHL_ZhuangLiaoStep = 0;
                YX_CTHL_HSLiao_VAVLE = 0; 
            }
            break;
        case 4:
            if(dwTickCount >= YX_CTHL_ZhuangLiaoDelay)
            {
                YX_CTHL_ZhuangLiaoStep = 5;
                YX_CTHL_JLTou_VAVLE = 1;    //接拉头输出,不需要时间
                YX_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
            }  
            break;
        case 5:    
            if(YX_CTHL_JLiao_Limit_IN)
            {
                YX_CTHL_ZhuangLiaoStep = 6;
                YX_CTHL_ZhuangLiaoDelay = dwTickCount + YX_CTHL_PARAM_MaGou_DELAY;
            }
            else if(dwTickCount >= YX_CTHL_ZhuangLiaoDelay)  //接拉头(接料)到们信号异常
            {
                 YX_CTHL_SetAlarmCode(YX_CTHL_JLiao_Origin_ALARM,bRunning);
                 YX_CTHL_HSLiao_VAVLE = 0; 
                 YX_CTHL_JLTou_VAVLE = 0; 
                 YX_CTHL_ZhuangLiaoStep = 0;
            }
            break;
        case 6:       
            if(dwTickCount >= YX_CTHL_ZhuangLiaoDelay)  //延时码勾
            {
                YX_CTHL_MGuo_VAVLE = 1; //码勾输出
                YX_CTHL_ZhuangLiaoStep = 7;
                YX_CTHL_ZhuangLiaoDelay = dwTickCount + YX_CTHL_PARAM_HSLiaoOff_DELAY;
                bTuiLaTouOkFlg = 0;   //认为拉头没法带出
            }
            break;
        case 7:  
            if(dwTickCount >= YX_CTHL_ZhuangLiaoDelay)
            {
                YX_CTHL_HSLiao_VAVLE = 0; 
                YX_CTHL_ZhuangLiaoStep = 8;
                YX_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
            }
            break;
        case 8:  
            if(!YX_CTHL_HSLiao_Limit_IN)  //
            {
                YX_CTHL_ZhuangLiaoStep = 9;
                YX_CTHL_TLiaoStep = 1; //推拉头
                YX_CTHL_ZhuangLiaoDelay = dwTickCount + YX_CTHL_PARAM_CheckLT_DELAY;   //也是给下模能上升的时间
            }          
            else if (YX_CTHL_LTou_Check)  //时间内信号这说明没有拉头
            {
                //重送3次
               YX_CTHL_TryCnt++;
               YX_CTHL_JLTou_VAVLE = 0;    //接拉头关
                YX_CTHL_MGuo_VAVLE = 0;     //码勾打开
                if(YX_CTHL_TryCnt >= 3)
                {
                    YX_CTHL_TryCnt = 0;
                    bZhuangLiaoOkFlg = 0;
                    YX_CTHL_ZhuangLiaoStep = 0;
                     YX_CTHL_wltzi=1;
                    YX_CTHL_SetAlarmCode(YX_CTHL_ZLT_ALARM,bRunning);
                      
                }
                else
                {
                    YX_CTHL_ZhuangLiaoStep = 20;    //重新送3次   
                    YX_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
                }  
            }
            else if(dwTickCount >= YX_CTHL_ZhuangLiaoDelay)
            {
              YX_CTHL_wltzi=1;
                YX_CTHL_SetAlarmCode(YX_CTHL_HSLiao_Limit_ALARM,bRunning);
                if(bRunning)
                {
                    YX_CTHL_ZhuangLiaoStep = 0 ;
                }  
            }  
            break;
        case 9:   
            if(YX_CTHL_LTou_Check)  //时间内信号这说明没有拉头
            {
                YX_CTHL_TryCnt++;
                YX_CTHL_JLTou_VAVLE = 0;    //接拉头关
                YX_CTHL_MGuo_VAVLE = 0;     //码勾打开
                if(YX_CTHL_TryCnt >= 3)
                {
                  YX_CTHL_TryCnt=0;
                    bZhuangLiaoOkFlg = 0;
                    YX_CTHL_ZhuangLiaoStep = 0;
                      YX_CTHL_wltzi=1;
                    YX_CTHL_SetAlarmCode(YX_CTHL_ZLT_ALARM,bRunning);
                      
                }
                else
                {
                    YX_CTHL_ZhuangLiaoStep = 20;    //重新送3次   
                    YX_CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME;
                }  
            }
            else if(dwTickCount >= YX_CTHL_ZhuangLiaoDelay)
            {
                bZhuangLiaoOkFlg = 1;
                YX_CTHL_ZhuangLiaoStep = 0;
            }
            break;
          //装拉头不成攻重新送拉头
        case 20: 
            if(YX_CTHL_HSLiao_Origin_IN)
            {
                YX_CTHL_JLTou_VAVLE = 0;    //接拉头关
                YX_CTHL_MGuo_VAVLE = 0;     //码勾打开
                YX_CTHL_ZhuangLiaoStep = 1;
                YX_CTHL_ZhuangLiaoDelay = dwTickCount + 10;
            }
            else if(dwTickCount >= YX_CTHL_ZhuangLiaoDelay)
            {
              YX_CTHL_wltzi=1;
                YX_CTHL_SetAlarmCode(YX_CTHL_HSLiao_Origin_ALARM,bRunning);
            }  
            break;
    }  
}

void YX_CTHL_TLiao_Step(void)
{
    switch(YX_CTHL_TLiaoStep)
    {
        case 1:
            if(bTuiLaTouOkFlg)
            {
                YX_CTHL_TLiaoStep = 0; 
            }
            else
            {
                YX_CTHL_TLiao_Delay = dwTickCount + YX_CTHL_VAVLE_ERROR_TIME;
                YX_CTHL_TLiaoStep = 2;
            }
            break;
        case 2:    //横送料在原位
            if(YX_CTHL_HSLiao_Origin_IN && !YX_CTHL_HSLiao_Limit_IN && !YX_CTHL_HSLiao_VAVLE)
            {
                YX_CTHL_TLiao_VAVLE = 1;    //推料(推拉头)输出
                YX_CTHL_TLiao_Delay = dwTickCount + YX_CTHL_VAVLE_ERROR_TIME;
                YX_CTHL_TLiaoStep = 3;
            }
            else if(dwTickCount >= YX_CTHL_TLiao_Delay) 
            {
                YX_CTHL_SetAlarmCode(YX_CTHL_HSLiao_Origin_ALARM,1);   
                YX_CTHL_wltzi=1;
            }
            break;
        case 3:  //推拉头到位
            if(!YX_CTHL_TLiao_Origin_IN)
            { 
                YX_CTHL_TLiao_Delay = dwTickCount + YX_CTHL_PARAM_TLiao_BACKDELAY;
                YX_CTHL_TLiaoStep = 4;
            }
            else if(dwTickCount >= YX_CTHL_TLiao_Delay) 
            {
                YX_CTHL_SetAlarmCode(YX_CTHL_TLiao_Limit_ALARM,1);  
                YX_CTHL_wltzi=1;
            }
            break;
        case 4:
            if(dwTickCount >= YX_CTHL_TLiao_Delay)
            {
                YX_CTHL_TLiao_VAVLE       = 0;
                YX_CTHL_TLiao_Delay = dwTickCount + YX_CTHL_VAVLE_ERROR_TIME; 
                YX_CTHL_TLiaoStep = 5;
            }
            break;
        case  5:
            if(YX_CTHL_TLiao_Origin_IN)
            {
                YX_CTHL_TLiaoStep = 0;
                YX_CTHL_TLiao_Delay = dwTickCount;
                bTuiLaTouOkFlg    = 1;
            }
            else if(dwTickCount >= YX_CTHL_TLiao_Delay) 
            {
                YX_CTHL_SetAlarmCode(YX_CTHL_TLiao_Limit_ALARM,0);   
                YX_CTHL_wltzi=1;
            }
            break;
    } 
   
}
//振动盘控制
void YX_CTHL_ZhenDongAction(void)
{
 
#if 0  
    if((YX_CTHL_bDLP || bRunning) && YX_CTHL_ZhenDongPian_OUT && YX_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(!XY_CTHL_PARAM_ZDP_AUTO)
       {

            if(YX_CTHL_ZhenDongPian_OUT == 0)
            {
                if(!YX_CTHL_ZDP_IN)
                {
                    if(dwTickCount >= YX_CTHL_ZDP_Delay)
                    {
                        YX_CTHL_ZhenDongPian_OUT = 1;
                        YX_CTHL_ZDP_Delay = dwTickCount + YX_CTHL_PARAM_ZDP_Stop_TIME;//YX_CTHL_PARAM_ZDP_Stop_TIME + 50;
                    }
                }
                else
                {
                    YX_CTHL_ZDP_Delay = dwTickCount + YX_CTHL_PARAM_ZDP_Start_TIME;//YX_CTHL_PARAM_ZDP_Start_TIME;
                }   
            }
            else
            {  
                if(YX_CTHL_ZDP_IN)
                {  
                    if(dwTickCount >= YX_CTHL_ZDP_Delay)
                    {
                         YX_CTHL_ZhenDongPian_OUT = 0;  
                         YX_CTHL_ZDP_Delay = dwTickCount + YX_CTHL_PARAM_ZDP_Start_TIME;//YX_CTHL_PARAM_ZDP_Start_TIME
                    }
                }
                else
                    YX_CTHL_ZDP_Delay = dwTickCount + YX_CTHL_PARAM_ZDP_Stop_TIME;//YX_CTHL_PARAM_ZDP_Stop_TIME+ 50;
            }
   
       }
 
      else if (YX_CTHL_PARAM_ZDP_AUTO ==1 )
       {
       YX_CTHL_ZhenDongPian_OUT=0;
       YX_CTHL_ZDP_Delay = dwTickCount + 0;
       }
       else if(YX_CTHL_PARAM_ZDP_AUTO == 2)
       {
        YX_CTHL_ZhenDongPian_OUT=1;
       YX_CTHL_ZDP_Delay = dwTickCount + 0;
       
       }
#endif    
}





//手动动作
void YX_CTHL_ManualAction(void)
{ 

 
 
    if(bRunning == 0)  
    {   
        if(bClearTotal) //切断计数清零
        {
            bClearTotal       = 0;
            ClrcToTal(QDCT_TOTAL_ADDR);
        }
                if(YX_CTHL_bHSL)     //手动横送料   
        {
            YX_CTHL_bHSL= 0;
       
            if(YX_CTHL_HSLiao_VAVLE)
            {
                YX_CTHL_HSLiao_VAVLE = 0;//~XY_CTHL_HSLiao_VAVLE;
            }
            else if(!YX_CTHL_TLiao_VAVLE   && !YX_CTHL_JLTou_VAVLE && !YX_CTHL_CTXM_VAVLE  && (!YX_CTHL_bMG || (YX_CTHL_bMG && YX_CTHL_LTou_Check)))
            {

              if(YX_CTHL_CTM_Origin_IN)
                YX_CTHL_HSLiao_VAVLE = 1;

              }
              else
              {
               YX_CTHL_SetAlarmCode(YX_CTHL_JLiao_Origin_ALARM,0);
              }
            }  
        
           
          
        if(YX_CTHL_bTLiao )  //手动推料(推拉头)
        {
            YX_CTHL_bTLiao = 0;           
            if(YX_CTHL_TLiao_VAVLE)
               YX_CTHL_TLiao_VAVLE = 0;//YX_CTHL_TLiao_VAVLE; 
            else if(!YX_CTHL_HSLiao_VAVLE && YX_CTHL_HSLiao_Origin_IN)
            {
                YX_CTHL_TLiao_VAVLE = 1;
            } 
            else//条件警告
            {
              YX_CTHL_SetAlarmCode(YX_CTHL_HSLiao_Origin_ALARM,0); // 横送料不能输出
            }

        } 
        
         if(YX_CTHL_bJLT)                  //接拉头
        {
            YX_CTHL_bJLT = 0;
            if(YX_CTHL_JLTou_VAVLE)
                YX_CTHL_JLTou_VAVLE = 0;
            else if( !YX_CTHL_CTXM_VAVLE && YX_CTHL_CTM_Origin_IN)
            {
                YX_CTHL_MGuo_VAVLE = 0;
                YX_CTHL_JLTou_VAVLE = 1;
            } 
            else//条件警告
            {
                 YX_CTHL_SetAlarmCode(YX_CTHL_CTM_Origin_ALARM,0); 
            }
        } 
             
        if(YX_CTHL_bMG)                   //码勾
        {
            YX_CTHL_bMG = 0;
            YX_CTHL_MGuo_VAVLE = !YX_CTHL_MGuo_VAVLE;
        } 


   
       if(YX_CTHL_bLYLun)              //前压轮
        {
            YX_CTHL_bLYLun = 0;
            YX_CTHL_LYLun_VAVLE = ~YX_CTHL_LYLun_VAVLE;
        }  
         
          
        if(YX_CTHL_bFKGD)                 //方块固定
        {
            YX_CTHL_bFKGD = 0;
            YX_CTHL_FKGD_VAVLE = ~YX_CTHL_FKGD_VAVLE;
        } 
        
        if(YX_CTHL_bCXC) //插小插
        {
        YX_CTHL_bCXC=0;
        YX_CTHL_CXC_VAVLE = ~YX_CTHL_CXC_VAVLE;
        }
        
        if(YX_CTHL_bXCDWGYZ) //固定前码
        {
        YX_CTHL_bXCDWGYZ=0;
        YX_CTHL_GDQM_VAVLE  = ~YX_CTHL_GDQM_VAVLE ;
        }
        
         if(YX_CTHL_bFKGM) //方块勾模
        {
        YX_CTHL_bFKGM=0;
        YX_CTHL_TQM_VAVLE  = ~YX_CTHL_TQM_VAVLE ;
        }
      if(YX_CTHL_bRYLun)               //后压轮
        {
            YX_CTHL_bRYLun= 0;
            YX_CTHL_RYLun_VAVLE = ~YX_CTHL_RYLun_VAVLE;
        }  
       
        if(YX_CTHL_bCTXM)                  //下模
        {
            YX_CTHL_bCTXM = 0;
            if(YX_CTHL_CTXM_VAVLE)
            {
                YX_CTHL_CTXM_VAVLE = 0;
                YX_CTHL_JLTou_VAVLE = 0;
            }    
            else if(!YX_CTHL_HSLiao_VAVLE && YX_CTHL_HSLiao_Origin_IN)                
            { 
              
                YX_CTHL_CTXM_VAVLE = 1;
            }  
        } 
  
        //压带
        if(YX_CTHL_bYD)
        {
            YX_CTHL_bYD = 0;
            YX_CTHL_YADAI_VAVLE =~YX_CTHL_YADAI_VAVLE;
        }  
       
       

        //自动装料
        if(YX_CTHL_bAutoZhuangLiao)
        {
            YX_CTHL_bAutoZhuangLiao = 0;
            if(YX_CTHL_ZhuangLiaoStep == 0)
            {
                bTuiLaTouOkFlg = 1;
                bZhuangLiaoOkFlg = 0;
                YX_CTHL_ZhuangLiaoStep = 1;
                YX_CTHL_MGuo_VAVLE = 1;   //判断是否已经有料
                YX_CTHL_ZhuangLiaoDelay = dwTickCount + 150;
            }  
        }
        if(YX_CTHL_bYJDW)  //一键定位
        {
          YX_CTHL_bYJDW=0;
          if(YX_CTHL_TD_MotorStep==0)
          {
                         
            YX_CTHL_CTXM_VAVLE =0;           
            YX_CTHL_FKGD_VAVLE =0;         
            YX_CTHL_YADAI_VAVLE = 0;     
            YX_CTHL_LYLun_VAVLE = 0;   //
            YX_CTHL_RYLun_VAVLE = 0;   //Y16插销压轮
            YX_CTHL_CXC_VAVLE = 0;   
            YX_CTHL_GDQM_VAVLE =0;
            YX_CTHL_TQM_VAVLE=0;     
  
             if((GetEn(X_AXIS) == MOTOR_DISEN))
                {
                    SetEn(X_AXIS, MOTOR_EN);   
                     SetEn(Y_AXIS, MOTOR_EN);  
                    YX_CTHL_TD_MotorDelay = dwTickCount + 200; 
                   
                } 
                else
                    YX_CTHL_TD_MotorDelay = dwTickCount + 50; 
          YX_CTHL_TD_MotorStep=1;
          }
          

        } 
     
     //X轴Y轴电机测试 
        if(YX_CTHL_bCLMotor_N)
        {           
           SetEn(Y_AXIS, MOTOR_EN);
            SetEn(X_AXIS, MOTOR_EN);
                if(!X_DRV)
                {
                  //                Y轴       运行速度                    启动速度        加速度  减速度
                 
                    AxisContinueMoveAcc(X_AXIS,2000,DIR_P,1000,1000,15,15);
                     AxisContinueMoveAcc(Y_AXIS,2000,DIR_N,1000,1000,15,15); 
                }
        }
        
        if(YX_CTHL_bCLMotor_P)  //后退限位已经取消
        {
          SetEn(Y_AXIS, MOTOR_EN);
            SetEn(X_AXIS, MOTOR_EN);
           if(!X_DRV)
           {  
              //                Y轴       运行速度                    启动速度        加速度  减速度
              AxisContinueMoveAcc(Y_AXIS,2000,DIR_P,1000,1000,15,15);
              AxisContinueMoveAcc(X_AXIS,2000,DIR_N,1000,1000,15,15);
           }  
        }
        
        if(!YX_CTHL_bCLMotor_P && !YX_CTHL_bCLMotor_N && (YX_CTHL_TD_MotorStep == 0) && YX_CTHL_ColorStep==0)
        {
              AxisEgmStop(Y_AXIS);
              AxisEgmStop(X_AXIS);
        }

 //Z轴电机测试    
      if(YX_CTHL_bCLMotor_Z)
        {           
           SetEn(Z_AXIS, MOTOR_EN);
        
                if(!Z_DRV)
                {
                  //                Y轴       运行速度                    启动速度        加速度  减速度
                   AxisContinueMoveAcc(Z_AXIS,2000,DIR_P,1000,1000,15,15);    
             
                }
        }
        

        
        if(!YX_CTHL_bCLMotor_Z && (YX_CTHL_TD_MotorStep == 0) && !YX_CTHL_MOTOR_MODE)
        {
             AxisEgmStop(Z_AXIS);
   
        }
        




      
    
    }        
}

#endif