From 7c013ff4a4e203549f671487b0c074a71f5a3757 Mon Sep 17 00:00:00 2001
From: topquark12 <aw@m-labs.hk>
Date: Sat, 26 Dec 2020 11:47:21 +0800
Subject: [PATCH] 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>
---
 src/pid.rs | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/pid.rs b/src/pid.rs
index 996c538..f23d636 100644
--- a/src/pid.rs
+++ b/src/pid.rs
@@ -60,20 +60,25 @@ impl Controller {
         let time_delta = time_delta.get::<second>();
 
         // error
-        let error = input - self.target;
+        let error = self.target - input;
 
         // proportional
         let p = f64::from(self.parameters.kp) * error;
 
         // 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() {
             self.integral = self.parameters.integral_min.into();
         }
         if 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
         let d = match self.last_input {