# pylint: disable=invalid-unary-operand-type
from time import time
[docs]class PIDController:
def __init__(self, set_point: float, *, p=0.0, i=0.0, d=0.0, sample_time=0.0, windup=None):
self.p = float(p)
self.i = float(i)
self.d = float(d)
self.windup = windup
self.sample_time = float(sample_time)
self.set_point = float(set_point)
self.last_time = time()
self.last_error = 0.0
self.output = None
self.i_cum = 0
[docs] def update(self, feedback, current_time=None):
error = self.set_point - float(feedback)
current_time = current_time or time()
delta_error = error - self.last_error
delta_time = current_time - self.last_time
if delta_time >= self.sample_time:
# Proportional part
p_term = self.p * error
# Integral part
self.i_cum += error * delta_time
if self.windup and self.i_cum < -self.windup:
self.i_cum = -self.windup
elif self.windup and self.i_cum > self.windup:
self.i_cum = self.windup
i_term = self.i * self.i_cum
# Derivative part
d_term = 0
if delta_time > 0:
d_term = self.d * (delta_error / delta_time)
# Finish calculation
self.last_time = current_time
self.last_error = error
self.output = p_term + i_term + d_term
return self.output
@property
def state(self):
return dict(i_cum=self.i_cum)
@state.setter
def state(self, new):
self.i_cum = new.get('i_cum', 0)