From e4f45242e4dae21d91af5c9c01c1a701830b9701 Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Fri, 18 Sep 2026 15:14:06 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8D=A2=E9=81=93=E9=80=BB=E8=BE=91=E8=B0=83?= =?UTF-8?q?=E9=80=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- RBcore/include/BHBF.h | 3 +- RBcore/msp_Timer.c | 63 ++++++++++++++++++----- project/paint_robot_new/paint_robot_new.c | 10 +--- 3 files changed, 54 insertions(+), 22 deletions(-) diff --git a/RBcore/include/BHBF.h b/RBcore/include/BHBF.h index 31ad17d..7280fed 100644 --- a/RBcore/include/BHBF.h +++ b/RBcore/include/BHBF.h @@ -120,7 +120,8 @@ typedef enum { TIMER_CMD_STRAIGHT_DRIVE, // 机器人直线行驶命令格式为BHBF_straight_drive_Cmd TIMER_CMD_RESET_STRAIGHT, // 上一条命令支持计时前进，这里重置计时 - TIMER_CMD_TURN_ANGLE, // 机器人转向目标角度（带PID） + TIMER_CMD_TURN_ANGLE, // 机器人转向目标角度（带PID） + TIMER_CMD_TURN_UP, // 机器人转向头朝上 } BHBF_Cmd_e; #define THREAD_DEFAULT_STACK 4096 diff --git a/RBcore/msp_Timer.c b/RBcore/msp_Timer.c index 04efbfe..1475da1 100644 --- a/RBcore/msp_Timer.c +++ b/RBcore/msp_Timer.c @@ -20,6 +20,7 @@ #include "msg_center.h" #include "rd_time.h" #include "tim.h" +#include "lua_base.h" /*----------------------------------------------* * 外部变量说明 * @@ -62,22 +63,30 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg) static int g_dletAngle = 0; //PID最终计算补偿值 static uint32_t uiLastSendTick = 0; //上一次时间戳，用于控制直线行驶距离 static int bTimerStarted = 0; //直线行驶距离单次触发标志 + static int bstrgightfinish = 0; //直线行驶完成 switch (pstMsg->m_uiMsgID) { case TIMER_CMD_RESET_STRAIGHT: { uiLastSendTick = 0; bTimerStarted = 0; + bstrgightfinish = 0; break; } case TIMER_CMD_STRAIGHT_DRIVE: { - BHBF_straight_drive_Cmd stCmd = {0}; + static BHBF_straight_drive_Cmd stCmd = {0}; if (pstMsg->m_uiDataLen >= sizeof(stCmd)) { + if (1 == bstrgightfinish) // 这里换道完成认为第一步旋转已经完成不再根据计时卡状态 + { + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, NULL, 0); + break; + } + RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd)); - if (stCmd.m_iTime > 0 && 0 == bTimerStarted) + if (stCmd.m_iTime >= 0 && 0 == bTimerStarted) { uiLastSendTick = Rd_GetTime(); bTimerStarted = 1; // 开始计时 @@ -119,17 +128,14 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg) aiMotorSpeed[1] = g_dletAngle ; } - if (stCmd.m_iTime > 0 && (int32_t)(Rd_GetTime() - uiLastSendTick) >= stCmd.m_iTime)// 到时间停止 + if (stCmd.m_iTime >= 0 && (int32_t)(Rd_GetTime() - uiLastSendTick) >= stCmd.m_iTime)// 到时间停止 { aiMotorSpeed[0] = 0; aiMotorSpeed[1] = 0; + bstrgightfinish = 1; + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, NULL, 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; } @@ -139,10 +145,18 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg) if (pstMsg->m_uiDataLen >= sizeof(int)) { RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int)); + + if (1 == bstrgightfinish) // 这里换道完成认为第一步旋转已经完成不再根据角度卡状态 + { + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + break; + } + if (abs(g_RF_Angle_Roll - iTargetAngle) <= 50) { aiMotorSpeed[0] = 0; aiMotorSpeed[1] = 0; + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); } else if (abs(g_RF_Angle_Roll - iTargetAngle) <= 1000) { @@ -157,14 +171,37 @@ static void Timer_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; } + case TIMER_CMD_TURN_UP: + { + if (abs(g_RF_Angle_Roll - g_stCV.RobotUpAngleValue) <= 50) + { + aiMotorSpeed[0] = 0; + aiMotorSpeed[1] = 0; + } + else if (abs(g_RF_Angle_Roll - g_stCV.RobotUpAngleValue) <= 1000) + { + g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, g_stCV.RobotUpAngleValue, 1, 0,0.5, 10); + aiMotorSpeed[0] = -g_dletAngle ; + aiMotorSpeed[1] = g_dletAngle; + } + else + { + g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, g_stCV.RobotUpAngleValue, 2, 0, 0.5, 85); + aiMotorSpeed[0] = -g_dletAngle ; + aiMotorSpeed[1] = g_dletAngle ; + } + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); + break; + } + case COMMON_CMD_SHOW_INFO: + { + lua_print("\ng_dletAngle = %d\nuiLastSendTick = %d\nbTimerStarted = %d\nbstrgightfinish = %d\ng_RF_Angle_Roll = %d\n", + g_dletAngle, uiLastSendTick, bTimerStarted, bstrgightfinish, g_RF_Angle_Roll); + break; + } default: break; } diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 52392f4..02dda3f 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -177,13 +177,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } 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_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); - } + MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_UP, NULL, 0); break; } case CUSTOM_CMD_TURN_ANGLE: @@ -194,7 +188,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int)); if (iTargetAngle == g_stCV.RobotUpAngleValue) { - log_d("change line success"); + } else if (iTargetAngle == g_stCV.RobotLeftAngleValue) {