Simplify pid calculation

pycar/src/car/control/pid.py

@@ -6,10 +6,10 @@ import time
 
 
 class PIDController:
-    def __init__(self, set_point=0, kp=3, ki=None, kd=None):
+    def __init__(self, set_point=0, kp=3, ki=1, kd=1):
         """
-        Simple implementation of PID control. All calculations are serial. 
+        Simple implementation of PID control. All calculations are serial.
-        Updates will only occur when you call the compute_timestep function, so 
+        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.
@@ -36,15 +36,13 @@ class PIDController:
 
     def compute_timestep(self, process_value: float):
         """
-        Compute the manipulated value at the current timestep, given the current Process Value
+        Compute the manipulated value at the current timestep, given the current Process Value.
+        This is very simple, only considering the previous timestep for the derivative,
+        and not performing any smoothing on the integral
         """
         error = self._set_point - process_value
         dt = time.time - self._start_timestep
-        self._sum_errors += self._sum_errors + error * dt
+        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
+        return self._kp * error + self._ki * self._sum_errors + self._kd * derivative
-        # (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)