diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml b/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml index 07f8dd5..15fa460 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Inc/MSP/msp_PID.h b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Inc/MSP/msp_PID.h index fbbaf55..c7951a2 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Inc/MSP/msp_PID.h +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Inc/MSP/msp_PID.h @@ -1,27 +1,20 @@ + /* - * msp_PID.h - * - * Created on: 2024年1月22日 - * Author: Administrator + * msp_PID.h - 两轮差速机器人角度追踪PID（内置参数，数组输出） */ #ifndef INC_MSP_MSP_PID_H_ #define INC_MSP_MSP_PID_H_ +/** + * @brief 两轮差速角度控制（内置 dt=0.1s，最大速度 = 1.6 * |base_speed|） + * @param current_angle 当前角度（度） + * @param desired_angle 期望角度（度） + * @param base_speed 基础前进速度（可正可负） + * @param speeds 输出速度数组，speeds[0]=左轮，speeds[1]=右轮 + */ +void TwoWheel_AngleControl(float current_angle, float desired_angle, + float base_speed,float zoom, float speeds[2]); +void TwoWheel_AngleControl_Weld(float current_angle, float desired_angle, + float base_speed,float zoom, float speeds[2]); -#include "bsp_include.h" - -void GF_MSP_Auto_Motion_adj(double Current_Angle, double Auto_Speed, double Auto_Speed_Max, double System_time ,double *W_Speed_1); -void GF_MSP_Auto_adj_Unicycle(double Current_Angle_11,double Desire_Angle_11, double Auto_Speed, double Auto_Speed_Max, double System_time ,double *W_Speed_1); -void GF_MSP_Auto_Motion_adj_Com(double Current_Angle, double Horizontal_Compen_Angle ,double Auto_Speed, double Auto_Speed_Max, double System_time ,double *W_Speed_1); - -void GF_MSP_PID_Now_Der_adj_Com(double Current_Angle,double Desire_Angle_Input, double Auto_Speed, double Auto_Speed_Max, double System_time ,double *W_Speed_1); - - - - - - -int Angle_Tune_PID(float CurrentAngle, float TargetAngle, int Position_KP, - int Position_KI, int Position_KD, int MaxValue); - -#endif /* INC_MSP_MSP_PID_H_ */ +#endif diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_PID.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_PID.c index 8192b62..bd38ed1 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_PID.c +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_PID.c @@ -1,350 +1,151 @@ /* - * msp_PID.c - * - * Created on: 2024年1月18日 - * Author: Administrator + * msp_PID.c - 两轮差速机器人角度追踪PID（内置参数，数组输出，float版本） */ #include "msp_PID.h" #include - -void Speedl_PID(double CurrentValue, double TargetValue, double Sys_T, +#include + +// ========== 内置固定参数 ========== +#define CONTROL_DT 0.1f // 固定采样时间 0.1秒 +#define MAX_SPEED_FACTOR 1.6f // 最大速度 = 基础速度 * 1.6 + +// ========== 全局可调参数 ========== +float PID_KP = 0.5f; // 比例系数 +float PID_KD = 0.012f; // 微分系数 +float PID_KP_WELD = 0.1f; // 比例系数 +float PID_KD_WELD = 0.025f; // 微分系数 +float Kp1 = 1.0f; // 小角度增益因子 +float Kp2 = 0.95f; // 中小角度增益因子 +float Kp3 = 0.80f; // 中大角度增益因子 +float Kp4 = 0.50f; // 大角度增益因子 +double Desire_Angle = 0; //用于焊缝跟踪的期望角度 + +// 误差分段阈值（度） +static const float ANGLE_THRESHOLD1 = 2.0f; +static const float ANGLE_THRESHOLD2 = 5.0f; +static const float ANGLE_THRESHOLD3 = 10.0f; + +// ========== 内部静态变量 ========== +static float last_error = 0.0f; // 上一次误差（用于微分） + +// ========== 内部函数声明 ========== +static float normalize_angle(float angle); +static float compute_pd_output(float error,float KP,float KD); +static float get_gain_factor(float abs_error); +void Weld_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; -void Speedl_PID(double CurrentValue, double TargetValue, double Sys_T, - double *Gain_speed) +void GF_MSP_PID_Now_Der_adj_Com_Weld(double Current_Angle, double Desire_Angle_Input, + double Auto_Speed, double Auto_Speed_Max, double System_time, double *W_Speed_1); + +// ========== 函数实现 ========== + +/* 将角度归一化到 [-180, 180) */ +static float normalize_angle(float angle) { - double Error; - double P_Error; - double D_Error; - Error = TargetValue - CurrentValue; - if (Error >= 180) - { - Error = Error - 360; - } - else if (Error <= -180) - { - Error = Error + 360; - } - P_Error = Error; - D_Error = Error - k_1_error; - k_1_error = Error; - Gain_speed[0] = PID_KP * P_Error + PID_KD * D_Error / Sys_T; + while (angle >= 180.0f) angle -= 360.0f; + while (angle < -180.0f) angle += 360.0f; + return angle; } -double Kp1 = 1; -double Kp2 = 0.950; -double Kp3 = 0.80; -double Kp4 = 0.50; -/* Input: - */ -//void GF_MSP_Auto_Motion_adj(double Current_Angle, double Auto_Speed, double Auto_Speed_Max, double System_time ,double *W_Speed_1) -void GF_MSP_Auto_Motion_adj(double Current_Angle, double Auto_Speed, - double Auto_Speed_Max, double System_time, double *W_Speed_1) +/* PD控制器：输入误差，输出控制增量（使用固定 dt = CONTROL_DT） */ +static float compute_pd_output(float error,float KP,float KD) { - // double Desire_Angle=0; - //double Horizontal_Compen_Angle=0;, - double Horizontal_Compen_Angle = 0; - if ((Current_Angle >= 45) & (Current_Angle < 135)) - { - Desire_Angle = 90 + Horizontal_Compen_Angle; - } - else if ((Current_Angle <= -45) & (Current_Angle >= -135)) - { - Desire_Angle = -90 + Horizontal_Compen_Angle; - } - else if ((Current_Angle > -45) & (Current_Angle < 45)) - { - Desire_Angle = 0 + Horizontal_Compen_Angle; - } - else - { - Desire_Angle = 180 + Horizontal_Compen_Angle; - } - //水平 前进抬车头 - double Detal_Angle = fabs(Current_Angle - Desire_Angle); - double Incre_Speed[1]; - Speedl_PID(Current_Angle, Desire_Angle, System_time, Incre_Speed); - Incre_Speed[0] = 1 * Incre_Speed[0]; - double deltaAngle1 = 2; - double deltaAngle2 = 5; - double deltaAngle3 = 10; - //double deltaAngle4=25; - double LeftSpeed_Con_1; - double RightSpeed_Con_1; - double LeftSpeed_Con_2; - double RightSpeed_Con_2; - if (Detal_Angle <= deltaAngle1) - { - LeftSpeed_Con_1 = (Auto_Speed + Kp1 * Incre_Speed[0]); - RightSpeed_Con_1 = (Auto_Speed - Kp1 * Incre_Speed[0]); - LeftSpeed_Con_2 = Auto_Speed + Kp1 * Incre_Speed[0]; - RightSpeed_Con_2 = Auto_Speed - Kp1 * Incre_Speed[0]; -//小角度时，两前轮调整 - } - else if (Detal_Angle > deltaAngle1 && Detal_Angle <= deltaAngle2) - { - LeftSpeed_Con_1 = (Auto_Speed + Kp2 * Incre_Speed[0]); - RightSpeed_Con_1 = (Auto_Speed - Kp2 * Incre_Speed[0]); - LeftSpeed_Con_2 = Auto_Speed + Kp2 * Incre_Speed[0]; - RightSpeed_Con_2 = Auto_Speed - Kp2 * Incre_Speed[0]; -//中小角度时，两前轮差速，两后轮跟踪调整 - } - else if (Detal_Angle > deltaAngle2 && Detal_Angle <= deltaAngle3) - { - LeftSpeed_Con_1 = (Auto_Speed + Kp3 * Incre_Speed[0]); - RightSpeed_Con_1 = (Auto_Speed - Kp3 * Incre_Speed[0]); - LeftSpeed_Con_2 = Auto_Speed + Kp3 * Incre_Speed[0]; - RightSpeed_Con_2 = Auto_Speed - Kp3 * Incre_Speed[0]; - } - else - { - LeftSpeed_Con_1 = (Auto_Speed + Kp4 * Incre_Speed[0]); - RightSpeed_Con_1 = (Auto_Speed - Kp4 * Incre_Speed[0]); - LeftSpeed_Con_2 = Auto_Speed + Kp4 * Incre_Speed[0]; - RightSpeed_Con_2 = Auto_Speed - Kp4 * Incre_Speed[0]; - } - - double Jud_value = fabs(LeftSpeed_Con_1) - fabs(RightSpeed_Con_1); - double factor = 0; - if (Jud_value >= 0) - { - if (LeftSpeed_Con_1 > Auto_Speed_Max) - { - factor = Auto_Speed_Max / LeftSpeed_Con_1; - LeftSpeed_Con_1 = Auto_Speed_Max; - LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor; - RightSpeed_Con_1 = RightSpeed_Con_1 * factor; - RightSpeed_Con_2 = RightSpeed_Con_2 * factor; - } - else if (LeftSpeed_Con_1 < -Auto_Speed_Max) - { - factor = -Auto_Speed_Max / LeftSpeed_Con_1; - LeftSpeed_Con_1 = -Auto_Speed_Max; - LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor; - RightSpeed_Con_1 = RightSpeed_Con_1 * factor; - RightSpeed_Con_2 = RightSpeed_Con_2 * factor; - } - } - else - { - if (RightSpeed_Con_1 > Auto_Speed_Max) - { - factor = Auto_Speed_Max / RightSpeed_Con_1; - RightSpeed_Con_1 = Auto_Speed_Max; - RightSpeed_Con_2 = RightSpeed_Con_2 * factor; - - LeftSpeed_Con_1 = LeftSpeed_Con_1 * factor; - LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor; - } - else if (LeftSpeed_Con_2 < -Auto_Speed_Max) - { - factor = -Auto_Speed_Max / RightSpeed_Con_1; - RightSpeed_Con_1 = -Auto_Speed_Max; - RightSpeed_Con_2 = RightSpeed_Con_2 * factor; - - LeftSpeed_Con_1 = LeftSpeed_Con_1 * factor; - LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor; - } - } - - W_Speed_1[0] = LeftSpeed_Con_1; - W_Speed_1[1] = RightSpeed_Con_1; - W_Speed_1[2] = LeftSpeed_Con_2; - W_Speed_1[3] = RightSpeed_Con_2; - + float p_term = KP * error; + float d_term = KD * (error - last_error) / CONTROL_DT; + last_error = error; + return p_term + d_term; } -void GF_MSP_Auto_adj_Unicycle(double Current_Angle_11, double Desire_Angle_11, - double Auto_Speed, double Auto_Speed_Max, double System_time, - double *W_Speed_1) +/* 根据误差绝对值选择增益因子 */ +static float get_gain_factor(float abs_error) { - //水平 前进抬车头 - double Detal_Angle = fabs(Current_Angle_11 - Desire_Angle_11); - double Incre_Speed[1]; - - Speedl_PID(Current_Angle_11, Desire_Angle_11, System_time, Incre_Speed); - Incre_Speed[0] = 1 * Incre_Speed[0]; - - double deltaAngle0 = 1; - double deltaAngle1 = 2; - double deltaAngle2 = 5; - double deltaAngle3 = 10; - //double deltaAngle4=25; - double LeftSpeed_Con_1; - - if (Detal_Angle <= deltaAngle0) - { - LeftSpeed_Con_1 = 0; - } - else if (Detal_Angle <= deltaAngle1) - { - LeftSpeed_Con_1 = (Auto_Speed + Kp1 * Incre_Speed[0] / 2); - } - else if (Detal_Angle > deltaAngle1 && Detal_Angle <= deltaAngle2) - { - LeftSpeed_Con_1 = (Auto_Speed + Kp2 * Incre_Speed[0] / 2); - } - else if (Detal_Angle > deltaAngle2 && Detal_Angle <= deltaAngle3) - { - LeftSpeed_Con_1 = (Auto_Speed + Kp3 * Incre_Speed[0] / 2); - } - else - { - LeftSpeed_Con_1 = (Auto_Speed + Kp4 * Incre_Speed[0] / 2); - } - - if (LeftSpeed_Con_1 > Auto_Speed_Max) - { - LeftSpeed_Con_1 = Auto_Speed_Max; - } - else if (LeftSpeed_Con_1 < -Auto_Speed_Max) - { - LeftSpeed_Con_1 = -Auto_Speed_Max; - } - else - { - //LeftSpeed_Con_1=LeftSpeed_Con_1; - } - W_Speed_1[0] = LeftSpeed_Con_1; + if (abs_error <= ANGLE_THRESHOLD1) return Kp1; + if (abs_error <= ANGLE_THRESHOLD2) return Kp2; + if (abs_error <= ANGLE_THRESHOLD3) return Kp3; + return Kp4; } -void GF_MSP_Auto_Motion_adj_Com(double Current_Angle, - double Horizontal_Compen_Angle, double Auto_Speed, - double Auto_Speed_Max, double System_time, double *W_Speed_1) +/* 两轮差速角度控制（对外接口） */ +void TwoWheel_AngleControl(float current_angle, float desired_angle, + float base_speed,float zoom, float speeds[2]) { - // double Desire_Angle=0; - //double Horizontal_Compen_Angle=0; - if ((Current_Angle >= 45) & (Current_Angle < 135)) - { - Desire_Angle = 90 + Horizontal_Compen_Angle; - } - else if ((Current_Angle <= -45) & (Current_Angle >= -135)) - { - Desire_Angle = -90 + Horizontal_Compen_Angle; - } - else if ((Current_Angle > -45) & (Current_Angle < 45)) - { - Desire_Angle = 0 + Horizontal_Compen_Angle; - } - else - { - Desire_Angle = 180 + Horizontal_Compen_Angle; - if (Desire_Angle > 180) - { - Desire_Angle = Desire_Angle - 360; - } - } - //水平 前进抬车头 - double Detal_Angle = fabs(Current_Angle - Desire_Angle); - double Incre_Speed[1]; - Speedl_PID(Current_Angle, Desire_Angle, System_time, Incre_Speed); - Incre_Speed[0] = 1 * Incre_Speed[0]; - double deltaAngle1 = 2; - double deltaAngle2 = 5; - double deltaAngle3 = 10; - //double deltaAngle4=25; - double LeftSpeed_Con_1; - double RightSpeed_Con_1; - double LeftSpeed_Con_2; - double RightSpeed_Con_2; - if (Detal_Angle <= deltaAngle1) - { - LeftSpeed_Con_1 = (Auto_Speed + Kp1 * Incre_Speed[0]); - RightSpeed_Con_1 = (Auto_Speed - Kp1 * Incre_Speed[0]); - LeftSpeed_Con_2 = Auto_Speed + Kp1 * Incre_Speed[0]; - RightSpeed_Con_2 = Auto_Speed - Kp1 * Incre_Speed[0]; -//小角度时，两前轮调整 - } - else if (Detal_Angle > deltaAngle1 && Detal_Angle <= deltaAngle2) - { - LeftSpeed_Con_1 = (Auto_Speed + Kp2 * Incre_Speed[0]); - RightSpeed_Con_1 = (Auto_Speed - Kp2 * Incre_Speed[0]); - LeftSpeed_Con_2 = Auto_Speed + Kp2 * Incre_Speed[0]; - RightSpeed_Con_2 = Auto_Speed - Kp2 * Incre_Speed[0]; -//中小角度时，两前轮差速，两后轮跟踪调整 - } - else if (Detal_Angle > deltaAngle2 && Detal_Angle <= deltaAngle3) - { - LeftSpeed_Con_1 = (Auto_Speed + Kp3 * Incre_Speed[0]); - RightSpeed_Con_1 = (Auto_Speed - Kp3 * Incre_Speed[0]); - LeftSpeed_Con_2 = Auto_Speed + Kp3 * Incre_Speed[0]; - RightSpeed_Con_2 = Auto_Speed - Kp3 * Incre_Speed[0]; - } - else - { - LeftSpeed_Con_1 = (Auto_Speed + Kp4 * Incre_Speed[0]); - RightSpeed_Con_1 = (Auto_Speed - Kp4 * Incre_Speed[0]); - LeftSpeed_Con_2 = Auto_Speed + Kp4 * Incre_Speed[0]; - RightSpeed_Con_2 = Auto_Speed - Kp4 * Incre_Speed[0]; - } - - double Jud_value = fabs(LeftSpeed_Con_1) - fabs(RightSpeed_Con_1); - double factor = 0; - if (Jud_value >= 0) - { - if (LeftSpeed_Con_1 > Auto_Speed_Max) - { - factor = Auto_Speed_Max / LeftSpeed_Con_1; - LeftSpeed_Con_1 = Auto_Speed_Max; - LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor; - RightSpeed_Con_1 = RightSpeed_Con_1 * factor; - RightSpeed_Con_2 = RightSpeed_Con_2 * factor; - } - else if (LeftSpeed_Con_1 < -Auto_Speed_Max) - { - factor = -Auto_Speed_Max / LeftSpeed_Con_1; - LeftSpeed_Con_1 = -Auto_Speed_Max; - LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor; - RightSpeed_Con_1 = RightSpeed_Con_1 * factor; - RightSpeed_Con_2 = RightSpeed_Con_2 * factor; - } - } - else - { - if (RightSpeed_Con_1 > Auto_Speed_Max) - { - factor = Auto_Speed_Max / RightSpeed_Con_1; - RightSpeed_Con_1 = Auto_Speed_Max; - RightSpeed_Con_2 = RightSpeed_Con_2 * factor; - - LeftSpeed_Con_1 = LeftSpeed_Con_1 * factor; - LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor; - } - else if (LeftSpeed_Con_2 < -Auto_Speed_Max) - { - factor = -Auto_Speed_Max / RightSpeed_Con_1; - RightSpeed_Con_1 = -Auto_Speed_Max; - RightSpeed_Con_2 = RightSpeed_Con_2 * factor; + // 1. 计算归一化误差 + float raw_error = desired_angle - current_angle; + float error = normalize_angle(raw_error); + float abs_error = fabsf(error); + + // 2. PD控制量 + float delta = zoom*compute_pd_output(error,PID_KP,PID_KD); + + // 3. 根据误差大小选择增益因子 + float factor = get_gain_factor(abs_error); + + // 4. 计算左右轮速度 + float left = base_speed + factor * delta; + float right = base_speed - factor * delta; + + // 5. 内置最大速度 = 1.6 * |base_speed| + float max_speed = fabsf(base_speed) * MAX_SPEED_FACTOR; + + // 6. 限幅：等比缩放至最大速度以内 + float max_abs = fmaxf(fabsf(left), fabsf(right)); + if (max_abs > max_speed && max_abs > FLT_MIN) { + float scale = max_speed / max_abs; + left *= scale; + right *= scale; + } + + speeds[0] = left; + speeds[1] = right; +} - LeftSpeed_Con_1 = LeftSpeed_Con_1 * factor; - LeftSpeed_Con_2 = LeftSpeed_Con_2 * factor; - } - } +/* 两轮差速角度控制（对外接口） */ +void TwoWheel_AngleControl_Weld(float current_angle, float desired_angle, + float base_speed,float zoom, float speeds[2]) +{ + // 1. 计算归一化误差 + float raw_error = desired_angle - current_angle; + float error = normalize_angle(raw_error); + float abs_error = fabsf(error); + + // 2. PD控制量 + float delta = zoom*compute_pd_output(error,PID_KP_WELD,PID_KD_WELD); + + // 3. 根据误差大小选择增益因子 + float factor = get_gain_factor(abs_error); + + // 4. 计算左右轮速度 + float left = base_speed + factor * delta; + float right = base_speed - factor * delta; + + // 5. 内置最大速度 = 1.6 * |base_speed| + float max_speed = fabsf(base_speed) * MAX_SPEED_FACTOR; + + // 6. 限幅：等比缩放至最大速度以内 + float max_abs = fmaxf(fabsf(left), fabsf(right)); + if (max_abs > max_speed && max_abs > FLT_MIN) { + float scale = max_speed / max_abs; + left *= scale; + right *= scale; + } + + speeds[0] = left; + speeds[1] = right; +} - W_Speed_1[0] = LeftSpeed_Con_1; - W_Speed_1[1] = RightSpeed_Con_1; - W_Speed_1[2] = LeftSpeed_Con_2; - W_Speed_1[3] = RightSpeed_Con_2; -} -void GF_MSP_PID_Now_Der_adj_Com(double Current_Angle, double Desire_Angle_Input, - double Auto_Speed, double Auto_Speed_Max, double System_time, - double *W_Speed_1) +void GF_MSP_PID_Now_Der_adj_Com_Weld(double Current_Angle, double Desire_Angle_Input, double Auto_Speed, double Auto_Speed_Max, double System_time, double *W_Speed_1) { Desire_Angle = Desire_Angle_Input; //水平 前进抬车头 double Detal_Angle = fabs(Current_Angle - Desire_Angle); double Incre_Speed[1]; - Speedl_PID(Current_Angle, Desire_Angle, System_time, Incre_Speed); - Incre_Speed[0] = 1 * Incre_Speed[0]; - double deltaAngle1 = 2; + Weld_PID(Current_Angle, Desire_Angle, System_time, Incre_Speed); + Incre_Speed[0] = 8 * Incre_Speed[0]; + double deltaAngle1 = 0.2; double deltaAngle2 = 5; double deltaAngle3 = 10; //double deltaAngle4=25; @@ -352,36 +153,38 @@ void GF_MSP_PID_Now_Der_adj_Com(double Current_Angle, double Desire_Angle_Input, double RightSpeed_Con_1; double LeftSpeed_Con_2; double RightSpeed_Con_2; - if (Detal_Angle <= deltaAngle1) - { - LeftSpeed_Con_1 = (Auto_Speed + Kp1 * Incre_Speed[0]); - RightSpeed_Con_1 = (Auto_Speed - Kp1 * Incre_Speed[0]); - LeftSpeed_Con_2 = Auto_Speed + Kp1 * Incre_Speed[0]; - RightSpeed_Con_2 = Auto_Speed - Kp1 * Incre_Speed[0]; -//小角度时，两前轮调整 - } - else if (Detal_Angle > deltaAngle1 && Detal_Angle <= deltaAngle2) - { - LeftSpeed_Con_1 = (Auto_Speed + Kp2 * Incre_Speed[0]); - RightSpeed_Con_1 = (Auto_Speed - Kp2 * Incre_Speed[0]); - LeftSpeed_Con_2 = Auto_Speed + Kp2 * Incre_Speed[0]; - RightSpeed_Con_2 = Auto_Speed - Kp2 * Incre_Speed[0]; -//中小角度时，两前轮差速，两后轮跟踪调整 - } - else if (Detal_Angle > deltaAngle2 && Detal_Angle <= deltaAngle3) - { - LeftSpeed_Con_1 = (Auto_Speed + Kp3 * Incre_Speed[0]); - RightSpeed_Con_1 = (Auto_Speed - Kp3 * Incre_Speed[0]); - LeftSpeed_Con_2 = Auto_Speed + Kp3 * Incre_Speed[0]; - RightSpeed_Con_2 = Auto_Speed - Kp3 * Incre_Speed[0]; - } - else - { - LeftSpeed_Con_1 = (Auto_Speed + Kp4 * Incre_Speed[0]); - RightSpeed_Con_1 = (Auto_Speed - Kp4 * Incre_Speed[0]); - LeftSpeed_Con_2 = Auto_Speed + Kp4 * Incre_Speed[0]; - RightSpeed_Con_2 = Auto_Speed - Kp4 * Incre_Speed[0]; - } + + LeftSpeed_Con_1 = (Auto_Speed + Kp1 * Incre_Speed[0]); + RightSpeed_Con_1 = (Auto_Speed - Kp1 * Incre_Speed[0]); + LeftSpeed_Con_2 = Auto_Speed + Kp1 * Incre_Speed[0]; + RightSpeed_Con_2 = Auto_Speed - Kp1 * Incre_Speed[0]; +// if (Detal_Angle >= deltaAngle1) +// { +// +////小角度时，两前轮调整 +// } +// else if (Detal_Angle > deltaAngle1 && Detal_Angle <= deltaAngle2) +// { +// LeftSpeed_Con_1 = (Auto_Speed + Kp2 * Incre_Speed[0]); +// RightSpeed_Con_1 = (Auto_Speed - Kp2 * Incre_Speed[0]); +// LeftSpeed_Con_2 = Auto_Speed + Kp2 * Incre_Speed[0]; +// RightSpeed_Con_2 = Auto_Speed - Kp2 * Incre_Speed[0]; +////中小角度时，两前轮差速，两后轮跟踪调整 +// } +// else if (Detal_Angle > deltaAngle2 && Detal_Angle <= deltaAngle3) +// { +// LeftSpeed_Con_1 = (Auto_Speed + Kp3 * Incre_Speed[0]); +// RightSpeed_Con_1 = (Auto_Speed - Kp3 * Incre_Speed[0]); +// LeftSpeed_Con_2 = Auto_Speed + Kp3 * Incre_Speed[0]; +// RightSpeed_Con_2 = Auto_Speed - Kp3 * Incre_Speed[0]; +// } +// else +// { +// LeftSpeed_Con_1 = (Auto_Speed + Kp4 * Incre_Speed[0]); +// RightSpeed_Con_1 = (Auto_Speed - Kp4 * Incre_Speed[0]); +// LeftSpeed_Con_2 = Auto_Speed + Kp4 * Incre_Speed[0]; +// RightSpeed_Con_2 = Auto_Speed - Kp4 * Incre_Speed[0]; +// } double Jud_value = fabs(LeftSpeed_Con_1) - fabs(RightSpeed_Con_1); double factor = 0; @@ -433,78 +236,21 @@ void GF_MSP_PID_Now_Der_adj_Com(double Current_Angle, double Desire_Angle_Input, } -//喷漆机器人PID算法 -int32_t PositionalPID(double pidTargetValue, double pidCurrentValue, double Kp, - double Kd) -{ - double Error = 0; - double P_Error = 0; - double I_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); -} - -int Angle_Tune_PID(float CurrentAngle, float TargetAngle, int Position_KP, - int Position_KI, int Position_KD, int MaxValue) +double Weld_KP = 10; +double Weld_KD = 10; +//double Sys_T=0.01;//System time +double Weld_1_error = 0; +void Weld_PID(double CurrentValue, double TargetValue, double Sys_T, + double *Gain_speed) { - static float Bias, delta_Speed, Integral_bias, Last_Bias; - Bias = CurrentAngle - TargetAngle; - Integral_bias += Bias; - + double Error; + double P_Error; + double D_Error; + Error = TargetValue - CurrentValue; - 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 (int) delta_Speed; + P_Error = Error; + D_Error = Error - Weld_1_error; + Weld_1_error = Error; + Gain_speed[0] = Weld_KP * P_Error + Weld_KD * D_Error / Sys_T; } - -//int Angle_Tune_PID(float CurrentAngle, float TargetAngle, int Position_KP, -// int Position_KI, int Position_KD, int MaxValue) -//{ -// static float Bias, delta_Speed, Integral_bias, Last_Bias; -// 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 (int) delta_Speed; -//} - diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c index aa84a89..6a33201 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c @@ -20,6 +20,20 @@ double RP_Speed_Ctrl = 0; float speedAdj = 0; +#define MOTOR_TO_COUNT 164.433 + +void SetMoveMotorSpeed(double Speed) +{ + double Inspeed = (Speed*360/100/101/6*3.1415926*0.325);//转换为m/min进入PID + + float Outspeed[2] = {0}; + TwoWheel_AngleControl(GV.TL720DParameters.RF_Angle_Roll, 0, Inspeed, 1, Outspeed); + + GV.LeftFrontMotor.Target_Velcity = Outspeed[0] * MOTOR_TO_COUNT; + GV.RightFrontMotor.Target_Velcity = Outspeed[1] * MOTOR_TO_COUNT; + GV.LeftBackMotor.Target_Velcity = Outspeed[0] * MOTOR_TO_COUNT; + GV.RightBackMotor.Target_Velcity = Outspeed[1] * MOTOR_TO_COUNT; +} void Manual_State_Do(void) { @@ -402,10 +416,7 @@ void auto_forward_state_do(void) Act_Speed =Speed_Ctrl; if(Ref_Speed >= 5000) Ref_Speed = 5000; - GV.LeftFrontMotor.Target_Velcity = Ref_Speed; - GV.RightFrontMotor.Target_Velcity = -Ref_Speed; - GV.LeftBackMotor.Target_Velcity = Ref_Speed; - GV.RightBackMotor.Target_Velcity = -Ref_Speed; + SetMoveMotorSpeed(Ref_Speed); } void auto_backward_state_do(void) @@ -420,10 +431,7 @@ void auto_backward_state_do(void) Act_Speed =Speed_Ctrl; if(Ref_Speed >= 5000) Ref_Speed = 5000; - GV.LeftFrontMotor.Target_Velcity = -Ref_Speed; - GV.RightFrontMotor.Target_Velcity = Ref_Speed; - GV.LeftBackMotor.Target_Velcity = -Ref_Speed; - GV.RightBackMotor.Target_Velcity = Ref_Speed; + SetMoveMotorSpeed(-Ref_Speed); }