diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c index ec48a38..d399277 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c @@ -160,7 +160,7 @@ void IO_Control() uint8_t IsAllowRotation_AutoMode = 0; int32_t knife_descent_height = 0; // 自动作业停止后默认上升高度为5mm -uint8_t knife_rising_height = 5; +int32_t knife_rising_height = 500; int32_t knife_current_altitude = 0; uint16_t delay_counts = 0; uint8_t auto_work_mode_states = 5; @@ -168,7 +168,7 @@ uint8_t auto_work_halt_states = 10; void Automatic_Operation() { - knife_descent_height = GV.PV.knife_descending_height; + knife_descent_height = GV.PV.knife_descending_height * 100; if(GV.MK32_Key.CH7_SD == -1000) { @@ -180,35 +180,23 @@ void Automatic_Operation() auto_work_halt_states = 10; break; case 10: - CurrentFrontEndState = Manual_DOWN_STATE; IsAllowRotation_AutoMode = Knife_Detection(); if(IsAllowRotation_AutoMode == 1) { + CurrentFrontEndState = MANUAL_LOW_SPEED_DOWN_STATE; StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_MOTION_STATE; + if(knife_current_altitude - GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance + >= knife_descent_height) + { + CurrentFrontEndState = HALT_STATE; + CurrentMoveState = AUTO_FORWARD; + } } else{ - StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; - } - - if(knife_current_altitude - GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance - >= knife_descent_height) - { CurrentFrontEndState = HALT_STATE; - auto_work_mode_states = 20; - - } - break; - case 20: - - delay_counts = delay_counts + 1; - if(delay_counts >= 500) - { - delay_counts = 0; - CurrentMoveState = AUTO_FORWARD; + CurrentMoveState = Move_HALT; + StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; } - - break; - case 30: break; default: CurrentMoveState = Move_HALT; @@ -219,7 +207,6 @@ void Automatic_Operation() } } else{ - delay_counts = 0; CurrentMoveState = Move_HALT; if(auto_work_mode_states == 5) @@ -235,7 +222,7 @@ void Automatic_Operation() auto_work_halt_states = 20; break; case 20: - CurrentFrontEndState = Manual_UP_STATE; + CurrentFrontEndState = MANUAL_LOW_SPEED_UP_STATE; if(GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance - knife_current_altitude >= knife_rising_height) { @@ -250,7 +237,7 @@ void Automatic_Operation() } -uint8_t IsAllowRotation = 0; +uint8_t IsAllowRotation = 1; void Strong_Grinding_Machine_Control() { IsAllowRotation = Knife_Detection();