added gain scheduling

Author: asteppke

tests/test_pid.py

@@ -1,29 +1,75 @@
 import unittest
-from tinypid import PID
+from tinypid import PID, Gain, PIDGainScheduler
+
 
 class TestPID(unittest.TestCase):
     def test_output_with_no_manual_output(self):
-        pid = PID(K_p=1.0, K_i=0.5, K_d=0.2, setpoint=10.0, dt=0.1)
+        pid = PID(k_p=1.0, k_i=0.5, k_d=0.2, setpoint=10.0, dt=0.1)
         process_variable = 8.0
         expected_output = 6.1  # P = 1.0 * (10.0 - 8.0) = 2.0, I = 0.5 * (10.0 - 8.0) * 0.1 = 0.1, D = 0.2 * ((10.0 - 8.0) / 0.1) = 4
         output = pid(process_variable)
         self.assertAlmostEqual(output, expected_output)
 
     def test_output_with_manual_output(self):
-        pid = PID(K_p=1.0, K_i=0.5, K_d=0.2, setpoint=10.0, dt=0.1)
+        pid = PID(k_p=1.0, k_i=0.5, k_d=0.2, setpoint=10.0, dt=0.1)
         process_variable = 8.0
         manual_output = 5.0
         expected_output = manual_output  # Since manual_output is provided, the output should be equal to it
         output = pid(process_variable, manual_output=manual_output)
         self.assertAlmostEqual(output, expected_output)
 
     def test_output_with_anti_windup_disabled(self):
-        pid = PID(K_p=1.0, K_i=0.5, K_d=0.2, setpoint=10.0, dt=0.1)
+        pid = PID(k_p=1.0, k_i=0.5, k_d=0.2, setpoint=10.0, dt=0.1)
         process_variable = 8.0
         manual_output = 15.0
         expected_output = manual_output  # Since manual_output is provided, the output should be equal to it
         output = pid(process_variable, manual_output=manual_output, anti_windup=False)
         self.assertAlmostEqual(output, expected_output)
 
-if __name__ == '__main__':
-    unittest.main()
+
+class TestGainSchedule(unittest.TestCase):
+    def test_gain_scheduler_basic(self):
+        gains = [
+            Gain(setpoint_scope=(0, 20), k_p=1.0, k_i=0.5, k_d=0.2),
+            Gain(setpoint_scope=(20, 30), k_p=2.0, k_i=1.0, k_d=0.4),
+        ]
+
+        pid = PIDGainScheduler(gains=gains, setpoint=10)
+
+        pid.update_gain(10)
+        self.assertAlmostEqual(pid.k_p, 1.0)
+        self.assertAlmostEqual(pid.k_i, 0.5)
+        self.assertAlmostEqual(pid.k_d, 0.2)
+
+        pid.update_gain(25)
+        self.assertAlmostEqual(pid.k_p, 2.0)
+        self.assertAlmostEqual(pid.k_i, 1.0)
+        self.assertAlmostEqual(pid.k_d, 0.4)
+
+    def test_gain_scheduler_constant(self):
+        gains = [
+            Gain(setpoint_scope=(0, 20), k_p=1.0, k_i=0.5, k_d=0.2),
+            Gain(setpoint_scope=(20, 30), k_p=2.0, k_i=1.0, k_d=0.4),
+        ]
+
+        pid = PIDGainScheduler(gains=gains, setpoint=10, dt=0.1)
+
+        process_variable = 8.0
+        expected_output = 6.1  # P = 1.0 * (10.0 - 8.0) = 2.0, I = 0.5 * (10.0 - 8.0) * 0.1 = 0.1, D = 0.2 * ((10.0 - 8.0) / 0.1) = 4
+        output = pid(process_variable)
+        self.assertAlmostEqual(output, expected_output)
+
+    def test_gain_scheduler_no_gain_found(self):
+        gains = [
+            Gain(setpoint_scope=(0, 20), k_p=1.0, k_i=0.5, k_d=0.2),
+            Gain(setpoint_scope=(20, 30), k_p=2.0, k_i=1.0, k_d=0.4),
+        ]
+
+        pid = PIDGainScheduler(gains=gains, setpoint=10)
+
+        with self.assertRaises(ValueError):
+            pid.update_gain(40)
+
+
+if __name__ == "__main__":
+    unittest.main()

tinypid/__init__.py

@@ -1 +1 @@
-from .pid import PID
+from .pid import PID, Gain, PIDGainScheduler

tinypid/pid.py

@@ -10,7 +10,29 @@
 output = controller(10)
 
 """
-from typing import Optional, Tuple
+
+from typing import List, Optional, Tuple
+
+
+class Gain:
+    """
+    A simple class to store PID gains and the setpoint range for which they apply.
+    """
+    def __init__(self, setpoint_scope: Tuple[float, float], k_p: float, k_i: float, k_d: float) -> None:
+        """
+        Initializes a Gain object with a setpoint range and PID gains.
+
+        Parameters:
+            setpoint_scope: The range of setpoints for which these gains apply.
+            The range is inclusive of the lower bound and exclusive of the upper bound.
+            k_p : The proportional gain.
+            k_i : The integral gain.
+            k_d : The derivative gain.
+        """
+        self.setpoint_scope = setpoint_scope
+        self.k_p = k_p
+        self.k_i = k_i
+        self.k_d = k_d
 
 
 class PID:
@@ -25,9 +47,9 @@ class PID:
 
     def __init__(
         self,
-        K_p: float = 1,
-        K_i: float = 0.1,
-        K_d: float = 0,
+        k_p: float = 1,
+        k_i: float = 0.1,
+        k_d: float = 0,
         setpoint: float = 0,
         dt: float = 1,
         derivative_lowpass: float = 1,
@@ -38,9 +60,9 @@ def __init__(
         Initialize PID controller
 
         Parameters:
-            K_p : Proportional gain
-            K_i : Integral gain
-            K_d : Derivative gain
+            k_p : Proportional gain
+            k_i : Integral gain
+            k_d : Derivative gain
             dt : Time step
             derivative_lowpass: lowpass constant (between 1 and 0, 1 meaning no lowpass)
             upper_limit : Upper limit for the output
@@ -51,9 +73,9 @@ def __init__(
         if not 0 <= derivative_lowpass <= 1:
             raise ValueError("derivative_lowpass must be between 0 and 1")
 
-        self.K_p = K_p
-        self.K_i = K_i
-        self.K_d = K_d
+        self.k_p = k_p
+        self.k_i = k_i
+        self.k_d = k_d
         self.P, self.I, self.D = None, None, None
         self.dt = dt
         self.alpha = derivative_lowpass
@@ -108,22 +130,25 @@ def limit(self, output: float) -> Tuple[bool, float]:
         saturated = output != unlimited
 
         return saturated, output
-    
-    def update_gains(self, K_p: float, K_i: float, K_d: float) -> None:
+
+    def update_gains(self, k_p: float, k_i: float, k_d: float) -> None:
         """
         Update the PID gains.
 
         Parameters:
-            K_p : The new proportional gain
-            K_i : The new integral gain
-            K_d : The new derivative gain
+            k_p : The new proportional gain
+            k_i : The new integral gain
+            k_d : The new derivative gain
         """
-        self.K_p = K_p
-        self.K_i = K_i
-        self.K_d = K_d
+        self.k_p = k_p
+        self.k_i = k_i
+        self.k_d = k_d
 
     def __call__(
-        self, process_variable: float, manual_output: Optional[float] = None, anti_windup: bool = True
+        self,
+        process_variable: float,
+        manual_output: Optional[float] = None,
+        anti_windup: bool = True,
     ) -> float:
         """
         Process the input signal and return the controller output.
@@ -137,9 +162,9 @@ def __call__(
         self.integral += error * self.dt
         derivative = (error - self._previous_error) / self.dt if self.dt != 0 else 0
 
-        self.P = self.K_p * error
-        self.I = self.K_i * self.integral
-        self.D = self.K_d * (self.alpha * derivative + (1 - self.alpha) * self._previous_derivative)
+        self.P = self.k_p * error
+        self.I = self.k_i * self.integral
+        self.D = self.k_d * (self.alpha * derivative + (1 - self.alpha) * self._previous_derivative)
 
         output = self.P + self.I + self.D
 
@@ -154,7 +179,7 @@ def __call__(
 
         if manual_output:
             # Use setpoint tracking by calculating integral so that the output matches the manual setpoint
-            self.integral = -(self.P + self.D - manual_output) / self.K_i if self.K_i != 0 else 0
+            self.integral = -(self.P + self.D - manual_output) / self.k_i if self.k_i != 0 else 0
             output = manual_output
 
         return output
@@ -165,3 +190,64 @@ def __repr__(self):
             f"P: {self.P}, I: {self.I}, D: {self.D}\n"
             f"Limits: {self.lower_limit} < output < {self.upper_limit}"
         )
+
+
+class PIDGainScheduler(PID):
+    """
+    An extended Proportional-Integral-Derivative controller that uses
+    gain scheduling to allow different gains, i.e., k_p, k_i, and k_d depending on
+    the setpoint.
+
+    """
+
+    def __init__(
+        self,
+        gains: List[Gain],
+        setpoint: float = 0,
+        dt: float = 1,
+        derivative_lowpass: float = 1,
+        upper_limit: Optional[float] = None,
+        lower_limit: Optional[float] = None,
+    ) -> None:
+        """
+        Initialize PID controller
+
+        Parameters:
+            gains : List of Gain objects
+            setpoint : The setpoint
+            dt : Time step
+            derivative_lowpass: lowpass constant (between 1 and 0, 1 meaning no lowpass)
+            upper_limit : Upper limit for the output
+            lower_limit : Lower limit for the output
+        """
+
+        super().__init__(None, None, None, setpoint, dt, derivative_lowpass, upper_limit, lower_limit)
+
+        self.gains = gains
+        self.update_gain(setpoint)
+
+    def update_gain(self, setpoint: float) -> Tuple[float, float, float]:
+        """
+        Update the PID gains based on the current setpoint
+
+        Parameters:
+            output : The current output
+        """
+        for gain in self.gains:
+            lower, upper = gain.setpoint_scope
+            if lower <= setpoint < upper:
+                self.k_p, self.k_i, self.k_d = gain.k_p, gain.k_i, gain.k_d
+                break
+        else:
+            raise ValueError("No gain found for the given setpoint.")
+
+    def __call__(
+        self,
+        process_variable: float,
+        manual_output: Optional[float] = None,
+        anti_windup: bool = True,
+    ) -> float:
+        self.update_gain(self.setpoint)
+
+        output = super().__call__(process_variable, manual_output, anti_windup)
+        return output