PID: coefficients should be in front, i_rate should be i_dt (#36288)

pull/36289/head
felsager 5 days ago committed by GitHub
parent 4c9ca91b98
commit 22d5cbd0fa
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
  1. 10
      common/pid.py

@ -16,7 +16,7 @@ class PIDController:
self.set_limits(pos_limit, neg_limit)
self.i_rate = 1.0 / rate
self.i_dt = 1.0 / rate
self.speed = 0.0
self.reset()
@ -46,12 +46,12 @@ class PIDController:
def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False):
self.speed = speed
self.p = float(error) * self.k_p
self.f = feedforward * self.k_f
self.d = error_rate * self.k_d
self.p = self.k_p * float(error)
self.d = self.k_d * error_rate
self.f = self.k_f * feedforward
if not freeze_integrator:
i = self.i + error * self.k_i * self.i_rate
i = self.i + self.k_i * self.i_dt * error
# Don't allow windup if already clipping
test_control = self.p + i + self.d + self.f

Loading…
Cancel
Save