From 9395c993da1725e9328ed858a44bc349e99755a2 Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Fri, 11 Sep 2026 15:14:17 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=8D=A2=E9=81=93=E9=80=BB?= =?UTF-8?q?=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- RBcore/BHBF.c | 63 ++++++++--- RBcore/include/BHBF.h | 10 +- project/paint_robot_new/paint_robot_new.c | 130 ++++++++++++++++++++-- 3 files changed, 175 insertions(+), 28 deletions(-) diff --git a/RBcore/BHBF.c b/RBcore/BHBF.c index 0a1db00..3476b6f 100644 --- a/RBcore/BHBF.c +++ b/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; } diff --git a/RBcore/include/BHBF.h b/RBcore/include/BHBF.h index e7623c0..64a37df 100644 --- a/RBcore/include/BHBF.h +++ b/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 * *----------------------------------------------*/ diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index e4d9028..182b4b7 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/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)