增加停车自动关枪逻辑

project/paint_robot_new/paint_robot_new_motors.c

@@ -44,6 +44,7 @@ static Motor_Instance_t *g_apstMotors[MOTOR_INSTANCE_MAX] = {NULL, NULL, NULL, N
 static uint32_t g_uiMotorModuleID = 0;
 static int g_aiMotorSpeed[MOTOR_INSTANCE_MAX] = {0};
 static int g_isMotorInit = -1; // -1表示还在等待遥控器初始化完毕，0表示可以进行电机初始化，1表示电机初始化完毕，运行电机主循环
+static int g_iLastVehicleMove = 0;
 
 /*----------------------------------------------*
  * 常量定义                                     *
@@ -60,6 +61,7 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg)
         return;
     }
 
+    static int iVehicleMove = 0;
     switch (pstMsg->m_uiMsgID)
     {
         case MOTOR_SET_SPEED:
@@ -71,7 +73,20 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg)
             }
             g_aiMotorSpeed[0] = MotorMgr_SpeedConvert(g_apstMotors[0], aiMotorSpeed[0]);
             g_aiMotorSpeed[1] = MotorMgr_SpeedConvert(g_apstMotors[1], aiMotorSpeed[1]);
-            break;
+            if (0 == g_aiMotorSpeed[0] && 0 == g_aiMotorSpeed[1])
+			{
+				iVehicleMove = 0;
+			}
+			else
+			{
+				iVehicleMove = 1;
+			}
+			if (0 == iVehicleMove && 1 == g_iLastVehicleMove)
+			{
+				MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪
+			}
+			g_iLastVehicleMove = iVehicleMove;
+			break;
         }
         case MOTOR_POWER_ENABLE:
 		{
@@ -86,6 +101,12 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg)
         case COMMON_CMD_STOP_ALL:
         {
             memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed));
+            iVehicleMove = 0;
+            if (1 == g_iLastVehicleMove)
+			{
+				MsgCenter_SendTo(MODULE_NAME_CUSTOM, COMMON_CMD_STOP_ALL, NULL, 0);//先关枪
+			}
+			g_iLastVehicleMove = 0;
             break;
         }
         default: