119 lines
3.0 KiB
C
119 lines
3.0 KiB
C
#include "PID.h"
|
||
#include "main.h"
|
||
#include "M3508.h"
|
||
#include <math.h> // 用于isnan、isinf函数
|
||
#include "Remote.h"
|
||
#include "CToC.h"
|
||
/**
|
||
* @brief PID控制器初始化
|
||
*/
|
||
void PID_Init(PID_Controller_t* pid, float kp, float ki, float kd, float integral_limit, float output_limit)
|
||
{
|
||
pid->kp = kp;
|
||
pid->ki = ki;
|
||
pid->kd = kd;
|
||
pid->integral = 0.0f;
|
||
pid->prev_error = 0.0f;
|
||
pid->output = 0.0f;
|
||
pid->integral_limit = integral_limit;
|
||
pid->output_limit = output_limit;
|
||
}
|
||
|
||
/**
|
||
* @brief PID计算函数（带保护）
|
||
*/
|
||
float PID_Calculate(PID_Controller_t* pid, float error, float dt)
|
||
{
|
||
// 检查输入参数有效性
|
||
if (dt <= 0.0f || dt > 1.0f) {
|
||
dt = 0.01f; // 使用默认时间间隔
|
||
}
|
||
|
||
// 检查error是否为NaN或无穷大
|
||
if (isnan(error) || isinf(error)) {
|
||
error = 0.0f;
|
||
}
|
||
|
||
// 比例项
|
||
float proportional = pid->kp * error;
|
||
|
||
// 检查proportional是否为NaN
|
||
if (isnan(proportional) || isinf(proportional)) {
|
||
proportional = 0.0f;
|
||
}
|
||
|
||
// 积分项（带限幅）
|
||
pid->integral += error * dt;
|
||
if (pid->integral > pid->integral_limit) {
|
||
pid->integral = pid->integral_limit;
|
||
} else if (pid->integral < -pid->integral_limit) {
|
||
pid->integral = -pid->integral_limit;
|
||
}
|
||
float integral = pid->ki * pid->integral;
|
||
|
||
// 检查integral是否为NaN
|
||
if (isnan(integral) || isinf(integral)) {
|
||
integral = 0.0f;
|
||
pid->integral = 0.0f;
|
||
}
|
||
|
||
// 微分项（添加dt保护）
|
||
float derivative = 0.0f;
|
||
if (dt > 0.001f) { // 确保dt足够大，避免除以很小的数
|
||
derivative = pid->kd * (error - pid->prev_error) / dt;
|
||
}
|
||
|
||
// 检查derivative是否为NaN
|
||
if (isnan(derivative) || isinf(derivative)) {
|
||
derivative = 0.0f;
|
||
}
|
||
|
||
pid->prev_error = error;
|
||
|
||
// 计算总输出
|
||
pid->output = proportional + integral + derivative;
|
||
|
||
// 检查总输出是否为NaN
|
||
if (isnan(pid->output) || isinf(pid->output)) {
|
||
pid->output = 0.0f;
|
||
// 重置PID状态
|
||
pid->integral = 0.0f;
|
||
pid->prev_error = 0.0f;
|
||
}
|
||
|
||
// 输出限幅
|
||
if (pid->output > pid->output_limit) {
|
||
pid->output = pid->output_limit;
|
||
} else if (pid->output < -pid->output_limit) {
|
||
pid->output = -pid->output_limit;
|
||
}
|
||
|
||
return pid->output;
|
||
}
|
||
/**
|
||
* @brief 初始化所有电机的PID控制器
|
||
*/
|
||
void Motor_PID_Init(void)
|
||
{
|
||
// PID参数（需要根据实际电机调试）
|
||
float speed_kp = 400.0f; // 速度环比例系数
|
||
float speed_ki = 0.05f; // 速度环积分系数
|
||
float speed_kd = 300.0f; // 速度环微分系数
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
PID_Init(&motor_controller[i].speed_pid, speed_kp, speed_ki, speed_kd, 3000.0f, 3000.0f);
|
||
}
|
||
}
|
||
|
||
/*
|
||
*函数简介:位置式PID清理
|
||
*参数说明:位置式PID参数结构体
|
||
*返回类型:无
|
||
*备注:重置PID内部状态，用于模式切换或异常情况
|
||
*/
|
||
void PID_PositionClean(PID_Controller_t* pid)
|
||
{
|
||
pid->integral = 0.0f; // 清理积分项，防止积分饱和
|
||
pid->prev_error = 0.0f; // 清理上一次误差，防止微分冲击
|
||
pid->output = 0.0f; // 清理输出值
|
||
} |