【编译通过】增加自动作业和扫描功能

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h

@@ -41,7 +41,7 @@ typedef enum _Robot_Operation_Mode_t
 {
 	MANUAL_OPERATION = 1,
 	AUTO_OPERATION,
-
+    AUTO_LASER_SCAN
 } Robot_Operation_Mode_t;
 
 typedef struct _transition_t

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c

@@ -124,6 +124,10 @@ void Mode_Control()
 
 			// 前端强磨机控制
 			Strong_Grinding_Machine_Control();
+			break;
+        case AUTO_LASER_SCAN:
+            Automatic_Laser_Scanning();
+
             break;
         case AUTO_OPERATION:
 
@@ -165,17 +169,47 @@ void IO_Control()
 
 }
 
+int32_t MaxLaserSensor = -255; // 激光传感器最大值，初始化为非法值
+int32_t knife_descent_height = -255; // "下降高度"迭代值，初始赋值为非法值
+
+void Automatic_Laser_Scanning()
+{
+    CurrentMoveState = Manual_State; //正常速度手动行驶
+    CurrentFrontEndState = MANUAL_STEP_UP_STATE; //刀上升至最高点
+    StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; //刀具不转动
+    knife_descent_height = 0; // 这里是初始化为一个合理值防止，用于标记执行过扫描
+
+    int32_t a = GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance;
+    int32_t b = GV.ZHR29_200_measure_results.laser_sensor_2_measure_distance;
+    int32_t c = GV.ZHR29_200_measure_results.laser_sensor_3_measure_distance;
+
+    int32_t max = (a > b) ? ((a > c) ? a : c) : ((b > c) ? b : c);
+
+    MaxLaserSensor = (max > MaxLaserSensor) ? max : MaxLaserSensor; // 取出激光雷达最大值
+}
+
 #define knife_rising_height 500 // 自动作业停止后默认上升高度为5mm
 uint8_t auto_work_mode_states = 5;
 uint8_t auto_work_halt_states = 10; 
 
 void Automatic_Operation()
 {
-    int32_t knife_descent_height = 0; // 屏幕配置值转换后    
+    static int knife_flag = 1;
     int32_t knife_current_altitude = 0; // 激光1读数
     uint8_t IsAllowRotation_AutoMode = 0; // 是否允许转刀
 
-	knife_descent_height = GV.PV.knife_descending_height * 100;
+    if (-255 == knife_descent_height && -255 == MaxLaserSensor)
+    {
+        // 跑到这说明一上来直接自动作业了，没有先进行扫描
+        return;
+    }
+
+    if (knife_flag)
+    {
+        // 由于这里是循环，每次开自动作业只执行一次
+        knife_descent_height += GV.PV.knife_descending_height * 100;
+        knife_flag = 0;           
+    }
 
 	if(GV.MK32_Key.CH7_SD == -1000)
 	{
@@ -193,7 +227,7 @@ void Automatic_Operation()
 					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)
+							>= MaxLaserSensor + knife_descent_height)
 					{
 						CurrentFrontEndState = HALT_STATE;
 						CurrentMoveState = AUTO_FORWARD;
@@ -213,10 +247,16 @@ void Automatic_Operation()
 
 		}
 	}
-	else{
+	else if(GV.MK32_Key.CH7_SD == 1000)
+    {
+        knife_flag = 1; // 关闭自动作业时恢复标志
+        MaxLaserSensor = -255; // 恢复为未扫描状态
+        knife_descent_height = -255; // 恢复为未扫描状态
 		CurrentMoveState = Move_HALT;
+        CurrentFrontEndState = HALT_STATE;
+	    StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE;
 
-		if(auto_work_mode_states == 5)
+		/*if(auto_work_mode_states == 5)
 		{
 			CurrentFrontEndState = HALT_STATE;
 			StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE;
@@ -239,7 +279,13 @@ void Automatic_Operation()
 					}
 					break;
 			}
+			}*/
     }
+    else //SD开关在中间时只停止动作不恢复标志位
+    {
+        CurrentMoveState = Move_HALT;
+        CurrentFrontEndState = HALT_STATE;
+	    StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE;
     }
 }
 

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c

@@ -350,9 +350,6 @@ void Manual_step_Up_State_Do(void)
         GV.LS_FrontEnd_Motor.Target_Velcity = 0;
         KinfePosition = 0;
     }
-    else
-    {
-            }
 }
 
 void Manual_step_Down_State_Do(void)
@@ -372,10 +369,6 @@ void Manual_step_Down_State_Do(void)
     {
 		GV.LS_FrontEnd_Motor.Target_Velcity = 0;
     }
-    else
-    {
-             
-    }
 }
 
 void FrontEnd_Halt_State_Do(void)