电机增加急停，增加非无模式下的摇杆逻辑（PID竖直行走）

RBcore/include/BHBF.h

@@ -77,6 +77,8 @@ typedef enum {
     COMMON_CMD_STOP_ALL,         // 机器人停止（建议各个控制硬件的模块都要实现）
     MOTOR_START = 0x0100,        // *电机命令开始*
     MOTOR_SET_SPEED,		     // 设置速度
+	MOTOR_POWER_DISABLE,		 // 电机失电
+	MOTOR_POWER_ENABLE,		     // 电机上电
     RBCORE_START = 0x0200,       // *机器人共性命令开始*
 
 	RBCORE_CMD_STOP_ALL,		 // 此命令统一调用机器人各个模块的COMMON_CMD_STOP_ALL

project/paint_robot_new/paint_robot_new.c

@@ -123,6 +123,15 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 				RD_MEMCPY(&g_stMK32, pstMsg->m_aucData, sizeof(g_stMK32));
 			}
 
+			// 急停
+			if (g_stMK32.CH8_SE == -1000 && g_stMK32.CH9_SF == -1000)
+			{
+				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_DISABLE, NULL, 0);
+				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
+				break;
+			}
+
+			// 更新速度旋钮值
 			int iSpeedSelection = 3 * (g_stMK32.CH11_RD1 + 1000) / 200;
 			int iVehicleSpeed = 1;
 			if (iSpeedSelection > 0)
@@ -132,6 +141,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));
 
+			// 开关喷枪
 			if (g_stMK32.CH6_SC == -1000)
 			{
 				int paintstate = 0;
@@ -143,6 +153,22 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 				MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &paintstate, sizeof(int));
 			}
 
+			// 根据模式判断换道（仅在竖直向左或向右生效）
+			if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
+			{
+				if (g_stMK32.CH4_SA == -1000)
+				{
+					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNRIGHT, NULL, 0);
+					break;
+				}
+				else if(g_stMK32.CH4_SA == 1000)
+				{
+					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNLEFT, NULL, 0);
+					break;
+				}
+			}
+
+			// 自动巡航
 			if (g_stMK32.CH5_SB == -1000)
 			{
 				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0);
@@ -154,21 +180,40 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 				break;
 			}
 
+			// 【注意!!!】摇杆行程死区判断开始，除摇杆以外按键在上边都处理完，不然走不下去
 			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);
 				break;
 			}
+			// 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效
 
+			// 摇杆角度死区判断
 			int angle = atan2(g_stMK32.CH2_LY_V, g_stMK32.CH3_LY_H) * 180 / M_PI;
 
 			if (abs(angle - 90) <= g_stCV.Joy_Sticker_Angle_Allowance)
 			{
-				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0);
+				if (g_stPV.RunMode == 1)
+				{
+					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0);
+				}
+				else
+				{
+					int iMode[2] = {1, g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration};
+					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_VERTICAL, iMode, sizeof(iMode));
+				}
 			}
 			else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
 			{
-				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0);
+				if (g_stPV.RunMode == 1)
+				{
+					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0);
+				}
+				else
+				{
+					int iMode[2] = {-1 , g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration};
+					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_VERTICAL, iMode, sizeof(iMode));
+				}
 			}
 			else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)
 			{

project/paint_robot_new/paint_robot_new_motors.c

@@ -72,6 +72,16 @@ static void Motor_ModuleHandler(const Msg_t *pstMsg)
             g_aiMotorSpeed[1] = MotorMgr_SpeedConvert(g_apstMotors[1], aiMotorSpeed[1]);
             break;
         }
+        case MOTOR_POWER_ENABLE:
+		{
+			HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 0);
+			break;
+		}
+        case MOTOR_POWER_DISABLE:
+		{
+			HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 1);
+			break;
+		}
         case COMMON_CMD_STOP_ALL:
         {
             for (int i = 0; i < 2; i++)
@@ -116,6 +126,8 @@ static void decode_LeiSaiMotor(const char *_pBuffer, uint32_t _iSize)
 
 void MotorTask(void *argument)
 {
+	// 此时消息中心还未就绪，直接调用HAL库接口给电机上电
+	HAL_GPIO_WritePin(OUT_0_GPIO_Port, OUT_0_Pin, 0);
 	// 初始化FDCAN1，使用雷赛电机的回调
 	TCANUserData *ptCANUserData = CAN_userdata_init(&hfdcan1, 32, 0);
 	g_ptCAN1 = rd_ComCreate(check_LeiSaiMotor, decode_LeiSaiMotor, FDCAN1_Send, CONFIG_UART_BUFFER_SIZE, ptCANUserData);