Error mapping function implementation (m-lundberg#19)

CHANGELOG.md

@@ -9,6 +9,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+- Function to map the erro value, now is possibly to clip the error value, i.e., [-pi, pi[
+
 - Typing information through a stub file so that users of the library can use e.g.
   [mypy](https://github.com/python/mypy) to type check their code
 

README.md

@@ -99,6 +99,22 @@ To eliminate overshoot in certain types of systems, you can calculate the [propo
 pid.proportional_on_measurement = True
 ```
 
+
+#### Error mapping
+To transform the error value in another value, e.g., to get a degree value error in a yaw angle control with values between [-pi, pi)
+```python
+def pi_clip(angle):
+    if angle > 0:
+        if angle > np.pi:
+            return angle -2*math.pi
+    else:
+        if angle < -np.pi:
+            return angle +2*math.pi
+    return angle
+
+pid.error_map = pi_clip
+```
+
 ## Tests
 Use the following to run tests:
 ```

simple_pid/PID.py

@@ -35,6 +35,7 @@ def __init__(
         output_limits=(None, None),
         auto_mode=True,
         proportional_on_measurement=False,
+        error_map=None
     ):
         """
         Initialize a new PID controller.
@@ -57,6 +58,7 @@ def __init__(
         :param proportional_on_measurement: Whether the proportional term should be calculated on
             the input directly rather than on the error (which is the traditional way). Using
             proportional-on-measurement avoids overshoot for some types of systems.
+        :param error_map: Function to transform the error value in another constrained value.
         """
         self.Kp, self.Ki, self.Kd = Kp, Ki, Kd
         self.setpoint = setpoint
@@ -65,6 +67,7 @@ def __init__(
         self._min_output, self._max_output = None, None
         self._auto_mode = auto_mode
         self.proportional_on_measurement = proportional_on_measurement
+        self.error_map = error_map
 
         self._proportional = 0
         self._integral = 0
@@ -105,6 +108,10 @@ def __call__(self, input_, dt=None):
         error = self.setpoint - input_
         d_input = input_ - (self._last_input if self._last_input is not None else input_)
 
+        # check if must map the error
+        if self.error_map is not None:
+            error = self.error_map(error)
+
         # compute the proportional term
         if not self.proportional_on_measurement:
             # regular proportional-on-error, simply set the proportional term
@@ -136,7 +143,8 @@ def __repr__(self):
             'Kp={self.Kp!r}, Ki={self.Ki!r}, Kd={self.Kd!r}, '
             'setpoint={self.setpoint!r}, sample_time={self.sample_time!r}, '
             'output_limits={self.output_limits!r}, auto_mode={self.auto_mode!r}, '
-            'proportional_on_measurement={self.proportional_on_measurement!r}'
+            'proportional_on_measurement={self.proportional_on_measurement!r},'
+            'error_map={self.error_map!r}'
             ')'
         ).format(self=self)
 

tests/test_pid.py

@@ -216,3 +216,25 @@ def update_system(C, dt):
 
     # check if system has converged
     assert abs(PV - 5) < 0.1
+
+
+def test_error_map():
+    # error map function
+    import math
+
+    def pi_clip(angle):
+        """Transform the angle value to a [-pi, pi) range."""
+        if angle > 0:
+            if angle > math.pi:
+                return angle - 2 * math.pi
+        else:
+            if angle < -math.pi:
+                return angle + 2 * math.pi
+        return angle
+
+    sp = 0.  # setpoint
+    pid = PID(1, 0, 0, setpoint=sp, sample_time=0.1, error_map=pi_clip)  # include error mapping
+    PV = 5.  # process variable
+
+    # check if error value is mapped by the function
+    assert pid(PV) == pi_clip(sp - PV)  # clip the error