增加换道逻辑

RBcore/BHBF.c

@@ -60,6 +60,9 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
     static int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
     static int g_dletAngle = 0; //PID最终计算补偿值
     static int32_t g_iPIDAngle = 0; //当前角度
+
+    static uint32_t uiLastSendTick = 0; //上一次时间戳，用于控制直线行驶距离
+    static int bTimerStarted = 0; //直线行驶距离单次触发标志
     switch (pstMsg->m_uiMsgID)
     {
 		case RBCORE_CMD_STOP_ALL:
@@ -115,49 +118,72 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
 			}
 			break;
 		}
-        case RBCORE_CMD_VERTICAL:
+        case RBCORE_CMD_RESET_STRAIGHT:
+        {
+        	uiLastSendTick = 0;
+        	bTimerStarted = 0;
+        	break;
+        }
+        case RBCORE_CMD_STRAIGHT_DRIVE:
 		{
-			int iMode[2] = {0};//iMode[0]正数表示前进，负数表示倒退 iMode[1]是目标角度
-			if (pstMsg->m_uiDataLen >= sizeof(iMode))
+			BHBF_straight_drive_Cmd stCmd = {0};
+			if (pstMsg->m_uiDataLen >= sizeof(stCmd))
 			{
-				RD_MEMCPY(&iMode, pstMsg->m_aucData, sizeof(iMode));
-				if (0 == iMode[0])
+				RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd));
+
+				if (stCmd.m_iTime > 0 && 0 == bTimerStarted)
+				{
+					uiLastSendTick = Rd_GetTime();
+					bTimerStarted = 1; // 开始计时
+				}
+
+				if (0 == stCmd.m_iMode)
 				{
 					break;
 				}
 
-				if (abs(g_iPIDAngle - iMode[1]) <= g_stCV.PID_mid.PID_Angle)
+				if (abs(g_iPIDAngle - stCmd.m_iAngle) <= g_stCV.PID_mid.PID_Angle)
 				{
-					if (abs(g_iPIDAngle - iMode[1]) < g_stCV.PID_low.PID_Angle)
+					if (abs(g_iPIDAngle - stCmd.m_iAngle) < g_stCV.PID_low.PID_Angle)
 					{
-						g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iMode[1],
+						g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, stCmd.m_iAngle,
 								g_stCV.PID_low.Kp, g_stCV.PID_low.Ki, g_stCV.PID_low.Kd, 10);
 					}
 					else
 					{
-						g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iMode[1],
+						g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, stCmd.m_iAngle,
 								g_stCV.PID_mid.Kp, g_stCV.PID_mid.Ki, g_stCV.PID_mid.Kd, 10);
-
 					}
-					if (iMode[0] > 0)
+					if (stCmd.m_iMode > 0)
 					{
-						aiMotorSpeed[0] = iSpeed - g_dletAngle ;
-						aiMotorSpeed[1] = iSpeed + g_dletAngle ;
+						aiMotorSpeed[0] = (g_stCV.Lane_Change_Speed_m_per_min * 10) - g_dletAngle ;
+						aiMotorSpeed[1] = (g_stCV.Lane_Change_Speed_m_per_min * 10) + g_dletAngle ;
 					}
 					else
 					{
-						aiMotorSpeed[0] = -iSpeed - g_dletAngle ;
-						aiMotorSpeed[1] = -iSpeed + g_dletAngle ;
+						aiMotorSpeed[0] = -(g_stCV.Lane_Change_Speed_m_per_min * 10) - g_dletAngle ;
+						aiMotorSpeed[1] = -(g_stCV.Lane_Change_Speed_m_per_min * 10) + g_dletAngle ;
 					}
 				}
 				else
 				{
-					g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iMode[1],
+					g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, stCmd.m_iAngle,
 							g_stCV.PID_high.Kp, g_stCV.PID_high.Ki, g_stCV.PID_high.Kd,50);
 					aiMotorSpeed[0] = -g_dletAngle ;
 					aiMotorSpeed[1] = g_dletAngle ;
 				}
+
+				if (stCmd.m_iTime > 0 && (int32_t)(Rd_GetTime() - uiLastSendTick) >= stCmd.m_iTime)// 到时间停止
+				{
+					aiMotorSpeed[0] = 0;
+					aiMotorSpeed[1] = 0;
+				}
 				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+
+				if (aiMotorSpeed[0] == 0 && aiMotorSpeed[1] == 0)
+				{
+					MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
+				}
 			}
 			break;
 		}
@@ -185,6 +211,11 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg)
 					aiMotorSpeed[1] = g_dletAngle ;
 				}
 				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+
+				if (aiMotorSpeed[0] == 0 && aiMotorSpeed[1] == 0)
+				{
+					MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
+				}
 			}
 			break;
 		}

RBcore/include/BHBF.h

@@ -86,7 +86,8 @@ typedef enum {
     RBCORE_CMD_MANUAL_BACKWARD,  // 机器人手动后退
 	RBCORE_CMD_MANUAL_TURNLEFT,  // 机器人手动左转
 	RBCORE_CMD_MANUAL_TURNRIGHT, // 机器人手动右转
-	RBCORE_CMD_VERTICAL,         // 机器人竖直前进（带PID）
+	RBCORE_CMD_STRAIGHT_DRIVE,   // 机器人直线行驶命令格式为BHBF_straight_drive_Cmd
+	RBCORE_CMD_RESET_STRAIGHT,   // 上一条命令支持计时前进，这里重置计时
 	RBCORE_CMD_TURN_ANGLE,  	 // 机器人转向目标角度（带PID）
 	RBCORE_GET_TL720D_ROLL,      // 获取陀螺仪横滚角（PID计算）
 	RBCORE_GET_VEHICLE_SPEED,    // 获取机器人速度（内部速度归一化）
@@ -98,11 +99,18 @@ typedef enum {
     CUSTOM_GET_SPEED,            // 获取速度（电机回码）
     CUSTOM_GET_FAULT_CODE,       // 获取错误码（电机回码）
 	CUSTOM_CMD_PAINTGUN,         // 控制喷枪
+	CUSTOM_CMD_STRAIGHT_DRIVE,   // RBCORE_CMD_STRAIGHT_DRIVE命令停止后回调
+	CUSTOM_CMD_TURN_ANGLE,       // RBCORE_CMD_TURN_ANGLE命令停止后回调
 	SENDIV_START = 0x0400,       // *IV发送线程命令（一般低优先级事项也放到这）*
 
     SENDIV_SET_IV,				 // 设置IV
 } BHBF_Cmd_e;
 
+typedef struct {
+	int m_iMode;  //正数表示前进，负数表示倒退
+	int m_iAngle; //目标角度
+	int m_iTime;  //行驶时间(单位毫秒)计时，负数表示一直行走
+} BHBF_straight_drive_Cmd;
 /*==============================================*
  *      project-wide global variables           *
  *----------------------------------------------*/

project/paint_robot_new/paint_robot_new.c

@@ -58,6 +58,11 @@ static IV_struct_define g_stIV;
 
 #define IV_SEND_TIME 500 // IV上报周期（单位毫秒）
 
+static int32_t RunTime_DistanceCm_SpeedE_2MPMin(void)
+{
+	return (600 * (g_stPV.LaneChangeDistance + g_stCV.Vertical_ChangeLane_Compensation) / g_stCV.Lane_Change_Speed_m_per_min);
+}
+
 static void Custom_ModuleHandler(const Msg_t *pstMsg)
 {
     if (NULL == pstMsg)
@@ -65,6 +70,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
         return;
     }
 
+    static int iLeft_Compensation = 0;
+    static int iRight_Compensation = 0;
     switch (pstMsg->m_uiMsgID)
     {
     	case CUSTOM_GET_FAULT_CODE:
@@ -106,6 +113,76 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 			}
 			break;
 		}
+		case CUSTOM_CMD_STRAIGHT_DRIVE:
+		{
+			BHBF_straight_drive_Cmd stCmd = {0};
+			if (pstMsg->m_uiDataLen >= sizeof(stCmd))
+			{
+				RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd));
+				int iTargetAngle = g_stCV.RobotUpAngleValue;
+				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
+			}
+			break;
+		}
+		case CUSTOM_CMD_TURN_ANGLE:
+		{
+			int iTargetAngle = 0;
+			if (pstMsg->m_uiDataLen >= sizeof(int))
+			{
+				RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int));
+				if (iTargetAngle == g_stCV.RobotUpAngleValue)
+				{
+					log_d("change line success");
+				}
+				else if (iTargetAngle == g_stCV.RobotLeftAngleValue)
+				{
+					if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
+					{
+						BHBF_straight_drive_Cmd stCmd = {
+							.m_iMode = 1,
+							.m_iAngle = g_stCV.RobotLeftAngleValue + iLeft_Compensation,
+							.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin()
+							};
+						MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
+					}
+					else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
+					{
+						BHBF_straight_drive_Cmd stCmd = {
+							.m_iMode = -1,
+							.m_iAngle = g_stCV.RobotLeftAngleValue - iRight_Compensation,
+							.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin()
+							};
+						MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
+					}
+				}
+				else if (iTargetAngle == g_stCV.RobotRightAngleValue)
+				{
+					if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
+					{
+						BHBF_straight_drive_Cmd stCmd = {
+							.m_iMode = -1,
+							.m_iAngle = g_stCV.RobotRightAngleValue + iLeft_Compensation,
+							.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin()
+							};
+						MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
+					}
+					else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
+					{
+						BHBF_straight_drive_Cmd stCmd = {
+							.m_iMode = 1,
+							.m_iAngle = g_stCV.RobotRightAngleValue - iRight_Compensation,
+							.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin()
+							};
+						MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
+					}
+				}
+				else if (iTargetAngle == g_stCV.RobotDownAngleValue)
+				{
+
+				}
+			}
+			break;
+		}
 		case CUSTOM_CMD_PAINTGUN:
 		{
 			int paintstate = -1;
@@ -153,18 +230,41 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 				MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &paintstate, sizeof(int));
 			}
 
+			int iTargetAngle = 0;
 			// 根据模式判断换道（仅在竖直向左或向右生效）
-			if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
+			if (g_stMK32.CH4_SA == 1000)
 			{
-				if (g_stMK32.CH4_SA == -1000)
+				if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
 				{
-					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNRIGHT, NULL, 0);
-					break;
+					iTargetAngle = g_stCV.RobotLeftAngleValue;
+					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
 				}
-				else if(g_stMK32.CH4_SA == 1000)
+				else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
+				{
+					iTargetAngle = g_stCV.RobotRightAngleValue;
+					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
+				}
+				break;
+			}
+			else if (g_stMK32.CH4_SA == -1000)
+			{
+				if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
+				{
+					iTargetAngle = g_stCV.RobotRightAngleValue;
+					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
+				}
+				else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
+				{
+					iTargetAngle = g_stCV.RobotLeftAngleValue;
+					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle));
+				}
+				break;
+			}
+			else // 重置前进计时器以便下一次换道重新计数
+			{
+				if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
 				{
-					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_TURNLEFT, NULL, 0);
-					break;
+					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_RESET_STRAIGHT, NULL, 0);
 				}
 			}
 
@@ -199,8 +299,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 				}
 				else
 				{
-					int iMode[2] = {1, g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration};
-					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_VERTICAL, iMode, sizeof(iMode));
+					BHBF_straight_drive_Cmd stCmd = {
+						.m_iMode = 1,
+						.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
+						.m_iTime = -1
+						};
+					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
 				}
 			}
 			else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
@@ -211,8 +315,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 				}
 				else
 				{
-					int iMode[2] = {-1 , g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration};
-					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_VERTICAL, iMode, sizeof(iMode));
+					BHBF_straight_drive_Cmd stCmd = {
+						.m_iMode = -1,
+						.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
+						.m_iTime = -1
+						};
+					MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
 				}
 			}
 			else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)