#include "global.h" #if JUN_YUE_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 short XCCR_LENGTH; unsigned short CTHL_XCCR_Time;//小插传入次数 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_CTXMSS_Proc(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) { 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; //传递伺服齿轮比参数 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); 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,0);//Y轴正常模式。 SetServoComUse(Y_AXIS,1);//X轴伺服扭矩通讯。 ReadServoAlarm_Enable(Y_AXIS,1); ReadServoEncode_Enable(Y_AXIS,1); SetServoComUse(Z_AXIS,0);//Y轴正常模式。 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_Auto_RunState;//CTHL_PARAM_XMZM_MODE;//XCCR_LENGTH;//CTXM_DOWN_TIME;//dwYRealPos_com;//servo_y.init_step;//CTHL_ZhuangLiaoStep; user_datas[122] = bSingOneFlg;//;CTHL_CTXMSSStep; user_datas[123] = CTHL_TD_MotorStep; user_datas[124] = CTHL_LianLengthCheck;//CTHL_ZhuangLiaoStep; user_datas[125] = CTHL_LianLengthCheckOld;//CTHL_TLiaoStep; user_datas[126] = CTHL_AutoStep;//CTHL_FZ_MotorStep;//CTHL_TD_MotorStep; // user_datas[127] = CTHL_cZipCnt;//CTHL_ZhuangLiaoStep; // user_datas[128] = CTHL_FZ_MotorStep;//CTHL_LianLengthAutolengthcheck; CTHL_SERVO_COM=servo_y.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_CTXMSS_Proc(); } void CTHL_AlarmProtect(void) { } //自动动作 void CTHL_AutoStepAction(void) { unsigned short freq; //测试用 if(dwTickCount >= CTHL_TestDelay) { CTHL_TestDelay = dwTickCount + 200; CTHL_TestLED = !CTHL_TestLED; CTHL_Test = 0; } if(bRunning) { switch(CTHL_AutoStep) { case 1: CTHL_Auto_RunState=CTHL_STATE_START;//存在着装拉头，拖带，下模上升 if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_TXM_VAVLE=0; CTHL_AutoStep = 2; //设置反转电机的扭矩和速度的设置 freq=CTHL_PARAM_FZ_Speed+SERVO_PARAM_SPEED_DIFFER; if(CTHL_cZipCnt >=1) { set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMIN,freq,freq,SERVO_TARR_CW); } else { set_servo_postotarr_limit(Y_AXIS,SERVO_PARAM_TRARR_LIMITMAX,freq,freq,SERVO_TARR_CW); } Set_Ctrlmode_trans(Y_AXIS,POS_MODE); bCxcAuto=0; if(CTHL_cZipCnt > 1) CTHL_AutoDelay = dwTickCount + 2;//拉头检测时间 else CTHL_AutoDelay = dwTickCount + 100;//拉头检测时间 CTHL_GF_Change = 0; } } break; case 2: if(bZhuangLiaoOkFlg && (CTHL_ZhuangLiaoStep == 0)&&(CTHL_TD_MotorStep==0)&&(CTHL_CTXMSSStep==0)) { if(dwTickCount >= CTHL_AutoDelay ) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { //if(!CTHL_PARAM_XMZM_MODE) { // CTHL_QMDW_VAVLE = 1;//0为斜码，1为直码 //CTHL_FKDW_VAVLE = 0; //方块定位退 // } CTHL_QMDW_VAVLE = 1; CTHL_YXC_VAVLE=1; CTHL_YFK_VAVLE=1; CTHL_JXM_VAVLE = 1; //夹斜码电磁阀 if(CTHL_cZipCnt==0)CTHL_CTXM_VAVLE=1; CTHL_JLTou_VAVLE = 0; //接拉头 CTHL_AutoStep = 3; CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; CTHL_Auto_RunState=CTHL_STATE_QMDW; } } } 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(CTHL_QMDW_IN&&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,1500,-CTHL_PARAM_JMDW_LENGTH,1500,800,10,10,0); //前拖带松一点带 AxisContinueMoveAcc(Y_AXIS,1500,DIR_N,1500,610,15,5); //Set_Ctrlmode_trans(Y_AXIS,TARR_MODE); CTHL_AutoStep = 5; CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; //CTHL_JXM_VAVLE = 1; //夹斜码电磁阀 } } else if((dwTickCount >= CTHL_AutoDelay+3000)) { CTHL_SetAlarmCode(CTHL_QMDW_Limit_ALARM,bRunning);//前码定位感应异常 } 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_AutoDelay = dwTickCount + CTHL_PARAM_TXM_DELAY; CTHL_AutoStep = 6; } AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); //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); //前拖带松一点带 AxisMovePosAccDec(Y_AXIS,2000,20,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 ) //推斜码到位 { 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&&!Y_DRV) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_Auto_RunState=CTHL_STATE_FZDW; CTHL_QMDW_VAVLE=0; CTHL_FZ_MotorStep=1; //反转穿入 //CTHL_FKDW_VAVLE = 0; //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; CTHL_AutoDelay = dwTickCount + CTHL_PARAM_FKDW_FZSTOP_DELAY;//拖带定位 } } break; case 10: if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_Auto_RunState=CTHL_STATE_FZCL; 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_Auto_RunState=CTHL_STATE_CXCR; CTHL_JCBI_VAVLE = 1; CTHL_SD = 1; CTHL_AutoStep = 12; CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } } break; case 12: if(CTHL_TXC_Limit_IN) //摆臂到位 { //CTHL_DingDai_VAVLE=1; //暂时不用 CTHL_XCCR_Time=0; //if(CTHL_PARAM_FZCXC_LENGTH != 0) // AxisMovePosAccDec(Y_AXIS,2*CTHL_PARAM_XCCR_SPEED,CTHL_PARAM_FZCXC_LENGTH,800,800,15,15,0);//辅助小插插入 CTHL_AutoStep = 13; CTHL_AutoDelay = dwTickCount + CTHL_PARAM_XCIN_DELAY;//小插到位延时拖带 } else if(dwTickCount >= CTHL_AutoDelay) { bCxcAuto=1; //CTHL_AutoStep = 101; //CTHL_JCBI_VAVLE = 0; //AxisMovePosAccDec(X_AXIS,1000,-50,1000,1000,15,15,0);//前轮布放松一点 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_AutoDelay = dwTickCount + 0;//CTHL_PARAM_YDC_DELAY; AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_XCCR_SPEED,CTHL_PARAM_XCCR_LENGTH,800,800,15,15,50); if(CTHL_SD) { CTHL_SD = 0; AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_XCCR_SPEED+100,CTHL_PARAM_XCCR_LENGTH+CTHL_PARAM_FZCXC_LENGTH,800,800,15,15,0); } CTHL_DingDai_VAVLE=1; 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_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=0; CTHL_DingDai_VAVLE=0; CTHL_XSavePosBuff3 = dwXRealPos; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); //CTHL_AutoStep = 100; //CTHL_JCBI_VAVLE = 0; //Y16小插摆臂电磁阀 //AxisMovePosAccDec(X_AXIS,800,-100,800,800,15,15,0);//前轮布放松一点 CTHL_SetAlarmCode(CTHL_XCDW_Limit_ALARM,bRunning);//小插定位针异常 } break; case 15: if((dwXRealPos >= (CTHL_XSavePosBuff3 + 50))) { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); if(dwTickCount >= CTHL_AutoDelay) { CTHL_XCDW_VAVLE=0; CTHL_TFK_VAVLE = 0; CTHL_DingDai_VAVLE=0; if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { cDELAYONE=0; 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 ; bZhuangLiaoOkFlg=0; CTHL_AutoStep = 17; //AxisMovePosAccDec(X_AXIS,5000,-CTHL_PARAM_CRFZ_LENGTH,1500,800,10,10,0); //前拖带松一点带 CTHL_YXC_VAVLE=0; //Y14压小插电磁阀 CTHL_JCBI_VAVLE=0; } } break; case 100: if(CTHL_START_IN_UP) { SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); CTHL_AutoDelay = dwTickCount + 300 ; CTHL_AutoStep = 101; } break; case 101: if(dwTickCount >= CTHL_AutoDelay) { CTHL_XCCR_Time++; if(CTHL_XCCR_Time==1) { AxisMovePosAccDec(X_AXIS,800,-CTHL_XSavePosBuff3,800,800,15,15,0);//前轮布放松 AxisMovePosAccDec(Y_AXIS,800,-CTHL_XSavePosBuff3,800,800,15,15,0);//前轮布放松 } CTHL_AutoStep = 102; } break; case 102: if(!X_DRV&&!Y_DRV) { SetAlarmCode(CTHL_ALARM_ADDR,0); //AxisMovePosAccDec(X_AXIS,800,-100,800,800,15,15,0);//前轮布放松 CTHL_YFK_VAVLE=1; CTHL_JCBI_VAVLE = 1; //Y16小插摆臂电磁阀 // CTHL_XCDW_VAVLE=0; cDELAYONE=1; cBAOjin=1; CTHL_cZipCnt=0; bZhuangLiaoOkFlg=0; SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); // if(CTHL_FKGY_IN) // { // CTHL_AutoStep = 9; // CTHL_FZ_MotorStep==0; // } // else // { CTHL_AutoStep = 103; // } CTHL_AutoDelay = dwTickCount + 300 ; // 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 + 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 = 104; CTHL_AutoDelay = dwTickCount + 100 ; } break; case 104: if(dwTickCount >= CTHL_AutoDelay && !Y_DRV) { CTHL_AutoDelay = dwTickCount + 0;//CTHL_PARAM_YDC_DELAY; AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_XCCR_SPEED,CTHL_PARAM_XCCR_LENGTH,800,800,15,15,50); CTHL_AutoStep = 105; CTHL_XSavePosBuff3=dwXRealPos; } break; case 105: 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_AutoDelay = dwTickCount + CTHL_PARAM_TXCDW_DELAY; } else if((dwTickCount >= (CTHL_AutoDelay+1200)||(dwXRealPos>=CTHL_XSavePosBuff3+CTHL_PARAM_XCCRBH_LENGTH+300)) && (!CTHL_bHandWork_MODE)) { CTHL_XSavePosBuff3 = dwXRealPos; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); if(CTHL_XCCR_Time>=2) { bCxcAuto=1; CTHL_SetAlarmCode(CTHL_XCDW_Limit_ALARM,bRunning);//小插定位针异常 } else { CTHL_AutoStep = 100; CTHL_JCBI_VAVLE = 0; //Y16小插摆臂电磁阀 } } break; case 17: if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_Auto_RunState=CTHL_STATE_TD; cDELAYONE=0; CTHL_TFK_VAVLE=0; CTHL_TD_MotorStep =10; CTHL_TD_MotorDelay = dwTickCount+ CTHL_PARAM_XCLD_DELAY; CTHL_AutoStep = 18; } } break; case 18: //if(!CTHL_CTM_Limit_IN) { if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_JLTou_VAVLE = 0; //CTHL_CTXMSSStep=1; CTHL_AutoStep = 19; } } break; case 19: if((CTHL_TD_MotorStep == 0)) { CTHL_Auto_RunState=CTHL_STATE_TD; if(!CTHL_bDANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoStep = 1; if(bSingOneFlg) { CTHL_CTXM_VAVLE =0; CTHL_CTXMSSStep=0; CTHL_AutoStep = 0; bRunning = 0; bSingOneFlg = 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; CTHL_GF_Change = 0; CTHL_AutoDelay = dwTickCount+0; } } break; } } } //下模上升处理 void CTHL_CTXMSS_Proc(void) { static unsigned char hemoflag1=0,hemoflag2=0; if(bRunning) { switch(CTHL_CTXMSSStep) { case 1: if(!CTHL_CTM_Limit_IN) { CTHL_JXM_VAVLE=0; 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_AutoDelay1 = dwTickCount+ 3000; } 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_AutoDelay1) { //穿头模和一次穿入回位异常 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; CTHL_GDLT_VAVLE=1; } else if(dwTickCount >= CTHL_AutoDelay1) { //穿头模和一次穿入回位异常 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; if(!CTHL_LTou_Check && !CTHL_TXM_Limit_IN)//有拉头，推斜码到位不能亮 { if(!bSingOneFlg) { CTHL_CTXM_VAVLE=1; CTHL_JXM_VAVLE=1; CTHL_CTXMSSStep = 21; } else { CTHL_CTXMSSStep = 0; } } else { CTHL_wltzi=1; CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning); // 没有拉头 } } break; case 21: if(CTHL_CTM_Limit_IN) { CTHL_CTXMSSStep = 0; } break; } } } //长按停止处理 void CTHL_STOP_longPress_Proc(void) { if(bRunning)//保存停机位置 { CTHL_Save_AutoStep=CTHL_AutoStep; CTHL_Save_ZhuangLiaoStep=CTHL_ZhuangLiaoStep; CTHL_Save_TLiaoStep=CTHL_TLiaoStep; CTHL_Save_TD_MotorStep=CTHL_TD_MotorStep; CTHL_Save_FZ_MotorStep=CTHL_FZ_MotorStep; CTHL_Save_CTXMSSStep=CTHL_CTXMSSStep; } CTHL_AutoStep = 0; CTHL_FZ_MotorStep =0; CTHL_TD_MotorStep =0; CTHL_ZhuangLiaoStep=0; CTHL_TLiaoStep=0; CTHL_Save_CTXMSSStep=0; bSingOneFlg=0; cBAOjin=0; CTHL_wltzi=0; CTHL_TLiao_VAVLE=0; CTHL_HSLiao_VAVLE =0; CTHL_JLTou_VAVLE =0; CTHL_TXM_VAVLE=0; CTHL_CTXM_VAVLE =0; CTHL_MGuo_VAVLE = 0; CTHL_JXM_VAVLE =0; CTHL_QMDW_VAVLE =0; CTHL_TFK_VAVLE =0; CTHL_HL_VAVLE =0; CTHL_YFK_VAVLE =0; CTHL_YXC_VAVLE =0; CTHL_XCDW_VAVLE=0; CTHL_JCBI_VAVLE =0; CTHL_DingDai_VAVLE=0; CTHL_LINK_ALARM=0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); Set_Ctrlmode_trans(Y_AXIS,POS_MODE); SetEn(X_AXIS, MOTOR_EN); SetEn(Y_AXIS, MOTOR_EN); 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); bRunning=0; bStop=0; } //短按停止处理 void CTHL_STOP_ShortPress_Proc(void) { if(bRunning&&!bSingOneFlg) { CTHL_Save_AutoStep=CTHL_AutoStep; CTHL_Save_ZhuangLiaoStep=CTHL_ZhuangLiaoStep; CTHL_Save_TLiaoStep=CTHL_TLiaoStep; CTHL_Save_TD_MotorStep=CTHL_TD_MotorStep; CTHL_Save_FZ_MotorStep=CTHL_FZ_MotorStep; CTHL_Save_CTXMSSStep=CTHL_CTXMSSStep; if(CTHL_TD_RunState==CTHL_STATE_TDLL) { bRunning=0; CTHL_AutoStep=0; CTHL_TD_MotorStep=0; CTHL_FZ_MotorStep=0; CTHL_CTXMSSStep=0; CTHL_CTXM_VAVLE=0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); Set_Ctrlmode_trans(Y_AXIS,POS_MODE); SetEn(X_AXIS, MOTOR_EN); SetEn(Y_AXIS, MOTOR_EN); } else { bSingOneFlg=1; } } else { bRunning=0; bSingOneFlg=0; CTHL_Save_AutoStep=CTHL_AutoStep; CTHL_Save_ZhuangLiaoStep=CTHL_ZhuangLiaoStep; CTHL_Save_TLiaoStep=CTHL_TLiaoStep; CTHL_Save_TD_MotorStep=CTHL_TD_MotorStep; CTHL_Save_FZ_MotorStep=CTHL_FZ_MotorStep; CTHL_Save_CTXMSSStep=CTHL_CTXMSSStep; CTHL_AutoStep=0; CTHL_TD_MotorStep=0; CTHL_CTXMSSStep=0; CTHL_FZ_MotorStep=0; /* CTHL_TLiao_VAVLE=0; CTHL_HSLiao_VAVLE =0; CTHL_JLTou_VAVLE =0; CTHL_TXM_VAVLE=0; CTHL_CTXM_VAVLE =0; CTHL_JXM_VAVLE =0; CTHL_QMDW_VAVLE =0; CTHL_TFK_VAVLE =0; CTHL_HL_VAVLE =0; CTHL_YFK_VAVLE =0; CTHL_YXC_VAVLE =0; CTHL_XCDW_VAVLE=0; CTHL_JCBI_VAVLE =0; CTHL_DingDai_VAVLE=0; CTHL_LINK_ALARM=0; */ AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); Set_Ctrlmode_trans(Y_AXIS,POS_MODE); SetEn(X_AXIS, MOTOR_EN); SetEn(Y_AXIS, MOTOR_EN); 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); } } //停止出处理 void CTHL_STOP_Proc(void) { if((CTHL_STOP_IN||bStop))//长按停止1.5S { if(dwTickCount>=CTHL_Stop_PressLongTime) //长按停止处理 CTHL_STOP_longPress_Proc(); } else { CTHL_Stop_PressLongTime=dwTickCount+1000; } if(CTHL_STOP_IN_UP|| bStop)//短按处理 { //CTHL_Stop_PressTime=dwTickCount+100; CTHL_STOP_ShortPress_Proc(); } //if(CTHL_STOP_IN_DW)CTHL_Stop_PressTime = dwTickCount + 80; } 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_GDLT_VAVLE=0; 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); 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_wltzi) { if(!bRunning) { bRunning = 1; SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); CTHL_AutoStep = 1; CTHL_AutoDelay = dwTickCount + 100; if(CTHL_QMDW_VAVLE)CTHL_TD_MotorStep=0; else CTHL_TD_MotorStep = 1; CTHL_FZ_MotorStep = 0; CTHL_CTXMSSStep=0; CTHL_CTXMSS_FLAG=0; CTHL_ZhuangLiaoDelay = dwTickCount + 350; CTHL_ZhuangLiaoStep=1; bTuiLaTouOkFlg = 1; bZhuangLiaoOkFlg= 0; CTHL_cZipCnt = 0; CTHL_TryCnt = 0; bCxcAuto=0; CTHL_TLiao_VAVLE =0; CTHL_HSLiao_VAVLE =0; CTHL_JLTou_VAVLE =0; CTHL_CTXM_VAVLE =0; CTHL_JXM_VAVLE =0; CTHL_TXM_VAVLE = 0 ; CTHL_TFK_VAVLE =0; CTHL_HL_VAVLE =0; CTHL_YFK_VAVLE =0; CTHL_YXC_VAVLE =0; CTHL_XCDW_VAVLE =0; CTHL_JCBI_VAVLE =0; CTHL_DingDai_VAVLE=0; CTHL_MGuo_VAVLE = 1; //判断是否已经有料 CTHL_GDLT_VAVLE=0; SetAlarmCode(CTHL_ALARM_ADDR,0); CTHL_Auto_RunState=0; CTHL_TD_RunState=0; bFZ_LengthOK = 0; CTHL_LianLengthCheck = 0; CTHL_LianLengthCheckOld = 0; } bStart = 0; } //停止 CTHL_STOP_Proc(); if(bAlarmStop) { bRunning = 0; bAlarmStop=0; CTHL_cStopMode=0; CTHL_Save_AutoStep=CTHL_AutoStep; CTHL_Save_ZhuangLiaoStep=CTHL_ZhuangLiaoStep; CTHL_Save_TLiaoStep=CTHL_TLiaoStep; CTHL_Save_TD_MotorStep=CTHL_TD_MotorStep; CTHL_Save_FZ_MotorStep=CTHL_FZ_MotorStep; CTHL_Save_CTXMSSStep=CTHL_CTXMSSStep; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); Set_Ctrlmode_trans(Y_AXIS,POS_MODE); CTHL_XCDW_VAVLE=0; CTHL_AutoStep = 0; CTHL_FZ_MotorStep =0; CTHL_TD_MotorStep =0; CTHL_CTXMSSStep=0; } #endif } void CTHL_NoLaLian_Speed_Proc(void) { if(CTHL_NoLaLian_IN_UP) { AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,200, 1000,CTHL_PARAM_FKDWMAN_Speed/2,25,15,0); //YWX_CTHL_PARAM_TD_RunSpeed AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,DIR_P,1500,800,15,15); } } void CTHL_TDLength_BH_Proc(unsigned short local)//拖带长度保护 { if(local==0)//为感应到开关前 { if(CTHL_cZipCnt < 2) { if((dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length) && (!CTHL_bHandWork_MODE)) { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); user_datas[128]=CTHL_TD_MotorStep; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } else { if((dwXRealPos >=(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheck)) && (!CTHL_bHandWork_MODE)) { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); user_datas[128]=CTHL_TD_MotorStep; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } } else//感应到开关后 { if(CTHL_cZipCnt < 2) { if((dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length+CTHL_LianLengthAutoFZCRcheck) && (!CTHL_bHandWork_MODE)) { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); user_datas[128]=CTHL_TD_MotorStep; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } else { if((dwXRealPos >=(CTHL_PARAM_WLCDSHEZHI_LENTH + CTHL_LianLengthCheck+CTHL_LianLengthCheckOld)) && (!CTHL_bHandWork_MODE)) { AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); user_datas[128]=CTHL_TD_MotorStep; CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } } } //拖带合链 void CTHL_TDHL_Proc(void) { //合链结束 if(CTHL_cZipCnt < 2) { //合链 if(dwXRealPos >= (CTHL_PARAM_SMG_LENGTH + CTHL_XSavePosBuff1)&&!CTHL_bHandWork_MODE) { //CTHL_XSavePosBuff2=dwXRealPos; CTHL_HL_VAVLE=1; } if((dwXRealPos>(CTHL_LianLengthAutoFZCRcheck))) { CTHL_HL_VAVLE=0; } } else { if(dwXRealPos >= CTHL_PARAM_HL_LENGTH) { CTHL_HL_VAVLE=1; } if((dwXRealPos>(CTHL_PARAM_HLWZ_LENGTH+CTHL_LianLengthAutoFZCRcheck-CTHL_PARAM_HELIAN_LENGTH))) { CTHL_HL_VAVLE=0; } } } //无方块处理 void CTHL_TDWFK_Proc(void) { } //拖带电机控制动作 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); CTHL_SetAlarmCode(CTHL_SF_ALR_ALARM,bRunning); //无拉链自动停机 CTHL_JCBI_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); CTHL_AutoStep = 201; CTHL_SetAlarmCode(CTHL_SF_ALR_ALARM,bRunning); //无拉链自动停机 CTHL_JCBI_VAVLE = 0; // CTHL_TFK_VAVLE=0; } #if 1 switch(CTHL_TD_MotorStep) { //1步开始，纯定位, 没有穿入 case 1: if(dwTickCount >= CTHL_TD_MotorDelay) //给锁轴时间 { CTHL_TD_RunState=CTHL_STATE_TDLL; //CTHL_FKDW_VAVLE=0; if(CTHL_PARAM_Test) CTHL_TXM_VAVLE = 0 ; else CTHL_TXM_VAVLE = 1; //因推斜码和固定方块一起，要避开 CTHL_TD_MotorStep = 2; SetPos(X_AXIS, 0); AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_First_SPEED,DIR_P,1000,600,50,15); AxisContinueMoveAcc(Y_AXIS,CTHL_PARAM_First_SPEED * 4/5,DIR_P,1000,600,50,15); } break; case 2://检测到过链 if(dwXRealPos >= CTHL_PARAM_first_CL_MAX_Length) { CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } if(CTHL_FK_Check_UP) { CTHL_TD_RunState=CTHL_STATE_FKGY; CTHL_XSavePosBuff = dwXRealPos; AxisContinueMoveChangeSpeed(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,1000,50,20); AxisContinueMoveChangeSpeed(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,1000,50,20); //AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_TDDW_LENGTH, // 1000,1000,30,100,50); //AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed*4/5,CTHL_PARAM_TDDW_LENGTH, // 1000,1000,30,100,50); CTHL_TD_MotorStep = 3; } break; case 3: if(dwXRealPos>=CTHL_XSavePosBuff+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=1; // CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常 //} if(CTHL_FKDW_IN_UP) { CTHL_TD_RunState=CTHL_STATE_FKDW; CTHL_TD_MotorStep = 5; CTHL_TD_MotorDelay = dwTickCount+ 0; AxisMovePosAccDec(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH, 1000,1000,30,100,50); AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH, 1000,1000,30,100,50); } break; case 5: if(!X_DRV&&!Y_DRV) { CTHL_TD_MotorStep = 0; } break; //穿合拉头定位 case 10: if(dwTickCount >= CTHL_TD_MotorDelay) { CTHL_TD_RunState=CTHL_STATE_TDHL; CTHL_JCBI_VAVLE=0; CTHL_DingDai_VAVLE=0; CTHL_YXC_VAVLE=0; CTHL_YFK_VAVLE=0; CTHL_TXM_VAVLE=0; CTHL_TD_MotorStep = 11; SetPos(X_AXIS, 0); //启动位置设为0点 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,250,25,00); if(!CTHL_bHandWork_MODE)//在手动穿链模式下不用动电机同步 AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_HLT_SPEED_Speed,CTHL_PARAM_HL_LENGTH, 1000,CTHL_PARAM_HLT_SPEED_Speed,250,25,00); CTHL_XSavePosBuff = dwXRealPos; bFKCheck_Flag = 0; CTHL_XSavePosBuff3 = 0; } break; case 11: CTHL_NoLaLian_Speed_Proc(); //最后拉链慢速的处理 //慢速合链长度，移动方块检测，躲避方块 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; } //慢速合链长度 如果已经到达下一条的方块也必须开模 ,CTHL_NoLaLian_IN最后几条慢速时感应才退下模 if(((dwXRealPos >= CTHL_PARAM_HL_LENGTH) && (!CTHL_bHandWork_MODE) && !CTHL_NoLaLian_IN) || (bFKCheck_Flag)) { //码勾先打开15MS再退下模 if(CTHL_MGuo_VAVLE) { CTHL_MGuo_VAVLE=0; CTHL_GDLT_VAVLE=1; CTHL_TD_MotorDelay = dwTickCount + CTHL_PARAM_TMG_TXM_DELAY;; } else if(dwTickCount >= CTHL_TD_MotorDelay) { CTHL_XSavePosBuff1= dwXRealPos; CTHL_CTXM_VAVLE=0;//推下模 CTHL_CTXMSSStep=1; CTHL_TD_MotorStep = 12; } } break; case 12://退下模加速 CTHL_TDLength_BH_Proc(0); CTHL_NoLaLian_Speed_Proc(); if(CTHL_FK_Check_UP && !bFKCheck_Flag) { bFKCheck_Flag = 1; CTHL_XSavePosBuff3= dwXRealPos; cGFJAUTO=0; } if(CTHL_CTM_Limit_IN) { CTHL_TD_MotorDelay = dwTickCount + CTHL_PARAM_TuiXiaMo_DELAY; } else if(dwTickCount >= CTHL_TD_MotorDelay) //下模离开后延时加速 { if(!CTHL_bHandWork_MODE) { bHeLaTouOK = 1; if((CTHL_cZipCnt > 1) && (!CTHL_NoLaLian_IN)) { if(CTHL_LianLengthCheckOld == 0) { CTHL_DataBuff = 6000; CTHL_LENTH=CTHL_LianLengthCheck - CTHL_PARAM_HL_LENGTH; } else if((CTHL_LianLengthCheck < 4000) && (CTHL_LianLengthCheckOld <4000)) { CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED*3/4; CTHL_LENTH=CTHL_LianLengthCheck + CTHL_LianLengthCheckOld- dwXRealPos; } else if((CTHL_LianLengthCheck < 5000) && (CTHL_LianLengthCheckOld <5000)) { CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED*4/5; CTHL_LENTH=CTHL_LianLengthCheck + CTHL_LianLengthCheckOld- dwXRealPos; } else { CTHL_DataBuff = CTHL_PARAM_CLsudu_SPEED; CTHL_LENTH=CTHL_LianLengthCheck + CTHL_LianLengthCheckOld- dwXRealPos; } AxisMovePosAccDecNotStop(X_AXIS,CTHL_DataBuff,CTHL_LENTH, 1000,CTHL_PARAM_FKDW_Speed,250,250,300); } 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_TDLength_BH_Proc(0); CTHL_NoLaLian_Speed_Proc(); CTHL_TDHL_Proc(); //已经开完模具 if(CTHL_FK_Check_UP&& !bFKCheck_Flag) { bFKCheck_Flag = 1; CTHL_XSavePosBuff3= dwXRealPos; } else if(bFKCheck_Flag) //说明第一个方块已经检测到 { CTHL_TD_RunState=CTHL_STATE_TDLL; CTHL_TD_MotorStep = 14; } break; case 14: CTHL_TDLength_BH_Proc(1); CTHL_NoLaLian_Speed_Proc(); CTHL_TDHL_Proc(); //无链运行15公分 if(!CTHL_FK_Check && (dwXRealPos >= (CTHL_XSavePosBuff3 + 1500))) { CTHL_TD_MotorStep = 15; if(CTHL_cZipCnt > 1)CTHL_CTXMSS_FLAG=1; } break; case 15: CTHL_TDLength_BH_Proc(1); CTHL_NoLaLian_Speed_Proc(); CTHL_TDHL_Proc(); CTHL_TD_RunState=CTHL_STATE_TDLL; //第二次感应方块， 要做定位 if(CTHL_FK_Check_UP && (dwXRealPos >= (CTHL_XSavePosBuff3 + 1800))) { CTHL_TD_RunState=CTHL_STATE_FKGY; if(!CTHL_bHandWork_MODE) { if(!CTHL_NoLaLian_IN) { CTHL_XSavePosBuff = dwXRealPos; AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,300, 1000,CTHL_PARAM_FKDWMAN_Speed,30,80,50); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,300, 1000,CTHL_PARAM_FKDWMAN_Speed,30,80,50); } //长度测量 bFZ_LengthOK = 1; if(CTHL_cZipCnt) { CTHL_LianLengthCheckOld = CTHL_LianLengthCheck; } CTHL_LianLengthCheck = dwXRealPos - CTHL_XSavePosBuff3; //公分节处理 if((CTHL_LianLengthCheck <= CTHL_PARAM_GFJ_LENGTH) && (CTHL_cZipCnt>1)) { CTHL_TD_MotorStep=50; AxisContinueMoveAcc(X_AXIS,2000,DIR_P,1000,600,15,15); AxisContinueMoveAcc(Y_AXIS,2000,DIR_P,1000,600,15,15); CTHL_TD_MotorDelay = dwTickCount + 200; bFZ_LengthOK= 0; CTHL_cZipCnt = 0; } else { CTHL_TD_MotorStep = 16; CTHL_AutoDelay=dwTickCount; if(CTHL_cZipCnt<2)CTHL_CTXMSS_FLAG=1; } } CTHL_XSavePosBuff = dwXRealPos; } CTHL_TDWFK_Proc(); break; case 16: if(dwXRealPos>=CTHL_XSavePosBuff+200) { //CTHL_FKDW_VAVLE=1; CTHL_TD_MotorStep = 17; //CTHL_TD_MotorDelay = dwTickCount+ 1500; } break; case 17: //定位感应亮时、位置到达时，或X轴停止时，停止X轴 //if(dwTickCount >= CTHL_TD_MotorDelay)//3S为检测到停机 //{ // bCxcAuto=1; // CTHL_SetAlarmCode(CTHL_FXDW_ALARM,bRunning);//方块钩针定位异常 //} if(CTHL_FKDW_IN) { CTHL_TD_RunState=CTHL_STATE_FKDW; if(CTHL_cZipCnt > 1&&!CTHL_NoLaLian_IN_UP) { AxisMovePosAccDec(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_TDDW_LENGTH, 1000,1000,30,100,50); AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_TDDW_LENGTH, 1000,1000,30,100,50); } else { AxisMovePosAccDec(X_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH, 1000,1000,30,100,50); AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_FKDWMAN_Speed,CTHL_PARAM_XMCR_YJDW_LENGTH, 1000,1000,30,100,50); } CTHL_TD_MotorStep = 18; CTHL_TD_MotorDelay = dwTickCount+ 0; bFZ_LengthOK = 1; } break; case 18: if(!X_DRV&&!Y_DRV) { CTHL_TD_MotorStep = 0; } break; //由公分节调转 case 50: if(CTHL_FK_Check_DW) { CTHL_TD_MotorStep = 51; CTHL_TD_MotorDelay = dwTickCount + 100; bFZ_LengthOK = 0; CTHL_GF_Change = 1; SetPos(X_AXIS, 0); CTHL_CTXM_VAVLE=0; CTHL_XSavePosBuff3=0; } break; case 51: if(dwTickCount >= CTHL_TD_MotorDelay&&!CTHL_FK_Check) { CTHL_TD_MotorStep = 52; } break; case 52: if(dwXRealPos>=CTHL_PARAM_GFJ_LENGTH) { CTHL_TryCnt = 0; CTHL_LianLengthCheck = 0; CTHL_LianLengthCheckOld = 0; CTHL_TD_MotorStep = 15; } break; } #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; //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+500,DIR_N,1500,1000,30,50); } else { AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_JXM_SPEED,-CTHL_PARAM_XMCR_SLOW_LENGTH, 1000,CTHL_PARAM_JXM_SPEED,50,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; } break; case 2: if(abs(dwXRealPos) >=CTHL_PARAM_XMCR_SLOW_LENGTH-100){//提前转换模式 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) { 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,150,0); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZ_Speed+120,-CTHL_LENTH, 1000,CTHL_PARAM_FZDS_Speed,120,100,0); } CTHL_FZ_MotorStep++; } //电机速度差太大 if(CTHL_cZipCnt<1) { if(abs(dwXRealPos)>dwYRealPos_com+1500) { //CTHL_FZ_MotorStep=100; bCxcAuto=0;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); //SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM); CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机卡滞 } } else { if(abs(dwXRealPos)>dwYRealPos_com+3000) { //CTHL_FZ_MotorStep=100; bCxcAuto=0;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); //SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM); CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机卡滞 } } break; case 3: 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_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,1000,120,80,CTHL_PARAM_FZJSDS_LENGTH); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_FZDS_Speed,-CTHL_PARAM_FZJS_LENGTH, 1000,1000,120,80,CTHL_PARAM_FZJSDS_LENGTH); CTHL_FZ_MotorStep++; } } //长度超出报警 if(CTHL_cZipCnt>=1) { 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){ //位置大于第一条拉链长度 bCxcAuto=0;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度 } } //电机速度差太大 if(CTHL_cZipCnt<1) { if(abs(dwXRealPos)>dwYRealPos_com+1500) { //CTHL_FZ_MotorStep=100; bCxcAuto=0;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); //SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM); CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机 } } else { if(abs(dwXRealPos)>dwYRealPos_com+3000) { //CTHL_FZ_MotorStep=100; bCxcAuto=0;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM); 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) > 500) && CTHL_FKGY_IN_UP)//位置大于100，且方块感应上升沿 { CTHL_XSavePosBuff1 = abs(dwXRealPos); if(CTHL_cZipCnt<1||CTHL_NoLaLian_IN){//记录前两条的方块感应长度； //停机距离调整 CTHL_LENTH=10;//CTHL_PARAM_FZSTOP_LENGTH; AxisMovePosAccDec(X_AXIS,800,-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,1000,-CTHL_LENTH, 800,800,15,25,0); AxisMovePosAccDec(Y_AXIS,600,-CTHL_LENTH, 800,800,15,25,0); } CTHL_FZ_MotorStep++; } //长度超出报警 if(CTHL_cZipCnt>=1) { 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){ //位置大于第一条拉链长度 bCxcAuto=0;//停轴 CTHL_SetAlarmCode(CTHL_CRFK_ALARM,bRunning); //穿入方块超出预设长度 } } //电机速度差太大 if(CTHL_cZipCnt<1) { if(abs(dwXRealPos)>dwYRealPos_com+1500) { //CTHL_FZ_MotorStep=100; bCxcAuto=0;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); //SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM); CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机 } } else { if(abs(dwXRealPos)>dwYRealPos_com+3000) { //CTHL_FZ_MotorStep=100; bCxcAuto=0;//停轴 SetEn(Y_AXIS, MOTOR_DISEN); SetAlarmCode(CTHL_ALARM_ADDR,CTHL_FZTD_KZ_ALARM); 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); 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: if(CTHL_PARAM_CXDW_Length) { CTHL_FZ_MotorStep = 22; AxisMovePosAccDec(Y_AXIS,2*CTHL_PARAM_XCCR_SPEED,-CTHL_PARAM_CXDW_Length,1000,1000,20,20,0); AxisMovePosAccDec(X_AXIS,2*CTHL_PARAM_XCCR_SPEED,-CTHL_PARAM_CXDW_Length,1000,1000,20,20,0); } break; case 22: if(!Y_DRV && !X_AXIS ) { CTHL_FZ_MotorStep = 0; Set_Ctrlmode_trans(Y_AXIS,POS_MODE); } break; case 100: if(dwYRealPos_com>=CTHL_PARAM_XMCR_SLOW_LENGTH+100&&dwYRealPos_com<=CTHL_LianLengthAutoFZCRcheck-500)//穿过慢速长度 { SetPos(Y_AXIS, 0); CTHL_FZ_MotorStep = 101; CTHL_XSavePosBuff1=dwYRealPos_com; } else { bCxcAuto=0;//停机 CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机卡滞 } break; case 101: CTHL_FZ_MotorStep = 102; Set_Ctrlmode_trans(Y_AXIS,POS_MODE); CTHL_FZ_MotorDelay=dwTickCount+50; break; case 102: CTHL_FZ_MotorStep = 103; AxisMovePosAccDec(Y_AXIS,1500,-500,1500,1500,20,20,0); break; case 103: if(!Y_DRV) { if(dwYRealPos_com>=CTHL_XSavePosBuff1+500) { Set_Ctrlmode_trans(Y_AXIS,TARR_MODE); SetAlarmCode(CTHL_ALARM_ADDR,0); CTHL_FZ_MotorStep = 4; FZ_ALARM_FLG=1; } else { bCxcAuto=0;//停机 CTHL_SetAlarmCode(CTHL_FZTD_KZ_ALARM,bRunning); //反转电机卡滞 } } break; } #endif } //装拉头动作 void CTHL_ZLT_Step(void) { switch(CTHL_ZhuangLiaoStep) { case 1: if(bZhuangLiaoOkFlg) { CTHL_ZhuangLiaoStep = 0; } else { if(((CTHL_MGuo_VAVLE && CTHL_LTou_Check) || (!CTHL_MGuo_VAVLE))&&CTHL_GDLT_VAVLE )//说明没有拉头 { /*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_GDLT_VAVLE=1; 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 if(dwTickCount >= CTHL_ZhuangLiaoDelay)//已经有拉头直接跳到结束 { 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_GDLT_VAVLE = 1; CTHL_MGuo_VAVLE = 0; if(!CTHL_JLTou_VAVLE) { 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_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 + CTHL_PARAM_JLTOU_DELAY; } break; case 5: if(dwTickCount >= CTHL_ZhuangLiaoDelay) { CTHL_ZhuangLiaoStep = 6; CTHL_GDLT_VAVLE = 0; //接拉头输出,不需要时间 CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_GDLT_DELAY; } 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 + CTHL_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; //码勾打开 CTHL_GDLT_VAVLE = 1; if(CTHL_TryCnt == 1)//重新横送一次 { CTHL_ZhuangLiaoStep = 21; //重新送3次 CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } else if(CTHL_TryCnt == 2||CTHL_TryCnt == 3)//重新装料2次 { CTHL_ZhuangLiaoStep = 20; //重新送3次 CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_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; //码勾打开 CTHL_GDLT_VAVLE = 1; 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; if(CTHL_TLiaoStep==0)CTHL_TLiaoStep = 1; //推拉头 CTHL_ZhuangLiaoStep = 0; } break; //装拉头2次 case 20: if(CTHL_HSLiao_Origin_IN) { CTHL_JLTou_VAVLE = 0; //接拉头关 CTHL_MGuo_VAVLE = 0; //码勾打开 CTHL_GDLT_VAVLE = 1; 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_GDLT_VAVLE = 1; 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_Limit_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_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(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 } unsigned char Manu_HSL_STEP; unsigned long Manu_HSL_DELAY; void CTHL_Manu_HSongLiao(void) { switch(Manu_HSL_STEP) { case 1: if(dwTickCount>=Manu_HSL_DELAY) { CTHL_MGuo_VAVLE=1; Manu_HSL_STEP=2; Manu_HSL_DELAY=dwTickCount+1000; } break; case 2: if(CTHL_MGuo_VAVLE&&CTHL_LTou_Check)//无拉头 { CTHL_GDLT_VAVLE=1; CTHL_MGuo_VAVLE=0; Manu_HSL_DELAY=dwTickCount+CTHL_VAVLE_ERROR_TIME; Manu_HSL_STEP=3; } else if(dwTickCount>=Manu_HSL_DELAY) { Manu_HSL_STEP=0; CTHL_SetAlarmCode(CTHL_YLT_CHECK_ALARM,0);//夹拉头异常 } break; case 3: if(CTHL_CTM_Origin_IN &&!CTHL_TLiao_Limit_IN)//下模在原位 { CTHL_HSLiao_VAVLE = 1; Manu_HSL_STEP=0; } else if(dwTickCount>=Manu_HSL_DELAY) { Manu_HSL_STEP=0; if(!CTHL_CTM_Origin_IN)CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,0); if(CTHL_TLiao_Limit_IN)CTHL_SetAlarmCode(CTHL_TLiao_Limit_ALARM,0); } break; } } //手动动作 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) { CTHL_Manu_HSongLiao(); 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 { CTHL_CTXM_VAVLE=0; CTHL_TLiao_VAVLE=0; CTHL_JLTou_VAVLE=0; CTHL_GDLT_VAVLE=0; if(Manu_HSL_STEP==0)Manu_HSL_STEP=1; Manu_HSL_DELAY=dwTickCount+20; } } if(CTHL_bTLiao ) //手动推料(推拉头) { CTHL_bTLiao = 0; if(CTHL_TLiao_VAVLE)CTHL_TLiao_VAVLE = 0; 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_bGDLT)//夹拉头 { CTHL_bGDLT = 0; if(!CTHL_GDLT_VAVLE)CTHL_MGuo_VAVLE = 0; CTHL_GDLT_VAVLE = !CTHL_GDLT_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_bXCDW) //插销定位 { CTHL_bXCDW = 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_bCXDD) //小插摆臂 { CTHL_bCXDD = 0; CTHL_DingDai_VAVLE = !CTHL_DingDai_VAVLE; } if(CTHL_bCTXM) //下模 { CTHL_bCTXM = 0; CTHL_TXM_VAVLE=0; if(CTHL_CTXM_VAVLE) { CTHL_CTXM_VAVLE = 0; CTHL_JLTou_VAVLE = 0; CTHL_JXM_VAVLE=0; } else if(!CTHL_HSLiao_VAVLE && CTHL_HSLiao_Origin_IN && (CTHL_JXM_VAVLE || !CTHL_TXM_VAVLE)) { CTHL_CTXM_VAVLE = 1; CTHL_JLTou_VAVLE=0; CTHL_JXM_VAVLE=1; } } //自动装料 if(CTHL_bAutoZhuangLiao) { CTHL_bAutoZhuangLiao = 0; if(CTHL_ZhuangLiaoStep == 0) { bTuiLaTouOkFlg = 1; bZhuangLiaoOkFlg = 0; CTHL_ZhuangLiaoStep = 1; CTHL_MGuo_VAVLE = 1; //判断是否已经有料 CTHL_GDLT_VAVLE=0; CTHL_TryCnt = 0; CTHL_ZhuangLiaoDelay = dwTickCount + 500; } } 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_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; } } //X轴Y轴电机测试 if(CTHL_bCLMotor_P) { SetEn(Y_AXIS, MOTOR_DISEN); SetEn(X_AXIS, MOTOR_DISEN); //CTHL_FKDW_VAVLE=0; if(CTHL_FK_Check_UP) { SetPos(X_AXIS, 0); SetPos(Y_AXIS, 0); } if(!X_DRV) { // Y轴 运行速度 启动速度 加速度 减速度 AxisContinueMoveAcc(X_AXIS,600 ,DIR_P,600,600,15,15); AxisContinueMoveAcc(Y_AXIS,580,DIR_P,580,580,15,15); } } //在压方块电磁阀输出情况下，方块感应后电机要自动停止 if(CTHL_FKGY_IN && CTHL_YFK_VAVLE && CTHL_bCLMotor_P) { CTHL_bCLMotor_P=0; AxisEgmStop(Y_AXIS); AxisEgmStop(X_AXIS); } if(CTHL_bCLMotor_N) //后退限位已经取消 { SetEn(Y_AXIS, MOTOR_DISEN); SetEn(X_AXIS, MOTOR_DISEN); //CTHL_FKDW_VAVLE=0; if(CTHL_FK_Check_UP) { SetPos(X_AXIS, 0); SetPos(Y_AXIS, 0); } //压方块电磁阀没有输出时，方块感应信号不限制 if(!CTHL_FKGY_IN || !CTHL_YFK_VAVLE) { if(!X_DRV && (!CTHL_QMDW_IN ||!CTHL_QMDW_VAVLE)) { // Y轴 运行速度 启动速度 加速度 减速度 AxisContinueMoveAcc(Y_AXIS,630,DIR_N,630,630,15,15); AxisContinueMoveAcc(X_AXIS,600,DIR_N,600,600,15,15); } if(CTHL_FKGY_IN && CTHL_YFK_VAVLE) { CTHL_bCLMotor_P=0; AxisEgmStop(Y_AXIS); AxisEgmStop(X_AXIS); } } if(CTHL_QMDW_IN && CTHL_QMDW_VAVLE) { AxisEgmStop(Y_AXIS); AxisEgmStop(X_AXIS); } } if(!CTHL_bCLMotor_P && !CTHL_bCLMotor_N && (CTHL_TD_MotorStep == 0)) { AxisEgmStop(Y_AXIS); AxisEgmStop(X_AXIS); } } } #endif