diff --git a/tests/test_pid.py b/tests/test_pid.py index d4c6e66..75a32f8 100644 --- a/tests/test_pid.py +++ b/tests/test_pid.py @@ -1,16 +1,17 @@ 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 @@ -18,12 +19,57 @@ def test_output_with_manual_output(self): 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() \ No newline at end of file + +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() diff --git a/tinypid/__init__.py b/tinypid/__init__.py index 819b5a3..895cc14 100644 --- a/tinypid/__init__.py +++ b/tinypid/__init__.py @@ -1 +1 @@ -from .pid import PID \ No newline at end of file +from .pid import PID, Gain, PIDGainScheduler \ No newline at end of file diff --git a/tinypid/pid.py b/tinypid/pid.py index 240d7a7..549ac83 100644 --- a/tinypid/pid.py +++ b/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