- PID: Modified AWU code

components/pid/pid.py

@@ -31,23 +31,20 @@ class Pid:
         kd = params['kd']
         rho = params['rho']
 
-        yi = rho*self.yi + ki*dt * err
-        yd = err - self.err
-
-        _yp = kp * err
-        _yi = yi
-        _yd = kd/dt * yd
+        self.yi = rho*self.yi + ki*dt * err
+        yd = kd/dt*(err - self.err)
+        yp = kp * err
 
         # Reset yi on error sign changes
-        if np.sign(err) != np.sign(self.err):
-            _yi = 0
+        if yp > self.y_max or yp < self.y_min:
+            self.yi = 0
 
-        y = _yp + _yi + _yd
+        self.err = err
+
+        y = yp + self.yi + yd
 
         self.y = max(self.y_min, min(self.y_max, y))
-        print("y = {}".format(_yi))
-        self.yi = yi
-        self.err = err
+        print("yi = {}".format(self.yi))
 
     def get_y(self):
         return self.y