86 lines
2.4 KiB
Python
86 lines
2.4 KiB
Python
from typing import Optional, Tuple
|
|
|
|
|
|
def interp(v: float, table: Optional[Tuple]) -> float:
|
|
if table is None:
|
|
return 0
|
|
if v < table[0][0]:
|
|
return table[0][1]
|
|
if v > table[-1][0]:
|
|
return table[-1][1]
|
|
for i in range(1, len(table)):
|
|
if v <= table[i][0]:
|
|
x0, y0 = table[i - 1]
|
|
x1, y1 = table[i]
|
|
x = (v - x0) / (x1 - x0)
|
|
return y0 + (y1 - y0) * x
|
|
return table[-1][1]
|
|
|
|
|
|
class PID:
|
|
def __init__(self,
|
|
kp: float = 0,
|
|
ki: float = 0,
|
|
kd: float = 0,
|
|
ff: tuple = None,
|
|
deadband: Optional[tuple] = None,
|
|
windup_limit: Optional[float] = None,
|
|
lower_limit: float = None,
|
|
upper_limit: float = None):
|
|
self._kp = kp
|
|
self._ki = ki
|
|
self._kd = kd
|
|
self._windup_limit = windup_limit
|
|
self._ff = ff
|
|
self._deadband = deadband
|
|
self._lower_limit = lower_limit if lower_limit is not None else -1e12
|
|
self._upper_limit = upper_limit if upper_limit is not None else 1e12
|
|
self.enabled = True
|
|
|
|
self.sp = 0.
|
|
self._ti = 0.
|
|
self._errs = [0.] * 3
|
|
self._t = None
|
|
|
|
def step(self, t: float, pv: float) -> float:
|
|
err = self.sp - pv
|
|
|
|
td = 0
|
|
if self._t is not None:
|
|
dt = t - self._t
|
|
self._ti += err * dt
|
|
|
|
if self._windup_limit is not None:
|
|
self._ti = max(-self._windup_limit, min(self._windup_limit, self._ti))
|
|
self._t = t
|
|
|
|
if not self.enabled:
|
|
self._ti = 0
|
|
|
|
u = self._kp * err + self._ki * self._ti + self._kd * td + interp(pv, self._ff)
|
|
if self._deadband is not None:
|
|
if u >= self._deadband[0]:
|
|
u += self._deadband[1]
|
|
elif u <= -self._deadband[0]:
|
|
u -= self._deadband[1]
|
|
u = min(self._upper_limit, max(self._lower_limit, u))
|
|
|
|
return u
|
|
|
|
|
|
class LPF:
|
|
def __init__(self):
|
|
self.c_ = 0
|
|
self.vs_ = (0., 0., 0.)
|
|
self.fs_ = (0., 0., 0.)
|
|
|
|
def step(self, v: float) -> float:
|
|
c = self.c_
|
|
vs = (v, self.vs_[0], self.vs_[1])
|
|
fs = self.fs_
|
|
f = (1 / (1 + c * c + 1.414 * c)) * (
|
|
vs[2] + 2 * vs[1] + vs[0] - (c * c - 1.414 * c + 1) * fs[1] - (-2 * c * c + 2) * fs[0])
|
|
self.vs_ = vs
|
|
self.fs_ = (f, fs[0], fs[1])
|
|
return f
|