84 lines
1.9 KiB
C++
84 lines
1.9 KiB
C++
#pragma once
|
|
|
|
class PidController {
|
|
public:
|
|
PidController(float kp, float ki, float kd, float outputMin, float outputMax)
|
|
: kp_(kp),
|
|
ki_(ki),
|
|
kd_(kd),
|
|
outputMin_(outputMin),
|
|
outputMax_(outputMax) {}
|
|
|
|
void setTunings(float kp, float ki, float kd) {
|
|
kp_ = kp;
|
|
ki_ = ki;
|
|
kd_ = kd;
|
|
}
|
|
|
|
void setSetpoint(float setpoint) { setpoint_ = setpoint; }
|
|
|
|
void reset() {
|
|
integral_ = 0.0f;
|
|
prevInput_ = 0.0f;
|
|
firstSample_ = true;
|
|
}
|
|
|
|
float compute(float input, uint32_t nowMs) {
|
|
if (firstSample_) {
|
|
prevInput_ = input;
|
|
prevTimeMs_ = nowMs;
|
|
firstSample_ = false;
|
|
return outputMin_;
|
|
}
|
|
|
|
const float dt = static_cast<float>(nowMs - prevTimeMs_) / 1000.0f;
|
|
if (dt <= 0.0f) {
|
|
return lastOutput_;
|
|
}
|
|
|
|
const float error = setpoint_ - input;
|
|
integral_ += error * dt;
|
|
|
|
// Anti-windup: clamp integral so output cannot exceed limits
|
|
const float integralMax = (outputMax_ - outputMin_) / (ki_ > 0.0f ? ki_ : 1.0f);
|
|
if (integral_ > integralMax) {
|
|
integral_ = integralMax;
|
|
} else if (integral_ < 0.0f) {
|
|
integral_ = 0.0f;
|
|
}
|
|
|
|
const float derivative = (input - prevInput_) / dt;
|
|
float output = kp_ * error + ki_ * integral_ - kd_ * derivative;
|
|
|
|
if (output < outputMin_) {
|
|
output = outputMin_;
|
|
} else if (output > outputMax_) {
|
|
output = outputMax_;
|
|
}
|
|
|
|
prevInput_ = input;
|
|
prevTimeMs_ = nowMs;
|
|
lastOutput_ = output;
|
|
return output;
|
|
}
|
|
|
|
float lastOutput() const { return lastOutput_; }
|
|
|
|
float kp() const { return kp_; }
|
|
float ki() const { return ki_; }
|
|
float kd() const { return kd_; }
|
|
|
|
private:
|
|
float kp_;
|
|
float ki_;
|
|
float kd_;
|
|
float outputMin_;
|
|
float outputMax_;
|
|
float setpoint_ = 0.0f;
|
|
float integral_ = 0.0f;
|
|
float prevInput_ = 0.0f;
|
|
float lastOutput_ = 0.0f;
|
|
uint32_t prevTimeMs_ = 0;
|
|
bool firstSample_ = true;
|
|
};
|