clip instead

common/pid.py

@@ -64,9 +64,14 @@ class PIDController:
 
       # Update when changing i will move the control away from the limits
       # or when i will move towards the sign of the error
-      if (self.neg_limit <= control <= self.pos_limit or control * error < 0) and not freeze_integrator:
+      # if (self.neg_limit <= control <= self.pos_limit or control * error < 0) and not freeze_integrator:
+      if not freeze_integrator:
         self.i = i
 
+      control_no_i = self.p + self.d + self.f
+      control_no_i = clip(control_no_i, self.neg_limit, self.pos_limit)
+      self.i = clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
+
     control = self.p + self.i + self.d + self.f
 
     self.control = clip(control, self.neg_limit, self.pos_limit)