【待优化】增加关枪延迟停车逻辑，当前无论开不开抢均生效

RBcore/include/BHBF.h

@@ -81,6 +81,8 @@ typedef enum {
     MOTOR_SET_SPEED,		     // 设置速度
 	MOTOR_POWER_ENABLE,		     // 电机上电
 	MOTOR_POWER_DISABLE,		 // 电机失电
+	MOTOR_GET_VEHICLE_SPEED,     // 获取机器人速度（内部速度归一化）
+	MOTOR_CMD_STOP_AFTER,        // 先关枪后停车
     RBCORE_START = 0x0200,       // *机器人共性命令开始*
 
 	RBCORE_CMD_STOP_ALL,		 // 此命令统一调用机器人各个模块的COMMON_CMD_STOP_ALL

project/paint_robot_new/paint_robot_new.c

@@ -315,6 +315,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 			}
 			g_stIV.RobotMoveSpeed = iVehicleSpeed;
 			MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int));
+			MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int));
 
 			Move_Halt_AngleError();
 
@@ -438,7 +439,14 @@ 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_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
+				if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
+				{
+					MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP_AFTER, NULL, 0);
+				}
+				else
+				{
+					MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0);
+				}
 				break;
 			}
 			// 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效

project/paint_robot_new/paint_robot_new_motors.c

@@ -61,7 +61,7 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg)
         return;
     }
 
-    static int iVehicleMove = 0;
+    int iSpeed = 1;
     switch (pstMsg->m_uiMsgID)
     {
         case MOTOR_SET_SPEED:
@@ -75,18 +75,12 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg)
             g_aiMotorSpeed[1] = MotorMgr_SpeedConvert(g_apstMotors[1], aiMotorSpeed[1]);
             if (0 == g_aiMotorSpeed[0] && 0 == g_aiMotorSpeed[1])
 			{
-				iVehicleMove = 0;
+				g_iLastVehicleMove = 0;
 			}
 			else
 			{
-				iVehicleMove = 1;
+				g_iLastVehicleMove = 1;
 			}
-			if (0 == iVehicleMove && 1 == g_iLastVehicleMove)
-			{
-				MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_SET_STOP_OFF_PAINT, NULL, 0);
-				MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪
-			}
-			g_iLastVehicleMove = iVehicleMove;
 			break;
         }
         case MOTOR_POWER_ENABLE:
@@ -99,18 +93,48 @@ 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:
+        case MOTOR_GET_VEHICLE_SPEED:
+        {
+        	if (pstMsg->m_uiDataLen >= sizeof(int))
+			{
+				RD_MEMCPY(&iSpeed, pstMsg->m_aucData, sizeof(int));
+			}
+        	break;
+        }
+        case MOTOR_CMD_STOP_AFTER:
         {
-            memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed));
+			static uint32_t uiLastTime = 0;
-            iVehicleMove = 0;
+        	static int iIsDelayStop = 0;
-            if (1 == g_iLastVehicleMove)
+
+			if (1 == g_iLastVehicleMove)
 			{
 				MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_SET_STOP_OFF_PAINT, NULL, 0);
 				MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪
+				iIsDelayStop = 1;
 			}
+
 			g_iLastVehicleMove = 0;
+
+			if (1 == iIsDelayStop)
+			{
+				if (Rd_GetTime() - uiLastTime >= 600 * g_stCV.Paint_Gun_Shutdown_Distance / iSpeed)
+				{
+					memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed));
+					iIsDelayStop = 0;
+				}
+				break;
+			}
+			else
+			{
+				uiLastTime = Rd_GetTime();
+			}
             break;
         }
+		case COMMON_CMD_STOP_ALL:
+		{
+			memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed));
+			break;
+		}
         default:
             break;
     }