#include "global.h" #if ZHEN_YU_MACHINE == 1 static unsigned long cycletime,zlttime,cuanliantime,tuodaitime; static unsigned char comflag=0,JLTFLG,XMFLG,ONCERUNFLG1; void ExtiAcitionX02(void)//获取X，Y轴的实际位置 { } //故障报警 void CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag) { SetAlarmCode(CTHL_ALARM_ADDR,alarm_code); if(!CTHL_wltzi && !bCxcAuto ) bAlarmStop = 1; else if(bCxcAuto)//伺服告警松轴 { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); //SetEn(X_AXIS, MOTOR_DISEN); //SetEn(Y_AXIS, MOTOR_DISEN); } } //运行状态 void CTHL_Running_state(void) { user_datas[210] = CTHL_ZhuangLiaoStep; user_datas[211] = CTHL_FZ_MotorStep; user_datas[212] = CTHL_TD_MotorStep; user_datas[213] = CTHL_AutoStep; user_datas[214] = CTHL_LianLengthGLCheckFirst; //第一条过链感应测量的长度//Y轴测量的长度反转 user_datas[215] = CTHL_LianLengthGLCheck;//过链实时 user_datas[216] = CTHL_LianLengthCheckFirst;//测量第一条长度 user_datas[217] = CTHL_LianLengthCheck;//实时测量的长度 user_datas[218] = CTHL_LianLengthXFZCheckFirst;//X轴反转第一次 user_datas[219] = CTHL_LianLengthXFZCheck;//X轴实时 user_datas[220] = CTHL_LianLengthYFZCheckFirst;//Y轴第一次 user_datas[221] = CTHL_LianLengthYFZCheck; //Y轴实时 memcpy(&user_datas[222],&dwXRealPos,4); memcpy(&user_datas[224],&dwYRealPos_com,4); memcpy(&user_datas[226],&CTHL_FKGY_Length,4);//方块感应位置 memcpy(&user_datas[228],&CTHL_FKGY_STOP_Length,4);//方块停长度 memcpy(&user_datas[230],&CTHL_CXCD,4);//穿插销长度。CTHL_GLGY_FKDW_Length memcpy(&user_datas[232],&CTHL_GLGY_FKDW_Length,4); user_datas[234] = master1.send_count-master1.receive_count;//GetAlarm(X_AXIS); user_datas[235] = GetAlarm(Y_AXIS); user_datas[236] = servo_y.alarmcode; user_datas[237] = servo_y.IO_TO_COM; } void CTHL_AlarmProtect(void) { } //自动动作 void CTHL_AutoStepAction(void) { int freq; //测试用 if(dwTickCount >= CTHL_TestDelay) { CTHL_TestDelay = dwTickCount + 200; CTHL_TestLED = !CTHL_TestLED; CTHL_Test = 0; } if(bRunning) { switch(CTHL_AutoStep) { case 1: if(dwTickCount >= CTHL_AutoDelay) { cycletime=dwTickCount; bCxcAuto=0; CTHL_GF_Change = 0;//公分节转换 CTHL_AutoStep++; SetEn(X_AXIS, MOTOR_EN); //设置反转电机的扭矩和速度的设置 if(CTHL_cZipCnt<1) { freq=CTHL_PARAM_FZ_Speed+20000; set_servo_postotarr_limit(Y_AXIS,400,freq,freq,SERVO_TARR_CW); } else { freq=CTHL_PARAM_FZFirst_SPEED+SERVO_PARAM_SPEED_DIFFER; set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW); } Set_Ctrlmode_trans(Y_AXIS,POS_MODE); } break; case 2://有拉头，方块并定位 if(CTHL_ZhuangLiaoStep == 0&&bZhuangLiaoOkFlg&&CTHL_TD_MotorStep==0)//装拉头完成，方块定位 { if(CTHL_FKDW_IN)//无方块定位信号返回，//人工处理信号，不停机 { CTHL_wltzi=1;//不停机 CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);// E17方块钩针定位信号异常 } if(CTHL_LTou_Check)//无拉头信号返回，//人工处理信号，不停机 { CTHL_wltzi=1;//不停机 CTHL_SetAlarmCode(CTHL_XCDW_Limit_ALARM,bRunning);// E11拉头缺料感应异常 } if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)//自动与单步，单步用启动按钮进行 { if(!CTHL_CTXM_Limit_IN){//下模未到位，下模动作 CTHL_CTXM_VAVLE=1; CTHL_XCJC_VAVLE=1;//插销检测随着下模上升 CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; CTHL_AutoStep++; } else{//下模到位 CTHL_TXM_VAVLE = 1; //侧推 CTHL_FZYL_VAVLE=1;//压轮 CTHL_AutoStep=4; CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } } } break; case 3://下模到位 if(CTHL_CTXM_VAVLE&&CTHL_CTXM_Limit_IN) { CTHL_TXM_VAVLE = 1; //侧推 CTHL_FZYL_VAVLE=1;//压轮 CTHL_AutoStep++; CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE){//人工处理信号，不停机 CTHL_wltzi=1;//不停机 CTHL_SetAlarmCode(CTHL_CTXM_Limit_ALARM,bRunning); //E08 下模到位异常 } break; case 4://压轮到位，侧推布带到位，方块钩针升起 if(CTHL_TXM_VAVLE&&CTHL_TXM_Limit_IN)//侧推布带到位 { if(CTHL_FZYL_VAVLE&&CTHL_FZYL_Limit_IN)//压轮到位 { CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TXM_DELAY;//侧推延时时间 CTHL_AutoStep++; } else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE) { CTHL_wltzi=1;//不停机 CTHL_SetAlarmCode(CTHL_FZYL_Limit_ALARM,bRunning);//E22 右压轮到位异常 } } else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE) { CTHL_wltzi=1;//不停机 CTHL_SetAlarmCode(CTHL_TXM_Limit_ALARM,bRunning);//E13侧推布带到位异常 } break; case 5://侧推到位后延时一小段时间后，退方块定位 if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP){ CTHL_FKDW_VAVLE = 0; //方块钩针 if(servo_y.IO_TO_COM==0);//SetEn(X_AXIS, MOTOR_DISEN);//X轴松开使能 else { SetEn(Y_AXIS, MOTOR_DISEN);//设置扭矩做准备 if(get_tarr_set(Y_AXIS)!=2)//是否配置转矩限制失败 { SetEn(Y_AXIS, MOTOR_DISEN);//设置扭矩做准备 CTHL_wltzi=0;//停机 CTHL_SetAlarmCode(CTHL_SERVO_CONFIG_ALARM,bRunning);//伺服转矩配置失败 } } CTHL_AutoStep++; CTHL_AutoDelay = dwTickCount + CTHL_PARAM_FKDW_OUT_DELAY;//方块钩针回退延时 } } break; case 6://反转穿入 if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { //定位完成后，位置清零。 SetPos(X_AXIS, 0); SetPos(Y_AXIS, 0); CTHL_FZ_MotorStep=1; //反转穿入， CTHL_FZ_MotorDelay = dwTickCount +1;//延时穿入拖带 CTHL_AutoStep = 9; } } break; case 9://穿方块完成 if(CTHL_FZ_MotorStep==0)//等待穿入完成 { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { cBAOjin=0; CTHL_JCXBD_VAVLE=1;//插销夹布带， CTHL_AutoStep++; CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } } break; case 10://夹到位延时 if(CTHL_JCXBD_VAVLE&&CTHL_JCXBD_Limit_IN&&!X_DRV){ CTHL_AutoDelay = dwTickCount + CTHL_PARAM_JBD_DELAY+5; //布带先于气缸后退 AxisMovePosAccDec(X_AXIS,CTHL_PARAM_HLT_SPEED_Speed,-CTHL_PARAM_FZCXC_LENGTH, 1200,1200,20,20,0); if((CTHL_cZipCnt <1))//应该Y轴位置是准的，由于通讯的原因，二次读取。 { CTHL_LianLengthYFZCheckFirst=dwYRealPos_com; //若这两个值取值比较准的话，可以作为后面的参考 } CTHL_LianLengthYFZCheck=dwYRealPos_com; CTHL_AutoStep++; } else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE) { CTHL_wltzi=1;//不停机 CTHL_SetAlarmCode(CTHL_JCXBD_Limit_ALARM,bRunning);//E23 插销布带夹到位异常 } break; case 11://插销夹布后退 if(dwTickCount >= CTHL_AutoDelay){ if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP){ CTHL_CXHT_VAVLE=1;//插销布带后退动作 CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; CTHL_AutoStep++; } } break; case 12://插销夹布后退到位延时 if(CTHL_CXHT_VAVLE&&CTHL_CXHT_Limit_IN&&!X_DRV){ CTHL_AutoDelay = dwTickCount + CTHL_PARAM_CXBB_DELAY; CTHL_AutoStep++; } else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE) { CTHL_wltzi=1;//不停机 CTHL_SetAlarmCode(CTHL_CXHT_Limit_ALARM,bRunning);//插销回退到位异常 } break; case 13://插销摆臂动作 if(dwTickCount >= CTHL_AutoDelay){ if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP){ CTHL_XCBB_VAVLE=1; CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; CTHL_AutoStep++; } } break; case 14://插销摆臂到位延时 if(CTHL_XCBB_VAVLE&&CTHL_XCBB_Limit_IN){ CTHL_AutoDelay = dwTickCount + CTHL_PARAM_YSCX_DELAY; CTHL_AutoStep++; } else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE) { CTHL_wltzi=1;//不停机 CTHL_SetAlarmCode(CTHL_CXHT_Limit_ALARM,bRunning);//插销回退到位异常 } break; case 15:// if(dwTickCount >= CTHL_AutoDelay) { CTHL_AutoDelay = dwTickCount +0; CTHL_AutoStep++; } break; case 16: // if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP){ CTHL_TD_MotorStep =10; CTHL_TD_MotorDelay = dwTickCount+ 1; CTHL_AutoStep++; } } break; case 17: if((CTHL_TD_MotorStep == 0))//FZ电机运行完成 { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_PARAM_CYCLE_TIME=dwTickCount-cycletime; CTHL_AutoStep = 1; //开始循环 if(cStopMode==1 || bSingOneFlg) { cStopMode=2;//停机后循环完成动作 CTHL_ZhouWantwo=1;//做完两条 } AddToTal(CTHL_TOTAL_ADDR); AddToTal(CTHL_TRUST_ALARM_ADDR); CalProSP(CTHL_SPEED_ADDR); //公分节那条也不加 if((CTHL_cZipCnt<2) && !CTHL_GF_Change) CTHL_cZipCnt++; if((CTHL_NoLaLian_IN))CTHL_cZipCnt = 0; if(bSingOneFlg || CTHL_bHandWork_MODE) { CTHL_AutoStep = 0; bRunning = 0; bSingOneFlg = 0; } } } break; } } } void CTHL_CheckStart(void) { if(CTHL_START_IN_UP || bStart) { if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE)//伺服告警DB9模式 { SetClr(X_AXIS, MOTOR_NOALARM); SetClr(Y_AXIS, MOTOR_NOALARM); CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000;//清除伺服告警 } if(CTHL_PARAM_DBSF_MODE)//外部接线模式 { if(!CTHL_SF_ALR_IN){ CTHL_SF_ALR_CLR=1; CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000; } } SetAlarmCode(CTHL_ALARM_ADDR,0); //清除告警 } if(CTHL_wltzi){//装拉头故障标志 if(CTHL_START_IN_UP || bStart) { if(bRunning) { CTHL_wltzi=0;//清除标志 CTHL_AutoStep = 1;//自动重新开始 CTHL_ZhuangLiaoStep = 1;//重新装拉头 bTuiLaTouOkFlg = 1; bZhuangLiaoOkFlg= 0;//清除标志 CTHL_ZhuangLiaoDelay = dwTickCount + 150; } else{ CTHL_wltzi=0; } } } #if 1 //启动 自动启动 单一自动 if((CTHL_START_IN_UP || bStart || CTHL_bSingOne ) && !CTHL_wltzi) { if(!bRunning) { bRunning = 1; if( CTHL_bSingOne)bSingOneFlg=1;//单次 SetEn(X_AXIS, MOTOR_EN); SetEn(Y_AXIS, MOTOR_EN); Set_Ctrlmode_trans(Y_AXIS,POS_MODE); bFZ_LengthOK = 0; CTHL_FZ_MotorStep = 0; CTHL_PARAM_ZDP_AUTO=0;//振动盘控制 bCxcAuto=0;//运行过程中伺服报警 // CTHL_TLiao_VAVLE =0; CTHL_HSLiao_VAVLE =0; // CTHL_XCBB_VAVLE=0; CTHL_JCXBD_VAVLE=0; CTHL_CXHT_VAVLE=0; CTHL_FZYL_VAVLE=0; // CTHL_FKDW_VAVLE=0; CTHL_SMZ_VAVLE=0; CTHL_TXM_VAVLE=0; CTHL_HL_VAVLE=0; //装料拉头 CTHL_ZhuangLiaoDelay = dwTickCount + 150; CTHL_ZhuangLiaoStep=1; bTuiLaTouOkFlg = 1; bZhuangLiaoOkFlg= 0; bXiaoMOUpFlg=1; CTHL_TryCnt = 0;//重装拉头次数 //方块定位 CTHL_TD_MotorStep=1; //自动运行 CTHL_AutoDelay = dwTickCount + 10; CTHL_AutoStep = 1; CTHL_cZipCnt = 0;//头两条检测 // } bStart = 0; CTHL_bSingOne=0; } if(CTHL_ZhouWantwo)//按下停止后，该条完成 { CTHL_ZhouWantwo=0; if(CTHL_ZhouWanone) { CTHL_ZhouWanone=0; bRunning=0; cStopMode=0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE) { SetClr(X_AXIS, MOTOR_NOALARM); SetClr(Y_AXIS, MOTOR_NOALARM); CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000; } else if(CTHL_PARAM_DBSF_MODE) { if(!CTHL_SF_ALR_IN) { CTHL_SF_ALR_CLR=1; CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000; } } SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); bSingOneFlg=0; CTHL_AutoStep = 0; CTHL_FZ_MotorStep =0; CTHL_TD_MotorStep =0; } } //停止 if(CTHL_STOP_IN_UP || bStop) { if(cStopMode < 2) { cStopMode++;//停止模式 } if(cStopMode==1 && bRunning)CTHL_ZhouWanone=1;//按下停止后，记录标准 else if(cStopMode==2 && bRunning)//按下停止两次后，停机。 { bRunning=0; cStopMode=0; bSingOneFlg=0; bFZ_LengthOK = 0; cBAOjin=0;//伺服报警标志 CTHL_wltzi=0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); CTHL_AutoStep = 0; CTHL_FZ_MotorStep =0; CTHL_TD_MotorStep =0; CTHL_ZhuangLiaoStep=0; CTHL_TLiao_VAVLE=0; CTHL_HSLiao_VAVLE =0; CTHL_XCJC_VAVLE=0; CTHL_XCBB_VAVLE=0; CTHL_JCXBD_VAVLE=0; CTHL_HL_VAVLE=0; if(CTHL_CTXM_VAVLE){ CTHL_JLTou_VAVLE=1; CTHL_MGuo_VAVLE=0; } CTHL_CTXM_VAVLE =0; CTHL_TXM_VAVLE=0; CTHL_SMZ_VAVLE=0; CTHL_FKDW_VAVLE=0; CTHL_FZYL_VAVLE=0; if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE) { SetClr(X_AXIS, MOTOR_NOALARM); SetClr(Y_AXIS, MOTOR_NOALARM); CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000; } else if(CTHL_PARAM_DBSF_MODE) { if(!CTHL_SF_ALR_IN) { CTHL_SF_ALR_CLR=1; CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000; } } //SetPos(X_AXIS, 0); //SetPos(Y_AXIS, 0); //SetPos(Z_AXIS, 0); SetAlarmCode(CTHL_ALARM_ADDR,0); } else if(!bRunning) { bRunning=0; cStopMode=0; bSingOneFlg=0; CTHL_wltzi=0; bCxcAuto=0; cBAOjin=0; bFZ_LengthOK = 0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); SetAlarmCode(CTHL_ALARM_ADDR,0); CTHL_TLiao_VAVLE =0; CTHL_HSLiao_VAVLE =0; CTHL_XCJC_VAVLE=0; CTHL_JCXBD_VAVLE=0; if(CTHL_CTXM_VAVLE){ CTHL_JLTou_VAVLE=1; CTHL_MGuo_VAVLE=0; } CTHL_CTXM_VAVLE =0; CTHL_FKDW_VAVLE=0; CTHL_TXM_VAVLE=0; CTHL_SMZ_VAVLE=0; CTHL_FKDW_VAVLE=0; CTHL_FZYL_VAVLE=0; CTHL_XCBB_VAVLE=0; CTHL_HL_VAVLE=0; CTHL_AutoStep = 0; CTHL_ZhuangLiaoStep=0; CTHL_TLiaoStep=0; CTHL_TD_MotorStep=0; CTHL_SongLiaoStep =0; CTHL_FZ_MotorStep =0; CTHL_AutoStep1 =0; if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE) { SetClr(X_AXIS, MOTOR_NOALARM); SetClr(Y_AXIS, MOTOR_NOALARM); CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000; } if(CTHL_PARAM_DBSF_MODE) { if(!CTHL_SF_ALR_IN) { CTHL_SF_ALR_CLR=1; CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000; } } SetAlarmCode(CTHL_ALARM_ADDR,0); //SetPos(X_AXIS, 0); //SetPos(Y_AXIS, 0); CTHL_TLiaoStep = 0; CTHL_AutoStep = 0; //CTHL_LianLengthCheck = 0; } bStop=0; } if(bAlarmStop) { bRunning = 0; bAlarmStop=0; cStopMode=0; CTHL_TLiao_VAVLE =0; CTHL_HSLiao_VAVLE =0; CTHL_FKDW_VAVLE=0; CTHL_TXM_VAVLE=0; CTHL_FKDW_VAVLE=0; CTHL_FZYL_VAVLE=0; CTHL_XCBB_VAVLE=0; CTHL_JCXBD_VAVLE=0; CTHL_HL_VAVLE=0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); CTHL_AutoStep = 0; CTHL_FZ_MotorStep =0; CTHL_TD_MotorStep =0; } #endif } //拖带电机控制动作 X轴 void CTHL_TD_Motor(void) // { if(dwTickCount >= CTHL_bSFBBAOJIN_Delay&&CTHL_SF_ALR_CLR) //伺服报警时间 { CTHL_SF_ALR_CLR=0; SetClr(X_AXIS,MOTOR_ALARM); //SetClr(Y_AXIS,MOTOR_ALARM); } if((GetAlarm(Y_AXIS)==1 || GetAlarm(X_AXIS)==1) && !CTHL_PARAM_DBSF_MODE &&!cBAOjin) { bCxcAuto=1; cBAOjin=1; CTHL_SF_ALR_IN=0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); CTHL_AutoStep = 101; SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); CTHL_SetAlarmCode(CTHL_SF_ALR_ALARM,bRunning); //无拉链自动 } else { CTHL_SF_ALR_IN=1; } memcpy(&user_datas[420],&dwXRealPos,4); #if 1 switch(CTHL_TD_MotorStep) { //1步开始，纯定位, 没有穿入 case 1://判断是否有方块感应 if(dwTickCount >= CTHL_TD_MotorDelay) { if(!CTHL_FKDW_IN)CTHL_TD_MotorStep = 0; else { SetEn(X_AXIS, MOTOR_EN); SetEn(Y_AXIS, MOTOR_EN);//松轴 CTHL_FKDW_VAVLE=0; CTHL_SMZ_VAVLE=0; CTHL_TXM_VAVLE=0; CTHL_XCBB_VAVLE=0; CTHL_FZYL_VAVLE=0;//压轮松开 CTHL_TD_MotorDelay=dwTickCount+50; CTHL_TD_MotorStep++; } } break; case 2: if(dwTickCount >= CTHL_TD_MotorDelay){ //给锁轴时间 SetPos(X_AXIS, 0); AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1500,600,30,15); AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1500,600,30,15); CTHL_TD_MotorStep++; } break; case 3://检测到过链 if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块 bCxcAuto=1; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } if(CTHL_FK_Check_UP)//有方块信号； { CTHL_XSavePosBuff = dwXRealPos;//方块位置 CTHL_TD_MotorStep++; } break; case 4: if(dwXRealPos>=CTHL_XSavePosBuff+CTHL_PARAM_FKDW_DW_LENGTH) { CTHL_FKDW_VAVLE=1; } if(dwXRealPos>=CTHL_XSavePosBuff+CTHL_PARAM_TDDW_LENGTH) { AxisContinueMoveChangeSpeed(X_AXIS,600,600,20,10); AxisContinueMoveChangeSpeed(Y_AXIS,600,600,20,10); CTHL_TD_MotorDelay = dwTickCount + 1500; CTHL_TD_MotorStep++; } break; case 5: //定位感应亮时、位置到达时，或X轴停止时，停止X轴 if(!CTHL_FKDW_IN) { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); CTHL_TD_MotorDelay = dwTickCount + 50; CTHL_TD_MotorStep++; } if(dwTickCount >= CTHL_TD_MotorDelay)//1.5S为检测到停机 { bCxcAuto=1;; CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常 } if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块 bCxcAuto=1; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } break; case 6://压轮动作 if(dwTickCount >= CTHL_TD_MotorDelay&&!X_DRV&&!Y_DRV){ CTHL_FZYL_VAVLE=1; AxisMovePosAccDec(X_AXIS,1200,-10,1200,1200,20,20,0); CTHL_TD_MotorDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; CTHL_TD_MotorStep++; } break; case 7://压轮到位 if(CTHL_FZYL_VAVLE&&CTHL_FZYL_Limit_IN&&!X_DRV){ CTHL_TD_MotorStep = 0; CTHL_GLGY_FKDW_Length=dwXRealPos-CTHL_XSavePosBuff;//第一次获取过链感应与定位间的长度 } else if(dwTickCount >= CTHL_TD_MotorDelay){ CTHL_SetAlarmCode(CTHL_FZYL_Limit_ALARM,bRunning);//压轮到位异常 } break; case 10://开始穿小插 if(dwTickCount >= CTHL_TD_MotorDelay&&!X_DRV&&!Y_DRV) { tuodaitime=dwTickCount; CTHL_XSavePosBuff3=dwXRealPos; CTHL_CXHT_VAVLE=0; AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_HLT_SPEED_Speed,CTHL_PARAM_CRFZ_LENGTH+180, 1000,1000,20,20,CTHL_PARAM_XCCR_LENGTH); CTHL_TD_MotorStep++; CTHL_TD_MotorDelay = dwTickCount+ 1000; ONCERUNFLG1=1; } break; case 11: if(abs(dwXRealPos-CTHL_XSavePosBuff3)>=120)CTHL_JCXBD_VAVLE=0;//夹插销布带松开 if(abs(dwXRealPos-CTHL_XSavePosBuff3)>=50&&ONCERUNFLG1) { ONCERUNFLG1=0; AxisMovePosAccDec(Y_AXIS,1500,250,1500,1000,20,20,0);//Y轴送带 } if(CTHL_CXGY_IN) { CTHL_XCJC_VAVLE=0;//插销检测下降 CTHL_HL_VAVLE=1; CTHL_TD_MotorStep++; CTHL_TD_MotorDelay = dwTickCount+ CTHL_PARAM_XCIN_DELAY; } else if(dwTickCount >= CTHL_TD_MotorDelay) { bCxcAuto=1;//不停机 CTHL_SetAlarmCode(CTHL_CXGY_Check_ALARM,bRunning);//E28 插销感应到位检测异常 } break; case 12: // if(dwTickCount >= CTHL_TD_MotorDelay) //延时停X轴 { CTHL_TD_MotorStep=40; AxisEgmStop(X_AXIS); } break; case 40: // if(!X_DRV&&!Y_DRV) { CTHL_SMZ_VAVLE=0; CTHL_MGuo_VAVLE=0; CTHL_TD_MotorStep=41; CTHL_CXCD=abs(dwXRealPos-CTHL_XSavePosBuff3); CTHL_XSavePosBuff3 = dwXRealPos; CTHL_XSavePosBuff = dwXRealPos; } break; case 41: // if(CTHL_SML_Limit_IN) { CTHL_TD_MotorDelay = dwTickCount+ CTHL_PARAM_YSXC_DELAY;//延时合练 } else if(dwTickCount >= CTHL_TD_MotorDelay&&!X_DRV) //开始提速 { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP)//自动与单步，单步用启动按钮进行 { CTHL_TD_MotorStep=42; CTHL_FZYL_VAVLE=0; CTHL_XSavePosBuff3=dwXRealPos; } } break; case 42: // //if(dwTickCount >= CTHL_TD_MotorDelay) { CTHL_TD_MotorStep=13; } break; case 13: //退推方块延时到 ，合练 //if(dwTickCount >= CTHL_TD_MotorDelay&&CTHL_SML_Limit_IN)//走合练长度 { CTHL_HL_LENTH=abs(dwXRealPos-CTHL_XSavePosBuff3); if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN_UP) { //前两条用反转长度定位合练长度 if(CTHL_LianLengthXFZCheckFirst>=CTHL_PARAM_HELIAN_LENGTH) { CTHL_LENTH=CTHL_LianLengthXFZCheckFirst+CTHL_PARAM_HLDW_LENGTH+200-CTHL_PARAM_HELIAN_LENGTH-CTHL_HL_LENTH; } else//剩余合练设置错误 { bCxcAuto=1; CTHL_SetAlarmCode(CTHL_SYHL_ERRO_ALARM,bRunning); } //用第一次速度 AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_First_SPEED,CTHL_LENTH, 1000,CTHL_PARAM_First_SPEED,30,200,0);//快速合链 } else { //前两条用反转长度定位合练长度 if(CTHL_LianLengthCheckFirst>=CTHL_PARAM_HELIAN_LENGTH) { CTHL_LENTH=CTHL_LianLengthCheckFirst+CTHL_PARAM_HLDW_LENGTH-CTHL_FKGY_FKDW_Length-CTHL_PARAM_HELIAN_LENGTH-CTHL_HL_LENTH; } else//剩余合练设置错误 { bCxcAuto=1; CTHL_SetAlarmCode(CTHL_SYHL_ERRO_ALARM,bRunning); } //用合练速度 AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_TD_RunSpeed,CTHL_LENTH, 1200,CTHL_PARAM_TD_RunSpeed,50,100,100);//快速合链 } CTHL_XSavePosBuff3 = dwXRealPos; bFKCheck_Flag = 0; cDELAYONE=0; CTHL_TD_MotorStep++; JLTFLG=1; XMFLG=1; } break; case 14: if(abs(dwXRealPos-CTHL_XSavePosBuff3)>=80&&JLTFLG) { JLTFLG=0; CTHL_JLTou_VAVLE=1; } if(abs(dwXRealPos-CTHL_XSavePosBuff3)>=200&&XMFLG) { XMFLG=0; CTHL_CTXM_VAVLE=0; if(CTHL_ZhuangLiaoStep==0)//装拉头 { CTHL_ZhuangLiaoDelay = dwTickCount + 20; CTHL_ZhuangLiaoStep=1; bZhuangLiaoOkFlg= 0; bXiaoMOUpFlg=0; } } if(abs(dwXRealPos-CTHL_XSavePosBuff3)>=30) { AxisMovePosAccDecNotStop(Y_AXIS,2000,100, 1000,1000,30,150,0);//Y轴转动 } if((CTHL_FK_Check_UP)&& !bFKCheck_Flag){//第一方块信号检测 bFKCheck_Flag = 1; //置方块检测为1 cDELAYONE=1; CTHL_XSavePosBuff1= dwXRealPos;//第一个方块检测位置 } if(CTHL_cZipCnt<1) { //长度限制 if(abs(dwXRealPos-CTHL_XSavePosBuff) >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块 bCxcAuto=1; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } else { //长度限制 if((abs(dwXRealPos-CTHL_XSavePosBuff) >=(long)(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheckFirst))) { bCxcAuto=1; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } if(abs(dwXRealPos-CTHL_XSavePosBuff3) >=CTHL_LENTH-20){ CTHL_XCJC_VAVLE=0; CTHL_XCBB_VAVLE=0; CTHL_HL_VAVLE=0; } if(abs(dwXRealPos-CTHL_XSavePosBuff3) >=CTHL_LENTH+50) { CTHL_TD_MotorStep++;// } break; case 15: //加速 if(CTHL_FK_Check_UP && !bFKCheck_Flag)//为检测到第一方块继续检测 { bFKCheck_Flag = 1; cDELAYONE=2; CTHL_XSavePosBuff1= dwXRealPos; CTHL_TD_MotorDelay = dwTickCount+ 200; } if((!CTHL_NoLaLian_IN)) { if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN_UP) { AxisContinueMoveChangeSpeed(X_AXIS,CTHL_PARAM_First_SPEED,1000,20,50); //长度限制 if(abs(dwXRealPos-CTHL_XSavePosBuff) >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块 bCxcAuto=1; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } else { if((CTHL_LianLengthCheckFirst < 4000) && (CTHL_LianLengthCheck <4000)){ // 40公分 CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED*2/3;//长链拖带快速速度，小于4000的链条 } else if((CTHL_LianLengthCheckFirst < 5000) && (CTHL_LianLengthCheck <5000)) { CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED*4/5;//长链拖带快速速度，小于5000的链条 } else{ CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED;//长链拖带快速速度 } CTHL_LENTH=CTHL_LianLengthCheckFirst-CTHL_GLGY_FKDW_Length-dwXRealPos; AxisMovePosAccDecNotStop(X_AXIS,CTHL_DataBuff,CTHL_LENTH, 1000,CTHL_PARAM_FKDW_Speed,50,100,CTHL_PARAM_GJSY_LENGTH); //长度限制 if((abs(dwXRealPos-CTHL_XSavePosBuff) >=(long)(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheckFirst))) { bCxcAuto=1; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } } //CTHL_TD_MotorStep = 17; if(bFKCheck_Flag) CTHL_TD_MotorStep = 17; //说明第一个方块已经检测到 else CTHL_TD_MotorStep++; //第一个方块没检测到时先运动的第一个再检测定位 break; case 16: //检测方块 if(CTHL_FK_Check_UP&& !bFKCheck_Flag)//第一次检测到方块 { bFKCheck_Flag = 1; cDELAYONE=3; CTHL_XSavePosBuff1= dwXRealPos; CTHL_TD_MotorDelay = dwTickCount+ 200; CTHL_TD_MotorStep++; } if(CTHL_cZipCnt<1) { //长度限制 if(abs(dwXRealPos-CTHL_XSavePosBuff) >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块 bCxcAuto=1; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } else { //长度限制 if((abs(dwXRealPos-CTHL_XSavePosBuff) >=(long)(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheckFirst))) { bCxcAuto=1; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } break; case 17://需判断中间无方块的情况，或者是短链的情况 //在链上行走8公分 //检测方块 /*if(CTHL_FK_Check_UP&& !bFKCheck_Flag)//第一次检测到方块 { bFKCheck_Flag = 1; cDELAYONE=3; CTHL_XSavePosBuff1= dwXRealPos; }*/ if(!CTHL_FK_Check && (dwXRealPos >= 1000)) { CTHL_TD_MotorStep++; } break; case 18: //在链上行走8公分 if(!CTHL_FK_Check && dwXRealPos>400 &&(abs(dwXRealPos-CTHL_XSavePosBuff1) >= (CTHL_PARAM_XMUP_LENGTH)))//上摸位置 { CTHL_XCBB_VAVLE=0; bXiaoMOUpFlg=1; } //第二次感应方块， 要做定位，//感应到第一个方块插销位置 if(CTHL_FK_Check_UP &&dwTickCount >= CTHL_TD_MotorDelay&& (dwXRealPos >= 1600)) { CTHL_XSavePosBuff3= dwXRealPos; if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN_UP) { CTHL_LianLengthGLCheckFirst= dwXRealPos-CTHL_XSavePosBuff1; CTHL_LianLengthCheckFirst= dwXRealPos-CTHL_XSavePosBuff1; } else { //感应到方块后，走段距离减速 CTHL_LENTH=CTHL_PARAM_TDDW_LENGTH; AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDW_Speed,CTHL_LENTH, 1000,800,30,30,100); } CTHL_LianLengthGLCheck= dwXRealPos-CTHL_XSavePosBuff1; CTHL_TD_MotorStep++; //公分节 /*if((CTHL_LianLengthGLCheck <= CTHL_GLGY_FKDW_Length) && (CTHL_cZipCnt>1))//感应到方块后定位距离 { CTHL_TD_MotorStep=50; AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1500,600,30,15); AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1500,600,30,15); bFZ_LengthOK= 0; CTHL_cZipCnt = 0; } else { CTHL_TD_MotorStep++; }*/ } if(CTHL_cZipCnt<1) { //长度限制 if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块 bCxcAuto=1; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } else { //长度限制 if((dwXRealPos >=(long)(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheckFirst))) { bCxcAuto=1; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } break; case 19: //方块位置+方块钩针下降位置 if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN_UP){ if(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_FKDW_DW_LENGTH){CTHL_FKDW_VAVLE=1;} if(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_TDDW_LENGTH) { CTHL_TD_MotorStep++; CTHL_TD_MotorDelay = dwTickCount+ 1500; AxisContinueMoveChangeSpeed(X_AXIS,800,800,20,10); } } else { if(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_FKDW_DW_LENGTH) { CTHL_FKDW_VAVLE=1; CTHL_TD_MotorStep++; CTHL_TD_MotorDelay = dwTickCount+ 1500; } } break; case 20: //定位感应亮时、位置到达时，或X轴停止时，停止X轴 if(dwTickCount >= CTHL_TD_MotorDelay)//3S为检测到停机 { bCxcAuto=1; CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常 } if(!CTHL_FKDW_IN) { CTHL_TD_MotorStep++; CTHL_TD_MotorDelay = dwTickCount+ 0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); } if(CTHL_cZipCnt<1) { //长度限制 if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length){ //大于第一条最长长度。没有检测的方块 bCxcAuto=1; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } else { //长度限制 if((dwXRealPos >=(long)(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheckFirst))) { bCxcAuto=1; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } break; case 21://压轮动作，做一个反转动作，松一下拉链 if(dwTickCount >= CTHL_TD_MotorDelay&&!X_DRV) { CTHL_FZYL_VAVLE=1;//压轮 CTHL_TD_MotorStep++; AxisMovePosAccDec(X_AXIS,1500,-30,1000,1000,20,20,0); CTHL_TD_MotorDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; } break; case 22://到位 //if(CTHL_FZYL_VAVLE&&CTHL_FZYL_Limit_IN) { CTHL_TD_MotorStep = 0; CTHL_PARAM_TD_TIME=dwTickCount-tuodaitime; if(CTHL_cZipCnt<1) { //测量过链感应与方块定位的距离 // CTHL_LianLengthCheckFirst=dwXRealPos; CTHL_GLGY_FKDW_Length=dwXRealPos-CTHL_XSavePosBuff3; //CTHL_PARAM_TDDW_LENGTH=CTHL_GLGY_FKDW_Length; //测量方块感应与方块定位的距离 CTHL_FKGY_FKDW_Length=CTHL_LianLengthCheckFirst-CTHL_LianLengthXFZCheckFirst; } else { bFZ_LengthOK = 1; } CTHL_LianLengthCheck = dwXRealPos ; CTHL_TD_MotorDelay = dwTickCount + 0; } //尺寸变化较大，重新测量 if(CTHL_cZipCnt>2) { if(abs(CTHL_LianLengthCheck-CTHL_LianLengthCheckFirst)>2000) { CTHL_cZipCnt=0; } } break; //由公分节调转 case 50: if(CTHL_FK_Check_DW) { CTHL_TD_MotorStep = 51; CTHL_GF_Change = 1; SetPos(X_AXIS, 0); } break; case 51: if(CTHL_FK_Check) { CTHL_TD_MotorDelay = dwTickCount + 100; } else if(dwTickCount >= CTHL_TD_MotorDelay) { CTHL_TD_MotorStep = 52; } break; case 52: if(dwXRealPos>CTHL_GLGY_FKDW_Length+500) { if(CTHL_FK_Check_UP) { CTHL_XSavePosBuff = dwXRealPos;//方块位置 CTHL_TD_MotorStep=4; } } break; } #endif } //穿入电机动作，Y轴 void CTHL_FZ_Motor(void) // { short *revise_buff; #if 1 //1步开始, 穿链反转长度开始， //20步开始 插销定位开始 switch(CTHL_FZ_MotorStep) { //穿入长度位置 case 1: if(dwTickCount >= CTHL_FZ_MotorDelay) { cuanliantime=dwTickCount; //Set_Ctrlmode_trans(Y_AXIS,TARR_MODE); if(CTHL_cZipCnt<1)//前两次走 { AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_JXM_SPEED,DIR_N,1000,1000,30,50); AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_JXM_SPEED,DIR_N,1000,1000,30,50); } else { AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_JXM_SPEED+80,-CTHL_PARAM_XMCR_SLOW_LENGTH, 1000,CTHL_PARAM_JXM_SPEED,50,100,0); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_JXM_SPEED,-CTHL_PARAM_XMCR_SLOW_LENGTH, 1000,CTHL_PARAM_JXM_SPEED,50,100,0); } CTHL_FZ_MotorStep ++; } break; case 2: //if(dwYRealPos_com > CTHL_PARAM_TJXM_LENGTH)CTHL_TXM_VAVLE=0; //到达可推出侧推布带时 //if(dwYRealPos_com > CTHL_PARAM_SMXJ_LENGTH)CTHL_SMZ_VAVLE=1; //到达上模位置上摸下降 if(abs(dwXRealPos) >=CTHL_PARAM_XMCR_SLOW_LENGTH-50){//提前转换模式 Set_Ctrlmode_trans(Y_AXIS,TARR_MODE); } if(abs(dwXRealPos) >= CTHL_PARAM_XMCR_SLOW_LENGTH)//穿链结束，开始走链加速 { CTHL_FZ_MotorDelay = dwTickCount+0; if(CTHL_cZipCnt<1) { //Set_Ctrlmode_trans(Y_AXIS,TARR_MODE); AxisContinueMoveChangeSpeed(X_AXIS,CTHL_PARAM_FZFirst_SPEED,1000,20,100); AxisContinueMoveChangeSpeed(Y_AXIS,CTHL_PARAM_FZFirst_SPEED,1000,20,100); } else { //长度=反转长度-慢速长度-减速长度-修正长度; //CTHL_LENTH = CTHL_LianLengthXFZCheckFirst-CTHL_PARAM_XMCR_SLOW_LENGTH+CTHL_PARAM_FZXZ_LENGTH-CTHL_PARAM_FZJS_LENGTH; revise_buff=(short *)(&CTHL_PARAM_FZXZ_LENGTH); CTHL_LENTH = CTHL_LianLengthAutoFZCRcheck-abs(dwXRealPos)+(*revise_buff)-CTHL_PARAM_FZJS_LENGTH; AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FZ_Speed,-CTHL_LENTH, 1500,CTHL_PARAM_FZDS_Speed,100,120,0); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZ_Speed+100,-CTHL_LENTH, 1500,CTHL_PARAM_FZDS_Speed,100,120,0); } CTHL_FZ_MotorStep++; } //电机速度差太大 if(abs(dwXRealPos)>dwYRealPos_com+4500) { bCxcAuto=1;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); CTHL_SetAlarmCode(CTHL_FZMOTOR_ALARM,bRunning); //反转电机卡滞 } comflag=1; break; case 3: //if(dwYRealPos_com> CTHL_PARAM_TJXM_LENGTH)CTHL_TXM_VAVLE=0; //到达可推出侧推布带时 //if(dwYRealPos_com > CTHL_PARAM_SMXJ_LENGTH)CTHL_SMZ_VAVLE=1; //到达上模位置上摸下降 if(dwTickCount> CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH)CTHL_TXM_VAVLE=0; //到达可推出侧推布带时 if(dwTickCount > CTHL_FZ_MotorDelay+CTHL_PARAM_SMXJ_LENGTH)CTHL_SMZ_VAVLE=1; //到达上模位置上摸 if(CTHL_cZipCnt<1) { CTHL_FZ_MotorStep++; } else { if(abs(dwXRealPos)>=CTHL_LENTH+CTHL_PARAM_XMCR_SLOW_LENGTH) { AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FZDS_Speed,-CTHL_PARAM_FZJS_LENGTH, 1500,900,120,80,CTHL_PARAM_FZJSDS_LENGTH); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZDS_Speed,-CTHL_PARAM_FZJS_LENGTH, 1500,900,120,80,CTHL_PARAM_FZJSDS_LENGTH); CTHL_FZ_MotorStep++; } } //长度超出报警 if(CTHL_cZipCnt>=1) { if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthXFZCheckFirst)) //拉带报警长度 { bCxcAuto=1;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度 } //方块检测异常(光纤) if(abs(dwXRealPos) >= (CTHL_LianLengthXFZCheckFirst + 500)) { bCxcAuto=1;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning); } } else { if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length){ //位置大于第一条拉链长度 bCxcAuto=1;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度 } } //电机速度差太大 if(abs(dwXRealPos)>dwYRealPos_com+4500) { bCxcAuto=1;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); //CTHL_FZYL_VAVLE=0; CTHL_SetAlarmCode(CTHL_FZMOTOR_ALARM,bRunning); //穿入方块超出预设长度 } break; case 4: //if(dwYRealPos_com > CTHL_PARAM_TJXM_LENGTH)CTHL_TXM_VAVLE=0; //到达可推出侧推布带时 //if(dwYRealPos_com > CTHL_PARAM_SMXJ_LENGTH)CTHL_SMZ_VAVLE=1; //到达上模位置上摸下降 if(dwTickCount> CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH)CTHL_TXM_VAVLE=0; //到达可推出侧推布带时 if(dwTickCount > CTHL_FZ_MotorDelay+CTHL_PARAM_SMXJ_LENGTH)CTHL_SMZ_VAVLE=1; //到达上模位置上摸 if((abs(dwXRealPos) > 500) && CTHL_FKGY_IN_UP)//位置大于100，且方块感应上升沿 { //Set_Ctrlmode_trans(Y_AXIS,TARR_MODE); CTHL_XSavePosBuff1 = abs(dwXRealPos); if(CTHL_cZipCnt<1){//记录前两条的方块感应长度； //CTHL_FKGY_Length=abs(dwXRealPos); //停机距离调整 CTHL_LENTH=0; AxisMovePosAccDecNotStop(X_AXIS,500,-CTHL_LENTH, 800,800,15,25,0); AxisMovePosAccDecNotStop(Y_AXIS,600,-CTHL_LENTH, 800,800,15,25,0); } else { //停机距离调整 CTHL_LENTH=CTHL_PARAM_FZSTOP_LENGTH; AxisMovePosAccDecNotStop(X_AXIS,500,-CTHL_LENTH, 800,800,15,25,0); AxisMovePosAccDecNotStop(Y_AXIS,600,-CTHL_LENTH, 800,800,15,25,0); } CTHL_FZ_MotorStep++; //启动定时器 CTHL_FZ_MotorDelay = dwTickCount +2000;//定时2S定位完成 } //长度超出报警 if(CTHL_cZipCnt>=1) { if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthXFZCheckFirst)) //拉带报警长度 { bCxcAuto=1;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度 } //方块检测异常(光纤) if(abs(dwXRealPos) >= (CTHL_LianLengthXFZCheckFirst + 500)) { bCxcAuto=1;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning); } } else { if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length){ //位置大于第一条拉链长度 bCxcAuto=1;//停轴 CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度 } } //电机速度差太大 if(abs(dwXRealPos)>dwYRealPos_com+4500) { bCxcAuto=1;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); CTHL_FZYL_VAVLE=0; CTHL_SetAlarmCode(CTHL_FZMOTOR_ALARM,bRunning); //穿入方块超出预设长度 } break; case 5: //方块有亮到灭 ，完成 if(CTHL_cZipCnt<1) { if(!CTHL_FKGY_IN) { CTHL_FZ_MotorStep++; AxisDecStop(X_AXIS); AxisEgmStop(Y_AXIS); CTHL_FZ_MotorDelay = dwTickCount +1; Set_Ctrlmode_trans(Y_AXIS,POS_MODE); } else if(dwTickCount >= CTHL_FZ_MotorDelay){//方块检测异常 bCxcAuto=1;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning); } } else { if(!CTHL_FKGY_IN&&abs(dwXRealPos)>=CTHL_XSavePosBuff1+30) { CTHL_FZ_MotorStep++; AxisDecStop(X_AXIS); AxisEgmStop(Y_AXIS); CTHL_FZ_MotorDelay = dwTickCount +1; Set_Ctrlmode_trans(Y_AXIS,POS_MODE); } else if(dwTickCount >= CTHL_FZ_MotorDelay){//方块检测异常 bCxcAuto=1;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning); } } break; case 6: //定位到位后,X轴进行正转拖紧一下带； if(!Y_DRV && !X_DRV&&dwTickCount >= CTHL_FZ_MotorDelay ) { if(CTHL_cZipCnt <1) { CTHL_LianLengthXFZCheckFirst =abs(dwXRealPos); AxisMovePosAccDec(Y_AXIS,1500,CTHL_PARAM_FZSJ_LENGTH,1500,600,20,20,0); } else { if(CTHL_cZipCnt <2) { AxisMovePosAccDec(Y_AXIS,1500,CTHL_PARAM_FZSJ_LENGTH,1500,600,20,20,0); } else { AxisMovePosAccDec(Y_AXIS,1500,CTHL_PARAM_FZSJ_LENGTH,1500,600,20,20,0); } } CTHL_FKGY_STOP_Length=abs(dwXRealPos)-CTHL_XSavePosBuff1; CTHL_LianLengthXFZCheck=abs(dwXRealPos); CTHL_FZ_MotorStep++; } break; case 7: //定位到位后,X轴进行正转拖紧一下带； CTHL_FZ_MotorStep=0; CTHL_PARAM_CL_TIME=dwTickCount-cuanliantime; break; } #endif } //装拉头动作 void CTHL_ZLT_Step(void) { switch(CTHL_ZhuangLiaoStep) { case 1://判断是否有拉头 if(bZhuangLiaoOkFlg){//装拉头完成，则等待 CTHL_ZhuangLiaoStep = 0; } else if(dwTickCount >= CTHL_ZhuangLiaoDelay) { zlttime=dwTickCount; if(!CTHL_JLTou_VAVLE&&!CTHL_JLiao_Limit_IN && CTHL_LTou_Check||CTHL_JLTou_VAVLE ){ //夹拉头、无拉头到位信号、缺拉头有信号；说明没有拉头 if(CTHL_CTXM_Origin_IN && !CTHL_CTXM_VAVLE){ //判断下模是否在原位 CTHL_ZhuangLiaoDelay = dwTickCount + 5; if(CTHL_TLiaoStep == 0){//执行给拉头步骤动作 CTHL_TLiaoStep = 1; } CTHL_ZhuangLiaoStep++; } else if(dwTickCount >= CTHL_ZhuangLiaoDelay + QDCT_VAVLE_ERROR_TIME){ CTHL_wltzi=1;//装拉头故障标志 CTHL_SetAlarmCode(CTHL_CTXM_Origin_ALARM,bRunning);//警告下模原位异常 } } else if(!CTHL_JLTou_VAVLE&&CTHL_JLiao_Limit_IN && !CTHL_LTou_Check){//夹拉头、有拉头到位信号、缺拉头无信号；说明拉头 CTHL_ZhuangLiaoDelay = dwTickCount + 40; CTHL_MGuo_VAVLE = 1; CTHL_ZhuangLiaoStep = 12; } else{//由于上电已经置CTHL_JLTou_VAVLE=0，所有剩下的情况时拉头到位有信号、缺料到位有信号的情况，报警 CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_JLT_MZ_ALARM,bRunning);//夹拉头模组异常 } } break; case 2://延时松开夹，和码钩 if(dwTickCount >= CTHL_ZhuangLiaoDelay) { CTHL_JLTou_VAVLE = 1; //夹拉头松开 CTHL_MGuo_VAVLE = 0; //码钩松开 CTHL_ZhuangLiaoDelay = dwTickCount + 10; CTHL_ZhuangLiaoStep++; } break; case 3: // 给拉头动作步骤完成 if(CTHL_TLiaoStep == 0){ //推料已经完成 if(dwTickCount >= CTHL_ZhuangLiaoDelay){ if(!CTHL_JLiao_Limit_IN&&CTHL_JLTou_VAVLE)//夹拉头不在限位， { if(CTHL_XCJC_Origin_IN&&!CTHL_XCJC_VAVLE){//插销检测在原位 CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_HSLiao_FORWARDDELAY; CTHL_ZhuangLiaoStep++; } else if(dwTickCount >= CTHL_ZhuangLiaoDelay + QDCT_VAVLE_ERROR_TIME){ CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_XCJC_Origin_ALARM,bRunning);//插销钩针原位异常 } } else if(dwTickCount >= CTHL_ZhuangLiaoDelay + QDCT_VAVLE_ERROR_TIME){ CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_JLT_Limit_ALARM,bRunning);//夹拉头到位异常 } } } break; case 4://延时横送 if(dwTickCount >= CTHL_ZhuangLiaoDelay) { CTHL_HSLiao_VAVLE = 1; //横送阀动作 CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; CTHL_ZhuangLiaoStep++; } break; case 5://横推拉头到位后延时 if(CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_Origin_IN){//横送拉头在到位 CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_HSLiao_BACKDELAY;//停顿10ms，再接拉头 CTHL_ZhuangLiaoStep ++; } else if(dwTickCount >= CTHL_ZhuangLiaoDelay){//横送拉头超时，重新横送一次 CTHL_ZhuangLiaoStep = 30; } break; case 6://接拉头动作 if(dwTickCount >= CTHL_ZhuangLiaoDelay) { CTHL_JLTou_VAVLE = 0; CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; CTHL_ZhuangLiaoStep ++; } break; case 7: //判断接拉头情况 if(CTHL_JLiao_Limit_IN&&!CTHL_LTou_Check)//接拉头到位,且缺拉头无信号，有拉头 { CTHL_ZhuangLiaoStep ++; CTHL_ZhuangLiaoDelay=dwTickCount+CTHL_PARAM_JLiaoCH_DELAY;//延时检测 } else if(!CTHL_JLiao_Limit_IN&&CTHL_LTou_Check)//接拉头到位无信号,且缺拉头有信号，无拉头 { //重新装拉头3次 CTHL_TryCnt++; CTHL_JLTou_VAVLE = 1; //接拉头关 CTHL_HSLiao_VAVLE = 0; //横送回退 bTuiLaTouOkFlg = 0; if(CTHL_TryCnt >= 3) { CTHL_TryCnt = 0; bZhuangLiaoOkFlg = 0;//装拉头失败 CTHL_ZhuangLiaoStep = 0;//重新开始 CTHL_wltzi=1; //user_datas[127]=4;//测试用 CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);//装拉头异常 } else { CTHL_ZhuangLiaoStep = 19; //重新送3次 CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; } } else if(dwTickCount >= CTHL_ZhuangLiaoDelay+QDCT_VAVLE_ERROR_TIME) //接拉头(接料)到们信号异常 { CTHL_SetAlarmCode(CTHL_JLT_MZ_ALARM,bRunning);//接料到位异常 CTHL_HSLiao_VAVLE = 0; //横送会回位 CTHL_JLTou_VAVLE = 1; //接拉头松开 CTHL_ZhuangLiaoStep = 0;//本次装料结束 } break; case 8: //二次检测 if(dwTickCount >= CTHL_ZhuangLiaoDelay) { if(CTHL_JLiao_Limit_IN&&!CTHL_LTou_Check)//接拉头到位,且缺拉头无信号，有拉头 { CTHL_ZhuangLiaoStep ++; CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_MaGou_DELAY;//延时码钩 } else //接拉头到位无信号,且缺拉头有信号，无拉头 { //重新装拉头3次 CTHL_TryCnt++; CTHL_JLTou_VAVLE = 1; //接拉头关 CTHL_HSLiao_VAVLE = 0; //横送回退 bTuiLaTouOkFlg = 0; if(CTHL_TryCnt >= 3) { CTHL_TryCnt = 0; bZhuangLiaoOkFlg = 0;//装拉头失败 CTHL_ZhuangLiaoStep = 0;//重新开始 CTHL_wltzi=1; //user_datas[127]=4;//测试用 CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);//装拉头异常 } else { CTHL_ZhuangLiaoStep = 19; //重新送3次 CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; } } } break; case 9: //码钩动作 if(dwTickCount >= CTHL_ZhuangLiaoDelay) //延时码勾 { CTHL_MGuo_VAVLE = 1; //码勾输出 CTHL_ZhuangLiaoStep++; CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_HSLiao_BACKDELAY;//横送回退延时 bTuiLaTouOkFlg = 0; } break; case 10: //横送回退 if(dwTickCount >= CTHL_ZhuangLiaoDelay) { CTHL_HSLiao_VAVLE = 0; CTHL_ZhuangLiaoStep++; CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; } break; case 11: //横送原位，给拉头，并判断是否存在着拉头。 if(!CTHL_HSLiao_Limit_IN&&CTHL_HSLiao_Origin_IN) //横送原位 { CTHL_ZhuangLiaoStep++; CTHL_TLiaoStep = 1; //推拉头，步骤动作 CTHL_ZhuangLiaoDelay = dwTickCount+5 ; //再次检测拉头 } else if(dwTickCount >= CTHL_ZhuangLiaoDelay)//横送原位位异常 { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning); if(bRunning)//异常后自动暂停 { CTHL_ZhuangLiaoStep = 0 ;//重新开始 } } break; case 12: //再次检测是否存在拉头 if(dwTickCount >= CTHL_ZhuangLiaoDelay) { if(CTHL_JLiao_Limit_IN&&!CTHL_LTou_Check)//接拉头到位,且缺拉头无信号，有拉头 { //bZhuangLiaoOkFlg = 1;//装拉头完成 CTHL_ZhuangLiaoStep++; } else { //重新装拉头3次 CTHL_TryCnt++; CTHL_JLTou_VAVLE = 1; //接拉头关 CTHL_MGuo_VAVLE = 0; if(CTHL_TryCnt >= 3) { CTHL_TryCnt = 0; bZhuangLiaoOkFlg = 0;//装拉头失败 CTHL_ZhuangLiaoStep = 0;//重新开始 CTHL_wltzi=1; //user_datas[127]=4;//测试用 CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning);//装拉头异常 } else { CTHL_ZhuangLiaoStep = 20; //重新送3次 CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; } } } break; case 13: //检测是否有下模上升的条件 if(!CTHL_TXM_VAVLE && !CTHL_TXM_Limit_IN&&!CTHL_SMZ_VAVLE&&!CTHL_SML_Limit_IN&&bXiaoMOUpFlg)//侧推在原位、上摸在原位，下模上升 { CTHL_CTXM_VAVLE = 1; //下模上升 CTHL_XCJC_VAVLE=1;//插销检测随着下模上升 CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; CTHL_ZhuangLiaoStep++; } break; case 14: //下模上升到位，插销钩针到位 if(CTHL_CTXM_VAVLE&&CTHL_CTXM_Limit_IN)//下模上到位 { bZhuangLiaoOkFlg = 1;//装拉头完成 bXiaoMOUpFlg=0; CTHL_ZhuangLiaoStep = 0; CTHL_PARAM_ZLT_TIME=dwTickCount-zlttime; } else if((dwTickCount >= CTHL_ZhuangLiaoDelay)) { CTHL_SetAlarmCode(CTHL_CTXM_Limit_ALARM,bRunning);//横送料原位异常 } break; case 19: //横送原位，给拉头，并判断是否存在着拉头。 if(!CTHL_HSLiao_Limit_IN&&CTHL_HSLiao_Origin_IN) //横送原位 { CTHL_ZhuangLiaoStep = 20; } else if(dwTickCount >= CTHL_ZhuangLiaoDelay)//横送原位位异常 { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning); if(bRunning)CTHL_ZhuangLiaoStep = 0 ;//重新开始，结束本次装拉头 } break; case 20: //重装拉头 if(CTHL_JLTou_VAVLE&&!CTHL_JLiao_Limit_IN && !CTHL_LTou_Check )//夹拉头松开 { CTHL_ZhuangLiaoStep = 1; CTHL_ZhuangLiaoDelay = dwTickCount + 10; } else if(dwTickCount >=CTHL_ZhuangLiaoDelay) { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_JLT_MZ_ALARM,bRunning);//夹拉头松开异常 } break; // case 30://横送回退 CTHL_HSLiao_VAVLE=0; CTHL_ZhuangLiaoStep = 31; CTHL_ZhuangLiaoDelay = dwTickCount + 200; break; case 31://横送前进 if(dwTickCount >=CTHL_ZhuangLiaoDelay) { CTHL_HSLiao_VAVLE=1; CTHL_ZhuangLiaoStep = 32; CTHL_ZhuangLiaoDelay = dwTickCount + 3000; } break; case 32://横送完成 if(CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_Origin_IN) { CTHL_ZhuangLiaoStep = 5; CTHL_ZhuangLiaoDelay = dwTickCount + 10; } else if(dwTickCount >= CTHL_ZhuangLiaoDelay)//异常告警 { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_HSLiao_Limit_ALARM,bRunning); CTHL_ZhuangLiaoStep = 0; CTHL_HSLiao_VAVLE = 0; } } } //给拉头动作 void CTHL_TLiao_Step(void) { switch(CTHL_TLiaoStep) { case 1:// if(bTuiLaTouOkFlg)//推拉头完成 { CTHL_TLiaoStep = 0; } else { CTHL_TLiao_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME;//气缸电磁阀不动作判断时间 CTHL_TLiaoStep++; } break; case 2: //信号到位后，稳定一会给拉头 if(CTHL_HSLiao_Origin_IN && !CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_VAVLE)//横送料在原位，横送料不在限位，横送气缸不动作 { CTHL_TLiao_Delay = dwTickCount + CTHL_PARAM_TLiao_FORWARDDELAY;//信号到位后，稳定一段时间动作 CTHL_TLiaoStep++; } else if(dwTickCount >= CTHL_TLiao_Delay) { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning); // //横送料原位异常 } break; case 3: //给拉头 if(dwTickCount >= CTHL_TLiao_Delay) { CTHL_TLiao_VAVLE = 1; //推料(推拉头)输出 CTHL_TLiao_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME;//气缸电磁阀不动作判断时间 CTHL_TLiaoStep++; } break; case 4: //给拉头到位后，延时一段时间 if(CTHL_TLiao_Limit_IN)//给拉头到位 { CTHL_TLiao_Delay = dwTickCount + CTHL_PARAM_TLiao_BACKDELAY;//推拉头到位后停留时间 CTHL_TLiaoStep++; } else if(dwTickCount >= CTHL_TLiao_Delay) { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,bRunning); //推料到位异常 } break; case 5://推料到位，推料气缸回退 if(dwTickCount >= CTHL_TLiao_Delay) { CTHL_TLiao_VAVLE = 0;//推料回退 CTHL_TLiao_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME; //气缸电磁阀不动作判断时间 CTHL_TLiaoStep++; } break; case 6://推料气缸回退到位 if(!CTHL_TLiao_Limit_IN)//推拉头动作完成 { CTHL_TLiaoStep = 0; CTHL_TLiao_Delay = dwTickCount; bTuiLaTouOkFlg = 1; } else if(dwTickCount >= CTHL_TLiao_Delay) { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,0);//给料到位异常 } break; } } //振动盘控制 void CTHL_ZhenDongAction(void) { #if 0 if((CTHL_bDLP || bRunning) && CTHL_ZhenDongPian_OUT && 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(!CTHL_PARAM_ZDP_AUTO) { if(CTHL_ZhenDongPian_OUT == 0)//振动盘未启动 { if(!CTHL_ZDP_IN)//未检测启动 { if(dwTickCount >= CTHL_ZDP_Delay) { CTHL_ZhenDongPian_OUT = 1; CTHL_ZDP_Delay = dwTickCount + CTHL_PARAM_ZDP_Stop_TIME;//CTHL_PARAM_ZDP_Stop_TIME + 50; } } else { CTHL_ZDP_Delay = dwTickCount + CTHL_PARAM_ZDP_Start_TIME;//CTHL_PARAM_ZDP_Start_TIME; } } else { if(CTHL_ZDP_IN) { if(dwTickCount >= CTHL_ZDP_Delay) { CTHL_ZhenDongPian_OUT = 0; CTHL_ZDP_Delay = dwTickCount + CTHL_PARAM_ZDP_Start_TIME;//CTHL_PARAM_ZDP_Start_TIME } } else CTHL_ZDP_Delay = dwTickCount + CTHL_PARAM_ZDP_Stop_TIME;//CTHL_PARAM_ZDP_Stop_TIME+ 50; } } else if (CTHL_PARAM_ZDP_AUTO ==1 ) { CTHL_ZhenDongPian_OUT=0; CTHL_ZDP_Delay = dwTickCount + 0; } else if(CTHL_PARAM_ZDP_AUTO == 2) { CTHL_ZhenDongPian_OUT=1; CTHL_ZDP_Delay = dwTickCount + 0; } #endif } //手动动作 void CTHL_ManualAction(void) { if(CTHL_bBaoJin) //报警清除加松轴 { CTHL_bBaoJin=0; SetEn(Y_AXIS, MOTOR_DISEN); SetEn(X_AXIS, MOTOR_DISEN); SetAlarmCode(CTHL_ALARM_ADDR,0); if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE) { SetClr(X_AXIS, MOTOR_NOALARM); SetClr(Y_AXIS, MOTOR_NOALARM); CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000; } if(CTHL_PARAM_DBSF_MODE) { if(!CTHL_SF_ALR_IN) { CTHL_SF_ALR_CLR=1; CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000; } } } if(bRunning == 0) { if(bClearTotal) //切断计数清零 { bClearTotal = 0; ClrcToTal(QDCT_TOTAL_ADDR); } if(CTHL_bHSL) //手动横送料 { CTHL_bHSL= 0; if(CTHL_HSLiao_VAVLE) { CTHL_HSLiao_VAVLE = 0;//~XY_CTHL_HSLiao_VAVLE; } else if((!CTHL_CTXM_VAVLE && CTHL_CTXM_Origin_IN ) && (!CTHL_bMG || (CTHL_bMG && CTHL_LTou_Check)))//下模在原位 { if(!CTHL_TLiao_VAVLE&&!CTHL_TLiao_Limit_IN)//推拉头在原位 { if(!CTHL_XCJC_VAVLE&& CTHL_XCJC_Origin_IN)//插销钩针在原点 { if(CTHL_JLTou_VAVLE&&!CTHL_LTou_Check&&!CTHL_JLiao_Limit_IN) { CTHL_HSLiao_VAVLE = 1; } else { CTHL_SetAlarmCode(CTHL_JLT_Limit_ALARM,0);//夹拉头异常 } } else { CTHL_SetAlarmCode(CTHL_XCJC_Origin_ALARM,0);//插销钩针原位异常 } } else { CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,0); } } else { CTHL_SetAlarmCode(CTHL_CTXM_Origin_ALARM,0); } } if(CTHL_bJLT)//接拉头 { CTHL_bJLT = 0; CTHL_MGuo_VAVLE = 0; CTHL_JLTou_VAVLE = !CTHL_JLTou_VAVLE; } if(CTHL_bTLiao ) //手动推料(给拉头) { CTHL_bTLiao = 0; if(CTHL_TLiao_VAVLE) CTHL_TLiao_VAVLE = 0;//CTHL_TLiao_VAVLE; else if(!CTHL_HSLiao_VAVLE && CTHL_HSLiao_Origin_IN) { CTHL_TLiao_VAVLE = 1; } else//条件警告 { CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,0);//横送料原位异常 } } if(CTHL_bMG) //码勾 { CTHL_bMG = 0; CTHL_MGuo_VAVLE = !CTHL_MGuo_VAVLE; } if(CTHL_bFKDW) //方块检测 { CTHL_bFKDW = 0; CTHL_FKDW_VAVLE = !CTHL_FKDW_VAVLE; } if(CTHL_bCTXM) //下模 { CTHL_bCTXM = 0; if(CTHL_CTXM_VAVLE) { CTHL_CTXM_VAVLE = 0; } else if(!CTHL_HSLiao_VAVLE && CTHL_HSLiao_Origin_IN ) //横推在原点 { CTHL_CTXM_VAVLE=1; } else//条件警告 { CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,0);//横送料原位异常 } } //侧推布带 if(CTHL_bTXM) { CTHL_bTXM=0; if(CTHL_TXM_VAVLE) { CTHL_TXM_VAVLE = 0; } else if(!CTHL_SMZ_VAVLE&&!CTHL_SML_Limit_IN) { CTHL_TXM_VAVLE = 1; } else//条件警告 { CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,0);//上模原位异常 } } if(CTHL_bSM)//上模 { CTHL_bSM=0; if(CTHL_SMZ_VAVLE) { CTHL_SMZ_VAVLE = 0; } else if(!CTHL_TXM_VAVLE && !CTHL_TXM_Limit_IN ) //横推在原点 { CTHL_SMZ_VAVLE=1; } else//条件警告 { CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,0);//横送料原位异常 } } if(CTHL_bCXDW) //插销钩针 { CTHL_bCXDW = 0; CTHL_XCJC_VAVLE = !CTHL_XCJC_VAVLE; } if(CTHL_bJCXBD)//插销布带夹 { CTHL_bJCXBD=0; CTHL_JCXBD_VAVLE = !CTHL_JCXBD_VAVLE; } if( CTHL_bCXHT)//插销回退 { CTHL_bCXHT=0; CTHL_CXHT_VAVLE = !CTHL_CXHT_VAVLE; } if(CTHL_bCXBB)//插销摆动 { CTHL_bCXBB=0; CTHL_XCBB_VAVLE = !CTHL_XCBB_VAVLE; } if(CTHL_bFZYL)//右拖压轮 { CTHL_bFZYL=0; CTHL_FZYL_VAVLE = !CTHL_FZYL_VAVLE; } if(CTHL_bGLBG)//过链摆杆 { CTHL_bGLBG=0; CTHL_HL_VAVLE=~CTHL_HL_VAVLE; } //自动装料 if(CTHL_bAutoZhuangLiao) { CTHL_bAutoZhuangLiao = 0; if(CTHL_ZhuangLiaoStep == 0) { bTuiLaTouOkFlg = 1; bZhuangLiaoOkFlg = 0; bXiaoMOUpFlg=1; CTHL_ZhuangLiaoStep = 1; CTHL_MGuo_VAVLE = 0; CTHL_ZhuangLiaoDelay = dwTickCount + 150; } } //一键定位 if(CTHL_bYJDW) { CTHL_bYJDW=0; CTHL_FKDW_VAVLE = 0;//方块定位气缸 CTHL_TXM_VAVLE=0; //侧推布带气缸 CTHL_SMZ_VAVLE=0; //上模气缸 CTHL_XCBB_VAVLE=0; //插销摆臂 CTHL_JCXBD_VAVLE=0; //夹插销布带 CTHL_CXHT_VAVLE=0; //插销后退 CTHL_FZYL_VAVLE=0; //右拖带压轮 CTHL_TD_MotorDelay = dwTickCount + 100; if(CTHL_TD_MotorStep==0){ if(GetEn(X_AXIS) == MOTOR_DISEN){//未使能 SetEn(X_AXIS, MOTOR_EN);//X使能 CTHL_TD_MotorDelay = dwTickCount + 200; } else{ CTHL_TD_MotorDelay = dwTickCount + 100; } CTHL_TD_MotorStep=1; } } //X轴Y轴电机测试 if(CTHL_bCLMotor_N) { SetEn(Y_AXIS, MOTOR_EN);//使能X轴 SetEn(X_AXIS, MOTOR_EN);//使能Y轴 if(CTHL_FK_Check_UP){//检测到过链感应信号后，设置原位。 SetPos(X_AXIS, 0); SetPos(Y_AXIS, 0); } if(!X_DRV){ // Y轴— 运行速度—方向—启动速度—停止速度— 加速时间— 减速时间 //AxisMovePosAccDec(X_AXIS,3000,2953,600,600,100,100,900); //AxisMovePosAccDec(Y_AXIS,3000,2953,600,600,100,100,900); AxisContinueMoveAcc(X_AXIS,600 ,DIR_P,600,600,15,15); if(servo_y.IO_TO_COM==0)AxisContinueMoveAcc(Y_AXIS,580,DIR_P,580,580,15,15); } } if(CTHL_bCLMotor_P) { SetEn(Y_AXIS, MOTOR_EN);//使能X轴 SetEn(X_AXIS, MOTOR_EN);//使能Y轴 if(CTHL_FK_Check_UP){//检测到过链感应信号后，设置原位。 SetPos(X_AXIS, 0); SetPos(Y_AXIS, 0); CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,0);//方块定位有信号 } if(!X_DRV ){ // Y轴— 运行速度—方向—启动速度—停止速度— 加速时间— 减速时间 速度 AxisContinueMoveAcc(X_AXIS,600,DIR_N,600,600,15,15); if(servo_y.IO_TO_COM==0)AxisContinueMoveAcc(Y_AXIS,630,DIR_N,630,630,15,15); } } if(!CTHL_bCLMotor_P && !CTHL_bCLMotor_N && (CTHL_TD_MotorStep == 0)) { AxisEgmStop(X_AXIS); //if(servo_y.IO_TO_COM==0)AxisEgmStop(Y_AXIS);else SetEn(Y_AXIS, MOTOR_EN); } } } //初始化动作 void CTHL_InitAction(void) { float buff_pulse,buff_dist; buff_pulse = CTHL_PARAM_TDCYCLE_PULSE; buff_dist = CTHL_PARAM_TDCYCLE_LENGTH; XGearRatio = buff_pulse/buff_dist; buff_pulse = CTHL_PARAM_TDCYCLE_PULSE; buff_dist = CTHL_PARAM_FZCYCLE_LENGTH; YGearRatio = buff_pulse/buff_dist; //传递伺服齿轮比参数 #if X_USERING_TARR==1 set_com_servo_param(X_AXIS,CTHL_PARAM_TDCYCLE_PULSE,CTHL_PARAM_TDCYCLE_LENGTH); #endif #if Y_USERING_TARR==1 set_com_servo_param(Y_AXIS,CTHL_PARAM_TDCYCLE_PULSE,CTHL_PARAM_FZCYCLE_LENGTH); #endif SetAlarmCode(CTHL_ALARM_ADDR,0); SetEnReverse(X_AXIS, 0); SetEnReverse(Y_AXIS, 0); SetDirReverse(X_AXIS, 0); SetDirReverse(Y_AXIS, 0); //设置伺服告警极性, //伺服设置常开，则极性设置为0， 0为正常，1为告警，； //伺服设置为常闭，则极性设置为1，0，为正常，1位告警 SetAlarmReverse(X_AXIS,0); SetAlarmReverse(Y_AXIS,0); SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); SetPos(X_AXIS, 0); //启动位置设为0点 SetPos(Y_AXIS, 0); //启动位置设为0点 } //运行 void CTHL_Action(void) { dwXRealPos = GetPos(X_AXIS); dwYRealPos = GetPos(Y_AXIS); dwZRealPos = GetPos(Z_AXIS); dwYRealPos_com=get_encode_value(Y_AXIS); CTHL_ZhenDongAction(); CTHL_AlarmProtect(); CTHL_CheckStart(); CTHL_ZLT_Step();//装拉头 CTHL_TLiao_Step();//推拉头 CTHL_TD_Motor();//拖带电机X轴 CTHL_FZ_Motor();//穿拉头电机Y轴 CTHL_ManualAction(); CTHL_AutoStepAction(); CTHL_Test++; servo_com_run(); CTHL_Running_state(); } #endif