pid: move ki coefficient inside integration

This commit is contained in:
Astro 2020-10-11 01:56:22 +02:00
parent f12214a4df
commit 83a266852a
1 changed files with 4 additions and 4 deletions

View File

@ -52,20 +52,20 @@ impl Controller {
// partial // partial
let p = f64::from(self.parameters.kp) * error; let p = f64::from(self.parameters.kp) * error;
//integral // integral
self.integral += error; self.integral += f64::from(self.parameters.ki) * error;
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 = f64::from(self.parameters.ki) * f64::from(self.integral); let i = self.integral;
// derivative // derivative
let d = match self.last_input { let d = match self.last_input {
None => 0.0, None => 0.0,
Some(last_input) => f64::from(self.parameters.kd) * (last_input - input) Some(last_input) => f64::from(self.parameters.kd) * (last_input - input),
}; };
self.last_input = Some(input); self.last_input = Some(input);