#include "global.h" #if FU_XIAO_WEI_MACHINE == 1 unsigned long CTXM_DOWN_TIME=0,PRV_CTXM_DOWN_TIME=0; unsigned long HSLIAO_OUT_DELAY=0,PRV_HSLIAO_OUT_DELAY=0; unsigned char FZ_ALARM_FLG=0; unsigned char CD_ORIGIN_FLG=0; unsigned short XCCR_LENGTH; void CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag); void CTHL_ManualAction(void); void CTHL_AutoStepAction(void); void CTHL_TD_Motor(void); void CTHL_FZ_Motor(void); void CTHL_CheckStart(void); void CTHL_TLiao_Step(void); //推料(推拉头) void CTHL_AlarmProtect(void); void CTHL_ZLT_Step(void); void CTHL_ZhenDongAction(void); void CTHL_CUDAI_Motor(void); void ExtiAcitionX02(void) { } //故障报警 void CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag) { SetAlarmCode(CTHL_ALARM_ADDR,alarm_code); if(!CTHL_wltzi && !bCxcAuto ) bAlarmStop = 1; if(bCxcAuto) { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); //SetEn(X_AXIS, MOTOR_EN); //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; buff_pulse = CTHL_PARAM_TDCYCLE_PULSE; buff_dist = CTHL_PARAM_CDCYCLE_LENGTH; ZGearRatio = buff_pulse/buff_dist; //传递伺服齿轮比参数 set_com_servo_param(X_AXIS,CTHL_PARAM_TDCYCLE_PULSE,CTHL_PARAM_TDCYCLE_LENGTH); set_com_servo_param(Y_AXIS,CTHL_PARAM_TDCYCLE_PULSE,CTHL_PARAM_FZCYCLE_LENGTH); set_com_servo_param(Z_AXIS,CTHL_PARAM_TDCYCLE_PULSE,CTHL_PARAM_CDCYCLE_LENGTH); SetAlarmCode(CTHL_ALARM_ADDR,0); user_datas[121] = 0; user_datas[122] = 0; user_datas[123] = 0; SetEnReverse(X_AXIS, 0); SetEnReverse(Y_AXIS, 0); SetEnReverse(Z_AXIS, 0); SetDirReverse(X_AXIS, 0); SetDirReverse(Y_AXIS, 0); SetDirReverse(Z_AXIS, 0); //设置伺服告警极性, //伺服设置常开，则极性设置为0， 0为正常，1为告警，； //伺服设置为常闭，则极性设置为1，0，为正常，1位告警 //SetAlarmReverse(X_AXIS,1); //SetAlarmReverse(Y_AXIS,0); SetServoComUse(X_AXIS,1);//X轴正常模式。 ReadServoAlarm_Enable(X_AXIS,0);//X轴正常模式。 ReadServoEncode_Enable(X_AXIS,0); SetServoComUse(Y_AXIS,1);//Y轴伺服扭矩通讯。 ReadServoAlarm_Enable(Y_AXIS,1); ReadServoEncode_Enable(Y_AXIS,1); SetServoComUse(Z_AXIS,1);//Z轴伺服扭矩通讯。 ReadServoAlarm_Enable(Z_AXIS,0); ReadServoEncode_Enable(Z_AXIS,0); SetEn(X_AXIS, MOTOR_EN); SetEn(Y_AXIS, MOTOR_EN); SetEn(Z_AXIS, MOTOR_EN); SetPos(X_AXIS, 0); //启动位置设为0点 SetPos(Y_AXIS, 0); //启动位置设为0点 SetPos(Z_AXIS, 0); //启动位置设为0点 cBAOjin=1; } void CTHL_Action(void) { dwXRealPos = GetPos(X_AXIS); dwYRealPos = GetPos(Y_AXIS); dwZRealPos = GetPos(Z_AXIS); dwYRealPos_com=get_encode_value(Y_AXIS); user_datas[121] = CTHL_LianLengthCheckFK_GL;//servo_z.vdi_value;//servo_z.init_step;//;//CTXM_DOWN_TIME;//dwYRealPos_com;//servo_y.init_step;//CTHL_ZhuangLiaoStep; user_datas[122] = CTHL_LianLengthCheckFK_DW; user_datas[123] = CTHL_AutoStep; user_datas[124] = CTHL_LianLengthCheck;//CTHL_ZhuangLiaoStep; user_datas[125] = CTHL_LianLengthCheckOld;//CTHL_TLiaoStep; user_datas[126] = CTHL_CD_MotorStep;//CTHL_FZ_MotorStep;//CTHL_TD_MotorStep; // user_datas[127] = CTHL_cZipCnt;//CTHL_ZhuangLiaoStep; // user_datas[128] = CTHL_FZ_MotorStep;//CTHL_LianLengthAutolengthcheck; CTHL_bServo_Com_X=servo_x.IO_TO_COM; CTHL_bServo_Com_Y=servo_y.IO_TO_COM; CTHL_bServo_Com_Z=servo_z.IO_TO_COM; CTHL_ZhenDongAction(); CTHL_AlarmProtect(); CTHL_CheckStart(); CTHL_ZLT_Step(); CTHL_TLiao_Step(); CTHL_TD_Motor(); CTHL_FZ_Motor(); CTHL_ManualAction(); CTHL_AutoStepAction(); CTHL_Test++; servo_com_run(); CTHL_CUDAI_Motor(); } void CTHL_AlarmProtect(void) { } //自动动作 void CTHL_AutoStepAction(void) { unsigned short freq; static unsigned char hemoflag1=0,hemoflag2=0; //测试用 if(dwTickCount >= CTHL_TestDelay) { CTHL_TestDelay = dwTickCount + 200; CTHL_TestLED = !CTHL_TestLED; CTHL_Test = 0; } if(CTHL_bHandWork_MODE)CTHL_bHandWork_State=1;else CTHL_bHandWork_State=0; if(bRunning) { switch(CTHL_AutoStep) { case 1: if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_YXC_VAVLE=1; CTHL_YFK_VAVLE=1; CTHL_TXM_VAVLE=0; //设置反转电机的扭矩和速度的设置 freq=CTHL_PARAM_FZ_Speed+SERVO_PARAM_SPEED_DIFFER; if(CTHL_cZipCnt >=1) { set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW); Set_Ctrlmode_trans(Z_AXIS,POS_MODE); } else { set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW); Set_Ctrlmode_trans(Z_AXIS,POS_MODE); } Set_Ctrlmode_trans(Y_AXIS,POS_MODE); set_servo_postotarr_limit(Z_AXIS,SERVO_PARAM_CUDAI_TRARRMAX,50000,50000,SERVO_TARR_CW); // set_servo_postotarr_limit(X_AXIS,80,20000,20000,SERVO_TARR_CCW); Set_Ctrlmode_trans(X_AXIS,POS_MODE); bCxcAuto=0; if(CTHL_cZipCnt > 1) CTHL_AutoDelay = dwTickCount + 1;//拉头检测时间 else CTHL_AutoDelay = dwTickCount + 180;//拉头检测时间 if(CTHL_ZhuangLiaoStep == 0) { CTHL_ZhuangLiaoStep = 1; CTHL_TryCnt = 0; } CTHL_GF_Change = 0; CTHL_AutoStep = 2; } } break; case 2: if(bZhuangLiaoOkFlg && (CTHL_ZhuangLiaoStep == 0)&&(CTHL_CTXMSSStep==0)) { if(dwTickCount >= CTHL_AutoDelay ) { if(CTHL_HSLiao_Origin_IN||CTHL_CTM_Minid_IN&&!CTHL_HSLiao_VAVLE) { //有拉头，推斜码到位不能亮 if(!CTHL_LTou_Check && !CTHL_TXM_Limit_IN) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { if(!CTHL_PARAM_XMZM_MODE) CTHL_QMDW_VAVLE = 1; CTHL_FKDW_VAVLE = 0; CTHL_CTXM_VAVLE = 1; //下模上升 CTHL_JLTou_VAVLE = 0; //接拉头 CTHL_AutoStep = 3; CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } } else if(dwTickCount >= (CTHL_AutoDelay+3000))//无拉头告警，并按下启动，重新装拉头 { CTHL_wltzi=1; user_datas[127]=1; CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning); // 没有拉头 } } else if(dwTickCount >= (CTHL_AutoDelay+4000)) { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning);//横送料原位异常 } } } break; case 3: if(CTHL_CTM_Limit_IN) //下模上到位 { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoDelay = dwTickCount + CTHL_PARAM_XCDWFZ_DELAY; if(!CTHL_PARAM_XMZM_MODE) CTHL_AutoStep = 4; else CTHL_AutoStep = 5; } } else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_bDANBU_MODE) { CTHL_SetAlarmCode(CTHL_CTM_Limit_ALARM,bRunning);//横送料原位异常 } break; case 4: if(dwTickCount >= CTHL_AutoDelay&&!X_DRV) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { //AxisContinueMoveAcc(X_AXIS,1000,DIR_N,1000,600,15,5); AxisMovePosAccDec(X_AXIS,2000,-CTHL_PARAM_JMDW_LENGTH,1000,800,10,10,0); //前拖带松一点带 AxisContinueMoveAcc(Y_AXIS,1000,DIR_N,1000,610,20,20); //Set_Ctrlmode_trans(Y_AXIS,TARR_MODE); CTHL_AutoStep = 5; CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; CTHL_JXM_VAVLE = 1; //夹斜码电磁阀 } } break; case 5: if(CTHL_QMDW_IN || CTHL_PARAM_XMZM_MODE) //if(!X_DRV|| CTHL_PARAM_XMZM_MODE) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_JXM_VAVLE = 1; //夹斜码电磁阀 CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TXM_DELAY; CTHL_AutoStep = 6; } AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); //set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW); //Set_Ctrlmode_trans(Y_AXIS,POS_MODE); } else if((dwTickCount >= CTHL_AutoDelay) && (!CTHL_bHandWork_MODE)) { CTHL_SetAlarmCode(CTHL_QMDW_Limit_ALARM,bRunning);//前码定位感应异常 } break; case 6: if(dwTickCount >= CTHL_AutoDelay) //顶斜码后拖反转 { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { AxisMovePosAccDec(X_AXIS,2000,-CTHL_PARAM_XMDD_LENGTH,2000,800,10,10,0); //前拖带松一点带 CTHL_TXM_VAVLE=1; //Y06推斜码电磁阀 CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; CTHL_AutoStep = 7; } } break; case 7: if(CTHL_TXM_Limit_IN && CTHL_TXM_VAVLE ) //推斜码到位 { Set_Ctrlmode_trans(Z_AXIS,TARR_MODE); CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TQMDW_DELAY; if(get_tarr_set(Y_AXIS)!=2)//伺服配置转矩限制失败 { SetEn(Y_AXIS, MOTOR_DISEN);//设置扭矩做准备 CTHL_wltzi=0;//停机 CTHL_SetAlarmCode(CTHL_SERVO_CONFIG_ALARM,bRunning);//伺服转矩配置失败 } CTHL_AutoStep = 8; } else if((dwTickCount >= CTHL_AutoDelay) && (!CTHL_bDANBU_MODE)) { CTHL_SetAlarmCode(CTHL_TXM_Limit_ALARM,bRunning);//推斜码到异常 } break; case 8: if(dwTickCount >= CTHL_AutoDelay&& (!X_DRV)) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_QMDW_VAVLE=0; CTHL_FZ_MotorStep=1; //反转穿入 //Set_Ctrlmode_trans(Y_AXIS,POS_MODE); CTHL_FZ_MotorDelay = dwTickCount + CTHL_PARAM_CRTD_DELAY;//延时穿入拖带 CTHL_AutoStep = 9; } } break; case 9: if(CTHL_FZ_MotorStep==0) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { cBAOjin=0; CTHL_TFK_VAVLE=1; CTHL_YFK_VAVLE=0; CTHL_XCDW_VAVLE=1; CTHL_AutoStep = 10; set_servo_postotarr_limit(Z_AXIS,SERVO_PARAM_CUDAI_TRARRMIN,10000,10000,SERVO_TARR_CW); CTHL_AutoDelay = dwTickCount + CTHL_PARAM_FKDW_FZSTOP_DELAY;//拖带定位 } } break; case 10: if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_FZ_MotorStep=20; CTHL_AutoStep = 11; CTHL_FZ_MotorDelay = dwTickCount + 0; } } break; case 11: if(CTHL_FZ_MotorStep==0 && !X_DRV && !Y_DRV) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_JCBI_VAVLE = 1; CTHL_AutoStep = 12; CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } } break; case 12: if(CTHL_TXC_Limit_IN) //摆臂到位 { CTHL_DingLian_VAVLE=1; //暂时不用 AxisMovePosAccDec(Y_AXIS,2*CTHL_PARAM_XCCR_SPEED,CTHL_PARAM_FZCXC_LENGTH,1000,1000,15,15,0);//辅助小插插入 //AxisMovePosAccDec(X_AXIS,2*CTHL_PARAM_XCCR_SPEED,-CTHL_PARAM_FZCXC_LENGTH,1000,1000,15,15,0);//辅助小插插入 Set_Ctrlmode_trans(Y_AXIS,POS_MODE); Set_Ctrlmode_trans(X_AXIS,POS_MODE); CTHL_AutoStep = 13; CTHL_AutoDelay = dwTickCount + CTHL_PARAM_XCIN_DELAY;//小插到位延时拖带 } else if(dwTickCount >= CTHL_AutoDelay) { bCxcAuto=1; CTHL_AutoStep = 100; CTHL_JCBI_VAVLE = 0; //Y16小插摆臂电磁阀 CTHL_SetAlarmCode(CTHL_XCBI_Limit_ALARM,bRunning);//小插摆臂到位异常 } break; case 13: if(dwTickCount >= CTHL_AutoDelay )//&& !Y_DRV) { SetPos(X_AXIS, 0); if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_YXC_VAVLE=0; CTHL_AutoDelay = dwTickCount + 0;//CTHL_PARAM_YDC_DELAY; AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_XCCR_SPEED,CTHL_PARAM_XCCR_LENGTH,800,800,15,15,50); //AxisMovePosAccDec(Y_AXIS,3*CTHL_PARAM_XCCR_SPEED,(CTHL_PARAM_XCCR_LENGTH+CTHL_PARAM_FZCXC_LENGTH),1000,1000,15,15,0); CTHL_AutoStep = 14; } CTHL_XSavePosBuff3=dwXRealPos; } break; case 14: if(CTHL_XCDW_Limit_IN)//||(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_XCCRBH_LENGTH)) { XCCR_LENGTH=dwXRealPos-CTHL_XSavePosBuff3; CTHL_XSavePosBuff3 = dwXRealPos; CTHL_AutoStep = 15; CTHL_XCDW_VAVLE=0; CTHL_XCQM_VAVLE=0; //CTHL_TFK_VAVLE = 0; CTHL_AutoDelay = dwTickCount + CTHL_PARAM_TXCDW_DELAY; } //else if((dwTickCount >= (CTHL_AutoDelay+1000)) && (!CTHL_bHandWork_MODE)) else if((dwTickCount >= (CTHL_AutoDelay+1200)||(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_XCCRBH_LENGTH)) && (!CTHL_bHandWork_MODE)) { bCxcAuto=1; CTHL_SetAlarmCode(CTHL_XCDW_Limit_ALARM,bRunning);//小插定位针异常 CTHL_AutoStep = 100; CTHL_JCBI_VAVLE = 0; //Y16小插摆臂电磁阀 AxisMovePosAccDec(X_AXIS,800,-100,800,800,15,15,0);//前轮布放松一点 } break; case 15: if((dwXRealPos >= (CTHL_XSavePosBuff3 + 50))) { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); if(dwTickCount >= CTHL_AutoDelay) { CTHL_TFK_VAVLE = 0; if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { bZhuangLiaoOkFlg=0; CTHL_AutoStep = 16; CTHL_AutoDelay = dwTickCount + 0; } } } break; case 16: if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { if(dwTickCount >= CTHL_AutoDelay) { CTHL_AutoDelay = dwTickCount + 0;//CTHL_PARAM_TXCDW_DELAY ; CTHL_AutoStep = 17; CTHL_JCBI_VAVLE=0; } } break; case 100: if(CTHL_START_IN_UP) { CTHL_AutoStep = 101; } break; case 101: CTHL_AutoStep = 102; break; case 102: if(!X_DRV&&!Y_DRV) { SetAlarmCode(CTHL_ALARM_ADDR,0); CTHL_YFK_VAVLE=1; CTHL_JCBI_VAVLE = 1; //Y16小插摆臂电磁阀 cBAOjin=1; CTHL_cZipCnt=0; bZhuangLiaoOkFlg=0; SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); CTHL_AutoStep = 103; CTHL_AutoDelay = dwTickCount + 300 ; SetClr(X_AXIS, MOTOR_NOALARM); SetClr(Y_AXIS, MOTOR_NOALARM); CTHL_bSFBBAOJIN_Delay= dwTickCount + 200; if(CTHL_PARAM_DBSF_MODE) { if(!CTHL_SF_ALR_IN) { CTHL_SF_ALR_CLR=1; CTHL_bSFBBAOJIN_Delay= dwTickCount + 200; } } } break; case 103: if(dwTickCount >= CTHL_AutoDelay) { CTHL_AutoStep = 13; CTHL_AutoDelay = dwTickCount + 100 ; } break; case 17: if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_TFK_VAVLE=0; if(!CTHL_bHandWork_MODE)CTHL_TD_MotorStep =10; else { CTHL_YXC_VAVLE=0; CTHL_AutoDelay = dwTickCount + 1000 ; } CTHL_TD_MotorDelay = dwTickCount+ CTHL_PARAM_XCLD_DELAY; CTHL_AutoStep = 18; } } break; case 18: if(!CTHL_bHandWork_MODE) { CTHL_AutoStep = 19; } else if(dwTickCount>=CTHL_AutoDelay) { CTHL_MGuo_VAVLE=0; CTHL_CTXM_VAVLE =0; CTHL_CTXMSSStep=1; CTHL_AutoStep = 19; } break; case 19: if((CTHL_TD_MotorStep == 0)) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoStep = 1; if(CTHL_cStopMode==1 || bSingOneFlg) { CTHL_cStopMode=2; CTHL_ZhouWantwo=1; CTHL_CTXM_VAVLE =0; } AddToTal(CTHL_TOTAL_ADDR); AddToTal(CTHL_TRUST_ALARM_ADDR); CalProSP(CTHL_SPEED_ADDR); //公分节那条也不加 if((CTHL_cZipCnt<3) && !CTHL_GF_Change) CTHL_cZipCnt++; if((CTHL_NoLaLian_IN)) CTHL_cZipCnt = 0; if(bSingOneFlg || CTHL_bHandWork_MODE) { CTHL_AutoStep = 0; bRunning = 0; bSingOneFlg = 0; CTHL_bHandWork_MODE=0; } CTHL_GF_Change = 0; } } break; } //退下模后启动送拉头 switch(CTHL_CTXMSSStep) { case 1: if(!CTHL_CTM_Limit_IN) { PRV_CTXM_DOWN_TIME= dwTickCount; if(CTXM_DOWN_TIME)HSLIAO_OUT_DELAY=dwTickCount+CTXM_DOWN_TIME*7/8; hemoflag1=1; hemoflag2=1; CTHL_CTXMSSStep = 2; CTHL_CTXMSS_Delay = dwTickCount+ 1000; } break; case 2: //测量时间 if(CTHL_CTM_Origin_IN&&hemoflag1) { hemoflag1=0; PRV_CTXM_DOWN_TIME=dwTickCount-PRV_CTXM_DOWN_TIME; CTXM_DOWN_TIME=PRV_CTXM_DOWN_TIME; } // /*if(CTXM_DOWN_TIME&&(CTHL_cZipCnt > 1))//测量时间后的动作 { if(dwTickCount>=HSLIAO_OUT_DELAY&&hemoflag2) { hemoflag2=0; CTHL_ZhuangLiaoStep = 1; CTHL_TryCnt = 0; bZhuangLiaoOkFlg=0; CTHL_ZhuangLiaoDelay = dwTickCount; } if(CTHL_CTM_Origin_IN) { CTHL_CTXMSSStep = 0; CTHL_MGuo_VAVLE = 0; CTHL_JLTou_VAVLE = 0; } else if(dwTickCount >= CTHL_CTXMSS_Delay) { //穿头模和一次穿入回位异常 if(!CTHL_CTM_Origin_IN) CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning); } } else *///测量时间前的动作 { if(CTHL_CTM_Origin_IN) { CTHL_ZhuangLiaoStep = 1; CTHL_TryCnt = 0; bZhuangLiaoOkFlg=0; CTHL_ZhuangLiaoDelay = dwTickCount; CTHL_CTXMSSStep = 20; CTHL_MGuo_VAVLE = 0; CTHL_JLTou_VAVLE = 0; } else if(dwTickCount >= CTHL_CTXMSS_Delay) { //穿头模和一次穿入回位异常 if(!CTHL_CTM_Origin_IN) CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning); } } break; case 20: if(bZhuangLiaoOkFlg && (CTHL_ZhuangLiaoStep == 0)&&CTHL_CTXMSS_FLAG) { CTHL_CTXMSS_FLAG=0; CTHL_CTXMSSStep = 21; CTHL_CTXMSS_Delay = dwTickCount+ 1000; } break; case 21: if(!CTHL_LTou_Check && !CTHL_TXM_Limit_IN)//有拉头，推斜码到位不能亮 { if(!bSingOneFlg) { CTHL_CTXM_VAVLE=1; CTHL_CTXMSSStep = 22; } else { CTHL_CTXMSSStep = 0; } } else if(dwTickCount >= CTHL_CTXMSS_Delay) { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning); // 没有拉头 } break; case 22: if(CTHL_CTM_Limit_IN) { CTHL_CTXMSSStep = 0; } break; } } } void CTHL_CheckStart(void) { if(!CTHL_SafeDoor_IN && bRunning) { CTHL_SetAlarmCode(CTHL_SafeDoor_ALARM,0); // 安全门关上才能启动 } if(CTHL_START_IN_UP || bStart) { 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); } 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_MGuo_VAVLE = 1; //判断是否已经有料 CTHL_ZhuangLiaoDelay = dwTickCount + 150; if((GetAlarm(X_AXIS)==1 || GetAlarm(Y_AXIS)==1) && !CTHL_PARAM_DBSF_MODE) { SetClr(X_AXIS, MOTOR_NOALARM); SetClr(Y_AXIS, MOTOR_NOALARM); SetClr(Z_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; } } } else { CTHL_wltzi=0; } } } #if 1 //启动 自动启动 单一自动 if((CTHL_START_IN_UP || bStart || CTHL_bSingOne ||CTHL_bHandWork_Run ) && !CTHL_wltzi) { if(!bRunning) { if(!CTHL_CTM_Origin_IN) CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning); bRunning = 1; if( CTHL_bSingOne) { bSingOneFlg=1; } if(CTHL_bHandWork_Run) { CTHL_bHandWork_MODE=1; CTHL_bHandWork_Run=0; } bFZ_LengthOK = 0; SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); SetEn(Z_AXIS, MOTOR_DISEN); CTHL_FZ_MotorStep = 0; CTHL_CTXMSSStep=0; CTHL_CTXMSS_FLAG=0; CTHL_CD_MotorStep=0; bTuiLaTouOkFlg = 1; bZhuangLiaoOkFlg= 0; CTHL_TryCnt = 0; CTHL_PARAM_ZDP_AUTO=0; bCxcAuto=0; bCL_EN_FLG = 0; CTHL_MOTOR_MODE=0; CTHL_stop=0; CTHL_TLiao_VAVLE =0; CTHL_HSLiao_VAVLE =0; CTHL_JLTou_VAVLE =0; CTHL_CTXM_VAVLE =0; // CTHL_MGuo_VAVLE =0; CTHL_JXM_VAVLE =0; CTHL_TXM_VAVLE = 0 ; CTHL_QMDW_VAVLE =0; CTHL_TFK_VAVLE =0; // CTHL_FKJC_VAVLE =1; CTHL_HL_VAVLE =0; CTHL_YFK_VAVLE =0; CTHL_YXC_VAVLE =0; CTHL_XCDW_VAVLE =0; CTHL_JCBI_VAVLE =0; CTHL_XCQM_VAVLE=0; CTHL_cZipCnt = 0; SetAlarmCode(CTHL_ALARM_ADDR,0); CTHL_AutoDelay = dwTickCount + 0; CTHL_TD_MotorStep = 0; CTHL_AutoStep = 1; CTHL_MGuo_VAVLE = 1; //判断是否已经有料 CTHL_ZhuangLiaoDelay = dwTickCount + 350; CTHL_ZhuangLiaoStep=1; CTHL_LianLengthAutoCheck = 0; } bStart = 0; CTHL_bSingOne=0; } if(CTHL_ZhouWantwo) { CTHL_ZhouWantwo=0; if(CTHL_ZhouWanone) { CTHL_ZhouWanone=0; bRunning=0; CTHL_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); SetClr(Z_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; } } SetEn(X_AXIS, MOTOR_EN); SetEn(Y_AXIS, MOTOR_EN); SetEn(Z_AXIS, MOTOR_EN); //Set_VDI_Value(Z_AXIS,0); CTHL_CD_MotorStep=0; bSingOneFlg=0; CTHL_AutoStep = 0; CTHL_FZ_MotorStep =0; CTHL_TD_MotorStep =0; CTHL_CTXM_VAVLE =0; } } //停止 if(CTHL_STOP_IN_UP || bStop) { if(CTHL_cStopMode < 2) CTHL_cStopMode++; if(CTHL_cStopMode==1 && bRunning) CTHL_ZhouWanone=1; if(bRunning && CTHL_cStopMode == 2) { bRunning=0; CTHL_cStopMode=0; bSingOneFlg=0; bFZ_LengthOK = 0; cBAOjin=0; CTHL_wltzi=0; CTHL_MOTOR_MODE=0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); AxisEgmStop(Z_AXIS); Set_Ctrlmode_trans(Y_AXIS,POS_MODE); SetEn(X_AXIS, MOTOR_EN); SetEn(Y_AXIS, MOTOR_EN); SetEn(Z_AXIS, MOTOR_EN); //Set_VDI_Value(Z_AXIS,0); CTHL_bHandWork_MODE=0; CTHL_bHandWork_Run=0; CTHL_AutoStep = 0; CTHL_stop=0; CTHL_FZ_MotorStep =0; CTHL_TD_MotorStep =0; CTHL_CD_MotorStep=0; CTHL_bResting_Z=0; // CTHL_ZhuangLiaoStep=0; // CTHL_TLiao_VAVLE=0; // CTHL_HSLiao_VAVLE =0; // CTHL_JLTou_VAVLE =0; CTHL_CTXM_VAVLE =0; CTHL_MGuo_VAVLE = 0; CTHL_JXM_VAVLE =0; CTHL_QMDW_VAVLE =0; CTHL_TFK_VAVLE =0; // CTHL_FKJC_VAVLE =0; CTHL_HL_VAVLE =0; CTHL_YFK_VAVLE =0; CTHL_YXC_VAVLE =0; CTHL_XCDW_VAVLE=0; CTHL_JCBI_VAVLE =0; CTHL_XCQM_VAVLE=0; 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); SetClr(Z_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; } } SetPos(X_AXIS, 0); SetPos(Y_AXIS, 0); SetPos(Z_AXIS, 0); SetAlarmCode(CTHL_ALARM_ADDR,0); } else if(!bRunning) { bRunning=0; CTHL_cStopMode=0; bSingOneFlg=0; CTHL_wltzi=0; bCxcAuto=0; cBAOjin=0; CTHL_stop=0; bFZ_LengthOK = 0; CTHL_MOTOR_MODE=0; CTHL_RunStepSave = 0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); Set_Ctrlmode_trans(Y_AXIS,POS_MODE); SetEn(X_AXIS, MOTOR_EN); SetEn(Y_AXIS, MOTOR_EN); SetEn(Z_AXIS, MOTOR_EN); //Set_VDI_Value(Z_AXIS,0); SetAlarmCode(CTHL_ALARM_ADDR,0); CTHL_TLiao_VAVLE =0; CTHL_HSLiao_VAVLE =0; CTHL_JLTou_VAVLE =0; CTHL_CTXM_VAVLE =0; CTHL_MGuo_VAVLE =0; CTHL_JXM_VAVLE =0; if(CTHL_PARAM_Test) CTHL_TXM_VAVLE = 0 ; else CTHL_TXM_VAVLE = 1; CTHL_QMDW_VAVLE =0; CTHL_TFK_VAVLE =0; CTHL_FKDW_VAVLE=0; CTHL_HL_VAVLE =0; CTHL_YFK_VAVLE =0; CTHL_YXC_VAVLE =0; CTHL_XCDW_VAVLE =0; CTHL_JCBI_VAVLE =0; CTHL_XCQM_VAVLE =0; CTHL_AutoStep = 0; CTHL_ZhuangLiaoStep=0; CTHL_TLiaoStep=0; CTHL_TD_MotorStep=0; CTHL_SongLiaoStep =0; CTHL_FZ_MotorStep =0; CTHL_CTXMSSStep=0; CTHL_bResting_Z=0; CTHL_CD_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); SetClr(Z_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); SetPos(Z_AXIS, 0); CTHL_TLiaoStep = 0; CTHL_AutoStep = 0; CTHL_LianLengthCheck = 0; CTHL_LianLengthCheckOld = 0; } bStop=0; } if(bAlarmStop) { bRunning = 0; bAlarmStop=0; CTHL_cStopMode=0; CTHL_XCDW_VAVLE=0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); AxisEgmStop(Z_AXIS); Set_Ctrlmode_trans(Y_AXIS,POS_MODE); Set_Ctrlmode_trans(Z_AXIS,POS_MODE); SetEn(X_AXIS, MOTOR_EN); SetEn(Y_AXIS, MOTOR_EN); SetEn(Z_AXIS, MOTOR_EN); CTHL_AutoStep = 0; CTHL_FZ_MotorStep =0; CTHL_TD_MotorStep =0; } #endif } //无带处理 void CTHL_NoLaLian_Speed_Proc(void) { if(CTHL_NoLaLian_IN_UP) { CTHL_cZipCnt=0; AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_First_SPEED/2,DIR_P,1000,800,50,50); AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_First_SPEED/2,DIR_P,1000,800,50,50); } } //合链处理 void CTHL_HeLian_Proc(void) { if((CTHL_cZipCnt < 2) || CTHL_NoLaLian_IN) { //松码钩后输出合链长度 if(dwXRealPos >= ((CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1) + 600)) { CTHL_XSavePosBuff2=dwXRealPos; CTHL_HL_VAVLE=1; } } else { //松码钩后输出合链长度 if(dwXRealPos >= (CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1)) { CTHL_XSavePosBuff2=dwXRealPos; CTHL_HL_VAVLE=1; } } // 方块感应到了之后，最长15公分也要退 if(((dwXRealPos+1700)>((CTHL_LianLengthAutoFZCRcheck+2700)-CTHL_PARAM_HELIAN_LENGTH)) || (bFKCheck_Flag && (dwXRealPos >= (CTHL_XSavePosBuff3 + 2500)))) { CTHL_HL_VAVLE=0; } } void CTHL_TDLength_BH_Proc(unsigned short local)//拖带长度保护 { if(local==0)//为感应到开关前 { if(CTHL_cZipCnt < 2) { if((dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length)) { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } else { if((dwXRealPos >=(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheck)) ) { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } } else//感应到开关后 { if(CTHL_cZipCnt < 2) { if((dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length+CTHL_LianLengthAutoFZCRcheck)) { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } else { if((dwXRealPos >=(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheck+CTHL_LianLengthCheckOld)) ) { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } } } //无方块处理 void CTHL_TDWFK_Proc(void) { //这里判断中间某条没有方块 if(CTHL_cZipCnt > 1&&(dwXRealPos >= CTHL_LianLengthCheckFK_DW+200)) { //说明下一条没有方块 AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed/2,DIR_P,1000,800,50,50); AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed/2,DIR_P,1000,800,50,50); CTHL_TD_MotorStep = 51; } } //拖带电机控制动作 X轴 void CTHL_TD_Motor(void) // { if(dwTickCount >= CTHL_bSFBBAOJIN_Delay) { CTHL_SF_ALR_CLR=0; SetClr(X_AXIS,MOTOR_ALARM); SetClr(Y_AXIS,MOTOR_ALARM); } if(CTHL_PARAM_DBSF_MODE) { if(!CTHL_SF_ALR_IN) { bCxcAuto=1; CTHL_AutoStep = 201; CTHL_FZ_MotorStep =0; CTHL_XCDW_VAVLE=0; CTHL_TD_MotorStep =0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); SetEn(X_AXIS, MOTOR_EN); SetEn(Y_AXIS, MOTOR_EN); SetEn(Z_AXIS, MOTOR_EN); CTHL_SetAlarmCode(CTHL_SF_ALR_ALARM,bRunning); //无拉链自动停机 CTHL_JCBI_VAVLE = 0; CTHL_XCQM_VAVLE=0; } } if((GetAlarm(Y_AXIS)==1 || GetAlarm(X_AXIS)==1) && !CTHL_PARAM_DBSF_MODE && !cBAOjin) { bCxcAuto=1; cBAOjin=1; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); SetEn(X_AXIS, MOTOR_EN); SetEn(Y_AXIS, MOTOR_EN); SetEn(Z_AXIS, MOTOR_EN); CTHL_AutoStep = 201; CTHL_SetAlarmCode(CTHL_SF_ALR_ALARM,bRunning); //无拉链自动停机 CTHL_JCBI_VAVLE = 0; CTHL_XCQM_VAVLE=0; // CTHL_TFK_VAVLE=0; } #if 1 switch(CTHL_TD_MotorStep) { //1步开始，纯定位, 没有穿入 case 1: if(dwTickCount >= CTHL_TD_MotorDelay) //给锁轴时间 { SetPos(X_AXIS, 0); CTHL_FKDW_VAVLE=0; CD_ORIGIN_FLG=0; if(CTHL_PARAM_Test) CTHL_TXM_VAVLE = 0 ; else CTHL_TXM_VAVLE = 1; //因推斜码和固定方块一起，要避开 CTHL_TD_MotorStep = 2; AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1000,600,10,15); AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1000,600,10,15); } break; case 2://检测到过链 if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length) { AxisEgmStop(X_AXIS); CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } if(CTHL_FK_Check_UP) { CTHL_XSavePosBuff = dwXRealPos; AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH, 1000,1000,100,100,50); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed*4/5,CTHL_PARAM_XMCR_YJDW_LENGTH, 1000,1000,100,100,50); CTHL_TD_MotorStep = 3; } break; case 3: if(dwXRealPos>=CTHL_XSavePosBuff+CTHL_PARAM_XMCR_YJDW_LENGTH-200) { CTHL_FKDW_VAVLE=1; CTHL_TXM_VAVLE=0; CTHL_TD_MotorStep = 4; CTHL_TD_MotorDelay = dwTickCount+ 1500; } break; case 4: //定位感应亮时、位置到达时，或X轴停止时，停止X轴 if(dwTickCount >= CTHL_TD_MotorDelay)//3S为检测到停机 { bCxcAuto=0; CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常 } if(CTHL_FKDW_IN) { CTHL_TD_MotorStep = 5; CTHL_TD_MotorDelay = dwTickCount+ 0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); } break; case 5: AxisMovePosAccDec(X_AXIS,1000,-30,1000,1000,30,20,0); CTHL_TD_MotorStep = 6; break; case 6: if(!X_DRV) { CTHL_TD_MotorStep = 0; } break; //穿合拉头定位 case 10: if(dwTickCount >= CTHL_TD_MotorDelay) { SetPos(X_AXIS, 0); //启动位置设为0点 CTHL_CD_MotorStep=1; Set_Ctrlmode_trans(Z_AXIS,POS_MODE); CTHL_CD_MOTORDelay=dwTickCount+10; CTHL_DingLian_VAVLE=0; CTHL_YXC_VAVLE=0; CTHL_TXM_VAVLE=0; CTHL_TD_MotorStep = 11; if(CTHL_cZipCnt == 0)CTHL_LianLengthCheckOld = 0; AxisMovePosAccDecNotStop(X_AXIS,(CTHL_PARAM_HLT_SPEED_Speed),CTHL_PARAM_HL_LENGTH, 1000,CTHL_PARAM_HLT_SPEED_Speed,150,25,00); //在手动穿链模式下不用动电机同步 if(!CTHL_bHandWork_MODE) AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_HLT_SPEED_Speed,CTHL_PARAM_HL_LENGTH ,1000,1000,50,50,00); CTHL_XSavePosBuff = dwXRealPos; bFKCheck_Flag = 0; CTHL_XSavePosBuff3 = 0; } break; case 11: //最后拉链的处理 CTHL_TDLength_BH_Proc(0); //第一方块信号检测 if(CTHL_FK_Check_UP && !bFKCheck_Flag) { bFKCheck_Flag = 1; CTHL_XSavePosBuff3= dwXRealPos; } //慢速合链长度 如果已经到达下一条的方块也必须开模 ,CTHL_NoLaLian_IN最后几条慢速时感应才退下模 if((dwXRealPos >= CTHL_PARAM_HL_LENGTH)|| (bFKCheck_Flag)) { //码勾先打开15MS再退下模 if(CTHL_MGuo_VAVLE) { CTHL_MGuo_VAVLE=0; CTHL_TD_MotorDelay = dwTickCount + CTHL_PARAM_TMG_TXM_DELAY;; } else if(dwTickCount >= CTHL_TD_MotorDelay) { CTHL_XSavePosBuff1= dwXRealPos; CTHL_MGuo_VAVLE=0; CTHL_CTXM_VAVLE=0; CTHL_CTXMSSStep=1; CTHL_TD_MotorStep = 12; CTHL_TD_MotorDelay = dwTickCount + CTHL_PARAM_TuiXiaMo_DELAY; } } break; case 12: CTHL_HeLian_Proc(); CTHL_TDLength_BH_Proc(0); if(dwXRealPos >= CTHL_PARAM_HL_LENGTH+200) { if(CTHL_PARAM_Test) CTHL_TXM_VAVLE = 0 ; else CTHL_TXM_VAVLE = 1; } if(CTHL_FK_Check_UP && !bFKCheck_Flag) { bFKCheck_Flag = 1; CTHL_XSavePosBuff3= dwXRealPos; } if(CTHL_CTM_Limit_IN) { CTHL_TD_MotorDelay = dwTickCount + CTHL_PARAM_TuiXiaMo_DELAY; } else if(dwTickCount >= CTHL_TD_MotorDelay) //下模离开后延时加速 { bHeLaTouOK = 1; if((CTHL_cZipCnt > 1) && (!CTHL_NoLaLian_IN)) { CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED; CTHL_LENTH=CTHL_LianLengthCheckFK_DW- dwXRealPos; AxisMovePosAccDecNotStop(X_AXIS,CTHL_DataBuff,CTHL_LENTH,1000,CTHL_PARAM_FKDW_Speed,150,250,CTHL_PARAM_TDDW_LENGTH); } else { if(CTHL_NoLaLian_IN) CTHL_DataBuff = CTHL_PARAM_FKDWMAN_Speed/2; else CTHL_DataBuff = CTHL_PARAM_First_SPEED; AxisContinueMoveAcc(X_AXIS,CTHL_DataBuff,DIR_P,1000,800,15,15); } AxisContinueMoveAcc(Y_AXIS,CTHL_DataBuff,DIR_P,1000,800,15,15); CTHL_TD_MotorStep = 13; } break; case 13: CTHL_NoLaLian_Speed_Proc(); CTHL_HeLian_Proc(); CTHL_TDLength_BH_Proc(0); //此步为已经开模， 开始加速 if(dwXRealPos >= CTHL_PARAM_HL_LENGTH+200) { if(CTHL_PARAM_Test) CTHL_TXM_VAVLE = 0 ; else CTHL_TXM_VAVLE = 1; } //已经开完模具 if(CTHL_FK_Check_UP&& !bFKCheck_Flag) { bFKCheck_Flag = 1; CTHL_XSavePosBuff3= dwXRealPos; } else if(bFKCheck_Flag) //说明第一个方块已经检测到 { CTHL_TD_MotorStep = 14; if(CTHL_cZipCnt > 1)CTHL_CTXMSS_FLAG=1; } break; case 14: CTHL_NoLaLian_Speed_Proc(); CTHL_HeLian_Proc(); CTHL_TDLength_BH_Proc(1); if(!CTHL_FK_Check && (dwXRealPos >= (CTHL_XSavePosBuff3 + 800))) { CTHL_TD_MotorStep = 15; } break; case 15: CTHL_NoLaLian_Speed_Proc(); CTHL_HeLian_Proc(); CTHL_TDLength_BH_Proc(1); CTHL_TDWFK_Proc(); //第二次感应方块， 要做定位 if(CTHL_FK_Check_UP && (dwXRealPos >= (CTHL_XSavePosBuff3 + 1800))) { //测量距离 bFZ_LengthOK = 1; CTHL_XSavePosBuff = dwXRealPos; if(CTHL_cZipCnt)CTHL_LianLengthCheckOld = CTHL_LianLengthCheck; CTHL_LianLengthCheck = dwXRealPos - CTHL_XSavePosBuff3; CTHL_LianLengthCheckFK_GL=dwXRealPos; CTHL_LENTH=abs(CTHL_LianLengthCheckOld-CTHL_LianLengthCheck); if(CTHL_cZipCnt > 1&&!CTHL_NoLaLian_IN_UP) { //公分节判断 if(CTHL_LianLengthCheck<=CTHL_PARAM_GFJ_LENGTH) { CTHL_TD_MotorStep=50; AxisContinueMoveAcc(X_AXIS,2000,DIR_P,1000,600,15,15); AxisContinueMoveAcc(Y_AXIS,1600,DIR_P,1000,600,15,15); CTHL_TD_MotorDelay = dwTickCount + 200; bFZ_LengthOK= 0; CTHL_cZipCnt = 0; } else if(CTHL_LENTH>=1500) { AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH, 1000,1000,30,80,100); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH, 1000,1000,30,80,100); CTHL_TD_MotorStep=16; CTHL_TXM_VAVLE=0; if(CTHL_cZipCnt<2)CTHL_CTXMSS_FLAG=1; CTHL_cZipCnt=0; } else { CTHL_TD_MotorStep=16; if(CTHL_cZipCnt<2)CTHL_CTXMSS_FLAG=1; } } else { CTHL_TD_MotorStep=16; CTHL_TXM_VAVLE=0; if(CTHL_cZipCnt<2)CTHL_CTXMSS_FLAG=1; AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH, 1000,1000,30,80,50); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH, 1000,1000,30,80,50); } } break; case 16: if(CTHL_cZipCnt > 1&&!CTHL_NoLaLian_IN_UP) { if(dwXRealPos>=CTHL_XSavePosBuff+CTHL_PARAM_FKDW_OUT_DELAY) { CTHL_TXM_VAVLE=0; CTHL_FKDW_VAVLE=1; CTHL_TD_MotorStep = 17; CTHL_TD_MotorDelay = dwTickCount+ 1500; } } else { if(dwXRealPos>=CTHL_XSavePosBuff+CTHL_PARAM_XMCR_YJDW_LENGTH-200) { CTHL_FKDW_VAVLE=1; CTHL_TXM_VAVLE=0; CTHL_TD_MotorStep = 17; CTHL_TD_MotorDelay = dwTickCount+ 1500; } } break; case 17: //定位感应亮时、位置到达时，或X轴停止时，停止X轴 if(dwTickCount >= CTHL_TD_MotorDelay)//3S为检测到停机 { bCxcAuto=0; CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常 } if(CTHL_FKDW_IN) { CTHL_TD_MotorStep = 18; CTHL_TD_MotorDelay = dwTickCount+ 0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); AxisEgmStop(Z_AXIS); CTHL_CD_MotorStep=0; Set_Ctrlmode_trans(Z_AXIS,TARR_MODE); bFZ_LengthOK = 1; CTHL_TXM_VAVLE=0; CTHL_LianLengthCheckFK_DW=dwXRealPos; } break; case 18: //AxisMovePosAccDec(X_AXIS,1000,-40,1000,1000,30,20,0); CTHL_TD_MotorStep = 0; break; //由公分节调转 case 50: if(CTHL_FK_Check_DW) { CTHL_cZipCnt = 0; CTHL_TD_MotorStep = 51; CTHL_TD_MotorDelay = dwTickCount + 15; bFZ_LengthOK = 0; CTHL_GF_Change = 1; SetPos(X_AXIS, 0); } break; case 51: if(CTHL_FK_Check) { CTHL_TD_MotorDelay = dwTickCount + 50; } else if(dwTickCount >= CTHL_TD_MotorDelay) { // CTHL_MGuo_VAVLE=0; CTHL_CTXM_VAVLE=0; CTHL_TD_MotorStep = 52; } break; case 52: if(dwXRealPos>=CTHL_PARAM_GFJ_LENGTH) { if(CTHL_FK_Check) { CTHL_XSavePosBuff = dwXRealPos; AxisContinueMoveChangeSpeed(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_FKDWMAN_Speed,20,5); AxisContinueMoveChangeSpeed(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed * 4/5,CTHL_PARAM_FKDWMAN_Speed* 4/5,20,5); CTHL_TD_MotorStep = 53; } } break; case 53: if(dwXRealPos >= (CTHL_PARAM_FKDW_OUT_DELAY + CTHL_XSavePosBuff + 100)) { CTHL_TXM_VAVLE=0; CTHL_FKDW_VAVLE=1; } //定位感应亮时也要停止 if(CTHL_FKDW_IN_UP || !X_DRV) { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); if(CTHL_FKDW_IN) { CTHL_TD_MotorStep = 54; CTHL_TD_MotorDelay = dwTickCount + 50; } else { CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning); } } break; case 54: if(dwTickCount >= CTHL_TD_MotorDelay)//YWX_CTHL_bTiaoShiStep) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { bStart = 1; CTHL_LianLengthCheck = 0; CTHL_LianLengthCheckOld = 0; // bRunning = 0; CTHL_TD_MotorStep = 0; CTHL_TD_MotorDelay = dwTickCount + 000; CTHL_TryCnt = 0; bCL_EN_FLG = 0; } } break; } /*if(!CD_ORIGIN_FLG&&CTHL_Z_ORIGN_IN&&CTHL_TD_MotorStep>=10) { CD_ORIGIN_FLG=1; Set_Ctrlmode_trans(Z_AXIS,POS_MODE); }*/ #endif } //穿入电机动作，Y轴 void CTHL_FZ_Motor(void) // { #if 1 //1步开始, 穿链反转长度开始 //20步开始 插销定位开始 switch(CTHL_FZ_MotorStep) { //穿入长度位置 case 1: if(dwTickCount >= CTHL_FZ_MotorDelay) { SetPos(X_AXIS, 0); //启动位置设为0点 SetPos(Y_AXIS, 0); //启动位置设为0点 FZ_ALARM_FLG=0; if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN||CTHL_bHandWork_MODE)//前两次走 { AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_FZFirst_SPEED,DIR_N,1000,1000,30,50); AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_FZFirst_SPEED+100,DIR_N,1500,1000,30,50); } else { AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_JXM_SPEED,-CTHL_PARAM_XMCR_SLOW_LENGTH, 1000,CTHL_PARAM_JXM_SPEED,150,100,0); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_JXM_SPEED+500,-CTHL_PARAM_XMCR_SLOW_LENGTH, 1000,CTHL_PARAM_JXM_SPEED,50,100,0); } CTHL_FZ_MotorStep = 2; CTHL_FZMOTOR_KADAI_Delay=dwTickCount+500; } break; case 2: if((abs(dwXRealPos) >=CTHL_PARAM_XMCR_SLOW_LENGTH-100)&&!CTHL_NoLaLian_IN&&!CTHL_bHandWork_MODE){//提前转换模式 Set_Ctrlmode_trans(Y_AXIS,TARR_MODE); } if(abs(dwXRealPos) >= CTHL_PARAM_XMCR_SLOW_LENGTH)//穿链结束，开始走链加速 { CTHL_FZ_MotorDelay = dwTickCount+0; if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN||CTHL_bHandWork_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_LianLengthAutoFZCRcheck-abs(dwXRealPos);//-CTHL_PARAM_FZJS_LENGTH; AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FZ_Speed,-CTHL_LENTH, 1000,CTHL_PARAM_FZDS_Speed,250,250,CTHL_PARAM_FZJS_LENGTH); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZ_Speed+120,-CTHL_LENTH, 1000,CTHL_PARAM_FZDS_Speed,120,250,CTHL_PARAM_FZJS_LENGTH); //AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FZ_Speed,-CTHL_LENTH, // 1000,CTHL_PARAM_FZDS_Speed,250,150,0); //AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZ_Speed+120,-CTHL_LENTH, // 1000,CTHL_PARAM_FZDS_Speed,120,150,0); } CTHL_FZ_MotorStep++; } if(dwTickCount>=CTHL_FZMOTOR_KADAI_Delay&&dwYRealPos_com<400)//穿入前码卡带 { bCxcAuto=0;//停轴 SetEn(Y_AXIS, MOTOR_EN); CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机 } //电机速度差太大 if((abs(dwXRealPos)>dwYRealPos_com+4500)&&!CTHL_bHandWork_MODE) { FZ_ALARM_FLG=1; CTHL_FZ_MotorStep=100; CTHL_XSavePosBuff1=dwYRealPos_com; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); //bCxcAuto=0;//停轴 //SetEn(Y_AXIS, MOTOR_DISEN); SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM); Set_Ctrlmode_trans(Y_AXIS,POS_MODE); //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机 } break; case 3: CTHL_FZ_MotorStep++; /*if(dwTickCount> CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10)CTHL_TXM_VAVLE=0; //到达可推出侧推布带时 if((dwTickCount>CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10 ) && !CTHL_TXM_VAVLE && !CTHL_TXM_Limit_IN)CTHL_JXM_VAVLE=0; if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN||CTHL_bHandWork_MODE) { 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, 1000,1500,120,100,CTHL_PARAM_FZJSDS_LENGTH); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZDS_Speed,-CTHL_PARAM_FZJS_LENGTH, 1000,1500,120,100,CTHL_PARAM_FZJSDS_LENGTH); CTHL_FZ_MotorStep++; } } //长度超出报警 if(CTHL_cZipCnt>=1&&!CTHL_bHandWork_MODE) { if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthAutoFZCRcheck)) //拉带报警长度 { bCxcAuto=0;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度 } //方块检测异常(光纤) if(abs(dwXRealPos) >= (CTHL_LianLengthAutoFZCRcheck + 300)) { bCxcAuto=0;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning); } } else { if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length&&!CTHL_bHandWork_MODE){ //位置大于第一条拉链长度 bCxcAuto=0;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度 } } //电机速度差太大 if((abs(dwXRealPos)>dwYRealPos_com+4500)&&!CTHL_bHandWork_MODE) { FZ_ALARM_FLG=1; CTHL_FZ_MotorStep=100; CTHL_XSavePosBuff1=dwYRealPos_com; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); //bCxcAuto=0;//停轴 //SetEn(Y_AXIS, MOTOR_DISEN); SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM); Set_Ctrlmode_trans(Y_AXIS,POS_MODE); //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机 }*/ break; case 4: if(dwTickCount> CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10)CTHL_TXM_VAVLE=0; //到达可推出侧推布带时 if((dwTickCount>CTHL_FZ_MotorDelay+CTHL_PARAM_TJXM_LENGTH*10) && !CTHL_TXM_VAVLE && !CTHL_TXM_Limit_IN)CTHL_JXM_VAVLE=0; if((abs(dwXRealPos) > 1500) && CTHL_FKGY_IN_UP)//位置大于100，且方块感应上升沿 { CTHL_XSavePosBuff1 = abs(dwXRealPos); set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMIN,50000,50000,SERVO_TARR_CW); if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN||CTHL_bHandWork_MODE){//记录前两条的方块感应长度； //停机距离调整 CTHL_LENTH=CTHL_PARAM_FZSTOP_LENGTH; AxisMovePosAccDec(X_AXIS,2000,-CTHL_LENTH, 800,800,15,25,0); AxisMovePosAccDec(Y_AXIS,600,-CTHL_LENTH, 800,800,15,25,0); } else { //停机距离调整 CTHL_LENTH=CTHL_PARAM_FZSTOP_LENGTH; AxisMovePosAccDec(X_AXIS,2000,-CTHL_LENTH, 800,800,15,25,0); AxisMovePosAccDec(Y_AXIS,600,-CTHL_LENTH, 800,800,15,25,0); } CTHL_FZ_MotorStep++; //小插前码定位 //CTHL_XCQM_VAVLE=1; } else { //长度超出报警 if(CTHL_cZipCnt>=1&&!CTHL_bHandWork_MODE) { if(abs(dwXRealPos)>= (CTHL_PARAM_WLCDSHEZHI_LENTH+ CTHL_LianLengthAutoFZCRcheck)) //拉带报警长度 { bCxcAuto=0;//停轴 SetEn(Y_AXIS, MOTOR_EN); CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度 } //方块检测异常(光纤) if(abs(dwXRealPos) >= (CTHL_LianLengthAutoFZCRcheck + 500)) { bCxcAuto=1;//停轴 CTHL_FZ_MotorStep=100; SetEn(Y_AXIS, MOTOR_EN); CTHL_SetAlarmCode(CTHL_FK_Check_ALARM,bRunning); } } else { if(abs(dwXRealPos) >= CTHL_PARAM_first_CL_MAX_Length&&!CTHL_bHandWork_MODE){ //位置大于第一条拉链长度 bCxcAuto=0;//停轴 CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度 } } if(dwTickCount>=CTHL_FZMOTOR_KADAI_Delay&&dwYRealPos_com<400)//穿入前码卡带 { bCxcAuto=1;//停轴 CTHL_FZ_MotorStep=100; SetEn(Y_AXIS, MOTOR_EN); CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机 } //电机速度差太大 if((abs(dwXRealPos)>dwYRealPos_com+4500)&&!FZ_ALARM_FLG&&!CTHL_bHandWork_MODE) { FZ_ALARM_FLG=1; CTHL_FZ_MotorStep=100; CTHL_XSavePosBuff1=dwYRealPos_com; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); //bCxcAuto=0;//停轴 //SetEn(Y_AXIS, MOTOR_DISEN); SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM); Set_Ctrlmode_trans(Y_AXIS,POS_MODE); //CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机 } } break; case 5: if(!Y_DRV && !X_DRV) { if((CTHL_cZipCnt <= 1)) { CTHL_LianLengthCheck = abs(dwXRealPos); //为第一条长度给值 CTHL_LianLengthAutoFZCRcheck = abs(dwXRealPos); //CTHL_LianLengthAutoFZcheck=abs(dwXRealPos); } Set_Ctrlmode_trans(Y_AXIS,POS_MODE); //前码扭矩 //Set_Ctrlmode_trans(Y_AXIS,POS_MODE); CTHL_LianLengthAutoFZcheck=abs(dwXRealPos); CTHL_FZ_MotorStep = 0; } break; //20步开始小插定位 case 20: //此处最好算好定位长度相减 if(dwTickCount >= CTHL_FZ_MotorDelay) { CTHL_FZ_MotorStep = 21; SetPos(Y_AXIS, 0); //启动位置设为0点 SetPos(X_AXIS, 0); //启动位置设为0点 } break; case 21: CTHL_FZ_MotorStep = 22; AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_XCCR_SPEED,-CTHL_PARAM_CXDW_Length,1000,1000,20,20,0); AxisMovePosAccDec(X_AXIS,CTHL_PARAM_XCCR_SPEED,-(CTHL_PARAM_CXDW_Length+100),1000,1000,20,20,0); CTHL_FZ_MotorDelay=dwTickCount+10+CTHL_PARAM_XCDW_TIME; break; case 22: if(!Y_DRV && !X_DRV ) { CTHL_FZ_MotorStep = 0; } //前码扭矩 /*if(!X_DRV && dwTickCount>=CTHL_FZ_MotorDelay&&GetTarr(Y_AXIS) ) { CTHL_FZ_MotorStep = 23; Set_Ctrlmode_trans(Y_AXIS,POS_MODE); CTHL_FZ_MotorDelay=dwTickCount+10; } */ break; case 23: if(dwTickCount>=CTHL_FZ_MotorDelay ) { CTHL_FZ_MotorStep = 0; Set_Ctrlmode_trans(X_AXIS,TARR_MODE); } break; case 100: if(CTHL_START_IN_UP) { CTHL_AutoStep = 101; } case 101: if(CTHL_FKGY_IN) { CTHL_FZ_MotorStep = 5; } else { if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN||CTHL_bHandWork_MODE) { AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_JXM_SPEED/2,DIR_N,1000,1000,30,50); AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_JXM_SPEED,DIR_N,1000,1000,30,50); } else { CTHL_LENTH = CTHL_LianLengthAutoFZCRcheck-abs(dwXRealPos); if(CTHL_LENTH>0) { AxisMovePosAccDec(X_AXIS,1000,-CTHL_LENTH,800,800,15,25,0); AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_JXM_SPEED,DIR_N,1000,1000,30,50); } } CTHL_FZ_MotorDelay=dwTickCount+3000; CTHL_FZ_MotorStep = 102; } break; case 102: if(CTHL_FKGY_IN_UP) { CTHL_FZ_MotorStep = 5; } else if(dwTickCount>CTHL_FZ_MotorDelay) { bCxcAuto=0;//停机 AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机卡滞 } break; } #endif } //储带电机 void CTHL_CUDAI_Motor(void) { #if 1 switch(CTHL_CD_MotorStep) { case 1: if(dwTickCount>=CTHL_CD_MOTORDelay) { set_servo_postotarr_limit(Z_AXIS,SERVO_PARAM_CUDAI_TRARRMIN,50000,50000,SERVO_TARR_CW); if((CTHL_cZipCnt >1)) { // AxisMovePosAccDecNotStop(Z_AXIS,CTHL_LianLengthAutoFZCRcheck/2,CTHL_PARAM_HIGH_SPEED, // 3000,CTHL_PARAM_SLOW_SPEED,50,50,300); AxisContinueMoveAcc(Z_AXIS,CTHL_PARAM_HIGH_SPEED,DIR_P,5000,1000,30,50); CTHL_CD_MotorStep=3; } else { AxisContinueMoveAcc(Z_AXIS,CTHL_PARAM_FIRST_SPEED,DIR_P,3000,1000,30,50); CTHL_CD_MotorStep=3; } } break; case 2: if(dwXRealPos>=CTHL_LianLengthAutoFZCRcheck*3/4) { Set_VDI_Value(Z_AXIS,2); AxisContinueMoveChangeSpeed(Z_AXIS,CTHL_PARAM_SLOW_SPEED,1000,80,80); CTHL_CD_MotorStep=3; } else if(CTHL_Z_ORIGN_IN) { Set_VDI_Value(Z_AXIS,2); AxisEgmStop(Z_AXIS); CTHL_CD_MotorStep=0; } break; case 3: if(CTHL_Z_ORIGN_IN) { Set_Ctrlmode_trans(Z_AXIS,TARR_MODE); Set_VDI_Value(Z_AXIS,2); AxisEgmStop(Z_AXIS); CTHL_CD_MotorStep=0; CTHL_bResting_Z=0; } break; } #endif } //装拉头动作 void CTHL_ZLT_Step(void) { switch(CTHL_ZhuangLiaoStep) { case 1: if(bZhuangLiaoOkFlg) { CTHL_ZhuangLiaoStep = 0; } else if(dwTickCount >= CTHL_ZhuangLiaoDelay) { if(((CTHL_MGuo_VAVLE && CTHL_LTou_Check) || (!CTHL_MGuo_VAVLE)) && !CTHL_JLiao_Limit_IN )//说明没有拉头 { /*if(bRunning&&CTXM_DOWN_TIME&&CTHL_cZipCnt>1) { if(CTHL_TLiaoStep == 0) { CTHL_TLiaoStep = 1; CTHL_ZhuangLiaoStep = 40; } } else*/ { //穿头模和一次穿入必须在原位,接料不能有输出 if(CTHL_CTM_Origin_IN && !CTHL_CTXM_VAVLE) { CTHL_JLTou_VAVLE = 0; CTHL_MGuo_VAVLE = 0; if(CTHL_TLiaoStep == 0) { CTHL_TLiaoStep = 1; CTHL_ZhuangLiaoStep = 2; } } else if(dwTickCount >= CTHL_ZhuangLiaoDelay + 1000) //警告横送拉必须保证穿头模和一次穿入在原位,接料不能有输出 { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning); } } } else //已经有拉头直接跳到结束 { CTHL_ZhuangLiaoDelay = dwTickCount + 40; CTHL_ZhuangLiaoStep = 9; } } break; /* case 40: if(CTHL_TLiaoStep == 0) //推料已经完成 { CTHL_HSLiao_VAVLE = 1; CTHL_ZhuangLiaoStep = 41; CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } break; case 41: if(!CTHL_HSLiao_Origin_IN&&!CTHL_CTM_Minid_IN)//离开原点 { CTHL_ZhuangLiaoStep = 42; CTHL_ZhuangLiaoDelay=dwTickCount+HSLIAO_CHECK_TIME+1;//横送过程检测 } break; case 42: if(dwTickCount>=CTHL_ZhuangLiaoDelay)//兼容没有做缓冲的情况 { if(CTHL_CTM_Origin_IN )//穿头模和第一次合链在原位 { CTHL_ZhuangLiaoStep = 3; } else { CTHL_ZhuangLiaoStep = 43; CTHL_HSLiao_VAVLE = 0; CTHL_ZhuangLiaoDelay = dwTickCount + 20; } } break; case 43: if(dwTickCount >= CTHL_ZhuangLiaoDelay) { if(CTHL_CTM_Origin_IN )//穿头模和第一次合链在原位 { CTHL_ZhuangLiaoStep = 3; CTHL_HSLiao_VAVLE = 1; CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } } break;*/ case 2: if(CTHL_TLiaoStep == 0) //推料已经完成 { CTHL_ZhuangLiaoDelay = dwTickCount+ 500; CTHL_JLTou_VAVLE = 0; CTHL_MGuo_VAVLE = 0; if(!CTHL_JLiao_Limit_IN) { CTHL_ZhuangLiaoStep = 3; CTHL_HSLiao_VAVLE = 1; PRV_HSLIAO_OUT_DELAY=dwTickCount; } } break; case 3: //横送到位 if(CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_Origin_IN) { HSLIAO_OUT_DELAY=dwTickCount-PRV_HSLIAO_OUT_DELAY; CTHL_ZhuangLiaoStep = 4; // CTHL_JLTou_VAVLE = 1; //接拉头输出,不需要时间 CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_H_JLiao_DELAY; } else if(dwTickCount >= CTHL_ZhuangLiaoDelay) { CTHL_ZhuangLiaoStep = 30; } break; case 4: if(dwTickCount >= CTHL_ZhuangLiaoDelay) { CTHL_ZhuangLiaoStep = 5; CTHL_JLTou_VAVLE = 1; //接拉头输出,不需要时间 CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; } break; case 5: if(CTHL_JLiao_Limit_IN) { CTHL_ZhuangLiaoStep = 6; CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_MaGou_DELAY; } else if(dwTickCount >= CTHL_ZhuangLiaoDelay) //接拉头(接料)到们信号异常 { CTHL_SetAlarmCode(CTHL_JLiao_Origin_ALARM,bRunning); CTHL_HSLiao_VAVLE = 0; CTHL_JLTou_VAVLE = 0; CTHL_ZhuangLiaoStep = 0; } break; case 6: if(dwTickCount >= CTHL_ZhuangLiaoDelay) //延时码勾 { CTHL_MGuo_VAVLE = 1; //码勾输出 CTHL_ZhuangLiaoStep = 7; CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_HSLiaoOff_DELAY; bTuiLaTouOkFlg = 0; //认为拉头没法带出 } break; case 7: if(dwTickCount >= CTHL_ZhuangLiaoDelay) { CTHL_HSLiao_VAVLE = 0; CTHL_ZhuangLiaoStep = 8; CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; } break; case 8: if(!CTHL_HSLiao_Limit_IN) // { CTHL_ZhuangLiaoStep = 9; //CTHL_TLiaoStep = 1; //推拉头 CTHL_ZhuangLiaoDelay = dwTickCount +CTHL_PARAM_CheckLT_DELAY; //也是给下模能上升的时间 } else if (CTHL_LTou_Check) //时间内信号这说明没有拉头 { //重送3次 CTHL_TryCnt++; CTHL_JLTou_VAVLE = 0; //接拉头关 CTHL_MGuo_VAVLE = 0; //码勾打开 if(CTHL_TryCnt == 1)//重新横送一次 { CTHL_ZhuangLiaoStep = 21; //重新送3次 CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; } else if(CTHL_TryCnt == 2||CTHL_TryCnt == 3)//重新装料2次 { CTHL_ZhuangLiaoStep = 20; //重新送3次 CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; } else if(CTHL_TryCnt == 4) { CTHL_TryCnt=0; bZhuangLiaoOkFlg = 0; CTHL_ZhuangLiaoStep = 0; CTHL_wltzi=1; user_datas[127]=5; CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning); } } else if(dwTickCount >= CTHL_ZhuangLiaoDelay) { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_HSLiao_Limit_ALARM,bRunning); if(bRunning) { CTHL_ZhuangLiaoStep = 0 ; } } break; case 9: if(CTHL_LTou_Check) //时间内信号这说明没有拉头 { CTHL_TryCnt++; CTHL_JLTou_VAVLE = 0; //接拉头关 CTHL_MGuo_VAVLE = 0; //码勾打开 if(CTHL_TryCnt == 1)//重新横送一次 { CTHL_ZhuangLiaoStep = 21; //重新送3次 CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; } else if(CTHL_TryCnt == 2||CTHL_TryCnt == 3)//重新装料2次 { CTHL_ZhuangLiaoStep = 20; //重新送3次 CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; } else if(CTHL_TryCnt == 4) { CTHL_TryCnt=0; bZhuangLiaoOkFlg = 0; CTHL_ZhuangLiaoStep = 0; CTHL_wltzi=1; user_datas[127]=5; CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning); } } else if(dwTickCount >= CTHL_ZhuangLiaoDelay) { bZhuangLiaoOkFlg = 1; CTHL_TLiaoStep = 1; //推拉头 CTHL_ZhuangLiaoStep = 0; } break; //装拉头2次 case 20: if(CTHL_HSLiao_Origin_IN) { CTHL_JLTou_VAVLE = 0; //接拉头关 CTHL_MGuo_VAVLE = 0; //码勾打开 CTHL_ZhuangLiaoStep = 1; CTHL_ZhuangLiaoDelay = dwTickCount + 10; } else if(dwTickCount >=CTHL_ZhuangLiaoDelay) { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning); } break; case 21: //重新横送一次 if(CTHL_HSLiao_Origin_IN) { CTHL_JLTou_VAVLE = 0; //接拉头关 CTHL_MGuo_VAVLE = 0; //码勾打开 CTHL_ZhuangLiaoStep = 2; CTHL_ZhuangLiaoDelay = dwTickCount + 10; } else if(dwTickCount >=CTHL_ZhuangLiaoDelay) { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_HSLiao_Origin_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 = 4; 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 = 2; } break; case 2: //横送料在原位 if(CTHL_HSLiao_Origin_IN && !CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_VAVLE) { CTHL_TLiao_VAVLE = 1; //推料(推拉头)输出 CTHL_TLiao_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME; CTHL_TLiaoStep = 3; } else if(dwTickCount >= CTHL_TLiao_Delay) { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,1); } break; case 3: //推拉头到位 if(!CTHL_TLiao_Origin_IN) { CTHL_TLiao_Delay = dwTickCount + CTHL_PARAM_TLiao_BACKDELAY; CTHL_TLiaoStep = 4; } else if(dwTickCount >= CTHL_TLiao_Delay) { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,1); } break; case 4: if(dwTickCount >= CTHL_TLiao_Delay) { CTHL_TLiao_VAVLE = 0; CTHL_TLiao_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME; CTHL_TLiaoStep = 5; } break; case 5: if(CTHL_TLiao_Origin_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_EN); SetEn(X_AXIS, MOTOR_EN); 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 + 500; } if(CTHL_PARAM_DBSF_MODE) { if(!CTHL_SF_ALR_IN) { CTHL_SF_ALR_CLR=1; CTHL_bSFBBAOJIN_Delay= dwTickCount + 500; } } } 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_bMG || (CTHL_bMG && CTHL_LTou_Check))) { if(!CTHL_TLiao_VAVLE) { if(!CTHL_JLTou_VAVLE) { if(CTHL_CTM_Origin_IN) CTHL_HSLiao_VAVLE = 1; } else { CTHL_SetAlarmCode(CTHL_JLiao_Origin_ALARM,0); } } else { CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,0); } } else { CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,0); } } 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_bJLT) //接拉头 { CTHL_bJLT = 0; if(CTHL_JLTou_VAVLE) CTHL_JLTou_VAVLE = 0; else if( !CTHL_CTXM_VAVLE && CTHL_CTM_Origin_IN) { CTHL_MGuo_VAVLE = 0; CTHL_JLTou_VAVLE = 1; } else//条件警告 { CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,0); } } if(CTHL_bMG) //码勾 { CTHL_bMG = 0; CTHL_MGuo_VAVLE = !CTHL_MGuo_VAVLE; } if(CTHL_bJXM) //夹前码 { CTHL_bJXM = 0; CTHL_JXM_VAVLE = !CTHL_JXM_VAVLE; } if(CTHL_bTXM) //推斜码 { CTHL_bTXM = 0; if((CTHL_JXM_VAVLE || !CTHL_CTXM_VAVLE) && !CTHL_TXM_VAVLE && !CTHL_TFK_VAVLE) { CTHL_TXM_VAVLE=1; } else if(CTHL_TXM_VAVLE) { CTHL_TXM_VAVLE=0; } else CTHL_SetAlarmCode(CTHL_JXM_Limit_ALARM,0); //夹前码要输出 } if(CTHL_bQMDW) //前码定位 { CTHL_bQMDW = 0; CTHL_QMDW_VAVLE = !CTHL_QMDW_VAVLE; } if(CTHL_bTFK) //推方块 { CTHL_bTFK = 0; if(!CTHL_TFK_VAVLE && !CTHL_TXM_VAVLE) CTHL_TFK_VAVLE = 1; else if(CTHL_TFK_VAVLE) CTHL_TFK_VAVLE = 0; } if(CTHL_bFKDW) //方块检测 { CTHL_bFKDW = 0; CTHL_FKDW_VAVLE = !CTHL_FKDW_VAVLE; } if(CTHL_bHL) //合链 { CTHL_bHL = 0; CTHL_HL_VAVLE = !CTHL_HL_VAVLE; } if(CTHL_bYFK) //压方块 { CTHL_bYFK = 0; CTHL_YFK_VAVLE = !CTHL_YFK_VAVLE; } if(CTHL_bYCX) //压小插 { CTHL_bYCX = 0; CTHL_YXC_VAVLE = !CTHL_YXC_VAVLE; } if(CTHL_bCXDW) //插销定位 { CTHL_bCXDW = 0; CTHL_XCDW_VAVLE = !CTHL_XCDW_VAVLE; } if(CTHL_bXCBI) //小插摆臂 { SetEn(X_AXIS, MOTOR_EN); SetEn(Y_AXIS, MOTOR_EN); CTHL_bXCBI = 0; CTHL_JCBI_VAVLE = !CTHL_JCBI_VAVLE; } if(CTHL_bXCQM) { CTHL_bXCQM=0; CTHL_XCQM_VAVLE=~CTHL_XCQM_VAVLE; } if(CTHL_bCTXM) //下模 { CTHL_bCTXM = 0; CTHL_TXM_VAVLE=0; if(CTHL_CTXM_VAVLE) { CTHL_CTXM_VAVLE = 0; CTHL_JLTou_VAVLE = 0; } else if(!CTHL_HSLiao_VAVLE && CTHL_HSLiao_Origin_IN && (CTHL_JXM_VAVLE || !CTHL_TXM_VAVLE)) { CTHL_CTXM_VAVLE = 1; } } //自动装料 if(CTHL_bAutoZhuangLiao) { CTHL_bAutoZhuangLiao = 0; if(CTHL_ZhuangLiaoStep == 0) { bTuiLaTouOkFlg = 1; bZhuangLiaoOkFlg = 0; CTHL_ZhuangLiaoStep = 1; CTHL_MGuo_VAVLE = 1; //判断是否已经有料 CTHL_TryCnt = 0; CTHL_ZhuangLiaoDelay = dwTickCount + 150; } } if(CTHL_bYJDW) //一键定位 { CTHL_bYJDW=0; CTHL_FKDW_VAVLE = 0; CTHL_YFK_VAVLE=0; CTHL_YXC_VAVLE=0; CTHL_HL_VAVLE=0; CTHL_CTXM_VAVLE=0; CTHL_TXM_VAVLE=0; CTHL_JCBI_VAVLE=0; CTHL_QMDW_VAVLE=0; CTHL_XCQM_VAVLE=0; CTHL_TD_MotorDelay = dwTickCount + 100; if(CTHL_TD_MotorStep==0) { CTHL_CTXM_VAVLE =0; if(GetEn(X_AXIS) == MOTOR_EN) { SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); CTHL_TD_MotorDelay = dwTickCount + 200; } else CTHL_TD_MotorDelay = dwTickCount + 100; CTHL_TD_MotorStep=1; SetEn(Z_AXIS, MOTOR_DISEN); CTHL_CD_MotorStep=1; CTHL_cZipCnt=0; } } //X轴Y轴电机测试 if(CTHL_bCLMotor_N) { SetEn(Y_AXIS, MOTOR_DISEN); SetEn(X_AXIS, MOTOR_DISEN); SetEn(Z_AXIS, MOTOR_DISEN); //Set_VDI_Value(Z_AXIS,1); CTHL_FKDW_VAVLE=0; if(CTHL_FK_Check_UP) { SetPos(X_AXIS, 0); SetPos(Y_AXIS, 0); } if(!X_DRV && (!CTHL_QMDW_IN ||!CTHL_QMDW_VAVLE)&&(!CTHL_FKGY_IN||CTHL_FKGY_IN&&CTHL_YXC_VAVLE&&!CTHL_YFK_VAVLE)) { // Y轴 运行速度 启动速度 加速度 减速度 AxisContinueMoveAcc(X_AXIS,600 ,DIR_N,600,600,15,15); AxisContinueMoveAcc(Y_AXIS,580,DIR_N,580,580,15,15); } if(CTHL_QMDW_IN && CTHL_QMDW_VAVLE||CTHL_FKGY_IN&&CTHL_YXC_VAVLE&&CTHL_YFK_VAVLE) { AxisEgmStop(Y_AXIS); AxisEgmStop(X_AXIS); } } //在压方块电磁阀输出情况下，方块感应后电机要自动停止 if((CTHL_XCDW_Limit_IN )&& CTHL_bCLMotor_P) { CTHL_bCLMotor_P=0; AxisEgmStop(Y_AXIS); AxisEgmStop(X_AXIS); } if(CTHL_bCLMotor_P) //后退限位已经取消 { SetEn(Y_AXIS, MOTOR_DISEN); SetEn(X_AXIS, MOTOR_DISEN); SetEn(Z_AXIS, MOTOR_DISEN); //Set_VDI_Value(Z_AXIS,1); CTHL_FKDW_VAVLE=0; if(CTHL_FK_Check_UP) { SetPos(X_AXIS, 0); SetPos(Y_AXIS, 0); } if(!CTHL_Z_ORIGN_IN) AxisContinueMoveAcc(Z_AXIS,630,DIR_P,630,630,15,15); //压方块电磁阀没有输出时，方块感应信号不限制 if(!CTHL_XCDW_Limit_IN) { if(!X_DRV ) { // Y轴 运行速度 启动速度 加速度 减速度 AxisContinueMoveAcc(Y_AXIS,630,DIR_P,630,630,15,15); AxisContinueMoveAcc(X_AXIS,600,DIR_P,600,600,15,15); } } } if(!CTHL_bCLMotor_P && !CTHL_bCLMotor_N && (CTHL_TD_MotorStep == 0)) { AxisEgmStop(Y_AXIS); AxisEgmStop(X_AXIS); if(CTHL_CD_MotorStep==0)AxisEgmStop(Z_AXIS); } if(GetEn(Z_AXIS)==0&&CTHL_Z_ORIGN_IN) { SetEn(Z_AXIS, MOTOR_DISEN); Set_Ctrlmode_trans(Z_AXIS,POS_MODE); } if(CTHL_bRest_Z) { CTHL_bRest_Z=0; CTHL_bResting_Z=1; CTHL_CD_MotorStep=1; CTHL_cZipCnt=0; SetEn(Z_AXIS, MOTOR_DISEN); CTHL_CD_MOTORDelay = dwTickCount + 100; } } } #endif