From 4eb5ba8a510299b4df7563355ff343d2b09e1f06 Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Mon, 28 Sep 2026 14:46:05 +0800 Subject: [PATCH] =?UTF-8?q?PID=E4=B8=8D=E5=86=8D=E4=BD=BF=E7=94=A8?= =?UTF-8?q?=E5=AE=9A=E6=97=B6=E5=99=A8=E4=B8=AD=E6=96=AD=EF=BC=8C=E9=81=BF?= =?UTF-8?q?=E5=85=8D=E6=B6=88=E6=81=AF=E4=B8=AD=E5=BF=83=E7=AB=9E=E6=80=81?= =?UTF-8?q?=E9=94=99=E8=AF=AF=E3=80=82=E5=85=B3=E9=97=AD=E5=8F=AF=E8=83=BD?= =?UTF-8?q?=E5=AF=BC=E8=87=B4=E5=BC=82=E5=B8=B8=E7=9A=84=E4=B8=B2=E5=8F=A3?= =?UTF-8?q?=E5=85=B3=E4=B8=AD=E6=96=AD=E6=93=8D=E4=BD=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- RBcore/BHBF.c | 111 +++++++++++++++++++++- RBcore/drv_interface.c | 2 +- bspMCU/My_print.c | 2 - project/paint_robot_new/paint_robot_new.c | 40 ++++---- 4 files changed, 130 insertions(+), 25 deletions(-) diff --git a/RBcore/BHBF.c b/RBcore/BHBF.c index 1f9edd1..36b5a1e 100644 --- a/RBcore/BHBF.c +++ b/RBcore/BHBF.c @@ -19,15 +19,17 @@ #include "BHBF.h" #include "common.h" #include "msg_center.h" +#include "lua_base.h" /*----------------------------------------------* * 外部变量说明 * *----------------------------------------------*/ - +extern volatile int32_t g_RF_Angle_Roll; /*----------------------------------------------* * 外部函数原型说明 * *----------------------------------------------*/ - +double Angle_Tune_PID(double CurrentAngle, double TargetAngle, double Position_KP, + double Position_KI, double Position_KD, double MaxValue); /*----------------------------------------------* * 内部函数原型说明 * *----------------------------------------------*/ @@ -55,6 +57,11 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg) return; } + int aiMotorSpeed[2] = {0}; + + static int g_dletAngle = 0; //PID最终计算补偿值 + static uint32_t uiLastSendTick = 0; //上一次时间戳，用于控制直线行驶距离 + static int bTimerStarted = 0; //直线行驶距离单次触发标志 switch (pstMsg->m_uiMsgID) { case RBCORE_CMD_STOP_ALL: @@ -63,6 +70,106 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg) MsgCenter_SendTo(MODULE_NAME_CUSTOM, CMD_STOP_ALL, NULL, 0); break; } + case TIMER_CMD_RESET_STRAIGHT: + { + uiLastSendTick = 0; + bTimerStarted = 0; + break; + } + case TIMER_CMD_STRAIGHT_DRIVE: + { + BHBF_straight_drive_Cmd stCmd = {0}; + if (pstMsg->m_uiDataLen >= sizeof(stCmd)) + { + 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_RF_Angle_Roll - stCmd.m_iAngle) <= g_stCV.PID_mid.PID_Angle) + { + if (abs(g_RF_Angle_Roll - stCmd.m_iAngle) < g_stCV.PID_low.PID_Angle) + { + g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, 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_RF_Angle_Roll, stCmd.m_iAngle, + g_stCV.PID_mid.Kp, g_stCV.PID_mid.Ki, g_stCV.PID_mid.Kd, 10); + } + if (stCmd.m_iMode > 0) + { + aiMotorSpeed[0] = (stCmd.m_iSpeed * 10) - g_dletAngle ; + aiMotorSpeed[1] = (stCmd.m_iSpeed * 10) + g_dletAngle ; + } + else + { + aiMotorSpeed[0] = -(stCmd.m_iSpeed * 10) - g_dletAngle ; + aiMotorSpeed[1] = -(stCmd.m_iSpeed * 10) + g_dletAngle ; + } + } + else + { + g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, 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_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, NULL, 0); + } + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); + } + break; + } + case TIMER_CMD_TURN_ANGLE: + { + int iTargetAngle = 0; + if (pstMsg->m_uiDataLen >= sizeof(int)) + { + RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int)); + + if (abs(g_RF_Angle_Roll - iTargetAngle) <= 50) + { + aiMotorSpeed[0] = 0; + aiMotorSpeed[1] = 0; + MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, NULL, 0); + } + else if (abs(g_RF_Angle_Roll - iTargetAngle) <= 1000) + { + g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, iTargetAngle, 1, 0,0.5, 10); + aiMotorSpeed[0] = -g_dletAngle ; + aiMotorSpeed[1] = g_dletAngle; + } + else + { + g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, iTargetAngle, 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 CMD_SHOW_INFO: + { + 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: break; } diff --git a/RBcore/drv_interface.c b/RBcore/drv_interface.c index c0111dd..de9f110 100644 --- a/RBcore/drv_interface.c +++ b/RBcore/drv_interface.c @@ -308,6 +308,6 @@ void Drv_InterfaceInit(void) TL720D_Init(); daemon_Init(); - Timer_Init(); + //Timer_Init(); log_i("BingooRobot Init success\nversion: %s", Rd_GetBuildTime()); } diff --git a/bspMCU/My_print.c b/bspMCU/My_print.c index fbab615..4453f3b 100644 --- a/bspMCU/My_print.c +++ b/bspMCU/My_print.c @@ -241,9 +241,7 @@ WEAK void Myprint_getchar(char *ch) { int ret; - __disable_irq(); ret = rd_ComRead(g_ptUartCtrl, ch, 1); - __enable_irq(); if (ret == 1) { diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 6b578a4..1bb51a6 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -96,7 +96,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_UP_Control(void) if (0 == g_ichangLineState) { iTargetAngle = g_stCV.RobotLeftAngleValue; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); } else if (1 == g_ichangLineState) { @@ -106,12 +106,12 @@ static void Vertical_Lane_Change_From_Left_To_Right_UP_Control(void) .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)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, 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)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); } } @@ -122,7 +122,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_Down_Control(void) if (0 == g_ichangLineState) { iTargetAngle = g_stCV.RobotRightAngleValue; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); } else if (1 == g_ichangLineState) { @@ -132,12 +132,12 @@ static void Vertical_Lane_Change_From_Left_To_Right_Down_Control(void) .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)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, 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)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); } } @@ -148,7 +148,7 @@ static void Vertical_Lane_Change_From_Right_To_Left_UP_Control(void) if (0 == g_ichangLineState) { iTargetAngle = g_stCV.RobotRightAngleValue; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); } else if (1 == g_ichangLineState) { @@ -158,12 +158,12 @@ static void Vertical_Lane_Change_From_Right_To_Left_UP_Control(void) .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)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, 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)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); } } @@ -174,7 +174,7 @@ static void Vertical_Lane_Change_From_Right_To_Left_Down_Control(void) if (0 == g_ichangLineState) { iTargetAngle = g_stCV.RobotLeftAngleValue; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); } else if (1 == g_ichangLineState) { @@ -184,19 +184,19 @@ static void Vertical_Lane_Change_From_Right_To_Left_Down_Control(void) .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)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, 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)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); } } static void Lane_Change_State_Reset(void) { g_ichangLineState = 0; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_RESET_STRAIGHT, NULL, 0); } static void Robot_Move_Forward(void) @@ -237,7 +237,7 @@ static void Robot_Move_Forward_PID(void) .m_iTime = -1, .m_iSpeed = g_iVehicleSpeed }; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); } } @@ -263,7 +263,7 @@ static void Robot_Move_Backward_PID(void) .m_iTime = -1, .m_iSpeed = g_iVehicleSpeed }; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); } } @@ -601,7 +601,7 @@ void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode) if (_iMode == 2 || _iMode == 3) { g_ichangLineState = 0; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_RESET_STRAIGHT, NULL, 0); } } @@ -652,7 +652,7 @@ void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode) .m_iTime = -1, .m_iSpeed = g_iVehicleSpeed }; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); } } return; @@ -680,7 +680,7 @@ void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode) .m_iTime = -1, .m_iSpeed = g_iVehicleSpeed }; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); } } return; @@ -727,7 +727,7 @@ void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode) .m_iTime = -1, .m_iSpeed = g_iVehicleSpeed }; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); } } } @@ -754,7 +754,7 @@ void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode) .m_iTime = -1, .m_iSpeed = g_iVehicleSpeed }; - MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); } } }