Comment change

1 files changed, 242 insertions, 0 deletions

diff --git a/PID.py b/PID.py
new file mode 100644
index 0000000..d8957c5
--- /dev/null
+++ b/PID.py
@@ -0,0 +1,242 @@
+import time
+
+
+def _clamp(value, limits):
+    lower, upper = limits
+    if value is None:
+        return None
+    elif (upper is not None) and (value > upper):
+        return upper
+    elif (lower is not None) and (value < lower):
+        return lower
+    return value
+
+
+try:
+    # Get monotonic time to ensure that time deltas are always positive
+    _current_time = time.monotonic
+except AttributeError:
+    # time.monotonic() not available (using python < 3.3), fallback to time.time()
+    _current_time = time.time
+
+
+class PID(object):
+    """A simple PID controller."""
+
+    def __init__(
+        self,
+        Kp=1.0,
+        Ki=0.0,
+        Kd=0.0,
+        setpoint=0,
+        sample_time=0.01,
+        output_limits=(None, None),
+        auto_mode=True,
+        proportional_on_measurement=False,
+        error_map=None,
+    ):
+        """
+        Initialize a new PID controller.
+
+        :param Kp: The value for the proportional gain Kp
+        :param Ki: The value for the integral gain Ki
+        :param Kd: The value for the derivative gain Kd
+        :param setpoint: The initial setpoint that the PID will try to achieve
+        :param sample_time: The time in seconds which the controller should wait before generating
+            a new output value. The PID works best when it is constantly called (eg. during a
+            loop), but with a sample time set so that the time difference between each update is
+            (close to) constant. If set to None, the PID will compute a new output value every time
+            it is called.
+        :param output_limits: The initial output limits to use, given as an iterable with 2
+            elements, for example: (lower, upper). The output will never go below the lower limit
+            or above the upper limit. Either of the limits can also be set to None to have no limit
+            in that direction. Setting output limits also avoids integral windup, since the
+            integral term will never be allowed to grow outside of the limits.
+        :param auto_mode: Whether the controller should be enabled (auto mode) or not (manual mode)
+        :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
+        self.sample_time = sample_time
+
+        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
+        self._derivative = 0
+
+        self._last_time = None
+        self._last_output = None
+        self._last_input = None
+
+        self.output_limits = output_limits
+        self.reset()
+
+    def __call__(self, input_, dt=None):
+        """
+        Update the PID controller.
+
+        Call the PID controller with *input_* and calculate and return a control output if
+        sample_time seconds has passed since the last update. If no new output is calculated,
+        return the previous output instead (or None if no value has been calculated yet).
+
+        :param dt: If set, uses this value for timestep instead of real time. This can be used in
+            simulations when simulation time is different from real time.
+        """
+        if not self.auto_mode:
+            return self._last_output
+
+        now = _current_time()
+        if dt is None:
+            dt = now - self._last_time if (now - self._last_time) else 1e-16
+        elif dt <= 0:
+            raise ValueError('dt has negative value {}, must be positive'.format(dt))
+
+        if self.sample_time is not None and dt < self.sample_time and self._last_output is not None:
+            # Only update every sample_time seconds
+            return self._last_output
+
+        # Compute error terms
+        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
+            self._proportional = self.Kp * error
+        else:
+            # Add the proportional error on measurement to error_sum
+            self._proportional -= self.Kp * d_input
+
+        # Compute integral and derivative terms
+        self._integral += self.Ki * error * dt
+        self._integral = _clamp(self._integral, self.output_limits)  # Avoid integral windup
+
+        self._derivative = -self.Kd * d_input / dt
+
+        # Compute final output
+        output = self._proportional + self._integral + self._derivative
+        output = _clamp(output, self.output_limits)
+
+        # Keep track of state
+        self._last_output = output
+        self._last_input = input_
+        self._last_time = now
+
+        return output
+
+    def __repr__(self):
+        return (
+            '{self.__class__.__name__}('
+            '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},'
+            'error_map={self.error_map!r}'
+            ')'
+        ).format(self=self)
+
+    @property
+    def components(self):
+        """
+        The P-, I- and D-terms from the last computation as separate components as a tuple. Useful
+        for visualizing what the controller is doing or when tuning hard-to-tune systems.
+        """
+        return self._proportional, self._integral, self._derivative
+
+    @property
+    def tunings(self):
+        """The tunings used by the controller as a tuple: (Kp, Ki, Kd)."""
+        return self.Kp, self.Ki, self.Kd
+
+    @tunings.setter
+    def tunings(self, tunings):
+        """Set the PID tunings."""
+        self.Kp, self.Ki, self.Kd = tunings
+
+    @property
+    def auto_mode(self):
+        """Whether the controller is currently enabled (in auto mode) or not."""
+        return self._auto_mode
+
+    @auto_mode.setter
+    def auto_mode(self, enabled):
+        """Enable or disable the PID controller."""
+        self.set_auto_mode(enabled)
+
+    def set_auto_mode(self, enabled, last_output=None):
+        """
+        Enable or disable the PID controller, optionally setting the last output value.
+
+        This is useful if some system has been manually controlled and if the PID should take over.
+        In that case, disable the PID by setting auto mode to False and later when the PID should
+        be turned back on, pass the last output variable (the control variable) and it will be set
+        as the starting I-term when the PID is set to auto mode.
+
+        :param enabled: Whether auto mode should be enabled, True or False
+        :param last_output: The last output, or the control variable, that the PID should start
+            from when going from manual mode to auto mode. Has no effect if the PID is already in
+            auto mode.
+        """
+        if enabled and not self._auto_mode:
+            # Switching from manual mode to auto, reset
+            self.reset()
+
+            self._integral = last_output if (last_output is not None) else 0
+            self._integral = _clamp(self._integral, self.output_limits)
+
+        self._auto_mode = enabled
+
+    @property
+    def output_limits(self):
+        """
+        The current output limits as a 2-tuple: (lower, upper).
+
+        See also the *output_limits* parameter in :meth:`PID.__init__`.
+        """
+        return self._min_output, self._max_output
+
+    @output_limits.setter
+    def output_limits(self, limits):
+        """Set the output limits."""
+        if limits is None:
+            self._min_output, self._max_output = None, None
+            return
+
+        min_output, max_output = limits
+
+        if (None not in limits) and (max_output < min_output):
+            raise ValueError('lower limit must be less than upper limit')
+
+        self._min_output = min_output
+        self._max_output = max_output
+
+        self._integral = _clamp(self._integral, self.output_limits)
+        self._last_output = _clamp(self._last_output, self.output_limits)
+
+    def reset(self):
+        """
+        Reset the PID controller internals.
+
+        This sets each term to 0 as well as clearing the integral, the last output and the last
+        input (derivative calculation).
+        """
+        self._proportional = 0
+        self._integral = 0
+        self._derivative = 0
+
+        self._integral = _clamp(self._integral, self.output_limits)
+
+        self._last_time = _current_time()
+        self._last_output = None
+        self._last_input = None