- PID: Reset yi on error sign changes

components/pid/pid.py

@@ -1,3 +1,4 @@
+import numpy as np
 
 
 class Pid:
@@ -6,12 +7,11 @@ class Pid:
         self.yi = 0
 
         # Differentiator
-        self.xd = 0
+        self.err = 0
 
         # Auto windup
         self.y_min = -1.0
         self.y_max = 1.0
-        self.d = 0
 
         # Output
         self.y = 0
@@ -23,7 +23,7 @@ class Pid:
 
     def reset(self):
         self.yi = 0
-        self.xd = 0
+        self.err = 0
 
     def process(self, dt, params, err):
         kp = params['kp']
@@ -31,21 +31,23 @@ class Pid:
         kd = params['kd']
         rho = params['rho']
 
-        yi = self.yi + ki*dt * err - ki*dt - self.d * rho
-        yd = err - self.xd
+        yi = rho*self.yi + ki*dt * err
+        yd = err - self.err
 
         _yp = kp * err
         _yi = yi
         _yd = kd/dt * yd
 
+        # Reset yi on error sign changes
+        if np.sign(err) != np.sign(self.err):
+            _yi = 0
+
         y = _yp + _yi + _yd
 
         self.y = max(self.y_min, min(self.y_max, y))
-        self.d = max(0, self.y - y)
-        if self.d < 0:
-            print("d = {}".format(self.d))
+        print("y = {}".format(_yi))
         self.yi = yi
-        self.xd = err
+        self.err = err
 
     def get_y(self):
         return self.y