PID_fix #35

Merged
sb10q merged 4 commits from PID_fix into master 2020-12-26 11:47:21 +08:00
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 {