flow_control/controllers.py

"""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)