修改自动作业逻辑，修改为边转边下刀

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;
+					CurrentMoveState = Move_HALT;
-
+					StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE;
-				}
-				break;
-			case 20:
-
-				delay_counts = delay_counts + 1;
-				if(delay_counts >= 500)
-				{
-					delay_counts = 0;
-					CurrentMoveState = AUTO_FORWARD;
 				}
-
-				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();