81 lines
1.8 KiB
C++
81 lines
1.8 KiB
C++
#include "pid.h"
|
|
|
|
PID PIDController::pid = {0, 0, 0, 0, 0};
|
|
|
|
PIDConfig PIDController::pidConfig = {
|
|
PID_KP,
|
|
PID_KI,
|
|
PID_KD};
|
|
|
|
void PIDController::reset()
|
|
{
|
|
pid.error = 0;
|
|
pid.last_error = 0;
|
|
pid.integral = 0;
|
|
pid.derivative = 0;
|
|
pid.lastTime = millis();
|
|
}
|
|
|
|
void PIDController::setConfig(float Kp, float Ki, float Kd,
|
|
float outputMax, float outputMin,
|
|
float integralMax)
|
|
{
|
|
pidConfig.Kp = Kp;
|
|
pidConfig.Ki = Ki;
|
|
pidConfig.Kd = Kd;
|
|
pidConfig.outputMax = outputMax;
|
|
pidConfig.outputMin = outputMin;
|
|
pidConfig.integralMax = integralMax;
|
|
}
|
|
|
|
float PIDController::update(float target, float current)
|
|
{
|
|
// 计算时间间隔
|
|
unsigned long now = millis();
|
|
float dt = (now - pid.lastTime) / 1000.0f; // 转换为秒
|
|
pid.lastTime = now;
|
|
|
|
// 如果时间间隔过大，说明可能是第一次运行或长时间未运行
|
|
if (dt > 1.0f)
|
|
{
|
|
dt = 0.01f; // 使用默认值
|
|
}
|
|
|
|
// 计算误差
|
|
pid.error = target - current;
|
|
|
|
// 计算积分项
|
|
pid.integral += pid.error * dt;
|
|
|
|
// 积分限幅
|
|
if (pid.integral > pidConfig.integralMax)
|
|
{
|
|
pid.integral = pidConfig.integralMax;
|
|
}
|
|
else if (pid.integral < -pidConfig.integralMax)
|
|
{
|
|
pid.integral = -pidConfig.integralMax;
|
|
}
|
|
|
|
// 计算微分项（使用错误微分而不是输出微分，避免微分突跳）
|
|
pid.derivative = (pid.error - pid.last_error) / dt;
|
|
pid.last_error = pid.error;
|
|
|
|
// 计算PID输出
|
|
float output = pidConfig.Kp * pid.error +
|
|
pidConfig.Ki * pid.integral +
|
|
pidConfig.Kd * pid.derivative;
|
|
|
|
// 输出限幅
|
|
if (output > pidConfig.outputMax)
|
|
{
|
|
output = pidConfig.outputMax;
|
|
}
|
|
else if (output < pidConfig.outputMin)
|
|
{
|
|
output = pidConfig.outputMin;
|
|
}
|
|
|
|
return output;
|
|
}
|