GC19038/Machine/HaiHuaMachine/JiaoYaDinCun.c

#include "global.h"
#include "JiaoYaDinCun.h"

#if HAI_HUA_MACHINE

long RealPosBuff              = 0;        //记录齿数
unsigned long ToothNumBuff    = 0;
unsigned long RemainKongWeiToothNum = 0; // 空位剩余齿数
unsigned char cFirstFlg = 0;        
unsigned short SecondToothNum = 0;
unsigned char SingOneFlg = 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);

/*
#define DATA_SIZE 100
#define MAX_FREQ 20000
#define MIN_FREQ 600
#define MAX_SPEED 99
#define FLEXIBLE 3  //2~15数值越大中间越陡刚启动和越接近最高速越平滑
#define ACC_TIME 200
#define DEC_TIME 200

unsigned short FreqTab[DATA_SIZE];
unsigned char  AccTab[DATA_SIZE];
unsigned char  DecTab[DATA_SIZE];
unsigned short CALFREQTab[DATA_SIZE];
void CalculateSModelLine(unsigned short *fre,float  len,  float fre_max,  float fre_min, float flexible)
{
    int i=0;
    float deno ;
    float melo ;
    float delt = fre_max-fre_min;
    float freq_buff;
    float acc_time_buff,dec_time_buff;
    float pulse_buff;
    
    acc_time_buff = ACC_TIME;
    acc_time_buff = acc_time_buff / (len+1);

    dec_time_buff = DEC_TIME;
    dec_time_buff = dec_time_buff / (len+1);    
    for(i=0; i<len; i++)
    {
        melo = flexible * (i-len/2) / (len/2);
        deno = 1.0 / (1 + expf(-melo));   //expf is a library function of exponential(e) 
        freq_buff = delt * deno + fre_min;
        *(fre+i)  = 11059200 / (unsigned short)freq_buff;
        CALFREQTab[i] = freq_buff;    
    }

    for(i = 0;i<(DATA_SIZE /2);i++)
	{
	   deno = i*acc_time_buff;   
	   AccTab[i] = (unsigned char)deno;
	   deno = (DATA_SIZE /2 - i)*acc_time_buff;  
       AccTab[(DATA_SIZE/2)+i] = (unsigned char)deno;
	   deno = i*dec_time_buff;  
	   DecTab[i] = (unsigned char)deno; 	
	   deno = (DATA_SIZE /2 - i)*dec_time_buff;  
       DecTab[(DATA_SIZE/2)+i] = (unsigned char)deno;
	}	

    for(i=(int)len;i<DATA_SIZE;i++)
    {
        *(fre+i)  = *(fre+(unsigned short)(len - 1));
        AccTab[i] = 0;
        DecTab[i] = 0;
    }
}
*/
void JYDC_SetAlarmCode(unsigned alarm_code)
{
    SetAlarmCode(JYDC_ALARM_ADDR,alarm_code);
    JYDC_bAlarmStop = 1;
}

void JYDC_DelayAction(void)
{
}




void JYDC_InitAction(void)
{   
//    float pulse_buff,length_buff;
   
//    JYDC_Param = (JYDC_ParamStrct *)&user_datas[0];
    
/*    pulse_buff = JYDC_Param->MotorCyclePulse;
    length_buff = JYDC_Param->MotorCyclePulse;
    
    XGearRatio = length_buff/pulse_buff;
    if(XGearRatio == 0)XGearRatio = 1;
    
    
    SetAccTime(X_AXIS,JYDC_Param->MotorAccTime);
    SetDecTime(X_AXIS,JYDC_Param->MotorDecTime);
     
    JYDC_SetAlarmCode(JYDC_NO_ALARM);
    SetPos(X_AXIS,0);
    */
    if(MODE_SELECT > 3)MODE_SELECT = 0;
    ModeSelect();
    CUR_LEN_JY = 0;
}

void SetZipperLength(unsigned short addr,unsigned long length)
{
    user_datas[addr + 1] = length >> 16;
    user_datas[addr]     = length & 0x00FF;
}



unsigned char test_buff[14]={1,2,3,4,5,6,7,8,9,10,11,12,13,14};
//手动动作
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;  
            JYDC_ChuiQi_VAVLE = 1;
            JYDC_ChuiQiDelay = dwTickCount + JYDC_PARAM_CHUIQI_TIME * 10;
        }
        
        if(JYDC_bChangeLength)
        {
            JYDC_bChangeLength = 0;    
            ModeSelect();

            SetZipperLength(31,CalInch(SET_ZIPPER_LENGTH));
            JYDC_PARAM_REVICHISHU = GetToothNum(SET_ZIPPER_LENGTH);
        }
        
        if(JYDC_bSetInch)
        {
            JYDC_bSetInch = 0;           
            SetZipperLength(6,CalLengthCM(SET_ZIPPER_LENGTH_INCH));
            JYDC_bChangeLength = 1;
        }
    }
}
//高速输入X30/X17外部中断
void JYDC_ExtiActionX30(void)
{
    CUR_LEN_JY++;
    DISPLAY_DATA0++;
    if((CUR_LEN_JY >= ToothNumBuff) && JYDC_bRunning)
    {
         AxisEgmStop(X_AXIS);
         if(JYDC_bTest)
         {
             JYDC_bRunning = 0;
             JYDC_AutoStep = 0;
             JYDC_SuoChi_VAVLE = 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);
}



//吹气动作
void JYDC_ChuiQi(void)
{
    if(JYDC_XiaChong_DW)
    {
        JYDC_ChuiQi_VAVLE = 1;
        JYDC_ChuiQiDelay = dwTickCount + JYDC_PARAM_CHUIQI_TIME * 10;		
    }
    
    if(JYDC_ChuiQi_VAVLE)
    {
        if(JYDC_ChuiQiDelay < dwTickCount)JYDC_ChuiQi_VAVLE = 0;			
    }
    
    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;
    }
    else if(JYDC_XiaChongStep == 5)
    {
        JYDC_XiaChongStep = 0;
    }
}

//启动停止故障停止动作
void JYDC_StartStopAction(void)
{
    if((START_IN_UP || JYDC_bStart) && !JYDC_bRunning && (JYDC_AutoStep ==0))
    {
        if(!JYDC_XiaChong_IN)JYDC_SetAlarmCode(JYDC_START_XIACHONG_ALARM);
        else
        {                      
            JYDC_AutoStep = 1;
            JYDC_bRunning = 1;
            JYDC_AutoDelay = 0;
        }
        JYDC_bStart   = 0;
    }
    
    if(JYDC_KaDai_IN && (GetAlarmCode(100) == 0))
    {
        JYDC_SetAlarmCode(JYDC_KADAI_ALARM);        
    }

            
    if(STOP_IN_UP || JYDC_bStop)
    {
        JYDC_bStop = 0;
        if(JYDC_bRunning)
        {
            SingOneFlg = 1;
/*            JYDC_ChuiQi_VAVLE = 1;
            JYDC_ChuiQiDelay = dwTickCount + JYDC_PARAM_CHUIQI_TIME * 10;
            JYDC_SuoChi_VAVLE = 1;
            JYDC_bRunning       = 0;
            JYDC_AutoStep       = 0;
            AxisEgmStop(X_AXIS);*/
        }
        else
        {
            CUR_LEN_JY = 0;
            JYDC_ChuiQi_VAVLE = 1;
            JYDC_ChuiQiDelay = dwTickCount + JYDC_PARAM_CHUIQI_TIME * 10;
            JYDC_SuoChi_VAVLE = 0;
            JYDC_bRunning       = 0;
            JYDC_AutoStep       = 0;
            AxisEgmStop(X_AXIS);
            DISPLAY_DATA0 = 0;
        }
    }
    
    if(JYDC_bAlarmStop)
    {
        JYDC_bAlarmStop        = 0;              
        AxisEgmStop(X_AXIS);
        JYDC_AutoStep     = 0;
        if(JYDC_bRunning)JYDC_SuoChi_VAVLE = 1;
        JYDC_bRunning          = 0;
    }
}

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

//自动运行动作
void JYDC_AutoRunStep(void)
{    
//    DISPLAY_DATA0 = JYDC_AutoStep;
//    DISPLAY_DATA1 = ToothNumBuff;
//    DISPLAY_DATA2 = SET_TOTAL;
 //   DISPLAY_DATA3 = GetTotal(JYDC_TOTAL_ADDR);
     
    if(JYDC_bRunning)
    {
        if(JYDC_AutoStep == 1)
        {
            JYDC_AutoStep     = 2;
        }
        else if((JYDC_AutoStep == 2))
        {
            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;
                       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 - CUR_LEN_JY) > JYDC_PARAM_LOW_LENGTH)
            {              
                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     = 5;               
            }
            else
            {
                AxisContinueMove(X_AXIS,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 - CUR_LEN_JY) <= (JYDC_PARAM_LOW_LENGTH))
                {
                    AxisChangeSpeed(X_AXIS,JYDC_PARAM_LOW_SPEED);
                    cFirstFlg = 0;
                    JYDC_AutoStep     = 6;
                }
                else if(dwTickCount >= JYDC_AutoDelay)
                {
                    JYDC_SetAlarmCode(JYDC_COUNT_ALARM);
                }   
            }
            else
            {
                if((ToothNumBuff - CUR_LEN_JY) <= JYDC_PARAM_LOW_LENGTH)
                {
                    AxisChangeSpeed(X_AXIS,JYDC_PARAM_LOW_SPEED);
                    JYDC_AutoStep     = 6;
                }
                else if(dwTickCount >= JYDC_AutoDelay)
                {
                    JYDC_SetAlarmCode(JYDC_COUNT_ALARM);
                }           
            }
        }       
        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;
                }
                else
                {
                    JYDC_AutoStep = 3;
                }

            }
        }
        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;
                  }
                  else
                  {
                      JYDC_SetAlarmCode(JYDC_TOTAL_ALARM);             
                  }
             }    
             else
             {
                  JYDC_AutoStep = 1;
                  JYDC_AutoDelay = JYDC_PARAM_CYCLE_DELAY * 10 + dwTickCount;
                  CalProSP(JYDC_PROSPEED_ADDR);
             }
        }    
    }
}

#endif