PID fixes

Flipped error calculation method to correct behavior of kP and kI terms.

Added anti integral windup to integral handling.

Changed how the i and integral term is calculated, to prevent old kI settings from affecting the current i term calculation when kI is being tuned. Especially noticable when kI is set from a non-zero value to zero.
Co-Authored-By: topquark12 <aw@m-labs.hk>
Co-Committed-By: topquark12 <aw@m-labs.hk>
This commit is contained in:
topquark12 2020-12-26 11:47:21 +08:00 committed by sb10q
parent 50a1b9f52d
commit 7c013ff4a4
1 changed files with 8 additions and 3 deletions

View File

@ -60,20 +60,25 @@ impl Controller {
let time_delta = time_delta.get::<second>(); let time_delta = time_delta.get::<second>();
// error // error
let error = input - self.target; let error = self.target - input;
// proportional // proportional
let p = f64::from(self.parameters.kp) * error; let p = f64::from(self.parameters.kp) * error;
// integral // integral
self.integral += f64::from(self.parameters.ki) * error * time_delta; if let Some(last_output_val) = self.last_output {
// anti integral windup
if last_output_val < self.parameters.output_max.into() && last_output_val > self.parameters.output_min.into() {
self.integral += error * time_delta;
}
}
if self.integral < self.parameters.integral_min.into() { if self.integral < self.parameters.integral_min.into() {
self.integral = self.parameters.integral_min.into(); self.integral = self.parameters.integral_min.into();
} }
if self.integral > self.parameters.integral_max.into() { if self.integral > self.parameters.integral_max.into() {
self.integral = self.parameters.integral_max.into(); self.integral = self.parameters.integral_max.into();
} }
let i = self.integral; let i = self.integral * f64::from(self.parameters.ki);
// derivative // derivative
let d = match self.last_input { let d = match self.last_input {