"""Control loop implementations."""
import asyncio
from heros import RemoteHERO
from heros.helper import log
[docs]
class ControlLoop:
"""Base class for (slow) control loops.
It measures the value of a sensor that implements the atomiq `Measurable` interface and and acts on an actor that
implements the atomiq `Parametrizable` interface. This makes it a versatile tool that can be used for a large
number of slow control tasks.
This class provides a basic structure for implementing control loops. Subclasses should implement the
`_next_control_value` method to define the specific control logic. See `PIDControlLoop` for an example.
Args:
sensor: object that measures the value to be controlled. Must implement the `Measurable` interface
actor: object that can act on the value to be controlled. Must implement the `Parametrizable` interface
actor_parameter: name of the parameter to act on in the actor.
initial_set_value: set value when the control loop starts. Can be changed during operation.
actor_min: minimum value to set for the control parameter on the actor.
actor_max: maximum value to set for the control parameter on the actor.
loop: asyncio loop to be used for control loop
update_rate: rate at which values should be measured and the actor should be updated in Hz (default 1Hz).
autostart: whether to close the control loop upon startup.
"""
set_value: float = 0.0
def __init__(self,
sensor: object | RemoteHERO,
actor: object | RemoteHERO,
actor_parameter: str,
initial_set_value: float,
actor_min: float | None = None,
actor_max: float | None = None,
loop: asyncio.AbstractEventLoop | None = None,
update_rate: float = 1.0,
autostart: bool = True):
self.sensor = sensor
self.actor = actor
self.actor_parameter = actor_parameter
self._loop = loop if loop is not None else asyncio.get_event_loop()
self.update_rate = update_rate
self._running = False
self.set_value = initial_set_value
self.actor_min = actor_min
self.actor_max = actor_max
self._run_flag = asyncio.Event()
self._stop_flag = asyncio.Event()
self._loop_task = self._loop.call_soon_threadsafe(asyncio.create_task, self._mainloop())
if autostart:
self.start_loop()
@property
def running(self):
return self._running
@running.setter
def running(self, status: bool):
if status:
self.start_loop()
else:
self.stop_loop()
[docs]
def start_loop(self) -> None:
"""Close the control loop."""
self._loop.call_soon_threadsafe(self._run_flag.set)
self._running = True
[docs]
def stop_loop(self) -> None:
"""Open the control loop."""
self._loop.call_soon_threadsafe(self._run_flag.clear)
self._running = False
[docs]
def _destroy_hero(self):
self._loop.call_soon_threadsafe(self._stop_flag.set)
[docs]
def _next_control_value(self, input_value: float) -> float:
raise NotImplementedError("Implement this method in a subclass")
[docs]
def _actor_value_bounded(self, value) -> float:
if self.actor_max is not None and value > self.actor_max:
return self.actor_max
if self.actor_min is not None and value < self.actor_min:
return self.actor_min
return value
[docs]
async def _mainloop(self):
next_run_time = self._loop.time()
while not self._stop_flag.is_set():
# calculate at which point in time we have to start the next loop
next_run_time += 1/self.update_rate
if self._run_flag.is_set():
try:
input = self.sensor.measure() # ty: ignore[unresolved-attribute]
except Exception as e: # noqa BLE001
log.warning(f"Could not read value from sensor: {e}")
continue
try:
output = self._actor_value_bounded(self._next_control_value(input))
except Exception as e: # noqa BLE001
log.warning(f"Could calculate next output value: {e}")
continue
if hasattr(self, "observable_data"):
self._emit_observable_data(input, output)
try:
self.actor.set_parameter(output, self.actor_parameter) # ty: ignore[unresolved-attribute]
except Exception as e: # noqa BLE001
log.warning(f"Could not write new value to actor: {e}")
# now we need to wait whatever time is still left until the point in time we determined previously
remaining_delay = next_run_time - self._loop.time()
if remaining_delay > 0:
await asyncio.sleep(remaining_delay)
else:
log.warning("One control loop update cycle took longer then 1/update_rate.")
[docs]
def _emit_observable_data(self, input_value: float, output_value: float) -> None:
if hasattr(self, "observable_data") and callable(self.observable_data):
self.observable_data({
"set_value": self.set_value,
"act_value": input_value,
"control_value": output_value
}) # ty: ignore[call-top-callable]
[docs]
class PIDControlLoop(ControlLoop):
"""PID (Proportional-Integral-Derivative) control loop.
This class implements a PID control loop, which is a common control loop
feedback mechanism widely used in industrial control systems. The PID
controller calculates an "error" value as the difference between a desired
setpoint and a measured process variable and attempts to minimize the error
by adjusting the process control inputs.
The PID formula is given by:
.. math :: u(t) = K_p e(t) + K_i \\int_{0}^{t} e(\tau) \\, d\tau + K_d \frac{de(t)}{dt}
where:
- ( u(t) ) is the control signal,
- ( e(t) ) is the error (difference between the setpoint and the measured value),
- ( K_p ) is the proportional gain,
- ( K_i ) is the integral gain,
- ( K_d ) is the derivative gain.
Args:
sensor: object that measures the value to be controlled. Must implement the `Measurable` interface
actor: object that can act on the value to be controlled. Must implement the `Parametrizable` interface
actor_parameter: name of the parameter to act on in the actor.
initial_set_value: set value when the control loop starts. Can be changed during operation.
actor_min: minimum value to set for the control parameter on the actor.
actor_max: maximum value to set for the control parameter on the actor.
loop: asyncio loop to be used for control loop
update_rate: rate at which values should be measured and the actor should be updated in Hz (default 1Hz).
autostart: whether to close the control loop upon startup.
default_p: initial value to set for the proportional gain. Can be changed during operation.
default_i: initial value to set for the integral gain. Can be changed during operation.
default_d: initial value to set for the differential gain. Can be changed during operation.
"""
p: float
i: float
d: float
integral: float
def __init__(self,
sensor: object | RemoteHERO,
actor: object | RemoteHERO,
actor_parameter: str,
initial_set_value: float,
actor_min: float | None = None,
actor_max: float | None = None,
loop: asyncio.AbstractEventLoop | None = None,
update_rate: float = 1.0,
autostart: bool = True,
default_p: float = 1.0,
default_i: float = 0.0,
default_d: float = 0.0,
integral_limit: float | None = None
):
self.p = default_p
self.i = default_i
self.d = default_d
self.last_value = 0.0
self.integral_limit = integral_limit
self.reset_integral()
super().__init__(sensor=sensor,
actor=actor,
actor_parameter=actor_parameter,
initial_set_value=initial_set_value,
actor_min=actor_min,
actor_max=actor_max,
loop=loop,
update_rate=update_rate,
autostart=autostart)
[docs]
def reset_integral(self):
"""Reset the time integral of the error to zero."""
self.integral = 0.0
[docs]
def _next_control_value(self, input_value: float) -> float:
"""Calculate the next value to set on the actor with the PID formula."""
diff = self.set_value - input_value
if self.integral_limit is None or abs(self.integral + diff) < self.integral_limit:
self.integral += diff
new_output = self.p*diff + self.i*self.integral + self.d*(input_value - self.last_value)
self.last_value = input_value
return new_output
[docs]
def _emit_observable_data(self, input_value: float, output_value: float) -> None:
if hasattr(self, "observable_data") and callable(self.observable_data):
self.observable_data({
"p": self.p,
"i": self.i,
"d": self.d,
"set_value": self.set_value,
"act_value": input_value,
"control_value": output_value
}) # ty: ignore[call-top-callable]