69 lines
1.3 KiB
C
69 lines
1.3 KiB
C
|
//
|
|||
|
|
// Created by Administrator on 2026/1/16.
|
|||
|
|
//
|
|||
|
|
|
|||
|
|
#ifndef RTTHREAD_PID_CONTROLLER_H
|
|||
|
|
#define RTTHREAD_PID_CONTROLLER_H
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
class PidController
|
|||
|
|
{
|
|||
|
|
public:
|
|||
|
|
PidController();
|
|||
|
|
|
|||
|
|
// 设置目标（通用 target）
|
|||
|
|
void setTarget(double target);
|
|||
|
|
|
|||
|
|
// PID 增益
|
|||
|
|
void setGains(double kp, double ki, double kd);
|
|||
|
|
|
|||
|
|
/// 设置积分上下限： lower <= I <= upper
|
|||
|
|
/// 若 upper <= lower，则不启用积分限幅
|
|||
|
|
void setIntegralLimits(double lower, double upper);
|
|||
|
|
|
|||
|
|
/// 设置误差死区： |error| < deadzone 时视为 0
|
|||
|
|
void setDeadzone(double deadzone);
|
|||
|
|
|
|||
|
|
/// 设置 D 项滤波系数（简单一阶滤波）
|
|||
|
|
/// coeff <= 0 表示不启用滤波
|
|||
|
|
void setDerivativeFilterCoeff(double coeff);
|
|||
|
|
|
|||
|
|
/// 重置内部状态（积分/上一拍误差/滤波器）
|
|||
|
|
void reset(double integral = 0.0, double prev_error = 0.0);
|
|||
|
|
|
|||
|
|
/// 核心：根据当前值计算控制量
|
|||
|
|
double compute(double x, double dt);
|
|||
|
|
|
|||
|
|
private:
|
|||
|
|
// gains
|
|||
|
|
double kp_;
|
|||
|
|
double ki_;
|
|||
|
|
double kd_;
|
|||
|
|
|
|||
|
|
// target
|
|||
|
|
double target_;
|
|||
|
|
|
|||
|
|
// state
|
|||
|
|
double integralError_;
|
|||
|
|
double prevError_;
|
|||
|
|
|
|||
|
|
// integral clamp
|
|||
|
|
double iLower_;
|
|||
|
|
double iUpper_;
|
|||
|
|
|
|||
|
|
// deadzone
|
|||
|
|
double deadzone_;
|
|||
|
|
|
|||
|
|
// derivative filter
|
|||
|
|
double dFilterCoeff_;
|
|||
|
|
double dErrorFiltered_;
|
|||
|
|
|
|||
|
|
bool initialized_;
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
|
|||
|
|
#endif //RTTHREAD_PID_CONTROLLER_H
|