- removed dbug prints

- PID: added Anti Windup code

components/pid/pid.py

@@ -11,6 +11,7 @@ class Pid:
         # Auto windup
         self.y_min = -1.0
         self.y_max = 1.0
+        self.d = 0
 
         # Output
         self.y = 0
@@ -30,7 +31,7 @@ class Pid:
         kd = params['kd']
         rho = params['rho']
 
-        yi = rho*self.yi + ki*dt * err
+        yi = self.yi + ki*dt * err - ki*dt - self.d * rho
         yd = err - self.xd
 
         _yp = kp * err
@@ -40,7 +41,9 @@ class Pid:
         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))
         self.yi = yi
         self.xd = err