123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475 |
- import numpy as np
- from numbers import Number
- from openpilot.common.numpy_fast import clip, interp
- class PIDController:
- def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
- self._k_p = k_p
- self._k_i = k_i
- self._k_d = k_d
- self.k_f = k_f # feedforward gain
- if isinstance(self._k_p, Number):
- self._k_p = [[0], [self._k_p]]
- if isinstance(self._k_i, Number):
- self._k_i = [[0], [self._k_i]]
- if isinstance(self._k_d, Number):
- self._k_d = [[0], [self._k_d]]
- self.pos_limit = pos_limit
- self.neg_limit = neg_limit
- self.i_unwind_rate = 0.3 / rate
- self.i_rate = 1.0 / rate
- self.speed = 0.0
- self.reset()
- @property
- def k_p(self):
- return interp(self.speed, self._k_p[0], self._k_p[1])
- @property
- def k_i(self):
- return interp(self.speed, self._k_i[0], self._k_i[1])
- @property
- def k_d(self):
- return interp(self.speed, self._k_d[0], self._k_d[1])
- @property
- def error_integral(self):
- return self.i/self.k_i
- def reset(self):
- self.p = 0.0
- self.i = 0.0
- self.d = 0.0
- self.f = 0.0
- self.control = 0
- def update(self, error, error_rate=0.0, speed=0.0, override=False, 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
- 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 ((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:
- self.i = i
- control = self.p + self.i + self.d + self.f
- self.control = clip(control, self.neg_limit, self.pos_limit)
- return self.control
|