diff --git a/common/pid.py b/common/pid.py index 80fec92758..a79d176986 100644 --- a/common/pid.py +++ b/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)