Add simple pid controller

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)