GC23005/Machine/YouGuanMachine/YGChuanTouJi.c

#include "global.h"

#if YOU_GUAN_MACHINE

void ChuanTou_AlarmProtect(void);
void ChuanTou_ManualAction(void);
void ChuanTou_AutoAction(void);
void ChuanTou_StepCheckStart(void);
void ChuanTou_Motor(void);
void ChuanTou_SongLiaoAction(void);
void ChuanTou_ZhuangLiaoStep(void);
void ChuanTou_CLT_Step(void);
void ChuanTou_ZhenDongAction(void);

static unsigned char cSongLiaoOk = 0;
static unsigned char cZhuangLiaoOk = 0;
static unsigned char cZLCnt      = 0;
static unsigned char STOP_IN_FLAG,STOP_IN_FLAG_OLD,STOP_FILTER;


void CT_SetAlarmCode(unsigned alarm_code)
{
    SetAlarmCode(CT_ALARM_ADDR,alarm_code);
    bAlarmStop = 1;
}

void ChuanTou_InitAction(void)
{
    float length_buff,pulse_buff;
    
    short i = 0;

/*    for (i = 0; i < 100; i++)
    {
        FreqTab_X[i] = 4000000/(FreqTab_XX[i]/14);
    }*/
    length_buff = CT_PARAM_CYCLE_LENGTH;
    pulse_buff  = CT_PARAM_CYCLE_PULSE;
    
    XGearRatio  = pulse_buff/length_buff;
    YGearRatio  = 1;
    axis_x->speed_unit = 100;
    
    STOP_IN_FLAG = STOP_IN;
    STOP_IN_FLAG_OLD = STOP_IN_FLAG;
    
    if(CT_PARAM_CR_MODE)
    {
        if(!CT_CR_ORIGIN_IN)
        {
            if(CT_MotorStep == 0)CT_MotorStep = 100;
        }
    }
    
}

void ChuanTou_Action(void)
{
    ChuanTou_AlarmProtect();
    ChuanTou_StepCheckStart();   // 调用脚踏开关检测程序
    ChuanTou_Motor();
    ChuanTou_ManualAction();
    ChuanTou_AutoAction();
    ChuanTou_CLT_Step();
    ChuanTou_ZhuangLiaoStep();
    ChuanTou_ZhenDongAction();
    ChuanTou_SongLiaoAction();
}

//手动动作
void ChuanTou_ManualAction(void)
{  
    if(bRunning == 0)  
    {   
        if(bClearTotal) //切断计数清零
        {
            bClearTotal       = 0;
            ClrcToTal(CT_TOTAL_ADDR);
        }
       
        if(CT_bFD)
        {
            CT_bFD = 0;
            CT_FD_VAVLE = ~CT_FD_VAVLE;
        }
        
        if(CT_bXM)
        {
            CT_bXM      = 0;
            CT_XM_VAVLE = ~CT_XM_VAVLE;
        }
        
        if(CT_bQYD)
        {
            CT_bQYD = 0;  
            CT_QYD_VAVLE = ~CT_QYD_VAVLE;
        }
        
        if(CT_bHYD)
        {
            CT_bHYD = 0;  
            CT_HYD_VAVLE = ~CT_HYD_VAVLE;
        }

        if(CT_bSL)
        {
            CT_bSL = 0;
            CT_SL_VAVEL = ~CT_SL_VAVEL;
        }
        
        if(CT_bHL)
        {
            CT_bHL = 0;
            CT_HL_VAVLE = ~CT_HL_VAVLE;
        }
        
        if(CT_bCR)
        {
            CT_bCR = 0;
            CT_CR_VAVLE = ~CT_CR_VAVLE;
        }
        
        
        if(CT_bHSL)
        {
            CT_bHSL = 0;
            CT_HSL_VAVLE = ~CT_HSL_VAVLE;
        }
        
        if(CT_bZL)
        {
            CT_bZL = 0;
            if(CT_SL_Step == 0)
            {
                cSongLiaoOk   = 0;
                CT_SL_Step    = 1;
            }
        }
        
        if(CT_bCLT)
        {
           CT_bCLT = 0;
           if(CT_CLT_Step == 0)CT_CLT_Step = 1;
        }
              
        if(CT_bCT_XM)
        {
            CT_bCT_XM = 0;
        }
        
        if(CT_bDW)
        {
            CT_bDW = 0;
            if(CT_MotorStep == 0)CT_MotorStep = 1;
        }
        
        if(CT_bAuto_ZL)
        {
            CT_bAuto_ZL = 0;
        }
        
        if(CT_bHM)
        {
            CT_bHM = 0;
        }
        
        if(CT_bMotor)
        {
            if(!X_DRV)AxisContinueMove(X_AXIS,20,CT_DIR_N);  
        }
        
        if((QD_MotorStep == 0) && !CT_bMotor && (CT_AutoStep == 0))
        {
            if(X_DRV)AxisEgmStop(X_AXIS);
        }
    
        if(CT_bZDP)
        {
            CT_bZDP = 0;
            CT_ZDP_Delay = dwTickCount + CT_PARAM_ZDP_TIME;
            CT_ZDP_OUT = 1;
        }
    }
    

}


void ChuanTou_AlarmProtect(void)
{ 

}


//振动盘控制
void ChuanTou_ZhenDongAction(void)
{
    if(bRunning)
    {
        if(CT_PARAM_ZDP_MODE)
        {
            if(!CT_ZDP_IN && (CT_ZDP_OUT == 0))
            {
                CT_ZDP_OUT = 1;
                CT_ZDP_Delay = dwTickCount + CT_PARAM_ZDP_TIME;
            }
            
            if((dwTickCount >= CT_ZDP_Delay) && (CT_ZDP_OUT == 1))
            {
                 CT_ZDP_OUT = 0;  
            }    
        }
        else
        {
            if(CT_ZDP_IN)CT_ZDP_OUT = 0;
            if(!CT_ZDP_IN)CT_ZDP_OUT = 1;
        }
    }
    else
    {
        if(!CT_ZDP_IN && (CT_ZDP_OUT == 0))
        {
            CT_ZDP_OUT = 1;
            CT_ZDP_Delay = dwTickCount + CT_PARAM_ZDP_TIME;
        }
        
        if((dwTickCount >= CT_ZDP_Delay) && (CT_ZDP_OUT == 1))
        {
             CT_ZDP_OUT = 0;  
        }     
    }
}

//送料
void ChuanTou_SongLiaoAction(void)
{
   switch(CT_SL_Step)
   {
       case 1:
       {
           if(cSongLiaoOk)
           {
                CT_SL_Step = 0;
           }
           else
           {
                CT_SL_Step  = 2;
                CT_SL_Delay = dwTickCount + 1000;
           }
       }break;
       case 2:
       {
           if(CT_SL_ORIGIN_IN && CT_CR_ORIGIN_IN && CT_XM_ORIGIN_IN)
           {
               CT_SL_Delay = dwTickCount + CT_PARAM_SL_DELAY; 
               CT_SL_Step  = 3;
           }
           else if(dwTickCount >= CT_SL_Delay)
           {
               if(!CT_SL_ORIGIN_IN)CT_SetAlarmCode(CT_SL_ORIGIN_ALARM);
               else if(!CT_CR_ORIGIN_IN)CT_SetAlarmCode(CT_CR_ORIGIN_ALARM);  
               else if(!CT_XM_ORIGIN_IN)CT_SetAlarmCode(CT_XM_ORIGIN_ALARM); 
           }
       }
        break;
       case 3:
       if(dwTickCount >= CT_SL_Delay)
       {
            CT_SL_VAVEL = 1;
            CT_SL_Delay = dwTickCount + VAVLE_ALARM_TIME;   
            CT_SL_Step  = 4;
       }
       break;  
       case 4:
       {
           if(CT_SL_LIMIT_IN)
           {
               CT_SL_Step  = 5;
               CT_SL_Delay = dwTickCount + CT_PARAM_SL_DELAY_BACK;
           }
           else if(dwTickCount >= CT_SL_Delay)
           {
               CT_SetAlarmCode(CT_SL_LIMIT_ALARM);
           }
       }break;
       case 5:
       {
           if(dwTickCount >= CT_SL_Delay)
           {
               CT_SL_VAVEL  = 0; 
               CT_SL_Delay = dwTickCount + VAVLE_ALARM_TIME;
               CT_SL_Step  = 6;  
           }
       }break;
       case 6:
       {
           if(!CT_SL_LIMIT_IN)
           {
               cSongLiaoOk = 1;
               CT_SL_Step  = 0;
           }
           else if(dwTickCount >= CT_SL_Delay)
           {
               CT_SetAlarmCode(CT_SL_LIMIT_ALARM);
           }       
       }
       break;
   }
}


//横送料
void ChuanTou_HengSongLiao_Action(void)
{
/*    switch(CT_HSL_Step)   
    {
        case 1:
        {
            if(CT_SONG_LIAO_ORIGIN_IN && CT_HSL_ORIGIN_IN && CT_XIA_MU_IN && cSongLiaoOk)
            {
                CT_HSL_Step = 2;
            }
            else if(!bRunning)
            {
                CT_HSL_Step = 0;
            }
        }
        break;
        case 2:
        {
            CT_HSL_VAVLE = 1;
            cSongLiaoOk  = 0;
            CT_HSL_Delay = dwTickCount + VAVLE_ALARM_TIME;
            CT_HSL_Step = 3;
        }
        break;
        case 3:
        {
            if(CT_HSL_LIMIT_IN)
            {
                CT_ZL_XM_VAVLE = 1;
                CT_HSL_Delay = dwTickCount + VAVLE_ALARM_TIME;
                CT_HSL_Step = 4;
            }
            else if(dwTickCount >= CT_HSL_Delay)
            {
                CT_SetAlarmCode(CT_HSL_LIMIT_ALARM);
            }
        }
        break;
        case 4: 
        { 
            if(CT_ZHUANG_LIAO_IN)
            {
                CT_HSL_Delay = dwTickCount + CT_PARAM_LOCK_TIME;
                CT_HSL_Step = 5;
            }
            else if(dwTickCount >= CT_HSL_Delay)
            {
                CT_SetAlarmCode(CT_ZL_LIMIT_ALARM);
            }            
        }
        break;
        case 5:
        if(dwTickCount >= CT_HSL_Delay)
        {
            CT_GJ_VAVLE = 1;
            CT_HSL_Delay = dwTickCount + CT_PARAM_GJ_TIME;
            CT_HSL_Step = 6;
        }
        break;
        case 6:
            CT_HSL_VAVLE = 0;
            CT_HSL_Delay = dwTickCount + VAVLE_ALARM_TIME;
            CT_HSL_Step = 7;             
        break;
        case 7:
            if(CT_HSL_ORIGIN_IN)
            {
                CT_ZL_XM_VAVLE = 0;
                CT_HSL_Step = 8;       
            }
            else if(dwTickCount >= CT_HSL_Delay)
            {
                CT_SetAlarmCode(CT_HSL_ORIGIN_ALARM);
            }
        break;
        case 8:
            CT_HSL_Step = 0;
            CT_SL_Step =  1;
        break;
    }
*/
}

void ChuanTou_ZhuangLiaoStep(void)
{
    switch(CT_ZL_Step)
    {
      case 1: 
      {
          if(cZhuangLiaoOk)CT_ZL_Step  = 0;
          else 
          {
              CT_CR_VAVLE = 0;
              CT_ZL_Delay = dwTickCount + VAVLE_ALARM_TIME;
              CT_ZL_Step  = 2;
          }
      }break;
      case 2: 
      {
          if(CT_CR_ORIGIN_IN)
          {
              if(CT_SL_Step == 0) CT_SL_Step = 1;
              CT_ZL_Step = 3;    
          }
          else if(dwTickCount >= CT_ZL_Delay)
          {
              CT_SetAlarmCode(CT_CR_ORIGIN_ALARM);
          }
      }
      break;
      case 3: 
      {
          if(CT_SL_Step == 0)
          {
              CT_ZL_Step = 4;
              CT_ZL_Delay = dwTickCount + 1000;
          }
      }break;
      case 4: 
      {
          if(CT_XM_ORIGIN_IN)
          {
               CT_XM_VAVLE = 1;
               cZLCnt++;
               CT_ZL_Delay = dwTickCount + 1000;
               CT_ZL_Step = 5;
          }
          else if(dwTickCount >= CT_ZL_Delay)
               CT_SetAlarmCode(CT_XM_ORIGIN_ALARM);
         
      }break;
      case 5: 
      {
          if(CT_DZ_IN)
          {
              cZhuangLiaoOk = 1;         
              cZLCnt        = 0;
              CT_ZL_Step = 0;
          }
          else if(dwTickCount >= CT_ZL_Delay)
          {
              if(cZLCnt >= 2)
              {
                  cSongLiaoOk = 0;
                  cZhuangLiaoOk = 0;  
                  CT_SetAlarmCode(CT_DZ_ALARM);
              }
              else 
              {
                  cSongLiaoOk = 0;
                  CT_XM_VAVLE = 0;  
                  CT_ZL_Step = 1;
              }
          }

      }
      break;
      default:;
    }
}

void ChuanTou_CLT_Step(void)
{
    static long xm_buff;
    static long dwRealPosY;
    
    dwRealPosY = GetPos(Y_AXIS);
    switch(CT_CLT_Step)
    {
        case 1:
          CT_CLT_Delay = dwTickCount + 1000;
          CT_QYD_VAVLE = 1;
          CT_HYD_VAVLE = 1;
          CT_CLT_Step = 2;
        break;
        case 2:          
          if(CT_CR_ORIGIN_IN)
          {
              CT_CLT_Delay = dwTickCount + CT_PARAM_FD_DELAY;
              CT_CLT_Step = 3;
          }
          else if(dwTickCount >= CT_CLT_Delay)
          {   
              CT_SetAlarmCode(CT_CR_ORIGIN_ALARM);
          }
        break;
        case 3:
          if(dwTickCount >= CT_CLT_Delay)
          {
              CT_FD_VAVLE = 1;
              if(CT_PARAM_HL_ENABLE)CT_HL_VAVLE = 1;
              CT_CLT_Delay = dwTickCount + VAVLE_ALARM_TIME;
              CT_CLT_Step = 4;
          }
        break;
        case 4:
          if(CT_FD_IN)
          {
              CT_CLT_Delay = dwTickCount + CT_PARAM_XM_DELAY;
              CT_CLT_Step = 5;
          }
          else if(dwTickCount >= CT_CLT_Delay)
              CT_SetAlarmCode(CT_FD_IN_ALARM);
        break;  
        case 5:
          if(dwTickCount >= CT_CLT_Delay)
          {
               if(CT_ZL_Step == 0)
               {
                   CT_ZL_Step = 1;
                   CT_CLT_Step = 6;
               }
          }
        break;
        case 6:
          if(CT_ZL_Step == 0)
          {
              if(CT_PARAM_CR_MODE == 0)
              {                 
                  CT_CR_Delay   = dwTickCount + CT_PARAM_CR_DELAY;  
                  CT_FDDelay   = dwTickCount + CT_PARAM_FD_BACK_DELAY;                          
                  CT_CLT_Delay = dwTickCount + VAVLE_ALARM_TIME;       
                  CT_CLT_Step = 7;
              }
              else
              {
                  xm_buff = dwRealPosY; 
                  CT_CRDR_OUT = CT_Y_DIR_P;
                  if(!Y_DRV)AxisContinueMove(Y_AXIS,CT_PARAM_CR_SPEED,QD_DIR_P);
                  
                  CT_CLT_Delay = dwTickCount + MOTOR_ALARM_TIME;       
                  CT_CLT_Step = 20;              
              }
          }  
        break;
        case 7:
              if(dwTickCount >= CT_CR_Delay)CT_CR_VAVLE  = 1; 
              if(dwTickCount >= CT_FDDelay)CT_FD_VAVLE  = 0;  
              
              if(CT_FD_IN_DW)
              {
                  if(CT_PARAM_HL_ENABLE)CT_HL_VAVLE  = 0;
                  CT_XM_VAVLE  = 0;  
                  CT_QYD_VAVLE = 0;
                  CT_HYD_VAVLE = 0;                
              }
          
              if(CT_CR_LIMIT_IN)
              {   
                  if(CT_PARAM_HL_ENABLE)CT_HL_VAVLE  = 0;
                  CT_FD_VAVLE  = 0;
                  CT_XM_VAVLE  = 0;  
                  CT_QYD_VAVLE = 0;
                  CT_HYD_VAVLE = 0;  
                  CT_CLT_Step = 8;
                  CT_CLT_Delay = dwTickCount + VAVLE_ALARM_TIME;  
              }
              else if(dwTickCount >= CT_CLT_Delay)
                  CT_SetAlarmCode(CT_CR_LIMIT_ALARM);
        break; 
        case 8:
          if(!CT_FD_IN)
          {
             CT_CLT_Step = 9;
             CT_CLT_Delay = dwTickCount + CT_PARAM_CR_BACK_DELAY;  
          }
          else if(dwTickCount >= CT_CLT_Delay)
              CT_SetAlarmCode(CT_FD_IN_ALARM);
        break;
        case 9:
          if(dwTickCount >= CT_CLT_Delay)
          {
             if(CT_PARAM_CR_MODE == 0)
             {
                 CT_CR_VAVLE = 0;
                 CT_CLT_Delay = dwTickCount + VAVLE_ALARM_TIME;  
                 CT_CLT_Step = 13;
             }
             else
             {
                 CT_CRDR_OUT = CT_Y_DIR_N;
                 if(!Y_DRV)AxisContinueMove(Y_AXIS,CT_PARAM_CR_SPEED,QD_DIR_N);               
                 CT_CLT_Delay = dwTickCount + VAVLE_ALARM_TIME;  
                 CT_CLT_Step = 30;
             }
          }
        break;
        case 13:
          if(CT_CR_ORIGIN_IN)
          {
             CT_CLT_Step = 0; 
             cSongLiaoOk = 0; // 穿完回到位就可以送料
             cZhuangLiaoOk = 0;
          }
          else if(dwTickCount >= CT_CLT_Delay)
              CT_SetAlarmCode(CT_CR_ORIGIN_ALARM);          
        break;
        case 20:
 //         if((dwRealPosY - xm_buff) >= CT_PARAM_XM_BACK_DELAY)CT_XM_VAVLE  = 0;   
          
          if(CT_CR_LIMIT_IN)
          {
              AxisDecStop(Y_AXIS);  
              CT_CLT_Step = 21; 
          }
          else if(dwTickCount >= CT_CLT_Delay)
            CT_SetAlarmCode(CT_CR_LIMIT_ALARM);
        break;
        case 21:
          if(!Y_DRV)
          {
             CT_CLT_Step = 7; 
          }
        break;
        case 30:
          if(CT_CR_ORIGIN_IN)
          {
             AxisDecStop(Y_AXIS);  
             CT_CLT_Step = 31; 
          }
          else if(dwTickCount >= CT_CLT_Delay)
              CT_SetAlarmCode(CT_CR_ORIGIN_ALARM);          
        break;
        case 31:
          if(!Y_DRV)
          {
             CT_CLT_Step = 0; 
             cSongLiaoOk = 0; // 穿完回到位就可以送料
          }
        break;
    }
}
void ChuanTou_AutoAction(void)
{  
    if(bRunning)
    {
        switch(CT_AutoStep)
        {
            case 1:
                  if(dwTickCount >= CT_AutoDelay)
                  {	
                      CT_AutoStep = 2;	    
                      if(CT_MotorStep == 0)
                      {
                          CT_MotorStep = 1;
                      }
                  }	
                  break;	
            case 2:	 
                  if(CT_MotorStep == 0)
                  {                        
                       CT_AutoStep = 3;	
                  }
                  break;	
            case 3:	
                  if((CT_MotorStep == 0) && (CT_CLT_Step == 0))
                  {   
                      CT_CLT_Step = 1;                 
                      CT_AutoStep = 4;	    
                  }              
                  break;
            case 4:
                     if((CT_CLT_Step == 0) || (CT_CLT_Step >= 10))
                     {
                         CT_AutoStep = 5;	
                     }
                  break;
            case 5:	 
                      dwZipCnt++;
                      AddToTal(CT_TOTAL_ADDR);
                      CalProSP(CT_PROSPEED_ADDR);
                      
                      
                      if(SingOneFlg)
                      {
                          bRunning = 0;
                          CT_AutoStep = 0; 
                          SingOneFlg = 0;
                      }
                      else
                      {
                          CT_AutoStep = 1;                        
                          CT_AutoDelay = dwTickCount + CT_PARAM_CYCLE_DELAY;
                      }
                  break;
        }	
    }
}


void ChuanTou_StepCheckStart(void)
{
//		启动      			
    if((CT_START_IN_UP) || bStart || CT_bSingle)
    {
        bStart = 0;
        
        if(!bRunning && (CT_AutoStep == 0))
        {
            if(CT_FD_IN)CT_SetAlarmCode(CT_FD_IN_ALARM);
            else if(CT_DZ_IN)CT_SetAlarmCode(CT_DZ_ALARM);
            else if(!CT_XM_ORIGIN_IN)CT_SetAlarmCode(CT_XM_ORIGIN_ALARM);
            else if(!CT_CR_ORIGIN_IN)CT_SetAlarmCode(CT_CR_ORIGIN_ALARM);
            else if(!CT_SL_ORIGIN_IN)CT_SetAlarmCode(CT_SL_ORIGIN_ALARM);
            else if(CT_QYD_VAVLE | CT_HYD_VAVLE | CT_HL_VAVLE)CT_SetAlarmCode(CT_OUT_ALARM);
            {
                bRunning    = 1;
                
                
                CT_AutoStep = 2;
                cSongLiaoOk   = 1;
                cZhuangLiaoOk = 0;
                if(CT_bSingle) SingOneFlg= 1;
                dwZipCnt      = 0;
            }
        }
        CT_bSingle = 0;
    }

   STOP_IN_FLAG_OLD = STOP_IN_FLAG;
    
   if(CT_STOP_IN)
   {
       if(STOP_FILTER < 200)
       {
           STOP_FILTER++;
       }
       else
           STOP_IN_FLAG = 1;
   }
   else if(STOP_FILTER > 100)
   {
       STOP_FILTER--;
   }
   else
       STOP_IN_FLAG = 0;

    //停止
   if((STOP_IN_FLAG && !STOP_IN_FLAG_OLD) || bStop)
   {
        bStop = 0;
        
        if(bRunning)
        {
            bRunning   = 0;
            CT_AutoStep   = 0;
            CT_MotorStep  = 0;
            CT_AutoDelay   = dwTickCount;
            CT_MotorDelay = dwTickCount;
            AxisDecStop(X_AXIS);  
            SingOneFlg  = 0;        
        }
        else
        {
            bRunning   = 0;
            CT_AutoStep   = 0;
            CT_MotorStep  = 0;
            CT_HeMuStep   = 0;
            CT_SL_Step    = 0;
            CT_HSL_Step   = 0;
            CT_ZL_Step    = 0;
            CT_CLT_Step   = 0;
            CT_ZD_Step    = 0;
            
            CT_AutoDelay   = dwTickCount;
            CT_MotorDelay  = dwTickCount;
            CT_SL_Delay    = dwTickCount;
            CT_HSL_Delay   = dwTickCount;
            CT_ZL_Delay    = dwTickCount;
            CT_FDDelay =  dwTickCount; 
            CT_CLT_Delay   =  dwTickCount; 
            CT_ZDP_Delay   =  dwTickCount; 
            
            SingOneFlg  = 0; 
            AxisDecStop(X_AXIS);  
            
            if(CT_PARAM_CR_MODE)//按停止电机复位
            {
                if(!CT_CR_ORIGIN_IN)
                {
                    if(CT_MotorStep == 0)CT_MotorStep = 100;
                }
            }
            
            CT_SZ_OUT   = CT_MOTOR_Y_DISEN;
            SetEn(X_AXIS,CT_MOTOR_DISEN);
            
            CT_FD_VAVLE = 0;
            CT_XM_VAVLE = 0;
            CT_SL_VAVEL = 0;
            CT_QYD_VAVLE = 0;
            CT_HL_VAVLE  = 0;
            CT_HYD_VAVLE = 0;
            CT_CR_VAVLE  = 0; 
            CT_ZDP_OUT   = 0;
            CT_HSL_VAVLE = 0;   
            
            cSongLiaoOk  = 0;
            cZhuangLiaoOk = 0;
            cZLCnt       = 0;
            if(GetAlarmCode(CT_ALARM_ADDR) != 0)SetAlarmCode(CT_ALARM_ADDR,0);
        }
    }
    
    if(bAlarmStop)
    {
        bAlarmStop = 0;
        bRunning   = 0;
        CT_AutoStep   = 0;
        CT_MotorStep  = 0;
        CT_AutoDelay   = dwTickCount;
        CT_MotorDelay = dwTickCount;
        SingOneFlg  = 0; 
        AxisDecStop(X_AXIS); 
        AxisDecStop(Y_AXIS);
    }
    
}   


//电机动作
void ChuanTou_Motor(void)	 //
{
        static long back_buff,save_buff;
        
        dwRealPos = GetPos(X_AXIS);
        
	switch(CT_MotorStep)
	{
		case 0:
			break;
                case 1:
                        if(GetDir(X_AXIS) == CT_MOTOR_EN)
                        {
                            SetEn(X_AXIS,CT_MOTOR_EN);
                            CT_MotorDelay = dwTickCount + 50;
                        }
                        
                        if(CT_SZ_OUT != CT_MOTOR_Y_EN)
                        {
                            CT_SZ_OUT   = CT_MOTOR_Y_EN;    
                            CT_MotorDelay = dwTickCount + 50;
                        }
                        
                        CT_MotorStep = 2;
                        break;
                case 2: 
                        if(dwTickCount >= CT_MotorDelay)
                        {
                            save_buff     = dwRealPos;
                            if(!X_DRV)AxisContinueMove(X_AXIS,CT_PARAM_HSPEED,QD_DIR_P);  
                            CT_MotorStep = 3;
                        }
                        break;
                case 3:
                        if(CT_GUO_LIAN_IN_DW)
                        {
                           // MoveChangSpeed(X_AXIS,CT_PARAM_LSPEED);
                            AxisChangeSpeed(X_AXIS,CT_PARAM_LSPEED);
                            CT_MotorStep   = 4;
                        }
                        else if((dwRealPos - save_buff) >= CT_PARAM_MAX_LENGTH)
                            CT_SetAlarmCode(CT_NO_ZIPPER_ALARM);
			break;
		case 4:
                        if(CT_GUO_LIAN_IN_UP)
                        {
                            back_buff      = dwRealPos;
                            CT_MotorStep   = 5;
                        }
                        else if((dwRealPos - save_buff) >= CT_PARAM_MAX_LENGTH)
                            CT_SetAlarmCode(CT_NO_ZIPPER_ALARM);
			break;
		case 5:
                        if((dwRealPos - back_buff) >= CT_PARAM_STOP_LENGTH)
                        {
                            AxisEgmStop(X_AXIS);
                            CT_MotorStep   = 6;
                        }
                     
			break;
		case 6:
                        if(!X_DRV)
                        {         
                            CT_MotorStep   = 0;
                            CT_QYD_VAVLE = 1;
                        }
			break;                  
                case 100:          //穿入电机慢慢复位动作
                        CT_CRDR_OUT = CT_Y_DIR_N;
                        if(!Y_DRV)AxisContinueMove(Y_AXIS,10,QD_DIR_N);   
                        
                        CT_MotorDelay = dwTickCount + MOTOR_ALARM_TIME;
                        CT_MotorStep   = 101;
                        break;
                case 101:
                        if(CT_CR_ORIGIN_IN)
                        {
                            AxisDecStop(Y_AXIS);  
                            CT_MotorStep   = 102;
                        }
                        else if(dwTickCount >= CT_MotorDelay)
                            CT_SetAlarmCode(CT_CR_ORIGIN_ALARM);
                        break;
                case 102:
                        if(!Y_DRV)
                        {
                            CT_MotorStep   = 0;
                        }
                        break;
	}
}



#endif