diff --git a/pycar/src/car/control/pid.py b/pycar/src/car/control/pid.py new file mode 100644 index 0000000..f4a5e79 --- /dev/null +++ b/pycar/src/car/control/pid.py @@ -0,0 +1,50 @@ +""" +PID Controller implementation. +""" + +import time + + +class PIDController: + def __init__(self, set_point=0, kp=3, ki=None, kd=None): + """ + Simple implementation of PID control. All calculations are serial. + Updates will only occur when you call the compute_timestep function, so + call it as often as is needed when using the control (ideally a set timestep). + You must ensure you set the set_point before calculations, otherwise you will see + that nothing will happen. + """ + self._set_point = set_point + self._kp = kp + self._ki = ki + self._kd = kd + self._reset_errors() + + @properrty + def set_point(self) -> float: + return self._set_point + + @set_point.setter + def set_point(self, new_point: float): + self._set_point = new_point + self._reset_errors() + + def _reset_errors(self): + self._previous_error = 0 + self._sum_errors = 0 + self._start_timestep = 0 + + def compute_timestep(self, process_value: float): + """ + Compute the manipulated value at the current timestep, given the current Process Value + """ + error = self._set_point - process_value + dt = time.time - self._start_timestep + self._sum_errors += self._sum_errors + error * dt + derivative = (error - self._previous_error) / dt + self._previous_error = error + # Calculate pid control, integral/deritive time are the sample rate + # (estimate standard form since we're doing serial calculation) if omitted + return self._kp * (error + + ((1/dt) if self._ki is None else self._ki / self._kp) * self._sum_errors + + ((1/dt) if self._kd is None else self._kd / self._kp) * derivative)