/* * msp_PID.c - 两轮差速机器人角度追踪PID（内置参数，数组输出，float版本） */ #include "msp_PID.h" #include #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); 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) { while (angle >= 180.0f) angle -= 360.0f; while (angle < -180.0f) angle += 360.0f; return angle; } /* PD控制器：输入误差，输出控制增量（使用固定 dt = CONTROL_DT） */ static float compute_pd_output(float error,float KP,float KD) { float p_term = KP * error; float d_term = KD * (error - last_error) / CONTROL_DT; last_error = error; return p_term + d_term; } /* 根据误差绝对值选择增益因子 */ static float get_gain_factor(float abs_error) { if (abs_error <= ANGLE_THRESHOLD1) return Kp1; if (abs_error <= ANGLE_THRESHOLD2) return Kp2; if (abs_error <= ANGLE_THRESHOLD3) return Kp3; return Kp4; } /* 两轮差速角度控制（对外接口） */ void TwoWheel_AngleControl(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,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; } /* 两轮差速角度控制（对外接口） */ 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; } 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]; 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; double LeftSpeed_Con_1; double RightSpeed_Con_1; double LeftSpeed_Con_2; double RightSpeed_Con_2; 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; 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; } 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) { double Error; double P_Error; double D_Error; Error = TargetValue - CurrentValue; 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; }