#include "global.h" #if FIE_YUE_MACHINE == 1 void CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag); void CTHL_ManualAction(void); void CTHL_AutoStepAction(void); void CTHL_TableAction(void); void CTHL_TD_Motor(void); void CTHL_CL_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_bFKCXTDDWStep(void); void CTHL_bCXCRStep(void); void CTHL_bCLFirstStep(void); void ExtiAcitionX02(void) { if(bCheckEN_FLG) { bCheckEN_FLG = 0; CTHL_CheckInX02PosBuff = GetPos(X_AXIS); } } //故障报警 void CTHL_SetAlarmCode(unsigned short alarm_code,unsigned short cStopFlag) { SetAlarmCode(CTHL_ALARM_ADDR,alarm_code); // bAlarmStop = 0; // if(cStopFlag) if(!cWULATOUCnt && !cXiaoChaCNT && !cCHULIANBAOJIN) { bAlarmStop = 1; } else { cWULATOUCnt=0; } } //初始化动作 void CTHL_InitAction(void) { float buff_pulse,buff_dist; buff_pulse = CTHL_PARAM_CYCLE_PULSE; buff_dist = CTHL_PARAM_CYCLE_LENGTH; XGearRatio = buff_pulse/buff_dist; buff_pulse = 2000;//CTHL_PARAM_CYCLE_PULSE; buff_dist = 750;//CTHL_PARAM_CYCLE_LENGTH; YGearRatio = buff_pulse/buff_dist; bYFirstDingWeiFlg = 1; user_datas[121] = 0; user_datas[122] = 0; user_datas[123] = 0; if(!CTHL_PARAM_MATOR_MODE) SetEnReverse(X_AXIS, 1); else SetEnReverse(X_AXIS, 0); SetEnReverse(Y_AXIS, 1); SetEnReverse(Z_AXIS, 1); SetDirReverse(X_AXIS, 0); SetDirReverse(Y_AXIS, 0); SetDirReverse(Z_AXIS, 0); SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); SetEn(Z_AXIS, MOTOR_DISEN); SetAlarmCode(CTHL_ALARM_ADDR,0); // QDCT_SZ_OUT = QDCT_MOTOR_DISEN; } void CTHL_Action(void) { user_datas[121] = CTHL_LianLengSave; // user_datas[122] =axis_x->cur_speed; // user_datas[123] = CTHL_bCXCR_Step; dwXRealPos = GetPos(X_AXIS); dwYRealPos = GetPos(Y_AXIS); dwZRealPos = GetPos(Z_AXIS); user_datas[124] = dwXRealPos; user_datas[125] = dwYRealPos; user_datas[126] = GetAlarm(X_AXIS); user_datas[127] = GetAlarm(Y_AXIS); user_datas[128] = CTHL_AutoStep; CTHL_ZhenDongAction(); CTHL_AlarmProtect(); CTHL_CheckStart(); CTHL_ZLT_Step(); CTHL_TLiao_Step(); CTHL_TD_Motor(); CTHL_CL_Motor(); CTHL_bFKCXTDDWStep(); CTHL_bCXCRStep(); CTHL_bCLFirstStep(); CTHL_ManualAction(); CTHL_AutoStepAction(); } void CTHL_AlarmProtect(void) { } //自动动作 void CTHL_AutoStepAction(void) { if(bRunning) { switch(CTHL_AutoStep) { case 1: if(CTHL_SF_Origin_IN) { /* CTHL_AutoStep = 10; //测试拖带定位 CTHL_AutoDelay = dwTickCount + 1000; break; */ if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_FKJD_VAVLE = 1; //方块夹带气缸输出 CTHL_XKTD_VAVLE = 1; //斜口推带气缸输出 CTHL_FKGD_VAVLE = 0; //方块固定关 // CTHL_XKDW_VAVLE = 1; CTHL_AutoStep = 2; CTHL_AutoDelay = dwTickCount + 50; if(CTHL_ZhuangLiaoStep == 0) { CTHL_ZhuangLiaoStep = 1; CTHL_TryCnt = 0; } } } break; case 2: if(!CTHL_XKTD_Origin_IN && (dwTickCount >= CTHL_AutoDelay)) //斜口推带原位离开 { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoStep = 3; CTHL_XKDW_VAVLE = 1; CTHL_AutoDelay = dwTickCount + 20; } } break; case 3://斜口定位 if(dwTickCount >= CTHL_AutoDelay) { CTHL_AutoStep = 4; // CTHL_AutoDelay = dwTickCount + CTHL_PARAM_XKDW_TD_DELAY; } break; case 4: if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { AxisMovePosAccDecNotStop(X_AXIS,1600,150,600,1000,5,3,00); CTHL_AutoStep = 5; CTHL_XSavePosBuff = dwXRealPos; // SetPos(X_AXIS, 0); //启动位置设为0点 if(cZipCnt < 1) CTHL_AutoDelay = dwTickCount +350; else CTHL_AutoDelay = dwTickCount + CTHL_PARAM_FKJD_DELAY; } break; case 5: if(dwTickCount >= CTHL_AutoDelay) { CTHL_FKJD_VAVLE = 1; //方块夹带气缸输出 } if(bCL_EN_FLG && (dwXRealPos >= 230) && CTHL_XKSC_Limit_IN) { CTHL_HLJJ_VAVLE = 1; //Y20 合链夹具电磁阀输出后,电机穿入 } if(CTHL_XKSC_Limit_IN) //方块上止到位 { AxisEgmStop(X_AXIS); // AxisMovePosAccDec(X_AXIS,1200,10,1200,1200,6,6,0); if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_QDXK_VAVLE = 1; CTHL_FKJD_VAVLE=0; CTHL_AutoStep = 6; CTHL_bLTCHECK_Delay = dwTickCount + 20; //电机穿入可以启动 bCL_OK_FLG = 1; //不用穿合链的话认为已经完成 if(bCL_EN_FLG) { bCL_OK_FLG = 0; CTHL_HLJJ_VAVLE = 1; //Y20 合链夹具电磁阀输出后,电机穿入 CTHL_CL_MotorStep = 1; CTHL_CL_MotorDelay = dwTickCount + CTHL_PARAM_HLJJ_CL_DELAY; CTHL_AutoDelay = dwTickCount + 50 + CTHL_PARAM_HLJJ_CL_DELAY; } else { CTHL_XKTD_VAVLE = 0; //斜口推带气缸关闭 CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } } } else if(dwXRealPos >= (CTHL_XSavePosBuff + 1500)) { CTHL_SetAlarmCode(CTHL_FKCXDW_ALARM,bRunning); //方块插销定位异常警告 } break; case 6: if(((dwYRealPos > 1000) || (!Y_DRV && dwYRealPos)) && CTHL_XKTD_VAVLE) { CTHL_XKTD_VAVLE = 0; //斜口推带气缸关闭 CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } if(CTHL_XKTD_Origin_IN) //斜口推带回到原位 { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { if(bZhuangLiaoOkFlg && (CTHL_ZhuangLiaoStep == 0)) { if(dwTickCount >= CTHL_bLTCHECK_Delay) { if(!CTHL_LTou_Check) { if(CTHL_PARAM_TDFZ_LENTH && !X_DRV) //拖带电机反转 { AxisMovePosAccDec(X_AXIS,8000,-CTHL_PARAM_DXKFZ_LENTH,800,600,6,6,0); if(!CTHL_PARAM_NLSL_MODE) CTHL_FKJD_VAVLE= 0; //方块夹带气缸关 } if(CTHL_PARAM_CXMODE) CTHL_CXVAVLE = 1; CTHL_CTXM_VAVLE = 1; //穿头下模上升 CTHL_AutoStep = 7; CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } else { cWULATOUCnt=1; CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning); // 没有拉头 } } } } } else if((dwTickCount >= CTHL_AutoDelay) && !CTHL_XKTD_VAVLE ) { CTHL_SetAlarmCode(CTHL_XKTD_Origin_ALARM,bRunning); //斜口推带回到异常 } break; case 7: if(!CTHL_CTM_Origin_IN) { CTHL_JLTou_VAVLE = 0; //接拉头 } if(CTHL_CTM_Limit_IN) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { if(CTHL_PARAM_NLSL_MODE) { CTHL_XKTD_VAVLE=1; } CTHL_JLTou_VAVLE = 0; CTHL_AutoStep = 8; CTHL_AutoDelay = dwTickCount + CTHL_PARAM_DXKYS_DELAY; } } else if(dwTickCount >= CTHL_AutoDelay) { CTHL_SetAlarmCode(CTHL_CTM_Limit_ALARM,bRunning); //穿头下模上升异常 } break; case 8: if(!X_DRV) //延时顶斜口 { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { if((dwTickCount >= CTHL_AutoDelay) || (CTHL_PARAM_NLSL_MODE)) { if(!CTHL_PARAM_NLSL_MODE) CTHL_DXK_VAVLE=1; CTHL_AutoDelay = dwTickCount + CTHL_PARAM_DXK_XKDW_Off_DELAY; CTHL_AutoStep = 9; } } } break; case 9: if((dwTickCount >= CTHL_AutoDelay) && (bCL_OK_FLG)) //顶斜口延时退斜口上止定位 { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { // AxisMovePosAccDec(X_AXIS,2000,-150,1200,1200,3,6,0); //反转1.5CM CTHL_XKDW_VAVLE = 0; //斜口上止定位关 CTHL_FKJD_VAVLE = 0; //方块夹带气缸关 CTHL_AutoStep = 10; CTHL_AutoDelay = dwTickCount + CTHL_PARAM_XK_CR_DELAY; } } break; case 10: if((dwTickCount >= CTHL_AutoDelay) && !X_DRV) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_TD_MotorStep = 10; //启动拖带穿入 CTHL_AutoStep = 11; } } break; case 11: if(CTHL_TD_MotorStep == 0) //穿入定位完成 { /* //测试拖带定位 CTHL_AutoStep = 1; CTHL_AutoDelay = dwTickCount + 1000; if(cZipCnt<3) cZipCnt++; break; */ if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_XKDW_VAVLE = 0; //斜口上止定位关 CTHL_XKTD_VAVLE = 1; //斜口推带气缸输出 CTHL_CXSCDW_VAVLE = 1; //Y21 插销上止定位电磁阀 cXcxcrCnt = 0; if(CTHL_CL_MotorStep == 0) { CTHL_CXQJD_VAVLE = 1; } CTHL_AutoStep = 12; // CTHL_AutoDelay = dwTickCount + 50; } } break; case 12: if((CTHL_CL_MotorStep == 0)&& (CTHL_SF_Origin_IN)) //插销前夹带回到原位 { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoStep = 14; CTHL_AutoDelay = dwTickCount + CTHL_PARAM_XCQJD_XCDW_DELAY; if(cXcxcrCnt != 1) { CTHL_CXQJD_VAVLE = 1; } } } break; case 14: if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_XKDW_VAVLE = 1; //斜口定位 CTHL_AutoStep = 15; CTHL_FKGD_VAVLE = 1; //方块固定 //Y轴启动插销 if(cXcxcrCnt != 1) { CTHL_CL_MotorStep = 20; //插销定位 } } } break; case 15: if(CTHL_CL_MotorStep == 0) //插销定位完成 { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoStep = 16; CTHL_CXDWZ_VAVLE = 1; //插销定位针(到位)电磁阀 CTHL_CXHJD_VAVLE = 1; //Y14 插销后夹带输出 CTHL_AutoDelay = dwTickCount + 10; //延时插销下移 } } break; case 16: if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoStep = 17; if(CTHL_PARAM_CXMODE) CTHL_CXVAVLE = 0; else CTHL_CXVAVLE = 1; //Y06 插销下移电磁阀 CTHL_CXHJD_VAVLE = 1; //Y14 插销后夹带输出 CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; //插销下移异常时间 } } break; case 17: if(CTHL_PARAM_CXMODE) { if(CTHL_CXLimit_IN) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoStep = 18; if( cXcxcrCnt==0 && !X_DRV) //拖带电机反转 { AxisMovePosAccDec(X_AXIS,8000,-CTHL_PARAM_TDFZ_LENTH,800,600,6,6,0); } CTHL_CXQJD_VAVLE = 0; //插销前夹带电磁阀 CTHL_CXSCDW_VAVLE = 0; //Y21 插销上止定位关 CTHL_CXHJD_VAVLE = 1; //Y14 插销后夹带输出 CTHL_AutoDelay = dwTickCount + CTHL_PARAM_SCDWOff_CXDW_DELAY; } } else if(dwTickCount >= CTHL_AutoDelay) { CTHL_SetAlarmCode(CTHL_CXLimit_ALARM,bRunning); //插销下移异常 } } else { CTHL_AutoStep = 18; if( cXcxcrCnt==0 && !X_DRV) //拖带电机反转 { AxisMovePosAccDec(X_AXIS,8000,-CTHL_PARAM_TDFZ_LENTH,800,600,6,6,0); } CTHL_CXQJD_VAVLE = 0; //插销前夹带电磁阀 CTHL_CXSCDW_VAVLE = 0; //Y21 插销上止定位关 CTHL_CXHJD_VAVLE = 1; //Y14 插销后夹带输出 CTHL_AutoDelay = dwTickCount + CTHL_PARAM_SCDWOff_CXDW_DELAY; } break; case 18: // if(START_IN_UP) { if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoStep = 19; cBAOHUCNT=0; CTHL_CXHDW_VAVLE = 1; //插销后定位(插销穿入) CTHL_AutoDelay = dwTickCount + 500; //插销穿入到位异常时间 CTHL_CL_MotorStep = 50; //合链电机回原点 } } } break; case 19: if(CTHL_CX_Limit_IN && (cXcxcrCnt<4)) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { cXcxcrCnt=0; cXiaoChaCNT=0; user_datas[100]=0; bZhuangLiaoOkFlg = 0; CTHL_CXVAVLE=0; CTHL_AutoStep = 901; CTHL_AutoDelay = dwTickCount + CTHL_PARAM_FKGD_DELAY; //退方块固定时间 CTHL_CXDWZ_VAVLE = 0; //插销定位针(到位)电磁阀 } } else if ((dwTickCount >= CTHL_AutoDelay) && (cXcxcrCnt<3) ) { cXcxcrCnt++; CTHL_CXHJD_VAVLE=0; CTHL_CXHDW_VAVLE=0; if(!CTHL_PARAM_CXMODE) CTHL_CXVAVLE=0; else CTHL_CXVAVLE=1; if(cXcxcrCnt<3) { CTHL_CXSCDW_VAVLE=1; CTHL_AutoStep = 12; CTHL_AutoDelay = dwTickCount + 120; } } else if((dwTickCount >= (CTHL_AutoDelay +3000)) && !CTHL_CX_Limit_IN ) { cXiaoChaCNT=1; if(!CTHL_PARAM_CXMODE) CTHL_CXVAVLE=0; else CTHL_CXVAVLE=1; CTHL_CXHJD_VAVLE =0; cBAOHUCNT=1; CTHL_SetAlarmCode(CTHL_CX_Limit_ALARM ,bRunning);//插销穿入到位异常 } if( cXcxcrCnt > 2) { cXiaoChaCNT=1; CTHL_CXVAVLE=0; CTHL_CXHJD_VAVLE =0; cBAOHUCNT=1; CTHL_SetAlarmCode(CTHL_CX_Limit_ALARM ,bRunning);//插销穿入到位异常 } break; case 901: if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_FKGD_VAVLE = 0; //方块固定退 CTHL_AutoStep = 20; if(!cBAOHUCNT) CTHL_AutoDelay = dwTickCount + CTHL_PARAM_First_CL_DELAY;//延时一次穿入 else { CTHL_AutoDelay = dwTickCount + 1000;//手动插销保护时间 cBAOHUCNT=0; } } } break; case 20: if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoStep = 21; CTHL_First_CT_VAVLE = 1; //一次穿入电磁阀输出 CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } } break; case 21: if(!CTHL_First_CT_Origin_IN) //原点离开后才开始时间 { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoStep = 22; CTHL_AutoDelay = dwTickCount + CTHL_PARAM_First_CL_Timer; //一次穿入时间 } } else if(dwTickCount >= CTHL_AutoDelay) { CTHL_SetAlarmCode(CTHL_YCCRYWYC_ALARM,bRunning); //一次穿入原位异常 } break; case 22: if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoStep = 23; CTHL_MGuo_VAVLE = 0; //码勾退 if(CTHL_PARAM_LTJJ_MODE) { CTHL_LTJJ_VAVLE=1; } CTHL_AutoDelay = dwTickCount + CTHL_PARAM_MGOff_XiaMo_DELAY; CTHL_FKGD_VAVLE = 0; //方块固定退 CTHL_FKJD_VAVLE=0; // CTHL_CXHJD_VAVLE = 0; //Y14 插销后夹带打开 CTHL_CXVAVLE = 0; //Y06 插销下移电磁阀关 } } break; case 23: if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { if(!CTHL_LTJJ_Origin_IN || !CTHL_PARAM_LTJJ_MODE) { CTHL_CTXM_VAVLE = 0; //穿头下模下降 CTHL_AutoStep = 24; CTHL_AutoDelay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } else if(dwTickCount >= CTHL_AutoDelay+3000) { CTHL_SetAlarmCode(CTHL_XMKH_Origin_ALARM,bRunning); } } } break; case 24: if(!CTHL_CTM_Limit_IN) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoStep = 25; CTHL_FKJD_VAVLE=0; CTHL_CXHJD_VAVLE = 0; CTHL_AutoDelay = dwTickCount + 30; //一次穿入延时返回 } } else if(dwTickCount >= CTHL_AutoDelay) { CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning);//穿头下模下降异常 } break; case 25: if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_AutoStep = 26; CTHL_First_CT_VAVLE = 0; //一次穿入电磁阀关 CTHL_AutoDelay = dwTickCount + 5; CTHL_CXHDW_VAVLE = 0; //插销后定位(插销穿入)关闭 //退下出错警告 CTHL_AutoDelay1 = dwTickCount + CTHL_VAVLE_ERROR_TIME; CTHL_AutoStep1 = 1; } } break; case 26: if(dwTickCount >= CTHL_AutoDelay) { if(!CTHL_DANBU_MODE || CTHL_START_IN_UP) { CTHL_FKJD_VAVLE = 1; //方块夹带气缸输出 CTHL_FKGD_VAVLE = 0; //方块固定关 CTHL_AutoStep = 4; bCL_EN_FLG = 1; cXcxcrCnt=0; AddToTal(CTHL_TOTAL_ADDR); AddToTal(CTHL_TRUST_ALARM_ADDR); AddToTal(CTHL_TEST_ALARM_ADDR); CalProSP(CTHL_SPEED_ADDR); if(cZipCnt<3) cZipCnt++; } } break; } } //退下模后启动送拉头 switch(CTHL_AutoStep1) { case 1: if(cStopMode==1 || bSingOneFlg ) { bCL_EN_FLG=1; cStopMode=2; bSingOneFlg=0; bStop=1; } if(CTHL_CTM_Origin_IN && CTHL_First_CT_Origin_IN) { CTHL_ZhuangLiaoStep = 1; CTHL_TryCnt = 0; CTHL_AutoStep1 = 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_First_CT_Origin_IN) CTHL_SetAlarmCode(CTHL_First_CT_Origin_ALARM,bRunning); } break; } } void CTHL_CheckStart(void) { if(cXCXQXQD) { if(CTHL_START_IN_UP || bStart) { bStart=0; if(!bRunning) { if(CTHL_SafeDoor_IN ) { bRunning=1; cXCXQXQD =0; CTHL_TD_MotorStep = 0; CTHL_AutoStep=11; } else { CTHL_SetAlarmCode(CTHL_SafeDoor_ALARM,0); // 安全门关上才能启动 } } } } if(cWULATOUCnt) // 无拉头警告后按 { if(CTHL_START_IN_UP || bStart) { bStart=0; if(!bRunning) { if(CTHL_SafeDoor_IN) { bRunning=1; CTHL_TryCnt=0; cWULATOUCnt=0; CTHL_ZhuangLiaoStep = 1; CTHL_AutoStep=6; bTuiLaTouOkFlg = 1; bZhuangLiaoOkFlg= 0; CTHL_MGuo_VAVLE = 1; //判断是否已经有料 CTHL_LTJJ_VAVLE=0; CTHL_ZhuangLiaoDelay = dwTickCount + 150; } else { CTHL_SetAlarmCode(CTHL_SafeDoor_ALARM,0); // 安全门关上才能启动 } } } } if(!CTHL_SafeDoor_IN && bRunning) { bStop=1; CTHL_SetAlarmCode(CTHL_SafeDoor_ALARM,0); // 安全门关上才能启动 } //启动 自动启动 单一自动 if((CTHL_START_IN_UP || bStart || CTHL_bSingleone) && !cWULATOUCnt && !cXCXQXQD) { if(cCHULIANBAOJIN) //无链停机启动 { cCHULIANBAOJIN=0; SetEn(X_AXIS, MOTOR_EN); // dwXRealPos=0; CTHL_TD_MotorStep=10; cZipCnt=0; CTHL_TD_MotorDelay = dwTickCount + 500; CTHL_AutoDelay = dwTickCount + 150; if(GetAlarmCode(CTHL_ALARM_ADDR) != 0) SetAlarmCode(CTHL_ALARM_ADDR,0); } if(!bRunning) { if(CTHL_SafeDoor_IN) { cStopMode=0; if(CTHL_bSingleone) { bSingOneFlg=1; } bRunning = 1; cWULATOUCnt=0; cHeLian=0; if(GetEn(X_AXIS)==MOTOR_DISEN) { SetEn(X_AXIS, MOTOR_EN); CTHL_AutoDelay = dwTickCount + 150; } SetEn(Y_AXIS, MOTOR_EN); CTHL_CL_MotorStep = 40; CTHL_AutoStep = 1; bTuiLaTouOkFlg = 1; CTHL_PARAM_ZDP_AUTO= 0; bZhuangLiaoOkFlg= 0; if(!CTHL_PARAM_LTJJ_MODE) { CTHL_LTJJ_VAVLE=0; } CTHL_MGuo_VAVLE = 1; //判断是否已经有料 CTHL_LTJJ_VAVLE=0; CTHL_ZhuangLiaoDelay = dwTickCount + 150; // bCL_EN_FLG = 0; cZipCnt = 0; cQDXK_DXK=0; CTHL_LianLengthCheck = 0; cStopInCnt = 0; } else { CTHL_SetAlarmCode(CTHL_SafeDoor_ALARM,0); // 安全门关上才能启动 } } CTHL_bSingleone=0; bStart = 0; } if(CTHL_STOP_INJT_UP) { bRunning=0; cXcxcrCnt=0; cStopMode=0; cCHULIANBAOJIN=0; cXCXQXQD=0; cWULATOUCnt=0; bCL_EN_FLG=0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); cStopMode=0; cQDXK_DXK=0; if(GetAlarm(Y_AXIS)==1 || GetAlarm(X_AXIS)==0) { SetClr(Y_AXIS, MOTOR_NOALARM); SetClr(X_AXIS, MOTOR_NOALARM); CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000; } CTHL_TLiao_VAVLE=0; CTHL_HSLiao_VAVLE =0; CTHL_JLTou_VAVLE =0; CTHL_XKTD_VAVLE =0; CTHL_XKDW_VAVLE =0; CTHL_DXK_VAVLE =0; CTHL_CXVAVLE =0; CTHL_CTXM_VAVLE =0; CTHL_First_CT_VAVLE =0; CTHL_FKGD_VAVLE =0; CTHL_CXQJD_VAVLE =0; CTHL_CXDWZ_VAVLE =0; CTHL_CXHJD_VAVLE =0; CTHL_CXHDW_VAVLE =0; CTHL_FKJD_VAVLE =0; CTHL_HLJJ_VAVLE =0; CTHL_CXSCDW_VAVLE =0; CTHL_MGuo_VAVLE =0; CTHL_QDXK_VAVLE=0; CTHL_LTJJ_VAVLE =0; CTHL_AutoStep = 0; CTHL_ZhuangLiaoStep=0; CTHL_TLiaoStep=0; CTHL_TD_MotorStep=0; CTHL_JiaLianStep =0; CTHL_SongLiaoStep =0; CTHL_CL_MotorStep =0; CTHL_AutoStep1 =0; CTHL_bFKCXTDDW_Step=0; CTHL_bCXCR_Step=0; CTHL_bCLFirst_Step=0; if(GetAlarmCode(CTHL_ALARM_ADDR) != 0) SetAlarmCode(CTHL_ALARM_ADDR,0); SetPos(X_AXIS, 0); CTHL_TLiaoStep = 0; CTHL_AutoStep = 0; //穿合链电机是往零点方向时可以停止 if(GetDir(Y_AXIS) == DIR_N) AxisEgmStop(Y_AXIS); } //停止 if(CTHL_STOP_IN_UP || bStop) { if(cStopMode < 2) cStopMode++; if(bRunning && cStopMode==2) { bRunning=0; cStopMode=0; cXcxcrCnt=0; bSingOneFlg=0; cXCXQXQD=0; cCHULIANBAOJIN=0; cWULATOUCnt=0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); CTHL_AutoStep = 0; CTHL_CL_MotorStep =0; CTHL_bCLFirst_Step=0; CTHL_ZhuangLiaoStep=0; // CTHL_XKTD_VAVLE = 0; cQDXK_DXK=0; // CTHL_XKDW_VAVLE =0; if(GetAlarm(Y_AXIS)==1 || GetAlarm(X_AXIS)==0) { SetClr(Y_AXIS, MOTOR_NOALARM); SetClr(X_AXIS, MOTOR_NOALARM); CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000; } if(GetAlarmCode(CTHL_ALARM_ADDR) != 0) SetAlarmCode(CTHL_ALARM_ADDR,0); } else if(!bRunning) { bRunning=0; cXcxcrCnt=0; cStopMode=0; cCHULIANBAOJIN=0; cXCXQXQD=0; cWULATOUCnt=0; bCL_EN_FLG=0; bSingOneFlg=0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); SetEn(X_AXIS, MOTOR_DISEN); SetEn(Y_AXIS, MOTOR_DISEN); cStopMode=0; cQDXK_DXK=0; if(GetAlarm(Y_AXIS)==1 || GetAlarm(X_AXIS)==0) { SetClr(Y_AXIS, MOTOR_NOALARM); SetClr(X_AXIS, MOTOR_NOALARM); CTHL_bSFBBAOJIN_Delay= dwTickCount + 2000; } CTHL_TLiao_VAVLE=0; CTHL_HSLiao_VAVLE =0; CTHL_JLTou_VAVLE =0; CTHL_XKTD_VAVLE =0; CTHL_XKDW_VAVLE =0; CTHL_DXK_VAVLE =0; CTHL_CXVAVLE =0; CTHL_CTXM_VAVLE =0; CTHL_First_CT_VAVLE =0; CTHL_FKGD_VAVLE =0; CTHL_CXQJD_VAVLE =0; CTHL_CXDWZ_VAVLE =0; CTHL_CXHJD_VAVLE =0; CTHL_CXHDW_VAVLE =0; CTHL_FKJD_VAVLE =0; CTHL_HLJJ_VAVLE =0; CTHL_CXSCDW_VAVLE =0; CTHL_MGuo_VAVLE =0; CTHL_QDXK_VAVLE=0; CTHL_LTJJ_VAVLE =0; CTHL_AutoStep = 0; CTHL_ZhuangLiaoStep=0; CTHL_TLiaoStep=0; CTHL_TD_MotorStep=0; CTHL_JiaLianStep =0; CTHL_SongLiaoStep =0; CTHL_CL_MotorStep =0; CTHL_AutoStep1 =0; CTHL_bFKCXTDDW_Step=0; CTHL_bCXCR_Step=0; CTHL_bCLFirst_Step=0; if(GetAlarmCode(CTHL_ALARM_ADDR) != 0) SetAlarmCode(CTHL_ALARM_ADDR,0); SetPos(X_AXIS, 0); CTHL_TLiaoStep = 0; CTHL_AutoStep = 0; //穿合链电机是往零点方向时可以停止 if(GetDir(Y_AXIS) == DIR_N) AxisEgmStop(Y_AXIS); } bStop=0; } if(bAlarmStop) { bRunning = 0; bAlarmStop=0; cStopMode=0; cXcxcrCnt=0; AxisEgmStop(X_AXIS); AxisEgmStop(Y_AXIS); CTHL_AutoStep = 0; cQDXK_DXK=0; CTHL_CL_MotorStep =0; CTHL_bCLFirst_Step=0; CTHL_SongLiaoStep =0; CTHL_ZhuangLiaoStep=0; // CTHL_XKTD_VAVLE = 0; // CTHL_XKDW_VAVLE =0; } } //拖带电机控制动作 X轴 void CTHL_TD_Motor(void) // { #if 1 switch(CTHL_TD_MotorStep) { //1步开始，纯定位, 没有穿入 case 1: if(dwTickCount >= CTHL_TD_MotorDelay) //给锁轴时间 { bCheckEN_FLG = 1; CTHL_CheckInX02PosBuff= 0; CTHL_TD_MotorStep = 2; SetPos(X_AXIS, 0); //启动位置设为0点 AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_TD_RunSpeed*2/3,DIR_P,800,600,15,15); CTHL_X_CXPosBuff=0; } break; case 2://检测到过链 if(dwXRealPos > 400) //穿入2CM后退 { if(CTHL_PARAM_NLSL_MODE) { CTHL_XKTD_VAVLE=0; } CTHL_DXK_VAVLE = 0; //顶斜口气缸关 CTHL_QDXK_VAVLE = 0; } if(dwXRealPos >= CTHL_PARAM_WLCDSHEZHI_LENTH) { CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } if(CTHL_FK_Check) { CTHL_DXK_VAVLE = 0; //顶斜口气缸关 CTHL_QDXK_VAVLE = 0; CTHL_XSavePosBuff = dwXRealPos; CTHL_TD_MotorStep = 3; user_datas[123] = dwXRealPos - CTHL_CheckInX02PosBuff; if((dwXRealPos >= CTHL_CheckInX02PosBuff) && CTHL_CheckInX02PosBuff) AxisMovePosAccDec(X_AXIS,CTHL_PARAM_TD_RunSpeed*2/3,CTHL_PARAM_FKDW_Length - (dwXRealPos- CTHL_CheckInX02PosBuff)-160, CTHL_PARAM_FKDW_Speed,CTHL_PARAM_FKDW_Speed,10,30,40); else AxisMovePosAccDec(X_AXIS,CTHL_PARAM_TD_RunSpeed*2/3,CTHL_PARAM_FKDW_Length-120, CTHL_PARAM_FKDW_Speed,CTHL_PARAM_FKDW_Speed,10,30,40); } break; case 3: if(CTHL_CXSC_Limit_IN_UP) { CTHL_X_CXPosBuff = dwXRealPos; } if(!X_DRV || (dwXRealPos >= (CTHL_XSavePosBuff + CTHL_PARAM_FKDW_Length))) { AxisEgmStop(X_AXIS); CTHL_TD_MotorStep =4 ; CTHL_LianLengthAutoCheck = dwXRealPos; } break; case 4: if(CTHL_PARAM_XCX_LENTH) //0的话不判断位置 { if ((dwXRealPos - CTHL_X_CXPosBuff) > (CTHL_PARAM_XCX_LENTH + CTHL_PARAM_XCXWC_LENTH)) { // user_datas[121] = 1; CTHL_TD_MotorStep = 0; CTHL_SetAlarmCode(CTHL_XCXXCYC_ALARM,bRunning); // 小插销行程异常 } else if(CTHL_X_CXPosBuff == 0) { // user_datas[121] = 3; CTHL_TD_MotorStep = 0; CTHL_SetAlarmCode(CTHL_XCXXCYC_ALARM,bRunning); // 小插销行程异常 } else { CTHL_TD_MotorStep = 0; CTHL_X_CXPosBuff=dwXRealPos; } } else { CTHL_TD_MotorStep = 0; // CTHL_X_CXPosBuff=dwXRealPos; } break; //穿拉头定位 case 10: if(dwTickCount >= CTHL_TD_MotorDelay) { CTHL_TD_MotorStep = 11; SetPos(X_AXIS, 0); //启动位置设为0点 dwXRealPos = 0; AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_TDCR_LowSpeed,DIR_P,200,200,5,5); CTHL_CheckInX02PosBuff= 0; CTHL_X_CXPosBuff=0; bCheckEN_FLG = 1; } break; case 11: if(CTHL_PARAM_NLSL_MODE) CTHL_Data_Buff = 600; else CTHL_Data_Buff = 400; if(dwXRealPos > CTHL_Data_Buff) //穿入2CM后退 { CTHL_CheckInX02PosBuff= 0; CTHL_DXK_VAVLE = 0; //顶斜口气缸关 CTHL_QDXK_VAVLE = 0; if(CTHL_PARAM_NLSL_MODE) { CTHL_XKTD_VAVLE=0; } CTHL_TD_MotorStep = 12; if((cZipCnt < 2) || ((CTHL_LianLengthCheck < CTHL_PARAM_TDCR_LowSpeedLength+300) && (CTHL_PARAM_NLSL_MODE == 0)) || ((CTHL_LianLengthCheck < CTHL_PARAM_TDCR_LowSpeedLength+600) && (CTHL_PARAM_NLSL_MODE == 1))) AxisContinueMoveAcc(X_AXIS,CTHL_PARAM_TD_RunSpeed*2/3,DIR_P,3000,600,10,10); else ////两段速度移动距离 { if(CTHL_PARAM_NLSL_MODE == 0) AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_TD_RunSpeed * 1.5,CTHL_LianLengthCheck-(CTHL_PARAM_TDCR_LowSpeedLength),3000,CTHL_PARAM_TD_RunSpeed*2/3,15,70,600); else //尼龙 { AxisMovePosAccDecNotStop(X_AXIS,CTHL_PARAM_TD_RunSpeed * 1.3,CTHL_LianLengthCheck-(CTHL_PARAM_TDCR_LowSpeedLength),3000,CTHL_PARAM_TD_RunSpeed*2/3,12,120,850); } } } if(CTHL_FK_Check) //在这里检测到说明速度还没有起来 { CTHL_DXK_VAVLE = 0; //顶斜口气缸关 CTHL_QDXK_VAVLE = 0; if(CTHL_PARAM_NLSL_MODE) { CTHL_XKTD_VAVLE=0; } CTHL_TD_MotorDelay= dwTickCount + CTHL_PARAM_FKJDSECONG_DEALY; CTHL_LianLengthReal = dwXRealPos; CTHL_XSavePosBuff = dwXRealPos; CTHL_TD_MotorStep = 13; // user_datas[122] = CTHL_PARAM_FKDW_Length - (dwXRealPos- CTHL_CheckInX02PosBuff); if((dwXRealPos >= CTHL_CheckInX02PosBuff) && CTHL_CheckInX02PosBuff) AxisMovePosAccDec(X_AXIS,CTHL_PARAM_TD_RunSpeed,CTHL_PARAM_FKDW_Length - (dwXRealPos- CTHL_CheckInX02PosBuff), CTHL_PARAM_FKDW_Speed,CTHL_PARAM_FKDW_Speed,10,150,10); else AxisMovePosAccDec(X_AXIS,CTHL_PARAM_TD_RunSpeed,CTHL_PARAM_FKDW_Length, CTHL_PARAM_FKDW_Speed,CTHL_PARAM_FKDW_Speed,10,150,10); //CTHL_TD_MotorStep = 0; } else if(CTHL_CXSC_Limit_IN_DW) { CTHL_SetAlarmCode(CTHL_DK_ERROR_ALARM,bRunning); } else if(CTHL_CXSC_Limit_IN_UP) { AxisMovePosAccDec(X_AXIS,CTHL_PARAM_TD_RunSpeed * 2/3,CTHL_PARAM_FKDW_Length, CTHL_PARAM_FKDW_Speed,CTHL_PARAM_FKDW_Speed,10,150,10); } break; case 12://检测到过链 if(cZipCnt<2) { if(dwXRealPos >= CTHL_PARAM_WLCDSHEZHI_LENTH) { if(bRunning) { cCHULIANBAOJIN=1; SetEn(X_AXIS, MOTOR_DISEN); AxisEgmStop(X_AXIS); SetPos(X_AXIS, 0); //启动位置设为0点 // dwXRealPos=0; } CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } else { if(dwXRealPos >= CTHL_LianLengthAutoCheckERROR + CTHL_PARAM_WUCD_LENTH) { if(bRunning) { cCHULIANBAOJIN=1; SetEn(X_AXIS, MOTOR_DISEN); AxisEgmStop(X_AXIS); SetPos(X_AXIS, 0); //启动位置设为0点 // dwXRealPos=0; } CTHL_SetAlarmCode(CTHL_WLZDTJ_ALARM,bRunning); //无拉链自动停机 } } if(CTHL_FK_Check) { CTHL_TD_MotorDelay= dwTickCount + CTHL_PARAM_FKJDSECONG_DEALY; CTHL_LianLengthReal = dwXRealPos; //if(cZipCnt == 1) // user_datas[122] = dwXRealPos; // user_datas[121] = dwXRealPos; if((cZipCnt == 1) || dwXRealPos < (CTHL_LianLengthCheck*0.85) || dwXRealPos>(CTHL_LianLengthCheck*1.3)) CTHL_LianLengthCheck = dwXRealPos; CTHL_LianLengthCheckOLD=CTHL_LianLengthNEWCheck; CTHL_LianLengthNEWCheck = dwXRealPos; //break; CTHL_XSavePosBuff = dwXRealPos; CTHL_TD_MotorStep = 13; // user_datas[122] = CTHL_PARAM_FKDW_Length - (dwXRealPos- CTHL_CheckInX02PosBuff); if((dwXRealPos >= CTHL_CheckInX02PosBuff) && CTHL_CheckInX02PosBuff) AxisMovePosAccDec(X_AXIS,CTHL_PARAM_TD_RunSpeed,CTHL_PARAM_FKDW_Length - (dwXRealPos- CTHL_CheckInX02PosBuff)-50, CTHL_PARAM_FKDW_Speed,CTHL_PARAM_FKDW_Speed,10,150,40); else AxisMovePosAccDec(X_AXIS,CTHL_PARAM_TD_RunSpeed,CTHL_PARAM_FKDW_Length-50, CTHL_PARAM_FKDW_Speed,CTHL_PARAM_FKDW_Speed,10,150,40); //CTHL_TD_MotorStep = 0; } else if(CTHL_CXSC_Limit_IN_DW) { CTHL_SetAlarmCode(CTHL_DK_ERROR_ALARM,bRunning); } else if(CTHL_CXSC_Limit_IN_UP) { AxisMovePosAccDec(X_AXIS,CTHL_PARAM_TD_RunSpeed * 2/3,CTHL_PARAM_FKDW_Length, CTHL_PARAM_FKDW_Speed,CTHL_PARAM_FKDW_Speed,10,150,40); } break; case 13: if(dwTickCount >= CTHL_TD_MotorDelay) { CTHL_FKJD_VAVLE = 1; //方块夹带气缸输出 } if(CTHL_CXSC_Limit_IN_UP) { CTHL_X_CXPosBuff = dwXRealPos; } if((!X_DRV ))// || (dwXRealPos >= (CTHL_XSavePosBuff + CTHL_PARAM_FKDW_Length))) { // user_datas[123] = dwXRealPos-CTHL_CheckInX02PosBuff; AxisEgmStop(X_AXIS); CTHL_TD_MotorStep = 14; /* if(cZipCnt>2) { if(((CTHL_LianLengthNEWCheck+CTHL_LianLengthCheckOLD)*0.5) < (CTHL_LianLengthCheck*0.95)) { cZipCnt=0; cHeLian=1; } }*/ if(cZipCnt==1) CTHL_LianLengthAutoCheckERROR = dwXRealPos; if(cZipCnt==1) CTHL_LianLengSave=dwXRealPos; CTHL_LianLengthAutoCheck = dwXRealPos; user_datas[122] = dwXRealPos-CTHL_X_CXPosBuff; } break; case 14: if(CTHL_PARAM_XCX_LENTH) //0的话不判断位置 { if ((dwXRealPos - CTHL_X_CXPosBuff) > (CTHL_PARAM_XCX_LENTH + CTHL_PARAM_XCXWC_LENTH)) { // user_datas[121] = 1; CTHL_TD_MotorStep = 0; cXCXQXQD=1; CTHL_SetAlarmCode(CTHL_XCXXCYC_ALARM,bRunning); // 小插销行程异常 } else if((dwXRealPos - CTHL_X_CXPosBuff) < (CTHL_PARAM_XCX_LENTH - CTHL_PARAM_XCXWC_LENTH)) { // user_datas[121] = 2; CTHL_TD_MotorStep = 0; cXCXQXQD=1; CTHL_SetAlarmCode(CTHL_XCXXCYC_ALARM,bRunning); // 小插销行程异常 } else if(CTHL_X_CXPosBuff == 0) { // user_datas[121] = 3; CTHL_TD_MotorStep = 0; cXCXQXQD=1; CTHL_SetAlarmCode(CTHL_XCXXCYC_ALARM,bRunning); // 小插销行程异常 } else { CTHL_TD_MotorStep = 15; } } else { CTHL_TD_MotorStep = 15; } break; case 15: // if(CTHL_START_IN_UP) CTHL_TD_MotorStep = 0; } #endif } //穿入电机动作，Y轴 void CTHL_CL_Motor(void) // { #if 0 //1步开始, 穿链长度开始 //20步开始 插销定位开始 //40步,回原点为 switch(CTHL_CL_MotorStep) { //穿入长度位置 case 1: if(dwTickCount >= CTHL_CL_MotorDelay) { // SetDir(Y_AXIS, DIR_P); if(CTHL_PARAM_CL_Length >= CTHL_PARAM_CL_MAX_Length) CTHL_PARAM_CL_Length = CTHL_PARAM_CL_MAX_Length; //穿入长度要根据检测长度减动一次穿入的1CM,反转的2CM if((CTHL_LianLengthAutoCheck < (CTHL_PARAM_CL_Length + 500 )) && bRunning) AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_CL_Speed,(CTHL_LianLengthAutoCheck-500),800,600,6,6,50); else AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_CL_Speed,(CTHL_PARAM_CL_Length-100),800,600,6,6,50); CTHL_CL_MotorStep = 2; } break; case 2: if((dwYRealPos + 120) > CTHL_PARAM_CL_Length) CTHL_HLJJ_VAVLE = 0; if(!Y_DRV || dwYRealPos >= CTHL_PARAM_CL_Length) { CTHL_HLJJ_VAVLE = 0; CTHL_CL_MotorStep = 3; bCL_OK_FLG = 1; CTHL_CL_MotorDelay = dwTickCount + 40; //延时数控回零点 } break; case 3: if(dwTickCount >= CTHL_CL_MotorDelay) { CTHL_CL_MotorStep = 50; } break; //夹带插销到穿入位置 case 20: //此处最好算好定位长度相减 if(cXcxcrCnt > 0) { CTHL_LENTH = CTHL_PARAM_CXDW_Length + 30; CTHL_CL_MotorStep = 21; } else { CTHL_LENTH = CTHL_PARAM_CXDW_Length ; CTHL_CL_MotorStep = 21; } break; case 21: SetDir(Y_AXIS, DIR_P); AxisMovePosAccDec(Y_AXIS,8000,CTHL_LENTH,800,500,6,6,50); CTHL_CL_MotorStep = 22; break; case 22: if(!Y_DRV || (dwYRealPos >= CTHL_LENTH)) //插销定位已经结束 { AxisEgmStop(Y_AXIS); CTHL_YsaveCXDWPosBuff = 0; CTHL_CL_MotorStep = 0; } break; //回原点开始 case 40: //要先离开 if(CTHL_SF_Origin_IN && (GetEn(Y_AXIS) == MOTOR_DISEN)) { CTHL_CL_MotorStep = 41; SetDir(Y_AXIS, DIR_P); } else { SetDir(Y_AXIS, DIR_N); CTHL_CL_MotorStep = 46; } if(GetEn(Y_AXIS) == MOTOR_DISEN) { SetEn(Y_AXIS, MOTOR_EN); CTHL_CL_MotorDelay = dwTickCount + 260; //锁轴时间 } else //已经锁轴 CTHL_CL_MotorDelay = dwTickCount + 5; break; case 41: if(dwTickCount >= CTHL_CL_MotorDelay) { CTHL_CL_MotorStep = 42; AxisContinueMoveAcc(Y_AXIS,5000,DIR_P,1000,800,15,15); } break; case 42: if(!CTHL_SF_Origin_IN) { CTHL_CL_MotorStep = 43; CTHL_YSavePosBuff = dwYRealPos; } break; case 43: //离开原点5CM停止 if((dwYRealPos - CTHL_YSavePosBuff) >= 800) { AxisEgmStop(Y_AXIS); CTHL_CL_MotorStep = 46; //留空几步备用 CTHL_CL_MotorDelay = dwTickCount + 50; } break; case 46: if(dwTickCount >= CTHL_CL_MotorDelay) { CTHL_CL_MotorStep = 47; //检测回到原位 if(!CTHL_SF_Origin_IN) AxisContinueMoveAcc(Y_AXIS,1500,DIR_N,1000,800,15,10); } break; case 47: if(CTHL_SF_Origin_IN) { CTHL_CL_MotorStep = 0; AxisEgmStop(Y_AXIS); SetPos(Y_AXIS, 0); } break; //数控回原位 (条件为自动工作，SON一直没有松开) case 50: if(dwYRealPos > 150) { //回零速度 SetDir(Y_AXIS, DIR_N); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_CL_Back_O_Speed,-dwYRealPos + 150,5000,2000,10,50,50); } else AxisContinueMoveAcc(Y_AXIS,1000,DIR_N,1000,1000,15,15); CTHL_CL_MotorStep = 51; break; case 51: if(CTHL_SF_Origin_IN) { CTHL_CL_MotorStep = 0; AxisEgmStop(Y_AXIS); SetPos(Y_AXIS, 0); } break; } #endif #if 1 //1步开始, 穿链长度开始 //20步开始 插销定位开始 //40步,回原点为 switch(CTHL_CL_MotorStep) { //穿入长度位置 case 1: if(dwTickCount >= CTHL_CL_MotorDelay) { if(!CTHL_PARAM_AUTO_MODE) { //穿入长度要根据检测长度减动一次穿入的1CM,反转的2CM if(CTHL_LianLengthAutoCheck <= 700) { CTHL_Y_CL_Length=CTHL_LianLengthAutoCheck-200; if(CTHL_Y_CL_Length<10) CTHL_Y_CL_Length=200; if(CTHL_Y_CL_Length >= CTHL_PARAM_CL_MAX_Length) CTHL_Y_CL_Length = CTHL_PARAM_CL_MAX_Length-40; AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_CL_Speed,CTHL_Y_CL_Length,800,600,6,6,50); } else if(CTHL_LianLengthAutoCheck>700) { user_datas[123] =CTHL_LianLengthAutoCheck; if(cZipCnt>1) CTHL_Y_CL_Length=CTHL_LianLengSave-CTHL_PARAM_HLTSY_LENTH-300; else { if(cZipCnt<1) CTHL_Y_CL_Length=CTHL_LianLengthAutoCheck-CTHL_PARAM_HLTSY_LENTH-100; else CTHL_Y_CL_Length=CTHL_LianLengthAutoCheck-CTHL_PARAM_HLTSY_LENTH-300; } if(cZipCnt>1) { if(CTHL_LianLengthAutoCheck<(CTHL_LianLengSave*0.7)) { CTHL_Y_CL_Length=CTHL_LianLengthAutoCheck-CTHL_PARAM_HLTSY_LENTH-300; cZipCnt=0; } } if(CTHL_Y_CL_Length<200) CTHL_Y_CL_Length=200; if(CTHL_Y_CL_Length >= CTHL_PARAM_CL_MAX_Length) CTHL_Y_CL_Length = CTHL_PARAM_CL_MAX_Length-40; AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_CL_Speed,CTHL_Y_CL_Length,800,600,6,6,50); } } else { if(CTHL_PARAM_CL_Length >= CTHL_PARAM_CL_MAX_Length) CTHL_PARAM_CL_Length = CTHL_PARAM_CL_MAX_Length; //穿入长度要根据检测长度减动一次穿入的1CM,反转的2CM if((CTHL_LianLengthAutoCheck < (CTHL_PARAM_CL_Length + 500 )) && bRunning) AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_CL_Speed,(CTHL_LianLengthAutoCheck-500),800,600,6,6,50); else AxisMovePosAccDec(Y_AXIS,CTHL_PARAM_CL_Speed,(CTHL_PARAM_CL_Length-100),800,600,6,6,50); } CTHL_CL_MotorStep = 2; } break; case 2: if(CTHL_PARAM_AUTO_MODE) { if((dwYRealPos + 120) > CTHL_PARAM_CL_Length) CTHL_HLJJ_VAVLE = 0; if(!Y_DRV || dwYRealPos >= CTHL_PARAM_CL_Length) { CTHL_HLJJ_VAVLE = 0; CTHL_CL_MotorStep = 3; bCL_OK_FLG = 1; CTHL_CL_MotorDelay = dwTickCount + 30; //延时数控回零点 } } else { if((dwYRealPos + 120) > CTHL_Y_CL_Length) CTHL_HLJJ_VAVLE = 0; if(!Y_DRV || dwYRealPos >= CTHL_Y_CL_Length) { CTHL_HLJJ_VAVLE = 0; CTHL_CL_MotorStep = 3; bCL_OK_FLG = 1; CTHL_CL_MotorDelay = dwTickCount + 30; //延时数控回零点 } } break; case 3: if(dwTickCount >= CTHL_CL_MotorDelay) { CTHL_CL_MotorStep = 50; } break; //夹带插销到穿入位置 case 20: //此处最好算好定位长度相减 if(cXcxcrCnt > 0) { if(cXcxcrCnt==1) CTHL_LENTH = CTHL_PARAM_CXDW_Length+CTHL_PARAM_TDFZ_LENTH + 50; if(cXcxcrCnt>1) CTHL_LENTH = CTHL_PARAM_CXDW_Length +CTHL_PARAM_TDFZ_LENTH + 120; CTHL_CL_MotorStep = 21; } else { CTHL_LENTH = CTHL_PARAM_CXDW_Length ; CTHL_CL_MotorStep = 21; } break; case 21: SetDir(Y_AXIS, DIR_P); CTHL_Y_CL_PosBuff = dwYRealPos; AxisMovePosAccDec(Y_AXIS,20000,CTHL_LENTH,800,500,6,6,50); CTHL_CL_MotorStep = 22; break; case 22: if(dwYRealPos >= CTHL_Y_CL_PosBuff+CTHL_LENTH-CTHL_PARAM_CXDW_Length) //插销定位已经结束 { CTHL_CXSCDW_VAVLE = 1; } if(!Y_DRV || (dwYRealPos >= CTHL_Y_CL_PosBuff+CTHL_LENTH)) //插销定位已经结束 { AxisEgmStop(Y_AXIS); CTHL_YsaveCXDWPosBuff = 0; CTHL_CL_MotorStep = 0; } break; case 30: CTHL_YsaveCXDWPosBuff = 0; CTHL_CL_MotorStep = 0; break; //回原点开始 case 40: //要先离开 if(CTHL_SF_Origin_IN && (GetEn(Y_AXIS) == MOTOR_DISEN)) { CTHL_CL_MotorStep = 41; SetDir(Y_AXIS, DIR_P); } else { SetDir(Y_AXIS, DIR_N); CTHL_CL_MotorStep = 46; } if(GetEn(Y_AXIS) == MOTOR_DISEN) { SetEn(Y_AXIS, MOTOR_EN); CTHL_CL_MotorDelay = dwTickCount + 260; //锁轴时间 } else //已经锁轴 CTHL_CL_MotorDelay = dwTickCount + 5; break; case 41: if(dwTickCount >= CTHL_CL_MotorDelay) { CTHL_CL_MotorStep = 42; AxisContinueMoveAcc(Y_AXIS,5000,DIR_P,1000,800,15,15); } break; case 42: if(!CTHL_SF_Origin_IN) { CTHL_CL_MotorStep = 43; CTHL_YSavePosBuff = dwYRealPos; } break; case 43: //离开原点5CM停止 if((dwYRealPos - CTHL_YSavePosBuff) >= 800) { AxisEgmStop(Y_AXIS); CTHL_CL_MotorStep = 46; //留空几步备用 CTHL_CL_MotorDelay = dwTickCount + 50; } break; case 46: if(dwTickCount >= CTHL_CL_MotorDelay) { CTHL_CL_MotorStep = 47; //检测回到原位 if(!CTHL_SF_Origin_IN) AxisContinueMoveAcc(Y_AXIS,1500,DIR_N,1000,800,15,10); } break; case 47: if(CTHL_SF_Origin_IN) { CTHL_CL_MotorStep = 0; AxisEgmStop(Y_AXIS); SetPos(Y_AXIS, 0); } break; //数控回原位 (条件为自动工作，SON一直没有松开) case 50: if(dwYRealPos > 151) { //回零速度 SetDir(Y_AXIS, DIR_N); AxisMovePosAccDecNotStop(Y_AXIS,CTHL_PARAM_CL_Back_O_Speed,-dwYRealPos + 150,5000,2000,10,50,100); } else AxisContinueMoveAcc(Y_AXIS,1000,DIR_N,1000,1000,15,15); CTHL_CL_MotorStep = 51; break; case 51: if(CTHL_SF_Origin_IN) { CTHL_CL_MotorStep = 0; AxisEgmStop(Y_AXIS); SetPos(Y_AXIS, 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(CTHL_CTM_Origin_IN && !CTHL_CTXM_VAVLE)//下模没有输出、下模在原位。 { CTHL_First_CT_VAVLE = 0;//？ CTHL_JLTou_VAVLE = 0;//夹拉头松开 CTHL_MGuo_VAVLE = 0; //码钩松开 if(CTHL_TLiaoStep == 0)//执行推料步骤动作 { CTHL_TLiaoStep = 1; CTHL_ZhuangLiaoStep = 2; } } else if(dwTickCount >= CTHL_ZhuangLiaoDelay + 500) //下模原位异常 { CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning); } } else //已经有拉头直接跳到结束 { CTHL_ZhuangLiaoDelay = dwTickCount + 40; CTHL_ZhuangLiaoStep = 9; } } break; case 2: //等待推料完成 if(CTHL_TLiaoStep == 0) //推料已经完成 { CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; CTHL_JLTou_VAVLE = 0;//夹拉头送开 CTHL_MGuo_VAVLE = 0; //码钩松开 if(!CTHL_JLiao_Limit_IN && CTHL_First_CT_Origin_IN) { CTHL_ZhuangLiaoStep = 3; CTHL_HSLiao_VAVLE = 1; if(CTHL_PARAM_LTJJ_MODE) { CTHL_LTJJ_VAVLE=1; } else CTHL_LTJJ_VAVLE=0; } } break; case 3: //横送到位 if(CTHL_HSLiao_Limit_IN && !CTHL_HSLiao_Origin_IN) { CTHL_ZhuangLiaoStep = 4; // CTHL_JLTou_VAVLE = 1; //接拉头输出,不需要时间 CTHL_ZhuangLiaoDelay = dwTickCount + 5; } else if(dwTickCount >= CTHL_ZhuangLiaoDelay) { CTHL_SetAlarmCode(CTHL_HSLiao_Limit_ALARM,bRunning); } 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_LTJJ_VAVLE=0; CTHL_ZhuangLiaoStep = 6; if(CTHL_PARAM_LTJJ_MODE) { CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_MaGou_DELAY+30; } else { CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_MaGou_DELAY; } } else if(dwTickCount >= CTHL_ZhuangLiaoDelay) //接拉头(接料)到们信号异常 { CTHL_SetAlarmCode(CTHL_JLiao_Limit_ALARM,bRunning); } break; case 6: if(dwTickCount >= CTHL_ZhuangLiaoDelay) //延时码勾 { if(!CTHL_PARAM_LTJJ_MODE || CTHL_LTJJ_Origin_IN) { CTHL_MGuo_VAVLE = 1; //码勾输出 CTHL_ZhuangLiaoStep = 7; CTHL_ZhuangLiaoDelay = dwTickCount + CTHL_PARAM_HSLiaoOff_DELAY; bTuiLaTouOkFlg = 0; //认为拉头没法带出 } else if(dwTickCount >= CTHL_ZhuangLiaoDelay+3000) { CTHL_SetAlarmCode(CTHL_XMKH_Origin_ALARM,bRunning); } } 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 >= CTHL_PARAM_SLT_NUMBER) { CTHL_TryCnt = 0; bZhuangLiaoOkFlg = 0; CTHL_ZhuangLiaoStep = 0; CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning); cWULATOUCnt=1; } else { CTHL_ZhuangLiaoStep = 20; //重新送3次 CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; } } else if(dwTickCount >= CTHL_ZhuangLiaoDelay) { 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 >= 3) { CTHL_TryCnt=0; bZhuangLiaoOkFlg = 0; CTHL_ZhuangLiaoStep = 0; CTHL_SetAlarmCode(CTHL_ZLT_ALARM,bRunning); cWULATOUCnt=1; } else { CTHL_ZhuangLiaoStep = 20; //重新送3次 CTHL_ZhuangLiaoDelay = dwTickCount + QDCT_VAVLE_ERROR_TIME; } } else if(dwTickCount >= CTHL_ZhuangLiaoDelay) { bZhuangLiaoOkFlg = 1; CTHL_ZhuangLiaoStep = 0; } break; //装拉头不成攻重新送拉头 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_SetAlarmCode(CTHL_HSLiao_Origin_ALARM,bRunning); } break; } } 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_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_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_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) // { /* switch(CTHL_PARAM_ZDP_AUTO) { case 0: 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; } break; case 1: CTHL_ZhenDongPian_OUT=0; CTHL_ZDP_Delay = dwTickCount; break; case 2: CTHL_ZhenDongPian_OUT=1; CTHL_ZDP_Delay = dwTickCount; break; } } else CTHL_DaLaPian_OUT = 0; */ if(CTHL_ZDP_IN) { if(CTHL_PARAM_LaPian_EN) { if(dwTickCount >= CTHL_DLP_Delay) { CTHL_DaLaPian_OUT = !CTHL_DaLaPian_OUT; if(CTHL_DaLaPian_OUT) CTHL_DLP_Delay = dwTickCount + CTHL_PARAM_DaLaPian; else CTHL_DLP_Delay = dwTickCount + CTHL_PARAM_DaLaPian+50; } } } else CTHL_DaLaPian_OUT = 0; #endif } void CTHL_bFKCXTDDWStep(void) { switch(CTHL_bFKCXTDDW_Step) { case 1: CTHL_FKJD_VAVLE = 1; //方块夹带气缸输出 CTHL_XKTD_VAVLE = 1; //斜口推带气缸输出 CTHL_FKGD_VAVLE = 0; //方块固定关 CTHL_bFKCXTDDW_Step = 2; break; case 2: if(!CTHL_XKTD_Origin_IN) //斜口推带原位离开 { CTHL_bFKCXTDDW_Step = 3; CTHL_bFKCXTDDW_Delay = dwTickCount + 20; } break; case 3://斜口定位 if(dwTickCount >= CTHL_bFKCXTDDW_Delay) { CTHL_XKDW_VAVLE = 1; CTHL_bFKCXTDDW_Step = 4; // CTHL_bFKCXTDDW_Delay = dwTickCount + CTHL_PARAM_XKDW_TD_DELAY; } break; case 4: if(dwTickCount >= CTHL_bFKCXTDDW_Delay) { AxisContinueMoveAcc(X_AXIS,1200,DIR_P,800,600,10,10);//拖带电机 CTHL_bFKCXTDDW_Step = 5; CTHL_XSavePosBuff = dwXRealPos; SetPos(X_AXIS, 0); //启动位置设为0点 dwXRealPos = 0; } break; case 5: if(CTHL_XKSC_Limit_IN) //方块到位 { AxisEgmStop(X_AXIS); CTHL_QDXK_VAVLE=1; CTHL_bFKCXTDDW_Step = 6; CTHL_XKTD_VAVLE = 0; //斜口推带气缸关闭 CTHL_bFKCXTDDW_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } else if(dwXRealPos >= (CTHL_XSavePosBuff + 800)) { CTHL_SetAlarmCode(CTHL_FKCXDW_ALARM,bRunning);//方块插销定位异常警告 } break; case 6: if(CTHL_PARAM_TDFZ_LENTH && !X_DRV) //拖带电机反转 AxisMovePosAccDec(X_AXIS,8000,-CTHL_PARAM_DXKFZ_LENTH,800,600,6,6,0); if(CTHL_PARAM_CXMODE) CTHL_CXVAVLE = 1; CTHL_bFKCXTDDW_Step = 0; break; } } //插销穿入动作 void CTHL_bCXCRStep(void) { switch(CTHL_bCXCR_Step) { case 11: CTHL_XKTD_VAVLE = 1; //斜口推带气缸输出 CTHL_FKGD_VAVLE = 0; //方块固定关 CTHL_CXSCDW_VAVLE=1; CTHL_bCXCR_Step = 12; break; case 12: if(!CTHL_XKTD_Origin_IN) //斜口推带原位离开 { CTHL_bCXCR_Step = 13; // CTHL_CXQJD_VAVLE = 1; //插销前夹带 CTHL_bCXCR_Delay = dwTickCount + 20; } break; case 13: if((CTHL_CL_MotorStep == 0) && (CTHL_SF_Origin_IN) && (dwTickCount >= CTHL_bCXCR_Delay)) //插销前夹带回到原位 { CTHL_bCXCR_Step = 14; // CTHL_FKGD_VAVLE = 1; //方块固定开 CTHL_CXQJD_VAVLE = 1; //插销前夹带 CTHL_bCXCR_Delay = dwTickCount + CTHL_PARAM_XCQJD_XCDW_DELAY; } break; case 14: if(dwTickCount >= CTHL_bCXCR_Delay) { CTHL_bCXCR_Step = 15; CTHL_FKGD_VAVLE = 1; //方块固定 //Y轴启动插销 CTHL_CL_MotorStep = 20; //插销定位 } break; case 15: if(CTHL_CL_MotorStep == 0) //插销定位完成 { CTHL_bCXCR_Step = 16; CTHL_CXDWZ_VAVLE = 1; //插销定位针(到位)电磁阀 CTHL_bCXCR_Delay = dwTickCount + 10; //延时插销下移 } break; case 16: if(dwTickCount >= CTHL_bCXCR_Delay) { CTHL_bCXCR_Step = 17; if(CTHL_PARAM_CXMODE) CTHL_CXVAVLE = 0; else CTHL_CXVAVLE = 1; //Y06 插销下移电磁阀 if(CTHL_PARAM_NLSL_MODE == 0) CTHL_bCXCR_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME; //插销下移异常时间 else CTHL_bCXCR_Delay = dwTickCount + 80; //插销下移异常时间 } break; case 17: if((CTHL_CXLimit_IN) || CTHL_PARAM_NLSL_MODE) { if(CTHL_CXLimit_IN || (dwTickCount >= CTHL_bCXCR_Delay)) { CTHL_bCXCR_Step = 18; if( cXcxcrCnt==0 && !X_DRV) //拖带电机反转 { AxisMovePosAccDec(X_AXIS,10000,-CTHL_PARAM_TDFZ_LENTH,800,600,6,6,0); } CTHL_CXQJD_VAVLE = 0; //插销前夹带电磁阀 CTHL_CXSCDW_VAVLE = 0; //Y21 插销上止定位关 CTHL_CXHJD_VAVLE = 1; //Y14 插销后夹带输出 CTHL_bCXCR_Delay = dwTickCount + CTHL_PARAM_SCDWOff_CXDW_DELAY; } } else if(dwTickCount >= CTHL_bCXCR_Delay) { CTHL_SetAlarmCode(CTHL_CXLimit_ALARM,bRunning);//插销下移异常 } break; case 18: // if(START_IN_UP) { if(dwTickCount >= CTHL_bCXCR_Delay) { CTHL_bCXCR_Step = 19; CTHL_CXHDW_VAVLE = 1; //插销后定位(插销穿入) CTHL_bCXCR_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME +1000; //插销穿入到位异常时间 CTHL_CL_MotorStep = 50; //合链电机回原点 } } break; case 19: if( cXcxcrCnt > 1) { CTHL_SetAlarmCode(CTHL_CX_Limit_ALARM,bRunning); //插销穿入到位异常 } if( CTHL_CX_Limit_IN && cXcxcrCnt<2 ) { cXcxcrCnt=0; bZhuangLiaoOkFlg = 0; CTHL_bCXCR_Step = 0; CTHL_CXDWZ_VAVLE = 0; //插销定位针(到位)电磁阀 } else if (dwTickCount >= CTHL_bCXCR_Delay) { cXcxcrCnt++; CTHL_CXHJD_VAVLE=0; CTHL_CXHDW_VAVLE=0; CTHL_CXDWZ_VAVLE = 0; if(CTHL_PARAM_CXMODE) CTHL_CXVAVLE = 0; else CTHL_CXVAVLE = 1; //Y06 插销下移电磁阀 CTHL_bCXCR_Step = 11; } break; } } void CTHL_bCLFirstStep(void) { switch(CTHL_bCLFirst_Step) { case 19: CTHL_JLTou_VAVLE = 0; //接拉头 bZhuangLiaoOkFlg = 0; CTHL_bCLFirst_Step = 20; CTHL_CXDWZ_VAVLE = 0; //插销定位针(到位)电磁阀 CTHL_CXVAVLE = 0; //Y06 插销下移电磁阀关 CTHL_bCLFirst_Delay = dwTickCount + CTHL_PARAM_First_CL_DELAY;//延时一次穿入 break; case 20: if(dwTickCount >= CTHL_bCLFirst_Delay) { CTHL_bCLFirst_Step = 21; CTHL_First_CT_VAVLE = 1; //一次穿入电磁阀输出 CTHL_bCLFirst_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } break; case 21: if(!CTHL_First_CT_Origin_IN) //原点离开后才开始时间 { CTHL_bCLFirst_Step = 22; CTHL_bCLFirst_Delay = dwTickCount + CTHL_PARAM_First_CL_Timer; //一次穿入时间 } else if(dwTickCount >= CTHL_bCLFirst_Delay) { CTHL_SetAlarmCode(CTHL_YCCRYWYC_ALARM,bRunning); //一次穿入原位异常 } break; case 22: if(dwTickCount >= CTHL_bCLFirst_Delay) { CTHL_bCLFirst_Step = 23; CTHL_MGuo_VAVLE = 0; //码勾退 CTHL_bCLFirst_Delay = dwTickCount + CTHL_PARAM_MGOff_XiaMo_DELAY; CTHL_FKGD_VAVLE = 0; //方块固定退 CTHL_CXHJD_VAVLE = 0; //Y14 插销后夹带打开 CTHL_CXVAVLE = 0; //Y06 插销下移电磁阀关 } break; case 23: if(dwTickCount >= CTHL_bCLFirst_Delay) { CTHL_bCLFirst_Step = 24; CTHL_CTXM_VAVLE = 0; //穿头下模下降 CTHL_CXHDW_VAVLE = 0; //插销后定位(插销穿入)关闭 CTHL_bCLFirst_Delay = dwTickCount + CTHL_VAVLE_ERROR_TIME; } break; case 24: if(!CTHL_CTM_Limit_IN) { CTHL_bCLFirst_Step= 25; CTHL_bCLFirst_Delay = dwTickCount + 25; //一次穿入延时返回 } else if(dwTickCount >= CTHL_bCLFirst_Delay) { CTHL_SetAlarmCode(CTHL_CTM_Origin_ALARM,bRunning); //穿头下模下降异常 } break; case 25: if(dwTickCount >= CTHL_bCLFirst_Delay) { CTHL_bCLFirst_Step= 0; CTHL_First_CT_VAVLE = 0; //一次穿入电磁阀关 CTHL_CXHDW_VAVLE = 0; //插销后定位(插销穿入)关闭 //退下出错警告 CTHL_AutoDelay1 = dwTickCount + CTHL_VAVLE_ERROR_TIME; CTHL_AutoStep1 = 1; bTuiLaTouOkFlg = 1; } break; } } //手动动作 void CTHL_ManualAction(void) { if(bRunning && user_datas[100]==0) // 运行灯输出 { CTHL_Run_State=1; CTHL_Stop_State=0; } else if(!bRunning && user_datas[100]==0) { CTHL_Run_State=0; CTHL_Stop_State=0; } if(user_datas[100] != 0) { CTHL_Run_State=0; CTHL_Stop_State=1; } if( (GetAlarm(Y_AXIS)==1) ) // 伺服报警 { // cCHULIANBAOJIN=1; // CTHL_SetAlarmCode(CTHL_SF_ALR_ALARM,0); } if(dwTickCount >= CTHL_bSFBBAOJIN_Delay) { SetClr(Y_AXIS,MOTOR_ALARM); SetClr(X_AXIS,MOTOR_ALARM); } if(bRunning == 0) { if(bClearTotal) //切断计数清零 { bClearTotal = 0; ClrcToTal(QDCT_TOTAL_ADDR); } 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_HENSONGLIAO_ALARM,0); // 横送料不能输出 } } if(CTHL_bHSL) //手动横送料 { CTHL_bHSL= 0; if(!CTHL_First_CT_VAVLE) { if(CTHL_HSLiao_VAVLE) { CTHL_HSLiao_VAVLE = 0;//~CTHL_HSLiao_VAVLE; } else if(!CTHL_TLiao_VAVLE && !CTHL_TLiao_Limit_IN && CTHL_CTM_Origin_IN && !CTHL_JLiao_Limit_IN && !CTHL_JLTou_VAVLE && !CTHL_CTXM_VAVLE && !CTHL_CXDWZ_VAVLE && (!CTHL_bMG || (CTHL_bMG && CTHL_LTou_Check))) { if(CTHL_PARAM_LTJJ_MODE) { CTHL_LTJJ_VAVLE=1; } else { CTHL_LTJJ_VAVLE=0; } CTHL_HSLiao_VAVLE = 1; } else //条件警告 最后一个括号条件为已经有拉头 { CTHL_SetAlarmCode(CTHL_YCCT_ALARM,0); } } } if(CTHL_bJLT) //接拉头 { CTHL_bJLT = 0; if(CTHL_JLTou_VAVLE) CTHL_JLTou_VAVLE = 0; else if(!CTHL_First_CT_VAVLE && !CTHL_CTXM_VAVLE && CTHL_CTM_Origin_IN && CTHL_First_CT_Origin_IN) { CTHL_MGuo_VAVLE = 0; CTHL_JLTou_VAVLE = 1; } else//条件警告 { CTHL_SetAlarmCode(CTHL_YCXM_Origin_ALARM,0); } } if(CTHL_bMG) //码勾 { CTHL_bMG = 0; CTHL_MGuo_VAVLE = !CTHL_MGuo_VAVLE; } if(CTHL_bLTJJ) //码勾 { CTHL_bLTJJ = 0; CTHL_LTJJ_VAVLE = !CTHL_LTJJ_VAVLE; } if(CTHL_bFKJD) //方块夹带 { CTHL_bFKJD = 0; CTHL_FKJD_VAVLE = !CTHL_FKJD_VAVLE; } if(CTHL_bXKTD) //斜口推带 { CTHL_bXKTD = 0; if(CTHL_XKTD_VAVLE) CTHL_XKTD_VAVLE = 0; else if(!CTHL_DXK_VAVLE && (!CTHL_XKSC_Limit_IN || !CTHL_CTXM_VAVLE|| CTHL_PARAM_NLSL_MODE)) CTHL_XKTD_VAVLE = 1; else//条件警告 { CTHL_SetAlarmCode(CTHL_DXKCTXM_Origin_ALARM,0); } } if(CTHL_bXKDW) //斜口定位 { CTHL_bXKDW = 0; CTHL_XKDW_VAVLE = ~CTHL_XKDW_VAVLE; } if(CTHL_bCTXM) //穿头下模 { CTHL_bCTXM = 0; if(CTHL_CTXM_VAVLE) { if(CTHL_PARAM_LTJJ_MODE) { CTHL_LTJJ_VAVLE=1; } else { CTHL_LTJJ_VAVLE=0; } CTHL_CTXM_VAVLE = 0; CTHL_MGuo_VAVLE = 0; CTHL_JLTou_VAVLE = 0; CTHL_CXVAVLE=0; } else if(!CTHL_HSLiao_VAVLE && CTHL_HSLiao_Origin_IN && (!CTHL_XKSC_Limit_IN || !CTHL_XKTD_VAVLE)) { if(CTHL_PARAM_CXMODE) CTHL_CXVAVLE = 1; CTHL_MGuo_VAVLE = 1; CTHL_CTXM_VAVLE = 1; CTHL_LTJJ_VAVLE=0; } } if(CTHL_bQianDXK) //前顶斜口 { CTHL_bQianDXK = 0; if(CTHL_QDXK_VAVLE) CTHL_QDXK_VAVLE = 0; else if(!CTHL_XKTD_VAVLE) CTHL_QDXK_VAVLE = 1; else //条件警告 { CTHL_SetAlarmCode(CTHL_XKTD_ALARM,0); } } if(CTHL_bDXK) //顶斜口 { CTHL_bDXK = 0; if(!CTHL_PARAM_NLSL_MODE) { if(CTHL_DXK_VAVLE) CTHL_DXK_VAVLE = 0; else if(!CTHL_XKTD_VAVLE) { if(!CTHL_PARAM_NLSL_MODE) CTHL_DXK_VAVLE = 1; } else //条件警告 { CTHL_SetAlarmCode(CTHL_XKTD_ALARM,0); } } else { if(CTHL_XKTD_VAVLE) CTHL_XKTD_VAVLE = 0; else if(!CTHL_DXK_VAVLE ) { if(!CTHL_XKSC_Limit_IN || !CTHL_CTXM_VAVLE || CTHL_PARAM_NLSL_MODE) CTHL_XKTD_VAVLE = 1; else { CTHL_SetAlarmCode(CTHL_YCXM_Origin_ALARM,0); } } else//条件警告 { CTHL_SetAlarmCode(CTHL_DXKCTXM_Origin_ALARM,0); } } } if(CTHL_bFKGD) //方块固定 { CTHL_bFKGD = 0; CTHL_FKGD_VAVLE = ~CTHL_FKGD_VAVLE; } if(CTHL_bCXQJD) //插销前夹带 { CTHL_bCXQJD = 0; CTHL_CXQJD_VAVLE = ~CTHL_CXQJD_VAVLE; } if(CTHL_bCXSY) //插销下移电磁阀 { CTHL_bCXSY = 0; CTHL_CXVAVLE = !CTHL_CXVAVLE; } if(CTHL_bCXDWZ) //Y14 插销定位针电磁阀 { CTHL_bCXDWZ = 0; if(CTHL_CXDWZ_VAVLE) CTHL_CXDWZ_VAVLE = 0; else if(CTHL_HSLiao_Origin_IN) { CTHL_CXDWZ_VAVLE = 1; } else //条件警告 { CTHL_SetAlarmCode(CTHL_HENSONGLIAO_ALARM,0); } } if(CTHL_bCXHJD) //Y15 插销后夹带电磁阀 { CTHL_bCXHJD = 0; CTHL_CXHJD_VAVLE =~CTHL_CXHJD_VAVLE; } if(CTHL_bCXHDW) //Y16 插销后定位电磁阀 { CTHL_bCXHDW = 0; if(CTHL_CXHDW_VAVLE) CTHL_CXHDW_VAVLE = 0; else if(!CTHL_CXQJD_VAVLE) CTHL_CXHDW_VAVLE = 1; else//条件警告 { CTHL_SetAlarmCode(CTHL_CXQJD_ALARM,0); } } if(CTHL_bFirst_CT) //Y20 一次穿头电磁阀 { CTHL_bFirst_CT = 0; CTHL_First_CT_VAVLE = ~CTHL_First_CT_VAVLE; } if(CTHL_bHLJJ) //Y21 合链夹具电磁阀 { CTHL_bHLJJ = 0; CTHL_HLJJ_VAVLE = ~CTHL_HLJJ_VAVLE; } if(CTHL_bCXSCDW) //Y23 插销上止定位电磁阀 { CTHL_bCXSCDW = 0; CTHL_CXSCDW_VAVLE = ~CTHL_CXSCDW_VAVLE; } //自动装料 if(CTHL_bAutoZhuangLiao) { CTHL_bAutoZhuangLiao = 0; if(CTHL_ZhuangLiaoStep == 0) { bTuiLaTouOkFlg = 1; bZhuangLiaoOkFlg = 0; CTHL_ZhuangLiaoStep = 1; CTHL_MGuo_VAVLE = 1; //判断是否已经有料 CTHL_LTJJ_VAVLE=0; CTHL_ZhuangLiaoDelay = dwTickCount + 150; } } //测试电机 if(CTHL_bCLMotor_N) { SetEn(Y_AXIS, MOTOR_EN); if(!CTHL_SF_Origin_IN) { if(!Y_DRV) // Y轴 运行速度 启动速度 加速度 减速度 AxisContinueMoveAcc(Y_AXIS,2000,DIR_N,1000,1000,15,15); } else { CTHL_bCLMotor_N = 0; } } if(CTHL_bCLMotor_P) //后退限位已经取消 { SetEn(Y_AXIS, MOTOR_EN); if(!Y_DRV) // Y轴 运行速度 启动速度 加速度 减速度 AxisContinueMoveAcc(Y_AXIS,2000,DIR_P,1000,1000,15,15); } if(!CTHL_bCLMotor_P && !CTHL_bCLMotor_N && (CTHL_CL_MotorStep == 0)) { AxisEgmStop(Y_AXIS);//AxisDecStop(Y_AXIS); } if(CTHL_bTDMotor_P) //后退限位已经取消 { SetEn(X_AXIS, MOTOR_EN); if(!X_DRV) // Y轴 运行速度 启动速度 加速度 减速度 AxisContinueMoveAcc(X_AXIS,2000,DIR_P,1000,1000,15,15); } if(CTHL_bTDMotor_N) //后退限位已经取消 { SetEn(X_AXIS, MOTOR_EN); if(!X_DRV) // Y轴 运行速度 启动速度 加速度 减速度 AxisContinueMoveAcc(X_AXIS,2000,DIR_N,1000,1000,15,15); } if(!CTHL_bTDMotor_P && !CTHL_bTDMotor_N && (CTHL_TD_MotorStep == 0)) { AxisEgmStop(X_AXIS);//AxisDecStop(Y_AXIS); } //合链电机返回原点 if(CTHL_bCLMotor_O) { CTHL_bCLMotor_O = 0; if(CTHL_CL_MotorStep == 0) CTHL_CL_MotorStep = 40; } //拖带方块定位 ,因这步为前拖带定位, 很多电磁阀要关掉 if(CTHL_bFKTDDW) { CTHL_bFKTDDW = 0; if(CTHL_TD_MotorStep == 0) { CTHL_XKDW_VAVLE = 0; //斜口定位 CTHL_CXQJD_VAVLE = 0; //插销前夹带 if(!CTHL_PARAM_CXMODE) CTHL_CXVAVLE = 0; //插销下移 CTHL_CXHJD_VAVLE = 0; //插销后夹带 CTHL_CXHDW_VAVLE = 0; //插销后定位 CTHL_CXDWZ_VAVLE = 0; //插销定位针 CTHL_bHLJJ = 0; //合链夹具 CTHL_FKJD_VAVLE = 0; //方块夹带 if((GetEn(X_AXIS) == MOTOR_DISEN)) { SetEn(X_AXIS, MOTOR_EN); CTHL_TD_MotorDelay = dwTickCount + 200; } else CTHL_TD_MotorDelay = dwTickCount + 50; CTHL_TD_MotorStep = 1; } } //拖带方块插销定位(斜口推带，斜口定位，顶斜口) if(CTHL_bFKCXTDDW) { CTHL_bFKCXTDDW = 0; if((CTHL_bFKCXTDDW_Step == 0) && (CTHL_CL_MotorStep == 0) && !CTHL_CTXM_VAVLE) { CTHL_bFKCXTDDW_Step = 1; if(GetEn(X_AXIS)==MOTOR_DISEN) { SetEn(X_AXIS, MOTOR_EN); CTHL_bFKCXTDDW_Delay = dwTickCount + 300; } CTHL_CXQJD_VAVLE = 0; //插销前夹带 CTHL_CXVAVLE = 0; //插销下移 CTHL_CXHJD_VAVLE = 0; //插销后夹带 CTHL_CXHDW_VAVLE = 0; //插销后定位 CTHL_CXDWZ_VAVLE = 0; //插销定位针 CTHL_HLJJ_VAVLE = 0; //合链夹具 } } //插销穿入(包含电机前定位,插销下移,后夹,后定位) if(CTHL_bCXCR) { CTHL_bCXCR = 0; //拉头模在上面到们并且码勾输出,判断有拉头 if(CTHL_CTXM_VAVLE && CTHL_MGuo_VAVLE && !CTHL_CXQJD_VAVLE && CTHL_CTM_Limit_IN && !CTHL_LTou_Check) { if(CTHL_CL_MotorStep == 0) { CTHL_bCXCR_Step = 11; SetEn(Y_AXIS, MOTOR_EN); if(!CTHL_SF_Origin_IN) CTHL_CL_MotorStep = 40; } } else //警告不能自动穿插销 { CTHL_SetAlarmCode(CTHL_WFZIDONGCX_ALARM,0); } } //电机合链动作 if(CTHL_bMotorHL) { CTHL_bMotorHL = 0; if(CTHL_SF_Origin_IN) { if((CTHL_CL_MotorStep == 0) && !CTHL_CTXM_VAVLE && (!CTHL_CTM_Limit_IN)) { bCL_OK_FLG = 0; CTHL_HLJJ_VAVLE = 1; //Y20 合链夹具电磁阀输出后,电机穿入 CTHL_CL_MotorStep = 1; CTHL_CL_MotorDelay = dwTickCount + CTHL_PARAM_HLJJ_CL_DELAY + 130; if((GetEn(Y_AXIS) == MOTOR_DISEN)) { SetEn(Y_AXIS, MOTOR_EN); CTHL_CL_MotorDelay = dwTickCount + 250; } } } else { CTHL_bCLMotor_O =1; } } //一次穿链动作 if(CTHL_bCL_First) { CTHL_bCL_First = 0; //拉头模在上面到们并且码勾输出,判断有拉头 if(CTHL_CTXM_VAVLE && CTHL_MGuo_VAVLE && !CTHL_CXQJD_VAVLE && CTHL_CTM_Limit_IN && !CTHL_LTou_Check && (CTHL_CL_MotorStep == 0)) { CTHL_bCLFirst_Step = 19; CTHL_CXHJD_VAVLE = 1; //插销后夹带 CTHL_CXDWZ_VAVLE = 0; //插销定位针(到位)电磁阀 CTHL_CXQJD_VAVLE = 0; //插销前夹带 } } } } #endif