mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-23 23:02:04 +08:00
Adeeb Shihadeh
60 lines
1.8 KiB
Python
60 lines
1.8 KiB
Python
import numpy as np
|
|
from collections.abc import Sequence
|
|
|
|
Gain = int | float | tuple[Sequence[float], Sequence[float]] | list[list[float]]
|
|
|
|
class PIDController:
|
|
def __init__(self, k_p: Gain, k_i: Gain, k_d: Gain = 0., pos_limit=1e308, neg_limit=-1e308, rate=100):
|
|
self._k_p = ([0], [k_p]) if isinstance(k_p, (int, float)) else k_p
|
|
self._k_i = ([0], [k_i]) if isinstance(k_i, (int, float)) else k_i
|
|
self._k_d = ([0], [k_d]) if isinstance(k_d, (int, float)) else k_d
|
|
|
|
self.set_limits(pos_limit, neg_limit)
|
|
|
|
self.i_dt = 1.0 / rate
|
|
self.speed = 0.0
|
|
|
|
self.reset()
|
|
|
|
@property
|
|
def k_p(self):
|
|
return np.interp(self.speed, self._k_p[0], self._k_p[1])
|
|
|
|
@property
|
|
def k_i(self):
|
|
return np.interp(self.speed, self._k_i[0], self._k_i[1])
|
|
|
|
@property
|
|
def k_d(self):
|
|
return np.interp(self.speed, self._k_d[0], self._k_d[1])
|
|
|
|
def reset(self):
|
|
self.p = 0.0
|
|
self.i = 0.0
|
|
self.d = 0.0
|
|
self.f = 0.0
|
|
self.control = 0
|
|
|
|
def set_limits(self, pos_limit, neg_limit):
|
|
self.pos_limit = pos_limit
|
|
self.neg_limit = neg_limit
|
|
|
|
def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False):
|
|
self.speed = speed
|
|
self.p = self.k_p * float(error)
|
|
self.d = self.k_d * error_rate
|
|
self.f = feedforward
|
|
|
|
if not freeze_integrator:
|
|
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
|
|
i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit
|
|
i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit
|
|
self.i = np.clip(i, i_lowerbound, i_upperbound)
|
|
|
|
control = self.p + self.i + self.d + self.f
|
|
self.control = np.clip(control, self.neg_limit, self.pos_limit)
|
|
return self.control
|