-added anti windup

components/pid/pid.py

@@ -11,30 +11,32 @@ class Pid:
         # Differentiator
         self.err = 0
 
-        # Auto windup
+        # Anti windup
         self.y_min = -1.0
         self.y_max = 1.0
+        self.awu = 0
 
         # Output
         self.y = 0
 
     @staticmethod
-    def params(kp, ki, kd, rho):
-        p = dict(kp=kp, ki=ki, kd=kd, rho=rho)
+    def params(kp, ki, kd, kt):
+        p = dict(kp=kp, ki=ki, kd=kd, kt=kt)
         return p
 
     def reset(self):
         self.yi = 0
         self.err = 0
+        self.awu = 0
 
     def process(self, err):
         dt = self.params['dt']
         kp = self.params['kp']
         ki = self.params['ki']
         kd = self.params['kd']
-        rho = self.params['rho']
+        kt = self.params['kt']
 
-        self.yi = rho*self.yi + ki*dt * err
+        self.yi = self.yi + ki*dt * err + kt*dt * self.awu
         yd = kd/dt*(err - self.err)
         yp = kp * err
 
@@ -44,5 +46,8 @@ class Pid:
 
         self.y = max(self.y_min, min(self.y_max, y))
 
+        # Anti windup
+        self.awu = self.y - y
+
     def get_y(self):
         return self.y