Revert "整理多余变量并将停止逻辑归一化"

This reverts commit a71799675b.

RBcore/BHBF.c

@@ -61,9 +61,8 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
     {
 		case RBCORE_CMD_STOP_ALL:
 		{
-    		aiMotorSpeed[0] = 0;
+    		MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
-			aiMotorSpeed[1] = 0;
+    		MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);
-			MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
 			break;
 		}
         case RBCORE_CMD_MANUAL_FORWARD:

RBcore/include/BHBF.h

@@ -76,6 +76,7 @@ extern "C"{
 
 typedef enum {
 	COMMON_CMD_SHOW_INFO,        // 终端信息展示
+    COMMON_CMD_STOP_ALL,         // 机器人停止（建议各个控制硬件的模块都要实现）
     MOTOR_START = 0x0100,        // *电机命令开始*
     MOTOR_SET_SPEED,		     // 设置速度
 	MOTOR_POWER_ENABLE,		     // 电机上电

project/paint_robot_new/paint_robot_new.c

@@ -47,6 +47,7 @@ static uint32_t g_uiCustomModuleID = 0;
 static SP_MSP_MK32_Button g_stMK32;
 static PV_struct_define g_stPV;
 static IV_struct_define g_stIV;
+static int g_RB_State = 0; // 机器人状态，1表示处于竖直行走模式下
 static int g_Paint_State = -1; // 喷枪状态，0表示打开，1表示关闭
 static int g_angle_protect_lock = 0; // 为1表示角度异常，触发停车逻辑Move_Halt_AngleError
 static int g_ipaintOffCount = 0; // Move_Halt_AngleError中的计时，即角度偏移超过多少时间触发停枪
@@ -75,11 +76,16 @@ static void Move_Halt_AngleError(void)
 		return;
 	}
 
+	if (0 == g_RB_State)
+	{
+		return;
+	}
+
 	if (1 == g_angle_protect_lock)
 	{
 		if (Rd_GetTime() - uiLastTime >= 600 * g_stCV.Paint_Gun_Shutdown_Distance / g_stIV.RobotMoveSpeed)
 		{
-			MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
+			MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
 		}
 		return;
 	}
@@ -92,8 +98,7 @@ static void Move_Halt_AngleError(void)
 	{
 		if(Rd_GetTime() - g_ipaintOffCount > 1000)
 		{
-			g_Paint_State = 1;
+			MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪
-			MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_Paint_State, sizeof(int));
 			g_angle_protect_lock = 1;
 			return;
 		}
@@ -240,6 +245,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 			}
 			break;
 		}
+		case COMMON_CMD_STOP_ALL:
+		{
+			HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, 1);
+			g_Paint_State = 1;
+			break;
+		}
 		case CUSTOM_CMD_PAINTGUN:
 		{
 			int paintstate = -1;
@@ -279,6 +290,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 			{
 				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_DISABLE, NULL, 0);
 				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
+				g_RB_State = 0;
 				g_Is_All_Button_Reset = 0;
 				break;
 			}
@@ -323,6 +335,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 					iTargetAngle = g_stCV.RobotRightAngleValue;
 					MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
 				}
+				g_RB_State = 0;
 				break;
 			}
 			else if (g_stMK32.CH4_SA == -1000)
@@ -337,6 +350,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 					iTargetAngle = g_stCV.RobotLeftAngleValue;
 					MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
 				}
+				g_RB_State = 0;
 				break;
 			}
 			else // 重置前进计时器以便下一次换道重新计数
@@ -344,6 +358,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 				if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
 				{
 					MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
+					g_RB_State = 0;
 				}
 			}
 
@@ -362,6 +377,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 						g_Paint_State = 1;
 						MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_Paint_State, sizeof(int));
 					}
+					g_RB_State = 0;
 				}
 				else if(g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
 				{
@@ -375,6 +391,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 						}
 					}
 					CH13_S2_Value = g_stMK32.CH13_S2;
+					g_RB_State = 0;
 				}
 			}
 
@@ -397,6 +414,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 							};
 						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));						
 					}
+					g_RB_State = 1;
 				}
 				break;
 			}
@@ -418,6 +436,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 							};
 						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
 					}
+					g_RB_State = 1;
 				}
 				break;
 			}
@@ -425,7 +444,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 			// 【注意!!!】摇杆行程死区判断开始，除摇杆以外按键在上边都处理完，不然走不下去
 			if (abs(g_stMK32.CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(g_stMK32.CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance)
 			{
-				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
+				MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
+				g_RB_State = 0;
 				break;
 			}
 			// 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效
@@ -451,6 +471,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 							};
 						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
 					}
+					g_RB_State = 1;
 				}
 			}
 			else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
@@ -471,19 +492,23 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 							};
 						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
 					}
+					g_RB_State = 1;
 				}
 			}
 			else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)
 			{
 				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNRIGHT, NULL, 0);
+				g_RB_State = 0;
 			}
 			else if (abs(angle - 180) <= g_stCV.Joy_Sticker_Angle_Allowance || abs(angle - (-180)) <= g_stCV.Joy_Sticker_Angle_Allowance)
 			{
 				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNLEFT, NULL, 0);
+				g_RB_State = 0;
 			}
 			else
 			{
-				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
+				MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
+				g_RB_State = 0;
 			}
 			break;
 		}
@@ -509,8 +534,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 			lua_print("PV Info\n");
 			lua_print("{%d, %d, %d, %d, %d, %ld}\n",
 					g_stPV.RunMode, g_stPV.RobotSpeed, g_stPV.LaneChangeDistance, (int)g_stPV.Vertical_Calibration, g_stPV.IV_IsRestart_Notified, (long long)g_stPV.TimeStamp);
-			lua_print("\ng_Paint_State = %d\ng_angle_protect_lock = %d\ng_ipaintOffCount = %d\ng_Is_All_Button_Reset = %d\n", 
+			lua_print("g_RB_State = %d\ng_Paint_State = %d\ng_angle_protect_lock = %d\ng_ipaintOffCount = %d\ng_Is_All_Button_Reset = %d\n", 
-					g_Paint_State, g_angle_protect_lock, g_ipaintOffCount, g_Is_All_Button_Reset);
+					g_RB_State, g_Paint_State, g_angle_protect_lock, g_ipaintOffCount, g_Is_All_Button_Reset);
 			break;
 		}
         default:

project/paint_robot_new/paint_robot_new_motors.c

@@ -83,6 +83,11 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg)
 			HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 1);
 			break;
 		}
+        case COMMON_CMD_STOP_ALL:
+        {
+            memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed));
+            break;
+        }
         default:
             break;
     }