|
| 1 | +import asyncio |
| 2 | +import time |
| 3 | + |
| 4 | +from .base import Powermeter |
| 5 | + |
| 6 | + |
| 7 | +class PidPowermeter(Powermeter): |
| 8 | + """ |
| 9 | + A wrapper around a powermeter that applies a PID (Proportional-Integral- |
| 10 | + Derivative) controller to steer the reported power toward zero (grid balance). |
| 11 | +
|
| 12 | + The PID controller uses the raw power-meter reading as its *process |
| 13 | + variable* and computes an adjustment that is either **added** to the raw |
| 14 | + reading (``mode="bias"``) or **used in place of** the raw reading |
| 15 | + (``mode="replace"``). |
| 16 | +
|
| 17 | + Positive PID output motivates the storage device to increase feed-in power; |
| 18 | + negative output motivates it to decrease feed-in power. |
| 19 | +
|
| 20 | + **Gain sensitivity:** in ``mode="bias"`` the PID and the storage device's own |
| 21 | + closed-loop controller act *together*. The effective closed-loop gain is |
| 22 | + ``(1 - Kp) * Kb``, where ``Kb`` is the device's internal gain. |
| 23 | + The system is stable for ``0 < Kp < 1``. Use ``Kp = 0.5`` as the |
| 24 | + recommended starting value. |
| 25 | +
|
| 26 | + **Anti-windup** is built in: the integral term is clamped so that the |
| 27 | + total PID output never exceeds ``[-output_max, +output_max]``, and |
| 28 | + integration is paused while the output is saturated. |
| 29 | +
|
| 30 | + Error convention: |
| 31 | + error = -measurement |
| 32 | + A positive grid import produces a negative error, causing the PID to |
| 33 | + reduce the reported value and motivate the storage device to cover the import. |
| 34 | +
|
| 35 | + To maintain a small import safety buffer (prevent export), set a small |
| 36 | + negative ``POWER_OFFSET`` (e.g. ``POWER_OFFSET = -20``) in the filter |
| 37 | + chain *before* the PID. |
| 38 | +
|
| 39 | + The controller runs on the **sum** of all phases (total grid power) |
| 40 | + and distributes its output equally across phases. |
| 41 | +
|
| 42 | + Config parameters: |
| 43 | + PID_KP Proportional gain (default 0 → PID disabled) |
| 44 | + PID_KI Integral gain (default 0) |
| 45 | + PID_KD Derivative gain (default 0) |
| 46 | + PID_OUTPUT_MAX Output clamp magnitude in watts (default 800) |
| 47 | + PID_MODE "bias" or "replace" (default "bias") |
| 48 | + """ |
| 49 | + |
| 50 | + VALID_MODES = ("bias", "replace") |
| 51 | + |
| 52 | + def __init__( |
| 53 | + self, |
| 54 | + wrapped_powermeter: Powermeter, |
| 55 | + kp: float = 0.0, |
| 56 | + ki: float = 0.0, |
| 57 | + kd: float = 0.0, |
| 58 | + output_max: float = 800.0, |
| 59 | + mode: str = "bias", |
| 60 | + ): |
| 61 | + """ |
| 62 | + Initialise the PID powermeter wrapper. |
| 63 | +
|
| 64 | + Args: |
| 65 | + wrapped_powermeter: The actual powermeter instance to wrap. |
| 66 | + kp: Proportional gain. |
| 67 | + ki: Integral gain. |
| 68 | + kd: Derivative gain. |
| 69 | + output_max: Maximum absolute PID output in watts. Must be > 0. |
| 70 | + mode: ``"bias"`` — add PID output to raw reading, or |
| 71 | + ``"replace"`` — use PID output as the reported value. |
| 72 | + """ |
| 73 | + if output_max <= 0: |
| 74 | + raise ValueError(f"PID output_max must be positive, got {output_max}") |
| 75 | + mode = mode.lower() |
| 76 | + if mode not in self.VALID_MODES: |
| 77 | + raise ValueError( |
| 78 | + f"PID mode must be one of {self.VALID_MODES}, got '{mode}'" |
| 79 | + ) |
| 80 | + |
| 81 | + self.wrapped_powermeter = wrapped_powermeter |
| 82 | + self.kp = kp |
| 83 | + self.ki = ki |
| 84 | + self.kd = kd |
| 85 | + self.output_max = output_max |
| 86 | + self.mode = mode |
| 87 | + |
| 88 | + # PID state |
| 89 | + self._integral: float = 0.0 |
| 90 | + self._prev_error: float | None = None |
| 91 | + self._prev_time: float | None = None |
| 92 | + self._lock = asyncio.Lock() |
| 93 | + |
| 94 | + async def wait_for_message(self, timeout=5): |
| 95 | + """Pass through to wrapped powermeter.""" |
| 96 | + return await self.wrapped_powermeter.wait_for_message(timeout) |
| 97 | + |
| 98 | + async def start(self): |
| 99 | + """Pass through to wrapped powermeter.""" |
| 100 | + await self.wrapped_powermeter.start() |
| 101 | + |
| 102 | + async def stop(self): |
| 103 | + """Pass through to wrapped powermeter.""" |
| 104 | + await self.wrapped_powermeter.stop() |
| 105 | + |
| 106 | + async def get_powermeter_watts(self) -> list[float]: |
| 107 | + """Return PID-adjusted power readings for each phase.""" |
| 108 | + async with self._lock: |
| 109 | + raw_values = await self.wrapped_powermeter.get_powermeter_watts() |
| 110 | + current_time = time.monotonic() |
| 111 | + |
| 112 | + # Compute error on the total power across all phases |
| 113 | + total_power = sum(raw_values) |
| 114 | + error = -total_power |
| 115 | + if self._prev_time is None: |
| 116 | + # First call — initialise state, no derivative yet |
| 117 | + self._prev_error = error |
| 118 | + self._prev_time = current_time |
| 119 | + dt = 0.0 |
| 120 | + else: |
| 121 | + dt = current_time - self._prev_time |
| 122 | + if dt <= 0: |
| 123 | + dt = 0.0 |
| 124 | + |
| 125 | + # --- Proportional --- |
| 126 | + p_term = self.kp * error |
| 127 | + |
| 128 | + # --- Derivative --- |
| 129 | + if dt > 0 and self._prev_error is not None: |
| 130 | + d_term = self.kd * (error - self._prev_error) / dt |
| 131 | + else: |
| 132 | + d_term = 0.0 |
| 133 | + |
| 134 | + # --- Integral with anti-windup --- |
| 135 | + if dt > 0: |
| 136 | + # Tentatively accumulate |
| 137 | + tentative_integral = self._integral + error * dt |
| 138 | + tentative_output = p_term + self.ki * tentative_integral + d_term |
| 139 | + # Only accept the new integral if output is not saturated, |
| 140 | + # or if the integral is moving toward zero (unwinding). |
| 141 | + if abs(tentative_output) <= self.output_max or ( |
| 142 | + self._integral != 0 and self._integral * error < 0 |
| 143 | + ): |
| 144 | + self._integral = tentative_integral |
| 145 | + i_term = self.ki * self._integral |
| 146 | + |
| 147 | + self._prev_error = error |
| 148 | + self._prev_time = current_time |
| 149 | + |
| 150 | + # --- Total output with clamping --- |
| 151 | + pid_output = p_term + i_term + d_term |
| 152 | + pid_output = max(-self.output_max, min(self.output_max, pid_output)) |
| 153 | + |
| 154 | + # --- Apply to readings --- |
| 155 | + num_phases = len(raw_values) |
| 156 | + per_phase = pid_output / num_phases if num_phases > 0 else 0.0 |
| 157 | + |
| 158 | + if self.mode == "bias": |
| 159 | + return [value + per_phase for value in raw_values] |
| 160 | + else: |
| 161 | + # replace mode: distribute PID output equally across phases |
| 162 | + return [per_phase] * num_phases |
0 commit comments