212 lines
7.3 KiB
Python
212 lines
7.3 KiB
Python
"""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):
|
|
"""矩形积分与后向差分微分形式的增量式 PID 系数。"""
|
|
self.a0 = self.kp + self.ki * self.dt + self.kd / self.dt
|
|
self.a1 = -self.kp - 2.0 * self.kd / self.dt
|
|
self.a2 = 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=100.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)
|