GC23005/Machine/XiDongMachine/XDJiaoYaDinCun.c

#include "global.h"

#if XI_DONG_MACHINE

long RealPosBuff              = 0;        //记录齿数
unsigned long ToothNumBuff    = 0;
unsigned long RemainKongWeiToothNum = 0; // 空位剩余齿数
unsigned char cFirstFlg = 0,cSeBiaoStep = 0;        
unsigned short SecondToothNum = 0;
unsigned char cCntEn = 0;
unsigned long dwSeBiaoDelayCnt = 0;

void StopProc(void);
void StartProc(void);
unsigned long CalEncodePulse(void);


void JYDC_DelayAction(void);
void JYDC_InitAction(void);
void JYDC_ExtiActionX30(void);
void JYDC_Action(void);


void ModeSelect(void);
void JYDC_ChuiQi(void);
void JYDC_XiaChong(void);
void JYDC_SetAlarmCode(unsigned alarm_code);
void JYDC_StartStopAction(void);
void JYDC_AutoRunStep(void);

unsigned long GetToothNum(unsigned long zipper_length);
unsigned long JYDC_ToothGetLength(unsigned long tooth_num);
unsigned char JYDC_STOP_IN_FLAG,JYDC_STOP_IN_FLAG_OLD,JYDC_STOP_FILTER;
unsigned char cAccSpeed = 5;
unsigned char cXC_OnceFlg = 0;

void JYDC_SetAlarmCode(unsigned alarm_code)
{
    SetAlarmCode(JYDC_ALARM_ADDR,alarm_code);
    JYDC_bAlarmStop = 1;
}

void JYDC_DelayAction(void)
{
}

void JYDC_InitAction(void)
{   
    if(MODE_SELECT > 3)MODE_SELECT = 0;
    ModeSelect();
    CUR_LEN_JY = 0;
    JYDC_STOP_IN_FLAG = STOP_IN;
    JYDC_STOP_IN_FLAG_OLD = JYDC_STOP_IN_FLAG;
    SetAlarmCode(JYDC_ALARM_ADDR,0);
}

//手动动作
void JYDC_ManualAction(void)
{              
    //警告跳出
    if(bClearTotal)
    {
        bClearTotal       = 0;
        ClrcToTal(JYDC_TOTAL_ADDR);
    }
    
    if(!JYDC_bRunning)
    {
        if(JYDC_bAddTooth)
        {
            JYDC_bAddTooth = 0;    
            if(JYDC_PARAM_REVICHISHU < 65535)JYDC_PARAM_REVICHISHU++;  
        }
              
        if(JYDC_bDecTooth)
        {
            JYDC_bDecTooth = 0;
            if(JYDC_PARAM_REVICHISHU != 0)JYDC_PARAM_REVICHISHU--;           
        }
        	       
        if(JYDC_bManXiaChong || JYDC_ManXiaChong_UP)
        {
            JYDC_bManXiaChong = 0;
            
            if(JYDC_XiaChongStep == 0)//if(!JYDC_bRunning && !STOP_IN && !X_DRV && (JYDC_XiaChongStep == 0))
            {
                JYDC_XiaChongStep = 1;
            }
        }
        
        if(JYDC_bManSuoChi)
        {
            JYDC_bManSuoChi = 0;       
            JYDC_SuoChi_VAVLE = ~JYDC_SuoChi_VAVLE; 
        }
        
        if(JYDC_bManChuiQi)
        {
            JYDC_bManChuiQi = 0;  
            if(JYDC_cChuiQiStep == 0)JYDC_cChuiQiStep = 1;
        }
        
        if(JYDC_bChangeLength)
        {
            JYDC_bChangeLength = 0;    
            ModeSelect();

            SetData32bits(31,CalInch(SET_ZIPPER_LENGTH));
            JYDC_PARAM_REVICHISHU = GetToothNum(SET_ZIPPER_LENGTH);
            CUR_LEN_JY        = 0;
            cFirstFlg  = 1;
            RemainKongWeiToothNum = 0;
            ToothNumBuff    = 0;
        }
        
        if(JYDC_bSetInch)
        {
            JYDC_bSetInch = 0;           
            SetData32bits(6,CalLengthCM(SET_ZIPPER_LENGTH_INCH));
            JYDC_bChangeLength = 1;
        }
        
        if(JYDC_bSetTooth)
        {
            JYDC_bSetTooth = 0;
            SetData32bits(6,JYDC_ToothGetLength(JYDC_PARAM_REVICHISHU));
            JYDC_bChangeLength = 1;
        }
        
        if(JYDC_bManXiChiMotor)
        {
            JYDC_bManXiChiMotor = 0;
            JYDC_XiChi_MOTOR    = ~JYDC_XiChi_MOTOR;
        }
        
        if(JYDC_bManMotor)
        {
            if(!X_DRV)
            {
                 JYDC_SuoChi_VAVLE = 0; 
                 AxisContinueMove(X_AXIS,10,0);
            }
        }
        else
        {
            if(X_DRV)
            {
                AxisEgmStop(X_AXIS);
                JYDC_SuoChi_VAVLE = 1;
            }
        }
    }
}

//高速输入X30/X16定时器中断
void JYDC_ExtiActionX30(void)
{
    CUR_LEN_JY++;
    
    if(JYDC_bRunning)
    {
        if(cAccSpeed > 1)cAccSpeed--;
    }
    if(JYDC_SEBIAO_MODE)
    {
        if(cSeBiaoStep == 1)
        {
            dwSeBiaoDelayCnt--;
            if(dwSeBiaoDelayCnt == 0)
            {
                CUR_LEN_JY = 0;
                cFirstFlg  = 1;
                RemainKongWeiToothNum = SET_KOUWEI_JY;
                cSeBiaoStep = 2;
            }
        }
        else if(cSeBiaoStep == 2)
        {
            if((CUR_LEN_JY >= ToothNumBuff) && JYDC_bRunning)
            {
                 AxisEgmStop(X_AXIS);
                 if(JYDC_bTest)
                 {
                     JYDC_bRunning = 0;
                     JYDC_AutoStep = 0;
                     JYDC_SuoChi_VAVLE = 1;
                     cSeBiaoStep   = 0;
                 }
            }        
        }
    }
    else
    {
        if((CUR_LEN_JY >= ToothNumBuff) && JYDC_bRunning)
        {
             AxisEgmStop(X_AXIS);
             if(JYDC_bTest)
             {
                 JYDC_bRunning = 0;
                 JYDC_AutoStep = 0;
                 JYDC_SuoChi_VAVLE = 1;
             }
        }
    }
}

//高速输入X31/X17外部色标信号中断
void JYDC_ExtiActionX31(void)
{
    if(cSeBiaoEn)
    {    cSeBiaoEn  = 0;
         dwSeBiaoDelayCnt = JYDC_SEBIAO_DELAY;
          if(dwSeBiaoDelayCnt == 0)
          {
              CUR_LEN_JY = 0;
              cFirstFlg  = 1;
              RemainKongWeiToothNum = SET_KOUWEI_JY;
              cSeBiaoStep = 2;
          }
          else
          {
               cSeBiaoStep = 1;          
          }
    }
}


//胶牙定寸动作
void JYDC_Action(void)
{	
//      DISPLAY_DATA0 = cNoPulseStopTime;
//      DISPLAY_DATA1 = GetCurSpeed(X_AXIS); 
//      DISPLAY_DATA2 = GetSetSpeed(X_AXIS); 
//      DISPLAY_DATA3 = GetState(X_AXIS); 
    if(JYDC_COUNT_IN_DW)  
    {
        JYDC_ExtiActionX30();
    }
    JYDC_ManualAction();
    JYDC_ChuiQi();
    JYDC_XiaChong();
    JYDC_StartStopAction();     
    JYDC_AutoRunStep();
}

void ModeSelect(void)
{
  switch(MODE_SELECT)   //模具选择
    {
        case 0:   //3#模具
                JY_JIZHUN_CISHU   = JY_JIZHUN_CISHU1;//基准长度对应齿数
                JY_JIZHUN_LEN     = JY_JIZHUN_LEN1;//基准长度
                SET_DAOKOU_JY     = SET_DAOKOU_JY1;   //机械模具刀口
                break;
        case 1:	//5#模具
                JY_JIZHUN_CISHU   = JY_JIZHUN_CISHU2;//基准长度对应齿数
                JY_JIZHUN_LEN     = JY_JIZHUN_LEN2;//基准长度
                SET_DAOKOU_JY     = SET_DAOKOU_JY2;   //机械模具刀口
                break;
        case 2:	//8#模具
                JY_JIZHUN_CISHU   = JY_JIZHUN_CISHU3;//基准长度对应齿数
                JY_JIZHUN_LEN     = JY_JIZHUN_LEN3;//基准长度
                SET_DAOKOU_JY     = SET_DAOKOU_JY3;   //机械模具刀口
                break;
        default: //特殊模具
                JY_JIZHUN_CISHU   = JY_JIZHUN_CISHU4;//基准长度对应齿数
                JY_JIZHUN_LEN     = JY_JIZHUN_LEN4;//基准长度
                SET_DAOKOU_JY     = SET_DAOKOU_JY4;   //机械模具刀口
                break;	
    }
    
    if((JY_JIZHUN_CISHU == 0) || (JY_JIZHUN_LEN == 0))
    {
        JY_JIZHUN_CISHU = 49;
        JY_JIZHUN_LEN = 1250;
        SET_DAOKOU_JY = 10;
    }	
}
//输入拉链长度返回所输入拉链长度所对应的齿数
unsigned long GetToothNum(unsigned long zipper_length)
{
    float length_buff1,tooth_buff,length_buff2;
    length_buff1 = zipper_length;
    tooth_buff   = JY_JIZHUN_CISHU;
    length_buff2 = JY_JIZHUN_LEN;
    return (unsigned long)round(length_buff1 * tooth_buff / length_buff2);
}

//输入齿数返回拉链长度
unsigned long JYDC_ToothGetLength(unsigned long tooth_num)
{
    float toothbuff1,tooth_buff,length_buff2;
    toothbuff1   = tooth_num;
    tooth_buff   = JY_JIZHUN_CISHU;
    length_buff2 = JY_JIZHUN_LEN;
    
    return (unsigned long)round((toothbuff1 * length_buff2 / tooth_buff));
}

//吹气动作
void JYDC_ChuiQi(void)
{   
      //吹气
    if(JYDC_XiaChong_IN_DW)
    {
        if(JYDC_cChuiQiStep == 0)JYDC_cChuiQiStep = 1;
    }
    
      
    switch(JYDC_cChuiQiStep)
    {
        case 1:
            JYDC_cChuiQiStep = 2;
            JYDC_ChuiQiDelay = dwTickCount+JYDC_PARAM_DELAY_CHUIQI_TIME;            
        break;
        case 2:
            if(dwTickCount >= JYDC_ChuiQiDelay)
            {
                JYDC_ChuiQiDelay = dwTickCount + JYDC_PARAM_CHUIQI_TIME;	
                JYDC_ChuiQi_VAVLE = 1;//
                JYDC_cChuiQiStep = 3;                
            }
        break;
        case 3:
            if(dwTickCount >= JYDC_ChuiQiDelay)
            {
                JYDC_ChuiQi_VAVLE = 0;//
                JYDC_cChuiQiStep = 0;
            }
        break;
    }
    
    if(JYDC_XiChiChuiQi_VAVLE)
    {
        if(JYDC_XiChiChuiQiDelay < dwTickCount)JYDC_XiChiChuiQi_VAVLE = 0;
    }
}
//下冲动作
void JYDC_XiaChong(void)
{
    if(JYDC_XiaChongStep == 1)
    {
         JYDC_ShaChe_VAVLE = 0; 
         JYDC_SuoChi_VAVLE = 1; //手动下冲不锁齿
         JYDC_XiaChongDelay = dwTickCount + XIACONG_DELAY_TIME*10;
         JYDC_XiaChongStep = 2;
    }
    else if((JYDC_XiaChongStep == 2) && (dwTickCount >= JYDC_XiaChongDelay))
    {
         JYDC_XiaChong_MOTOR = 1;
         JYDC_XiaChongDelay = dwTickCount + JYDC_PARAM_XC_ALARM_TIME * 10;
         JYDC_XiaChongStep = 3;
    }
    else if(JYDC_XiaChongStep == 3)
    {
        if(JYDC_XiaChong_IN_UP)
        {
            JYDC_XiaChong_MOTOR = 0;
            JYDC_XiaChongDelay = dwTickCount + JYDC_PARAM_SHACHE_DELAY_TIME * 10;
            JYDC_XiaChongStep = 4;
        }
        else if(dwTickCount >= JYDC_XiaChongDelay)
        {
            JYDC_SetAlarmCode(JYDC_XIACHONG_ARRIVE_ALARM);
            JYDC_XiaChong_MOTOR = 0;
            JYDC_ShaChe_VAVLE = 0;
        }
    }
    else if((JYDC_XiaChongStep == 4) && (dwTickCount >= JYDC_XiaChongDelay))
    {
         JYDC_ShaChe_VAVLE = 1;
         JYDC_XiaChongStep = 5;
         JYDC_XiaChongDelay = dwTickCount + JYDC_PARAM_SHACHE_TIME;
    }
    else if((JYDC_XiaChongStep == 5) && (dwTickCount >= JYDC_XiaChongDelay))
    {
         JYDC_ShaChe_VAVLE = 0;
         JYDC_XiaChongStep = 6;
    }
    else if(JYDC_XiaChongStep == 6)
    {
        if(!JYDC_bRunning)
        {
            CUR_LEN_JY = 0;    
            cFirstFlg  = 1;
            RemainKongWeiToothNum = 0;
        }
        JYDC_XiaChongStep = 0;
    }
}

//启动停止故障停止动作
void JYDC_StartStopAction(void)
{
    if((START_IN_UP || JYDC_bStart) && !JYDC_bRunning && (JYDC_AutoStep ==0))
    {
        if(GetTotal(JYDC_TOTAL_ADDR) >= SET_TOTAL)JYDC_SetAlarmCode(JYDC_TOTAL_ALARM);
        else
        {                      
            JYDC_AutoStep = 1;
            JYDC_bRunning = 1;
            JYDC_SuoChi_VAVLE = 0;
            JYDC_AutoDelay = 0;
            cAccSpeed     = 6;
        }
        JYDC_bStart   = 0;
    }
    
    if(JYDC_KaDai_IN && (GetAlarmCode(100) == 0))
    {
        JYDC_SetAlarmCode(JYDC_KADAI_ALARM);        
    }
    
    JYDC_STOP_IN_FLAG_OLD = JYDC_STOP_IN_FLAG;
    
   if(STOP_IN)
   {
       if(JYDC_STOP_FILTER < 50)
       {
           JYDC_STOP_FILTER++;
       }
       else
           JYDC_STOP_IN_FLAG = 1;
   }
   else if(JYDC_STOP_FILTER > 25)
   {
       JYDC_STOP_FILTER--;
   }
   else
       JYDC_STOP_IN_FLAG = 0;
   
    if((JYDC_STOP_IN_FLAG && !JYDC_STOP_IN_FLAG_OLD)|| JYDC_bStop)
    {
        JYDC_bStop = 0;

        if(JYDC_bRunning && (SingOneFlg == 0))
        {
            if(JYDC_cChuiQiStep == 0)JYDC_cChuiQiStep = 1;
            JYDC_AutoDelay  = dwTickCount;
            JYDC_bRunning       = 0;
            JYDC_AutoStep       = 0;
            JYDC_XiChi_MOTOR    = 0;
            AxisEgmStop(X_AXIS);
            JYDC_SuoChi_VAVLE = 1;
            SingOneFlg = 0;
        }
        else
        {
            if(JYDC_cChuiQiStep == 0)JYDC_cChuiQiStep = 1;
            JYDC_XiaChongDelay = dwTickCount;
            JYDC_AutoDelay  = dwTickCount;
            JYDC_SuoChi_VAVLE = 0;
            JYDC_bRunning       = 0;
            JYDC_AutoStep       = 0;
            JYDC_XiChi_MOTOR    = 0;
            AxisEgmStop(X_AXIS);
            SingOneFlg = 0;
        }
    }
    
    if(JYDC_bAlarmStop)
    {
        JYDC_bAlarmStop        = 0;     
        JYDC_XiaChongStep      = 0;
        JYDC_XiaChong_MOTOR    = 0;
        JYDC_ShaChe_VAVLE      = 1;
        AxisEgmStop(X_AXIS);
        JYDC_AutoStep     = 0;
        if(JYDC_bRunning)JYDC_SuoChi_VAVLE = 1;
        JYDC_bRunning          = 0;
        JYDC_XiaChongDelay = dwTickCount;
        JYDC_AutoDelay  = dwTickCount;
        SingOneFlg = 0;
    }
}

unsigned long  ToothTransPulse(unsigned long tooth)
{
    return(JY_JIZHUN_LEN * tooth * 2000 / 250 / JY_JIZHUN_CISHU);
}

unsigned char CheckSpeedError(unsigned char cspeed)
{
    unsigned char buff;
    buff = cspeed/cAccSpeed;
    
    if(buff < 5)return 5;
    else 
        return buff;
}

unsigned char CheckMaxLowSpeed(unsigned char cspeed)
{
    if(cspeed > JYDC_PARAM_SET_SPEED) 
      return JYDC_PARAM_SET_SPEED;
    else 
      return cspeed;
}
//自动运行动作
void JYDC_AutoRunStep(void)
{    
    DISPLAY_DATA0 = JYDC_AutoStep;
    DISPLAY_DATA1 = ToothNumBuff;
    DISPLAY_DATA2 = JYDC_XiaChongStep;
    DISPLAY_DATA3 = GetTotal(JYDC_TOTAL_ADDR);
     
    if(JYDC_bRunning)
    {
        unsigned short buff;
        if(JYDC_AutoStep == 1)
        {          
            if(JYDC_XICHI_MOTOR_MODE)JYDC_XiChi_MOTOR = 1;
            else JYDC_XiChi_MOTOR = 0;
            if(JYDC_SEBIAO_MODE)
            {
                JYDC_AutoStep     = 20;
                JYDC_SuoChi_VAVLE = 0;
                dwZipCnt          = 0;
            }
            else
            {
                if(CUR_LEN_JY >= JYDC_PARAM_REVICHISHU)
                {
                    CUR_LEN_JY        = 0;
                    JYDC_XiaChongStep = 1;
                    JYDC_AutoStep     = 2;
                }
                else
                   JYDC_AutoStep     = 2;
                
            }
        }              
        else if((JYDC_AutoStep == 2) && (JYDC_XiaChongStep == 0))
        {
            cFirstFlg       = 1;
            if(RemainKongWeiToothNum == 0)
            {
                RemainKongWeiToothNum = SET_KOUWEI_JY;    
            }
            else
            {
            
            }
               
            JYDC_AutoStep     = 3;
        }
        else if(JYDC_AutoStep == 3)
        {
            if(RemainKongWeiToothNum >= SET_DAOKOU_JY)
            {
                if(cFirstFlg)
                {   
                    if(JYDC_PARAM_REVICHISHU <= SET_KOUWEI_JY)
                    {
                        ToothNumBuff    = JYDC_PARAM_REVICHISHU + SET_DAOKOU_JY;
                        RemainKongWeiToothNum = 0;
                    }
                    else
                    {
//                        ToothNumBuff     = JYDC_PARAM_REVICHISHU + (RemainKongWeiToothNum-SET_DAOKOU_JY) + SET_DAOKOU_JY;
                      ToothNumBuff     = JYDC_PARAM_REVICHISHU+SET_DAOKOU_JY;
                      RemainKongWeiToothNum -= SET_DAOKOU_JY;

                    }
                }
                else
                {
                    ToothNumBuff     = ToothNumBuff + SET_DAOKOU_JY;
                    RemainKongWeiToothNum -= SET_DAOKOU_JY;
                }
            }
            else
            {
                if(cFirstFlg)
                {
                    ToothNumBuff = JYDC_PARAM_REVICHISHU - RemainKongWeiToothNum + SET_DAOKOU_JY;
                }
                else
                {
                    ToothNumBuff     = ToothNumBuff + RemainKongWeiToothNum;
                }
                RemainKongWeiToothNum = 0;
            }
            
            JYDC_SuoChi_VAVLE = 0;
//            JYDC_AutoDelay    = dwTickCount + 50;
            JYDC_AutoStep     = 4;
        }
        else if((JYDC_AutoStep == 4) && (dwTickCount >= JYDC_AutoDelay))
        {
            if((ToothNumBuff) > (JYDC_PARAM_LOW_LENGTH  +  CUR_LEN_JY))
            {              
                AxisContinueMove(X_AXIS,JYDC_PARAM_SET_SPEED,0);
                AxisChangeSpeed(X_AXIS,CheckSpeedError(JYDC_PARAM_SET_SPEED));
                JYDC_AutoDelay    = dwTickCount + NO_TOOTHCOUT_TIME * 10;
                JYDC_XiChiChuiQi_VAVLE = 1;
                JYDC_XiChiChuiQiDelay = dwTickCount + 200;
                JYDC_AutoStep     = 5;               
            }
            else
            {
                AxisContinueMove(X_AXIS,CheckMaxLowSpeed(JYDC_PARAM_LOW_SPEED),0);
                JYDC_AutoDelay    = dwTickCount + NO_TOOTHCOUT_TIME  * 10;
                JYDC_XiChiChuiQi_VAVLE = 1;
                JYDC_XiChiChuiQiDelay = dwTickCount + 200;
                JYDC_AutoStep     = 6;    
            } 
          
 /*           DISPLAY_DATA6 = ToothTransPulse(ToothNumBuff);
            DISPLAY_DATA7 = ToothTransPulse(JYDC_PARAM_LOW_LENGTH);
            AxisMoveTwoPos(X_AXIS,JYDC_PARAM_SET_SPEED,ToothTransPulse(ToothNumBuff - JYDC_PARAM_LOW_LENGTH),JYDC_PARAM_LOW_SPEED,ToothTransPulse(JYDC_PARAM_LOW_LENGTH+5),0);
            JYDC_AutoStep = 6;
            JYDC_AutoDelay    = dwTickCount + NO_TOOTHCOUT_TIME;*/
        }
        else if(JYDC_AutoStep == 5)
        {
            if(cFirstFlg)
            {
                if((ToothNumBuff) <= (JYDC_PARAM_LOW_LENGTH + CUR_LEN_JY))
                {
                    AxisChangeSpeed(X_AXIS,CheckMaxLowSpeed(JYDC_PARAM_LOW_SPEED));
                    cFirstFlg = 0;
                    JYDC_AutoStep     = 6;
                }
                else if(dwTickCount >= JYDC_AutoDelay)
                {
                    JYDC_SetAlarmCode(JYDC_COUNT_ALARM);
                }   
                else 
                {
                    if(X_DRV)AxisChangeSpeed(X_AXIS,CheckSpeedError(JYDC_PARAM_SET_SPEED));
                }
            }
            else
            {
                if((ToothNumBuff) <= (JYDC_PARAM_LOW_LENGTH + CUR_LEN_JY))
                {
                    AxisChangeSpeed(X_AXIS,CheckMaxLowSpeed(JYDC_PARAM_LOW_SPEED));
                    JYDC_AutoStep     = 6;
                }
                else if(dwTickCount >= JYDC_AutoDelay)
                {
                    JYDC_SetAlarmCode(JYDC_COUNT_ALARM);
                }    
                else
                {
                    if(X_DRV)AxisChangeSpeed(X_AXIS,CheckSpeedError(JYDC_PARAM_SET_SPEED));
                }
            }
        }       
        else if(JYDC_AutoStep == 6)
        {
            if(!X_DRV)
            {
                JYDC_XiaChongStep = 1;
                JYDC_AutoStep     = 7;
            }
            else if(dwTickCount >= JYDC_AutoDelay)
            {
                JYDC_SetAlarmCode(JYDC_COUNT_ALARM);
            }
        }
        else if(JYDC_AutoStep == 7)
        {
            if(JYDC_XiaChongStep == 0)
            {
                if(RemainKongWeiToothNum == 0)
                {
                    JYDC_AutoStep = 8;
                    JYDC_AutoDelay    = dwTickCount;
                }
                else
                {
                    JYDC_AutoStep = 3;
                    JYDC_AutoDelay    = dwTickCount;
                }

            }
        }
        else if((JYDC_AutoStep == 8) && (JYDCRunFlg == 1))
        {
             AddToTal(JYDC_TOTAL_ADDR);
             CUR_LEN_JY = 0;
             if((GetTotal(JYDC_TOTAL_ADDR) >= SET_TOTAL) || JYDC_bStop || SingOneFlg)
             {
                  if(JYDC_bStop || SingOneFlg)
                  {
                      JYDC_bStop    = 0;
                      JYDC_AutoStep = 0;
                      JYDC_bRunning = 0; 
                      SingOneFlg    = 0;
                      JYDC_SuoChi_VAVLE = 1;
                      JYDC_XiChi_MOTOR = 0;
                  }
                  else
                  {
                      JYDC_SetAlarmCode(JYDC_TOTAL_ALARM);             
                  }
             }    
             else
             {
                  JYDC_AutoStep = 1;
                  JYDC_AutoDelay = JYDC_PARAM_CYCLE_DELAY * 10 + dwTickCount;
                  CalProSP(JYDC_PROSPEED_ADDR);
             }
        } 
        else if((JYDC_AutoStep == 20)  && (dwTickCount >= JYDC_AutoDelay))   //色标动作
        {       
            cSeBiaoEn         = 1;   
            if(dwZipCnt == 0)
              AxisContinueMove(X_AXIS,CheckMaxLowSpeed(JYDC_PARAM_LOW_SPEED),0);
            else
              AxisContinueMove(X_AXIS,JYDC_PARAM_SET_SPEED,0);
            
            JYDC_AutoDelay    = dwTickCount + NO_TOOTHCOUT_TIME * 10;
            JYDC_XiChiChuiQi_VAVLE = 1;
            JYDC_XiChiChuiQiDelay = dwTickCount + 200;

            JYDC_AutoStep     = 21;               
        }
        else if(JYDC_AutoStep == 21) 
        {
            if(!X_DRV)
            {
                JYDC_AutoStep     = 22; 
                JYDC_AutoDelay =  dwTickCount;
            }
            else if(dwTickCount >= JYDC_AutoDelay)
            {
                JYDC_SetAlarmCode(JYDC_SEBIAO_ALARM);
            }
        }
        else if(JYDC_AutoStep == 22)
        {
            if(RemainKongWeiToothNum >= SET_DAOKOU_JY)
            {
                if(cFirstFlg)
                {   
                    if(JYDC_PARAM_REVICHISHU <= SET_KOUWEI_JY)
                    {
                        ToothNumBuff    = JYDC_PARAM_REVICHISHU + SET_DAOKOU_JY;
                        RemainKongWeiToothNum = 0;
                    }
                    else
                    {
                       ToothNumBuff     = JYDC_PARAM_REVICHISHU + (RemainKongWeiToothNum-SET_DAOKOU_JY) + SET_DAOKOU_JY;
                       RemainKongWeiToothNum -= SET_DAOKOU_JY;

                    }
                }
                else
                {
                    ToothNumBuff     = ToothNumBuff + SET_DAOKOU_JY;
                    RemainKongWeiToothNum -= SET_DAOKOU_JY;
                }         
            }
            else
            {
                if(cFirstFlg)
                {
                    ToothNumBuff = JYDC_PARAM_REVICHISHU - RemainKongWeiToothNum + SET_DAOKOU_JY;
                }
                else
                {
                    ToothNumBuff     = ToothNumBuff + RemainKongWeiToothNum;
                }
                RemainKongWeiToothNum = 0;
            }
            
            JYDC_AutoStep     = 23;
        }
        else if((JYDC_AutoStep == 23) && (dwTickCount >= JYDC_AutoDelay))
        {
            if((ToothNumBuff - CUR_LEN_JY) > JYDC_PARAM_LOW_LENGTH)
            {              
                if(!X_DRV)AxisContinueMove(X_AXIS,JYDC_PARAM_SET_SPEED,0);
                JYDC_AutoDelay    = dwTickCount + NO_TOOTHCOUT_TIME * 10;
                JYDC_XiChiChuiQi_VAVLE = 1;
                JYDC_XiChiChuiQiDelay = dwTickCount + 200;
                JYDC_AutoStep     = 24;               
            }
            else
            {
                if(!X_DRV)AxisContinueMove(X_AXIS,CheckMaxLowSpeed(JYDC_PARAM_LOW_SPEED),0);
                JYDC_AutoDelay    = dwTickCount + NO_TOOTHCOUT_TIME  * 10;
                JYDC_XiChiChuiQi_VAVLE = 1;
                JYDC_XiChiChuiQiDelay = dwTickCount + 200;
                JYDC_AutoStep     = 25;    
            } 
        }
        else if(JYDC_AutoStep == 24)
        {
            if(cFirstFlg)
            {
                if((ToothNumBuff - CUR_LEN_JY) <= (JYDC_PARAM_LOW_LENGTH))
                {
                    AxisChangeSpeed(X_AXIS,CheckMaxLowSpeed(JYDC_PARAM_LOW_SPEED));
                    cFirstFlg = 0;
                    JYDC_AutoStep     = 25;
                }
                else if(dwTickCount >= JYDC_AutoDelay)
                {
                    JYDC_SetAlarmCode(JYDC_COUNT_ALARM);
                }   
            }
            else
            {
                if((ToothNumBuff - CUR_LEN_JY) <= JYDC_PARAM_LOW_LENGTH)
                {
                    AxisChangeSpeed(X_AXIS,CheckMaxLowSpeed(JYDC_PARAM_LOW_SPEED));
                    JYDC_AutoStep     = 25;
                }
                else if(dwTickCount >= JYDC_AutoDelay)
                {
                    JYDC_SetAlarmCode(JYDC_COUNT_ALARM);
                }           
            }
        }       
        else if(JYDC_AutoStep == 25)
        {
            if(!X_DRV)
            {
                JYDC_XiaChongStep = 1;
                JYDC_AutoStep     = 26;
            }
            else if(dwTickCount >= JYDC_AutoDelay)
            {
                JYDC_SetAlarmCode(JYDC_COUNT_ALARM);
            }
        }
        else if(JYDC_AutoStep == 26)
        {
            if(JYDC_XiaChongStep == 0)
            {
                if(RemainKongWeiToothNum == 0)
                {
                    JYDC_AutoStep = 27;
                    JYDC_AutoDelay    = dwTickCount;
                }
                else
                {
                    JYDC_AutoStep = 22;
                    JYDC_AutoDelay    = dwTickCount;
                }
            }
        }
        else if((JYDC_AutoStep == 27) && (JYDCRunFlg == 1))
        {
             AddToTal(JYDC_TOTAL_ADDR);
             dwZipCnt++;
             CUR_LEN_JY = 0;
             cSeBiaoStep = 0;
             if((GetTotal(JYDC_TOTAL_ADDR) >= SET_TOTAL) || JYDC_bStop || SingOneFlg)
             {
                  if(JYDC_bStop || SingOneFlg)
                  {
                      JYDC_bStop    = 0;
                      JYDC_AutoStep = 0;
                      JYDC_bRunning = 0; 
                      SingOneFlg    = 0;
                      JYDC_SuoChi_VAVLE = 1;
                      JYDC_XiChi_MOTOR = 0;
                  }
                  else
                  {
                      JYDC_SetAlarmCode(JYDC_TOTAL_ALARM);             
                  }
             }    
             else
             {
                  JYDC_AutoStep = 1;
                  JYDC_AutoDelay = JYDC_PARAM_CYCLE_DELAY * 10 + dwTickCount;
                  CalProSP(JYDC_PROSPEED_ADDR);
             }
        } 
    }
}


#endif