diff --git a/common/pid.py b/common/pid.py index a79d176986..7f5134c132 100644 --- a/common/pid.py +++ b/common/pid.py @@ -59,15 +59,10 @@ class PIDController: if override: self.i -= self.i_unwind_rate * float(np.sign(self.i)) else: - i = self.i + error * self.k_i * self.i_rate - control = self.p + i + self.d + self.f - - # 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 not freeze_integrator: - self.i = i + self.i = self.i + error * self.k_i * self.i_rate + # Clip i to prevent exceeding control limits 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)