From 7e51b88785ecde452ffc29445a78492e5a11710e Mon Sep 17 00:00:00 2001 From: erg Date: Tue, 28 Feb 2023 20:09:47 +0100 Subject: Completely revorked --- PID.py | 244 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 244 insertions(+) create mode 100644 PID.py (limited to 'PID.py') diff --git a/PID.py b/PID.py new file mode 100644 index 0000000..db66c3f --- /dev/null +++ b/PID.py @@ -0,0 +1,244 @@ +import time +import warnings + + +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 + warnings.warn('time.monotonic() not available in python < 3.3, using time.time() as fallback') + + +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 -- cgit v1.2.3-65-gdbad