88 lines
2.7 KiB
Python
88 lines
2.7 KiB
Python
"""This module implements the PID (proportional integral derivative) controller."""
|
|
|
|
# Mathematics
|
|
from numpy import clip
|
|
from numpy import ndarray
|
|
|
|
# PID controller
|
|
from .pid import Pid
|
|
|
|
|
|
class AntiWindupPid(Pid):
|
|
|
|
"""The PID controller with anti-windup, i.e. a limited control output and integral part.
|
|
|
|
This controller resembles the PID with added clipping on the control signal to a user-set
|
|
maximum value. Furthermore, the integral of the error over time is pruned (anti windup).
|
|
|
|
Examples:
|
|
First, import some helper functions from numpy.
|
|
|
|
>>> from numpy import array
|
|
|
|
We then setup the Pid controller with some control constants.
|
|
|
|
>>> controller = AntiWindupPid(
|
|
... array([0.5]),
|
|
... array([0.1]),
|
|
... array([0.0]),
|
|
... 5.0,
|
|
... 0.1,
|
|
... )
|
|
|
|
We then specify an initial and desired state as well as the current state derivative.
|
|
|
|
>>> initial = array([5.0])
|
|
>>> desired = array([0.0])
|
|
>>> derivative = array([0.0])
|
|
|
|
Finally, we retrieve a control signal from the Pid based on the values we just set.
|
|
|
|
>>> signal = controller.control(desired, initial, derivative)
|
|
|
|
Args:
|
|
P: Proportional control constant ``(n,)``
|
|
I: Integral control constant ``(n,)``
|
|
D: Derivative control constant ``(n,)``
|
|
max_control: Limit of control signal
|
|
dt: Time between measurements
|
|
keep_trace: Whether to store a trace of control signals, states, etc.
|
|
"""
|
|
|
|
# In this context, we reproduce a common LQR notation
|
|
# pylint: disable=too-many-arguments
|
|
|
|
def __init__(
|
|
self,
|
|
P: ndarray,
|
|
I: ndarray, # noqa: E741
|
|
D: ndarray,
|
|
max_control: float,
|
|
dt: float,
|
|
keep_trace: bool = False,
|
|
) -> None:
|
|
# Setup internal PID controller and own attributes
|
|
super().__init__(P, I, D, dt, keep_trace)
|
|
self.max_control = max_control
|
|
|
|
def control(self, desired: ndarray, state: ndarray, state_derivative: ndarray) -> ndarray:
|
|
"""Compute the control signal based on proportional, integral and derivative terms.
|
|
|
|
Args:
|
|
desired: The desired state
|
|
state: The current state
|
|
state_derivative: The current state derivative
|
|
|
|
Returns:
|
|
The control signal
|
|
"""
|
|
|
|
# Get the basic PID control signal and clip to specified boundary
|
|
pid_signal = super().control(desired, state, state_derivative)
|
|
control_signal: ndarray = clip(pid_signal, -abs(self.max_control), abs(self.max_control))
|
|
|
|
# Prune integral part, i.e. apply anti wind up
|
|
self.summed_error -= (pid_signal - control_signal) / self.I
|
|
|
|
return control_signal
|