Add PID controller implementation

Author: TahmidSwad

Diff for: control_algorithms/pid.py

@@ -12,7 +12,7 @@
 
 
 class PID:
-    def __init__(self, kp: float, ki: float, kd: float, setpoint: float = 0):
+    def __init__(self, kp: float, ki: float, kd: float, setpoint: float = 0.0):
         """
         Initialize the PID controller.
 
@@ -26,8 +26,8 @@ def __init__(self, kp: float, ki: float, kd: float, setpoint: float = 0):
         self.kd = kd
         self.setpoint = setpoint
 
-        self.integral = 0
-        self.previous_error = 0
+        self.integral = 0.0
+        self.previous_error = 0.0
 
     def compute(self, measured_value: float, dt: float) -> float:
         """
@@ -38,8 +38,8 @@ def compute(self, measured_value: float, dt: float) -> float:
         :return: Control output
         """
         error = self.setpoint - measured_value
-        self.integral += error * dt if error != 0 else 0
-        derivative = (error - self.previous_error) / dt if dt > 0 else 0
+        self.integral += error * dt if error != 0 else 0.0
+        derivative = (error - self.previous_error) / dt if dt > 0 else 0.0
 
         output = (self.kp * error) + (self.ki * self.integral) + (self.kd * derivative)
         self.previous_error = error