|
YAMS
Yet Another Mechanism System
|
#include <ADRCController.hpp>
Classes | |
| struct | Reference |
Public Member Functions | |
| ADRCController (std::shared_ptr< ADRCConfig > config) | |
| void | Initialize (double b0, wpi::units::second_t period) |
| wpi::units::volt_t | Step (double reference, double output, const std::vector< double > &refDerivatives={}) |
| wpi::units::volt_t | Calculate (double measurement, double setpoint) |
| wpi::units::volt_t | Calculate (double measurement, double setpoint, wpi::units::volt_t appliedOutput) |
| wpi::units::volt_t | Calculate (double measurement, Reference now, Reference next, wpi::units::volt_t appliedOutput) |
| void | Reset (double measurement) |
| void | Reset (const std::vector< double > &xhatInit, wpi::units::volt_t uInit) |
| double | GetB0 () const |
| wpi::units::radians_per_second_t | GetBandwidth () const |
| wpi::units::radians_per_second_t | GetObserverBandwidth () const |
| wpi::units::second_t | GetSettlingTime () const |
| double | GetBandwidthScale () const |
| const TrackingDifferentiator * | GetTrackingDifferentiator () const |
| std::vector< double > | GetEstimatedStates () const |
| double | GetEstimatedDisturbance () const |
| wpi::units::volt_t | GetLastOutput () const |
| int | GetSystemOrder () const |
| int | GetControllerOrder () const |
| const std::shared_ptr< ADRCConfig > & | GetConfig () const |
| bool | IsInitialized () const |
Friends | |
| class | ADRCConfig |
Active disturbance rejection controller.
Written by FRC Team 8331, who contributed active disturbance rejection control to YAMS.
An active disturbance rejection controller treats every unmodeled effect (friction, gravity, back-EMF, coupling, dropped battery voltage, external loads) as a single "total disturbance" and estimates it online with an extended state observer. The controller then cancels the estimate while driving the modeled part of the plant with a bandwidth-parameterized control law, so it needs very little model: just the system order and an approximate input gain b0.
Most teams do not construct this class directly; they describe the controller with ADRCConfig and the motor controller builds and runs it.
Plant orders 1 through 4 are supported. The extended state observer is one order higher than the plant, with its final state holding the disturbance estimate. Gains come from a single closed-loop bandwidth wc; the observer bandwidth is 4 * wc.
The controller keeps only its running state: the observer's estimate, its last outputs and the tracking differentiator. Everything else it reads from its ADRCConfig every step, including the observer and control gains, which the config works out again whenever its settling time, bandwidth scale or b0 changes. The system order, input delay and tracking differentiator are fixed when the controller is initialized.
The controller does not limit or ramp its output. A motor controller running it limits and ramps the output itself, with WithClosedLoopMaxVoltage and WithClosedLoopRampRate, and tells the observer the voltage it applied. Run on its own, clamp the output to what the actuator can apply, ramp it with WPILib's SlewRateLimiter if it needs one, and tell the observer what was applied with Calculate(measurement, setpoint, applied), so a clamped output is not mistaken for a disturbance.
|
explicit |
Create a controller that reads its settings from a config, with the config's system order.
| config | Config this controller runs. |
| std::invalid_argument | if the config is null or has no system order. |
| wpi::units::volt_t yams::math::ADRCController::Calculate | ( | double | measurement, |
| double | setpoint ) |
Run one control step, as Step(setpoint, measurement).
| measurement | Measured output. |
| setpoint | Reference. |
| std::logic_error | if the controller has not been initialized. |
| wpi::units::volt_t yams::math::ADRCController::Calculate | ( | double | measurement, |
| double | setpoint, | ||
| wpi::units::volt_t | appliedOutput ) |
Run one control step, telling the observer what the actuator applied since the last step. A current limit, a low battery or saturation can apply less than asked; told the applied output, the observer does not mistake the shortfall for a disturbance.
| measurement | Measured output. |
| setpoint | Reference. |
| appliedOutput | Output the actuator applied, such as the motor controller's voltage. A non-finite value is ignored. |
| std::logic_error | if the controller has not been initialized. |
| wpi::units::volt_t yams::math::ADRCController::Calculate | ( | double | measurement, |
| Reference | now, | ||
| Reference | next, | ||
| wpi::units::volt_t | appliedOutput ) |
Run one control step on a moving setpoint, such as a motion profile's, telling the observer what the actuator applied since the last step.
The controller is given the setpoint where it is now, at the start of the control period, and where it will be at the end, as WPILib's feedforwards are. It follows the setpoint now, and feeds forward how it moves over the period: a first order loop the average rate over the period, and a higher order loop the rate now and the change of rate over the period. Given only the setpoint at the end of the period, its stiff feedback would pull the mechanism a period ahead of the profile. A setpoint standing still is Reference::Still(setpoint) for both. A tracking differentiator, when the config turns one on, replaces the rates with its own.
| measurement | Measured output. |
| now | The setpoint now, and its rate. |
| next | The setpoint one control period from now, and its rate. |
| appliedOutput | Output the actuator applied, such as the motor controller's voltage. A non-finite value is ignored. |
| std::logic_error | if the controller has not been initialized. |
| double yams::math::ADRCController::GetB0 | ( | ) | const |
| wpi::units::radians_per_second_t yams::math::ADRCController::GetBandwidth | ( | ) | const |
| double yams::math::ADRCController::GetBandwidthScale | ( | ) | const |
|
inline |
|
inline |
| double yams::math::ADRCController::GetEstimatedDisturbance | ( | ) | const |
| std::vector< double > yams::math::ADRCController::GetEstimatedStates | ( | ) | const |
| wpi::units::volt_t yams::math::ADRCController::GetLastOutput | ( | ) | const |
| wpi::units::radians_per_second_t yams::math::ADRCController::GetObserverBandwidth | ( | ) | const |
| wpi::units::second_t yams::math::ADRCController::GetSettlingTime | ( | ) | const |
|
inline |
| const TrackingDifferentiator * yams::math::ADRCController::GetTrackingDifferentiator | ( | ) | const |
| void yams::math::ADRCController::Initialize | ( | double | b0, |
| wpi::units::second_t | period ) |
Initialize the controller: give its config the values it cannot hold itself, which also works out its gains, and start the observer and the output at zero.
| b0 | Input gain used while the config leaves b0 unset, such as the one ADRCConfig::ResolveB0 derives. Must be finite and non-zero when used. |
| period | Control period. Must be finite and positive. |
| std::invalid_argument | if a value is invalid, or the tracking differentiator cannot run at this period. |
|
inline |
| void yams::math::ADRCController::Reset | ( | const std::vector< double > & | xhatInit, |
| wpi::units::volt_t | uInit ) |
Start the observers at a full state and the output at a value.
| xhatInit | Observer state, systemOrder + 1 long; empty starts at zero. |
| uInit | Output. |
| std::logic_error | if the controller has not been initialized. |
| std::invalid_argument | if the state has the wrong length or the output is not finite. |
| void yams::math::ADRCController::Reset | ( | double | measurement | ) |
Start the observer at a measurement, with the other states, the disturbance and the output at zero.
| measurement | Measured output, in the units the controller is stepped with. |
| std::logic_error | if the controller has not been initialized. |
| std::invalid_argument | if the measurement is not finite. |
| wpi::units::volt_t yams::math::ADRCController::Step | ( | double | reference, |
| double | output, | ||
| const std::vector< double > & | refDerivatives = {} ) |
|
friend |