From fabfd2a95e468e52be02ed9c28663ea826281cc4 Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Mon, 21 Sep 2026 14:33:48 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8D=A2=E9=81=93=E6=94=B9=E4=B8=BA=E7=8A=B6?= =?UTF-8?q?=E6=80=81=E6=9C=BA=E5=AE=9E=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- RBcore/include/BHBF.h | 1 - RBcore/msp_Timer.c | 43 +---- project/paint_robot_new/paint_robot_new.c | 181 +++++++++++++--------- 3 files changed, 115 insertions(+), 110 deletions(-) diff --git a/RBcore/include/BHBF.h b/RBcore/include/BHBF.h index c617755..4018efd 100644 --- a/RBcore/include/BHBF.h +++ b/RBcore/include/BHBF.h @@ -122,7 +122,6 @@ typedef enum { TIMER_CMD_STRAIGHT_DRIVE, // 机器人直线行驶命令格式为BHBF_straight_drive_Cmd TIMER_CMD_RESET_STRAIGHT, // 上一条命令支持计时前进，这里重置计时 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 0e57f6e..99e857a 100644 --- a/RBcore/msp_Timer.c +++ b/RBcore/msp_Timer.c @@ -63,14 +63,12 @@ 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: @@ -78,12 +76,6 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg) 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) @@ -132,7 +124,6 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg) { 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)); @@ -146,17 +137,11 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg) { 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)); + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, NULL, 0); } else if (abs(g_RF_Angle_Roll - iTargetAngle) <= 1000) { @@ -174,32 +159,10 @@ static void Timer_ModuleHandler(const Msg_t *pstMsg) } 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); + lua_print("\ng_dletAngle = %d\nuiLastSendTick = %d\nbTimerStarted = %d\ng_RF_Angle_Roll = %d\n", + g_dletAngle, uiLastSendTick, bTimerStarted, g_RF_Angle_Roll); break; } default: diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index ace3262..a1dff3d 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -48,6 +48,7 @@ static SP_MSP_MK32_Button g_stMK32; static PV_struct_define g_stPV; static IV_struct_define g_stIV; static int g_Is_All_Button_Reset = 0; // 1表示按钮正常工作，0表示需要复位 +static int g_ichangLineState = 0; // 换道状态，0表示起始状态，1表示第一次旋转完成，2表示计时行走 完成 /*----------------------------------------------* * 常量定义 * @@ -64,6 +65,110 @@ 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 Vertical_Lane_Change_From_Left_To_Right_UP_Control(int _iRight_Compensation) +{ + int iTargetAngle = 0; + if (0 == g_ichangLineState) + { + iTargetAngle = g_stCV.RobotLeftAngleValue; + MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + } + else if (1 == g_ichangLineState) + { + BHBF_straight_drive_Cmd stCmd = { + .m_iMode = -1, + .m_iAngle = g_stCV.RobotLeftAngleValue - _iRight_Compensation, + .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(), + .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min + }; + MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + } + else if (2 == g_ichangLineState) + { + iTargetAngle = g_stCV.RobotUpAngleValue; + MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + } +} + +// 竖直从左往右作业 下端 向右换道 最终头朝上 +static void Vertical_Lane_Change_From_Left_To_Right_Down_Control(int _iRight_Compensation) +{ + int iTargetAngle = 0; + if (0 == g_ichangLineState) + { + iTargetAngle = g_stCV.RobotRightAngleValue; + MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + } + else if (1 == g_ichangLineState) + { + BHBF_straight_drive_Cmd stCmd = { + .m_iMode = 1, + .m_iAngle = g_stCV.RobotRightAngleValue - _iRight_Compensation, + .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(), + .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min + }; + MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + } + else if (2 == g_ichangLineState) + { + iTargetAngle = g_stCV.RobotUpAngleValue; + MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + } +} + +// 竖直从右往左作业 上端 向左换道 最终头朝上 +static void Vertical_Lane_Change_From_Right_To_Left_UP_Control(int _iLeft_Compensation) +{ + int iTargetAngle = 0; + if (0 == g_ichangLineState) + { + iTargetAngle = g_stCV.RobotRightAngleValue; + MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + } + else if (1 == g_ichangLineState) + { + BHBF_straight_drive_Cmd stCmd = { + .m_iMode = -1, + .m_iAngle = g_stCV.RobotRightAngleValue + _iLeft_Compensation, + .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(), + .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min + }; + MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + } + else if (2 == g_ichangLineState) + { + iTargetAngle = g_stCV.RobotUpAngleValue; + MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + } +} + +// 竖直从右往左作业 下端 向左换道 最终头朝上 +static void Vertical_Lane_Change_From_Right_To_Left_Down_Control(int _iLeft_Compensation) +{ + int iTargetAngle = 0; + if (0 == g_ichangLineState) + { + iTargetAngle = g_stCV.RobotLeftAngleValue; + MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + } + else if (1 == g_ichangLineState) + { + BHBF_straight_drive_Cmd stCmd = { + .m_iMode = 1, + .m_iAngle = g_stCV.RobotLeftAngleValue + _iLeft_Compensation, + .m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(), + .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min + }; + MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + } + else if (2 == g_ichangLineState) + { + iTargetAngle = g_stCV.RobotUpAngleValue; + MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + } +} + static struct { int m_iPaintState; // 喷枪状态：0=打开，1=关闭 int m_iAnglelock; // 1=角度异常，触发 Move_Halt_AngleError 并屏蔽互斥代码 @@ -188,70 +293,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } case CUSTOM_CMD_STRAIGHT_DRIVE: { - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_UP, NULL, 0); + g_ichangLineState = 2; // 走到这说明换道直线走完了 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) - { - - } - 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(), - .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min - }; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_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(), - .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min - }; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_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(), - .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min - }; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_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(), - .m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min - }; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); - } - } - else if (iTargetAngle == g_stCV.RobotDownAngleValue) - { - - } - } + g_ichangLineState = 1; // 走到这说明换道第一次转完了 break; } case COMMON_CMD_STOP_ALL: @@ -340,19 +387,16 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) Move_Halt_AngleError(); - int iTargetAngle = 0; // 根据模式判断换道（仅在竖直向左或向右生效） if (g_stMK32.CH4_SA == 1000) { if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上 { - iTargetAngle = g_stCV.RobotLeftAngleValue; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + Vertical_Lane_Change_From_Right_To_Left_Down_Control(iLeft_Compensation); } else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上 { - iTargetAngle = g_stCV.RobotRightAngleValue; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + Vertical_Lane_Change_From_Left_To_Right_Down_Control(iRight_Compensation); } break; } @@ -360,13 +404,11 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) { if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上 { - iTargetAngle = g_stCV.RobotRightAngleValue; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + Vertical_Lane_Change_From_Right_To_Left_UP_Control(iLeft_Compensation); } else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上 { - iTargetAngle = g_stCV.RobotLeftAngleValue; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + Vertical_Lane_Change_From_Left_To_Right_UP_Control(iRight_Compensation); } break; } @@ -374,6 +416,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) { if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3) { + g_ichangLineState = 0; MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0); } }