手动模式下的四个方向行走已经实现

RBcore/BHBF.c

@@ -55,31 +55,62 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
         return;
     }
 
+    static int iSpeed = -1;
+    static int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
     switch (pstMsg->m_uiMsgID)
     {
-		case RBCORE_CMD_STOP_ALL:
+		case CMD_STOP_ALL:
 		{
-			//log_i("RBCORE_CMD_STOP_ALL");
+    		MsgCenter_SendTo(MODULE_NAME_MOTOR, pstMsg->m_uiMsgID, NULL, 0);
 			break;
 		}
         case RBCORE_CMD_MANUAL_FORWARD:
         {
-        	log_i("RBCORE_CMD_MANUAL_FORWARD");
+        	if (iSpeed >= 0)
+        	{
+        		aiMotorSpeed[0] = iSpeed;
+        		aiMotorSpeed[1] = iSpeed;
+        		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+        	}
             break;
         }
         case RBCORE_CMD_MANUAL_BACKWARD:
         {
-        	log_i("RBCORE_CMD_MANUAL_BACKWARD");
+        	if (iSpeed >= 0)
+			{
+				aiMotorSpeed[0] = -iSpeed;
+				aiMotorSpeed[1] = -iSpeed;
+				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+			}
             break;
         }
         case RBCORE_CMD_MANUAL_TURNLEFT:
 		{
-			log_i("RBCORE_CMD_MANUAL_TURNLEFT");
+			if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
+			{
+				aiMotorSpeed[0] = -g_stCV.LeftTurnSpeed;
+				aiMotorSpeed[1] = g_stCV.RightTurnSpeed;
+				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+			}
 			break;
 		}
         case RBCORE_CMD_MANUAL_TURNRIGHT:
 		{
-			log_i("RBCORE_CMD_MANUAL_TURNRIGHT");
+			if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
+			{
+				aiMotorSpeed[0] = g_stCV.LeftTurnSpeed;
+				aiMotorSpeed[1] = -g_stCV.RightTurnSpeed;
+				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+			}
+			break;
+		}
+        case RBCORE_GET_VEHICLE_SPEED:
+		{
+			if (pstMsg->m_uiDataLen >= sizeof(int))
+			{
+				RD_MEMCPY(&iSpeed, pstMsg->m_aucData, sizeof(int));
+				iSpeed *= 10;
+			}
 			break;
 		}
         default:

RBcore/include/BHBF.h

@@ -78,18 +78,18 @@ typedef enum {
 	LUA_SHOW_INFO,
     MOTOR_CMD_SET_SPEED,		 // 设置速度
     MOTOR_CMD_SET_POSITION,      // 设置位置
-    MOTOR_CMD_STOP,              // 停止
     MOTOR_CMD_GET_STATUS,        // 获取状态
     MOTOR_CMD_SET_HOME,          // 设置零点
     MOTOR_CMD_SPEED_MODE,        // 速度模式
     MOTOR_CMD_POSITION_MODE,     // 位置模式
     MOTOR_CMD_END = 0x0100,
 
-	RBCORE_CMD_STOP_ALL,         // 机器人停止
+	CMD_STOP_ALL,         // 机器人停止
     RBCORE_CMD_MANUAL_FORWARD,   // 机器人手动前进
     RBCORE_CMD_MANUAL_BACKWARD,  // 机器人手动后退
 	RBCORE_CMD_MANUAL_TURNLEFT,  // 机器人手动左转
 	RBCORE_CMD_MANUAL_TURNRIGHT, // 机器人手动右转
+	RBCORE_GET_VEHICLE_SPEED,    // 获取机器人速度
 	RBCORE_CMD_END = 0x0200,
 
 	CUSTOM_GET_PV,				 // 获取PV

project/paint_robot_new/paint_robot_new.c

@@ -93,7 +93,7 @@ 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_RBCORE, CMD_STOP_ALL, NULL, 0);
 				break;
 			}
 
@@ -117,7 +117,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 			}
 			else
 			{
-				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
+				MsgCenter_SendTo(MODULE_NAME_RBCORE, CMD_STOP_ALL, NULL, 0);
 			}
 			break;
 		}

project/paint_robot_new/paint_robot_new_motors.c

@@ -62,25 +62,22 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg)
     {
         case MOTOR_CMD_SET_SPEED:
         {
-            int iSpeed = 0;
-            if (pstMsg->m_uiDataLen >= sizeof(int))
+            int aiMotorSpeed[2] = {0};
+            if (pstMsg->m_uiDataLen >= sizeof(aiMotorSpeed))
             {
-                RD_MEMCPY(&iSpeed, pstMsg->m_aucData, sizeof(int));
+                RD_MEMCPY(aiMotorSpeed, pstMsg->m_aucData, sizeof(aiMotorSpeed));
             }
-            //log_i("Set Speed %d", iSpeed);
-            g_aiMotorSpeed[0] = iSpeed;
-            g_aiMotorSpeed[1] = iSpeed;
+            g_aiMotorSpeed[0] = MotorMgr_SpeedConvert(g_apstMotors[0], aiMotorSpeed[0]);
+            g_aiMotorSpeed[1] = MotorMgr_SpeedConvert(g_apstMotors[1], aiMotorSpeed[1]);
             break;
         }
-        case MOTOR_CMD_STOP:
+        case CMD_STOP_ALL:
         {
             for (int i = 0; i < 2; i++)
-            {
-                if (g_apstMotors[i] != NULL)
             {
             	MotorMgr_SetTargetSpeed(g_apstMotors[i], 0);
             }
-            }
+            memset(g_aiMotorSpeed, 0, sizeof(g_aiMotorSpeed));
             break;
         }
         case MOTOR_CMD_SET_HOME:
@@ -174,18 +171,18 @@ void MotorTask(void *argument)
     // 配置左轮电机
     Motor_Config_t stLeftMotorConfig = {
         .m_ucMotorID = 1,
-        .m_uiPulsePerRound = 10000,
-        .m_uiReductionRatio = 70,
-        .m_fWheelDiameter = 0.26f
+        .m_uiPulsePerRound = g_stCV.pulse_Per_Circle,
+        .m_uiReductionRatio = g_stCV.wheel_Reduction_Ratio,
+        .m_fWheelDiameter = g_stCV.wheel_Diameter_m
     };
     g_apstMotors[0] = MotorMgr_Create(&stLeftMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL);
 
     // 配置右轮电机
     Motor_Config_t stRightMotorConfig = {
         .m_ucMotorID = 2,
-        .m_uiPulsePerRound = 10000,
-        .m_uiReductionRatio = 70,
-        .m_fWheelDiameter = 0.26f
+        .m_uiPulsePerRound = g_stCV.pulse_Per_Circle,
+        .m_uiReductionRatio = g_stCV.wheel_Reduction_Ratio,
+        .m_fWheelDiameter = g_stCV.wheel_Diameter_m
     };
     g_apstMotors[1] = MotorMgr_Create(&stRightMotorConfig, g_ptCAN1, LeiSai_GetProtocol(), NULL);