first commit
This commit is contained in:
rangang
+211
@@ -0,0 +1,211 @@
|
||||
"""Generic incremental PID controller.
|
||||
|
||||
The controller operates on a generic setpoint and measurement. Any actuator
|
||||
mapping (for example, converting valve opening to motor travel) belongs in the
|
||||
caller or hardware layer, not in this module.
|
||||
"""
|
||||
|
||||
|
||||
class IncrementalPID:
|
||||
"""Incremental PID controller with output and output-rate limits.
|
||||
|
||||
``error`` is always calculated as ``setpoint - measurement``. The output
|
||||
is the accumulated controller command, bounded by ``out_min`` and
|
||||
``out_max``.
|
||||
|
||||
``output_rate_limit`` is expressed in output units per second. When it is
|
||||
set, the maximum output change in one update is
|
||||
``output_rate_limit * dt``. ``du_max`` remains available as a legacy
|
||||
per-update limit through :meth:`set_du_max` or the ``update`` keyword.
|
||||
``xa_full`` is accepted for compatibility with older callers but is not
|
||||
used; actuator travel and dead-zone mapping do not belong in a PID.
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
kp: float,
|
||||
ki: float,
|
||||
kd: float,
|
||||
dt: float,
|
||||
out_min: float,
|
||||
out_max: float,
|
||||
xa_full=None,
|
||||
output_rate_limit=None,
|
||||
):
|
||||
if dt <= 0:
|
||||
raise ValueError("dt must be greater than zero")
|
||||
if out_min > out_max:
|
||||
raise ValueError("out_min must not be greater than out_max")
|
||||
if output_rate_limit is not None and output_rate_limit < 0:
|
||||
raise ValueError("output_rate_limit must not be negative")
|
||||
|
||||
self.kp = float(kp)
|
||||
self.ki = float(ki)
|
||||
self.kd = float(kd)
|
||||
self.dt = float(dt)
|
||||
self.out_min = float(out_min)
|
||||
self.out_max = float(out_max)
|
||||
self.output_rate_limit = (
|
||||
None if output_rate_limit is None else float(output_rate_limit)
|
||||
)
|
||||
|
||||
# Kept only as a compatibility attribute. It has no control meaning
|
||||
# in this generic controller and is intentionally not used.
|
||||
self.xa_full = xa_full
|
||||
|
||||
self.setpoint = 0.0
|
||||
self.measurement = 0.0
|
||||
self.error = 0.0
|
||||
|
||||
self.a0 = 0.0
|
||||
self.a1 = 0.0
|
||||
self.a2 = 0.0
|
||||
self._calculate_coefficients()
|
||||
|
||||
self.prev_error = 0.0
|
||||
self.prev_error2 = 0.0
|
||||
self.output = 0.0
|
||||
|
||||
# Legacy per-update increment limit. The newer
|
||||
# output_rate_limit takes precedence when configured.
|
||||
self.du_max = None
|
||||
|
||||
def _calculate_coefficients(self):
|
||||
"""Calculate the discrete incremental-PID coefficients."""
|
||||
self.a0 = self.kp + (self.ki * self.dt / 2.0) + (2.0 * self.kd / self.dt)
|
||||
self.a1 = -self.kp + (self.ki * self.dt / 2.0) - (4.0 * self.kd / self.dt)
|
||||
self.a2 = (2.0 * self.kd) / self.dt
|
||||
|
||||
def set_values(self, measurement, setpoint):
|
||||
"""Set the current measurement and desired setpoint."""
|
||||
self.measurement = float(measurement)
|
||||
self.setpoint = float(setpoint)
|
||||
|
||||
# Public aliases retained for older code that still reads these names.
|
||||
# They are plain aliases and do not add pressure-specific control logic.
|
||||
@property
|
||||
def current_pressure(self):
|
||||
return self.measurement
|
||||
|
||||
@current_pressure.setter
|
||||
def current_pressure(self, value):
|
||||
self.measurement = float(value)
|
||||
|
||||
@property
|
||||
def target_pressure(self):
|
||||
return self.setpoint
|
||||
|
||||
@target_pressure.setter
|
||||
def target_pressure(self, value):
|
||||
self.setpoint = float(value)
|
||||
|
||||
def update_values(self, measurement, setpoint):
|
||||
"""Compatibility-friendly alias for :meth:`set_values`."""
|
||||
self.set_values(measurement, setpoint)
|
||||
|
||||
def update_pressure_values(self, current_pressure, target_pressure):
|
||||
"""Legacy alias; use :meth:`set_values` for new code."""
|
||||
self.set_values(current_pressure, target_pressure)
|
||||
|
||||
def update(
|
||||
self,
|
||||
measurement=None,
|
||||
setpoint=None,
|
||||
*,
|
||||
dt=None,
|
||||
output_rate_limit=None,
|
||||
du_max=None,
|
||||
):
|
||||
"""Run one controller step and return the bounded output.
|
||||
|
||||
``measurement`` and ``setpoint`` may be omitted when they were already
|
||||
supplied with :meth:`set_values` (or the legacy alias). ``dt`` is an
|
||||
optional per-step override and is measured in seconds.
|
||||
|
||||
``output_rate_limit`` is a per-second limit. The legacy ``du_max``
|
||||
keyword is a per-update limit and takes precedence for that call.
|
||||
"""
|
||||
if measurement is not None:
|
||||
self.measurement = float(measurement)
|
||||
if setpoint is not None:
|
||||
self.setpoint = float(setpoint)
|
||||
|
||||
if dt is not None:
|
||||
if dt <= 0:
|
||||
raise ValueError("dt must be greater than zero")
|
||||
if float(dt) != self.dt:
|
||||
self.dt = float(dt)
|
||||
self._calculate_coefficients()
|
||||
|
||||
if output_rate_limit is not None:
|
||||
if output_rate_limit < 0:
|
||||
raise ValueError("output_rate_limit must not be negative")
|
||||
rate_limit = float(output_rate_limit)
|
||||
else:
|
||||
rate_limit = self.output_rate_limit
|
||||
|
||||
self.error = self.setpoint - self.measurement
|
||||
delta = (
|
||||
self.a0 * self.error
|
||||
+ self.a1 * self.prev_error
|
||||
+ self.a2 * self.prev_error2
|
||||
)
|
||||
|
||||
if du_max is not None:
|
||||
if du_max < 0:
|
||||
raise ValueError("du_max must not be negative")
|
||||
self.du_max = float(du_max)
|
||||
|
||||
if du_max is not None:
|
||||
max_delta = float(du_max)
|
||||
elif rate_limit is not None:
|
||||
max_delta = rate_limit * self.dt
|
||||
elif self.du_max is not None:
|
||||
max_delta = abs(float(self.du_max))
|
||||
else:
|
||||
max_delta = None
|
||||
|
||||
if max_delta is not None:
|
||||
delta = max(-max_delta, min(max_delta, delta))
|
||||
|
||||
new_output = self.output + delta
|
||||
new_output = max(self.out_min, min(self.out_max, new_output))
|
||||
|
||||
self.prev_error2 = self.prev_error
|
||||
self.prev_error = self.error
|
||||
self.output = new_output
|
||||
return new_output
|
||||
|
||||
def reset(self, initial_output=0.0):
|
||||
"""Reset controller history and initialize the output command."""
|
||||
initial_output = float(initial_output)
|
||||
self.output = max(self.out_min, min(self.out_max, initial_output))
|
||||
self.prev_error = 0.0
|
||||
self.prev_error2 = 0.0
|
||||
self.error = 0.0
|
||||
|
||||
def update_parameters(self, kp: float, ki: float, kd: float):
|
||||
"""Update PID gains and recalculate the discrete coefficients."""
|
||||
self.kp = float(kp)
|
||||
self.ki = float(ki)
|
||||
self.kd = float(kd)
|
||||
self._calculate_coefficients()
|
||||
|
||||
def set_dt(self, dt: float):
|
||||
"""Set the controller period in seconds."""
|
||||
if dt <= 0:
|
||||
raise ValueError("dt must be greater than zero")
|
||||
self.dt = float(dt)
|
||||
self._calculate_coefficients()
|
||||
|
||||
def set_output_rate_limit(self, value):
|
||||
"""Set or clear the output slew-rate limit in output units/second."""
|
||||
if value is not None and value < 0:
|
||||
raise ValueError("output_rate_limit must not be negative")
|
||||
self.output_rate_limit = None if value is None else float(value)
|
||||
|
||||
def set_du_max(self, value):
|
||||
"""Legacy setter for a maximum output change per update."""
|
||||
if value is not None and value < 0:
|
||||
raise ValueError("du_max must not be negative")
|
||||
self.du_max = None if value is None else float(value)
|
||||
Reference in New Issue
Block a user