From 9481ffeaaf9441485a4c851d7d7884fc9da7391c Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Thu, 10 Sep 2026 13:50:22 +0800 Subject: [PATCH] =?UTF-8?q?PID=E7=A7=BB=E6=A4=8D=EF=BC=8C=E4=BB=A5?= =?UTF-8?q?=E5=8F=8A=E5=A2=9E=E5=8A=A0=E8=87=AA=E5=8A=A8=E5=B7=A1=E8=88=AA?= =?UTF-8?q?=E6=8C=89=E9=94=AESB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- RBcore/BHBF.c | 93 +++++++++++++++++++--- RBcore/TL720D.c | 1 + RBcore/include/BHBF.h | 11 +-- RBcore/msp_PID.c | 95 +++++++++++++++++++++++ project/paint_robot_new/paint_robot_new.c | 15 +++- 5 files changed, 196 insertions(+), 19 deletions(-) create mode 100644 RBcore/msp_PID.c diff --git a/RBcore/BHBF.c b/RBcore/BHBF.c index fd945ac..0a1db00 100644 --- a/RBcore/BHBF.c +++ b/RBcore/BHBF.c @@ -27,7 +27,8 @@ /*----------------------------------------------* * 外部函数原型说明 * *----------------------------------------------*/ - +double Angle_Tune_PID(double CurrentAngle, double TargetAngle, double Position_KP, + double Position_KI, double Position_KD, double MaxValue); /*----------------------------------------------* * 内部函数原型说明 * *----------------------------------------------*/ @@ -55,13 +56,23 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg) return; } - static int iSpeed = -1; + static int iSpeed = -1; // 表示由RD1转换来的速度值 static int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理 + static int g_dletAngle = 0; //PID最终计算补偿值 + static int32_t g_iPIDAngle = 0; //当前角度 switch (pstMsg->m_uiMsgID) { - case COMMON_CMD_STOP_ALL: + case RBCORE_CMD_STOP_ALL: { - MsgCenter_SendTo(MODULE_NAME_MOTOR, pstMsg->m_uiMsgID, NULL, 0); + MsgCenter_SendTo(MODULE_NAME_MOTOR, COMMON_CMD_STOP_ALL, NULL, 0); + break; + } + case RBCORE_GET_TL720D_ROLL: + { + if (pstMsg->m_uiDataLen >= sizeof(int32_t)) + { + RD_MEMCPY(&g_iPIDAngle, pstMsg->m_aucData, sizeof(int32_t)); + } break; } case RBCORE_CMD_MANUAL_FORWARD: @@ -106,17 +117,75 @@ static void RBcore_ModuleHandler(const Msg_t *pstMsg) } case RBCORE_CMD_VERTICAL: { - - break; - } - case RBCORE_CMD_HORIZONTAL_LEFT: - { - + int iMode[2] = {0};//iMode[0]正数表示前进，负数表示倒退 iMode[1]是目标角度 + if (pstMsg->m_uiDataLen >= sizeof(iMode)) + { + RD_MEMCPY(&iMode, pstMsg->m_aucData, sizeof(iMode)); + if (0 == iMode[0]) + { + break; + } + + if (abs(g_iPIDAngle - iMode[1]) <= g_stCV.PID_mid.PID_Angle) + { + if (abs(g_iPIDAngle - iMode[1]) < g_stCV.PID_low.PID_Angle) + { + g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iMode[1], + 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_stCV.PID_mid.Kp, g_stCV.PID_mid.Ki, g_stCV.PID_mid.Kd, 10); + + } + if (iMode[0] > 0) + { + aiMotorSpeed[0] = iSpeed - g_dletAngle ; + aiMotorSpeed[1] = iSpeed + g_dletAngle ; + } + else + { + aiMotorSpeed[0] = -iSpeed - g_dletAngle ; + aiMotorSpeed[1] = -iSpeed + g_dletAngle ; + } + } + else + { + g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iMode[1], + g_stCV.PID_high.Kp, g_stCV.PID_high.Ki, g_stCV.PID_high.Kd,50); + aiMotorSpeed[0] = -g_dletAngle ; + aiMotorSpeed[1] = g_dletAngle ; + } + MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); + } break; } - case RBCORE_CMD_HORIZONTAL_RIGHT: + case RBCORE_CMD_TURN_ANGLE: { - + int iTargetAngle = 0; + if (pstMsg->m_uiDataLen >= sizeof(int)) + { + RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int)); + if (abs(g_iPIDAngle - iTargetAngle) <= 50) + { + aiMotorSpeed[0] = 0; + aiMotorSpeed[1] = 0; + } + else if (abs(g_iPIDAngle - iTargetAngle) <= 1000) + { + g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, iTargetAngle, 1, 0,0.5, 10); + aiMotorSpeed[0] = -g_dletAngle ; + aiMotorSpeed[1] = g_dletAngle; + } + else + { + g_dletAngle = Angle_Tune_PID((double)g_iPIDAngle, 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 RBCORE_GET_VEHICLE_SPEED: diff --git a/RBcore/TL720D.c b/RBcore/TL720D.c index cce5dbc..0999a05 100644 --- a/RBcore/TL720D.c +++ b/RBcore/TL720D.c @@ -125,6 +125,7 @@ static void decode_TL720D(const char *buf, uint32_t _iSize) { uiLastSendTick = uiNowTick; MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_GET_TL720D_ROLL, (void *)&g_stTL720D.RF_Angle_Roll, sizeof(int32_t)); + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_GET_TL720D_ROLL, (void *)&g_stTL720D.RF_Angle_Roll, sizeof(int32_t)); } } diff --git a/RBcore/include/BHBF.h b/RBcore/include/BHBF.h index 5b50b1f..8b996ce 100644 --- a/RBcore/include/BHBF.h +++ b/RBcore/include/BHBF.h @@ -74,24 +74,25 @@ extern "C"{ typedef enum { COMMON_CMD_SHOW_INFO, // 终端信息展示 - COMMON_CMD_STOP_ALL, // 机器人停止 + COMMON_CMD_STOP_ALL, // 机器人停止（建议各个控制硬件的模块都要实现） MOTOR_START = 0x0100, // *电机命令开始* MOTOR_SET_SPEED, // 设置速度 RBCORE_START = 0x0200, // *机器人共性命令开始* + RBCORE_CMD_STOP_ALL, // 此命令统一调用机器人各个模块的COMMON_CMD_STOP_ALL RBCORE_CMD_MANUAL_FORWARD, // 机器人手动前进 RBCORE_CMD_MANUAL_BACKWARD, // 机器人手动后退 RBCORE_CMD_MANUAL_TURNLEFT, // 机器人手动左转 RBCORE_CMD_MANUAL_TURNRIGHT, // 机器人手动右转 - RBCORE_CMD_VERTICAL, // 机器人竖直前进（带PID） - RBCORE_CMD_HORIZONTAL_LEFT, // 机器人水平向左（带PID） - RBCORE_CMD_HORIZONTAL_RIGHT, // 机器人水平向右（带PID） + RBCORE_CMD_VERTICAL, // 机器人竖直前进（带PID） + RBCORE_CMD_TURN_ANGLE, // 机器人转向目标角度（带PID） + RBCORE_GET_TL720D_ROLL, // 获取陀螺仪横滚角（PID计算） RBCORE_GET_VEHICLE_SPEED, // 获取机器人速度（内部速度归一化） CUSTOM_START = 0x0300, // *机器人特性命令开始* CUSTOM_GET_PV, // 获取PV CUSTOM_GET_MK32, // 获取MK32 - CUSTOM_GET_TL720D_ROLL, // 获取陀螺仪横滚角 + CUSTOM_GET_TL720D_ROLL, // 获取陀螺仪横滚角（IV显示） CUSTOM_GET_SPEED, // 获取速度（电机回码） CUSTOM_GET_FAULT_CODE, // 获取错误码（电机回码） CUSTOM_CMD_PAINTGUN, // 控制喷枪 diff --git a/RBcore/msp_PID.c b/RBcore/msp_PID.c new file mode 100644 index 0000000..ed39f12 --- /dev/null +++ b/RBcore/msp_PID.c @@ -0,0 +1,95 @@ +/* + * msp_PID.c + * + * Created on: 2024年1月18日 + * Author: Administrator + */ + +#include +#include + +void Speedl_PID(double CurrentValue, double TargetValue, double Sys_T, + double *Gain_speed); +double Desire_Angle = 0; +double PID_KP = 50; +double PID_KD = 12; +//double Sys_T=0.01;//System time +double k_1_error = 0; + + +/* PID + * CurrentAngle :0.01度 + * TargetAngle :0.01度 + * MaxValue :0.1rpm + * */ +double Angle_Tune_PID(double CurrentAngle, double TargetAngle, double Position_KP, + double Position_KI, double Position_KD, double MaxValue) +{ + static double Bias, delta_Speed, Integral_bias, Last_Bias; + /* + if (TargetValue - CurrentValue >= 180) + { + Bias = (TargetValue - CurrentValue) - 360; + } + if (pidTargetValue - pidCurrentValue <= -180) + { + Bias = TargetValue - CurrentValue + 360; + } + */ + + Bias = CurrentAngle - TargetAngle; + Integral_bias += Bias; + + + delta_Speed = Position_KP * Bias + Position_KI * Integral_bias + + Position_KD * (Bias - Last_Bias); + Last_Bias = Bias; + if (delta_Speed >= MaxValue) + { + delta_Speed = MaxValue; + } + if (delta_Speed <= -MaxValue) + { + delta_Speed = -MaxValue; + } + + return delta_Speed; +} + + + +//喷漆机器人PID算法 +int32_t PositionalPID(double pidTargetValue, double pidCurrentValue, double Kp, + double Kd) +{ + double Error = 0; + double P_Error = 0; + double D_Error = 0; + double delta = 0; + + Error = pidTargetValue - pidCurrentValue; + // + if (pidTargetValue - pidCurrentValue >= 180) + { + Error = (pidTargetValue - pidCurrentValue) - 360; + } + if (pidTargetValue - pidCurrentValue <= -180) + { + Error = pidTargetValue - pidCurrentValue + 360; + } + //计算误差 + P_Error = Error; //比例环节 + + D_Error = Error - k_1_error; //微分环节 + + //deltaSpeed = Kp * P_Error + Ki * I_Error + Kd * D_Error; //计算Speed输出值 + + delta = Kp * P_Error + Kd * D_Error; + + k_1_error = Error; + + return (int32_t) (delta); +} + + + diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 1671f7c..e33f6f9 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -143,9 +143,20 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &paintstate, sizeof(int)); } + if (g_stMK32.CH5_SB == -1000) + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_FORWARD, NULL, 0); + break; + } + else if(g_stMK32.CH5_SB == 1000) + { + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_MANUAL_BACKWARD, NULL, 0); + break; + } + if (abs(g_stMK32.CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(g_stMK32.CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance) { - MsgCenter_SendTo(MODULE_NAME_RBCORE, COMMON_CMD_STOP_ALL, NULL, 0); + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); break; } @@ -169,7 +180,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) } else { - MsgCenter_SendTo(MODULE_NAME_RBCORE, COMMON_CMD_STOP_ALL, NULL, 0); + MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0); } break; }