You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
256 lines
8.1 KiB
256 lines
8.1 KiB
/*
|
|
* msp_PID.c - 两轮差速机器人角度追踪PID（内置参数，数组输出，float版本）
|
|
*/
|
|
|
|
#include "msp_PID.h"
|
|
#include <math.h>
|
|
#include <float.h>
|
|
|
|
// ========== 内置固定参数 ==========
|
|
#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;
|
|
}
|
|
|