From 7b80d4f0506fdc1581d547e6600d45622d91e01d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 8 Nov 2024 17:18:41 -0800 Subject: [PATCH] simplify that --- common/pid.py | 28 ++++------------------------ 1 file changed, 4 insertions(+), 24 deletions(-) diff --git a/common/pid.py b/common/pid.py index 2943e1c473..80fec92758 100644 --- a/common/pid.py +++ b/common/pid.py @@ -56,38 +56,18 @@ class PIDController: self.f = feedforward * self.k_f self.d = error_rate * self.k_d - # Check if integral this step brings us out of bounds. If so, unwind - i = self.i + error * self.k_i * self.i_rate - _control = self.p + i + self.d + self.f - - if override:# or control > self.pos_limit or control < self.neg_limit: + if override: self.i -= self.i_unwind_rate * float(np.sign(self.i)) else: - _control = self.p + i + self.d + self.f + 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 error >= 0: - # if not (_control <= self.pos_limit) and i < 0.0: - # raise Exception - # elif error <= 0: - # if not (_control >= self.neg_limit) and i > 0.0: - # raise Exception - - if self.neg_limit <= _control <= self.pos_limit: - - if ((error >= 0 and (_control <= self.pos_limit or i < 0.0)) or - (error <= 0 and (_control >= self.neg_limit or i > 0.0))) and \ - not freeze_integrator: + if (self.neg_limit <= control <= self.pos_limit or control * error < 0) and not freeze_integrator: self.i = i - _control = self.p + self.i + self.d + self.f - _control = clip(_control, self.neg_limit, self.pos_limit) - self.i = clip(self.i, self.neg_limit - _control, self.pos_limit - _control) - control = self.p + self.i + self.d + self.f - print('control', control) self.control = clip(control, self.neg_limit, self.pos_limit) return self.control