YAMS
Yet Another Mechanism System
Loading...
Searching...
No Matches
yams::math::ADRCController Class Reference

#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

Detailed Description

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.

auto config = std::make_shared<yams::math::ADRCConfig>();
config->WithSystemOrder(yams::math::ADRCConfig::SystemOrder::FIRST).WithSettlingTime(200_ms);
// b0, which the config leaves unset, and the control period.
adrc.Initialize(b0, 20_ms);
// Every loop: clamp the output, and tell the observer what was applied last loop.
wpi::units::volt_t volts = std::clamp(adrc.Calculate(measurement, setpoint, appliedVoltage), -12_V, 12_V);
@ FIRST
First order: the loop commands and measures velocity, such as a flywheel.
Definition ADRCConfig.hpp:90
Definition ADRCController.hpp:61
wpi::units::volt_t Calculate(double measurement, double setpoint)
void Initialize(double b0, wpi::units::second_t period)

Constructor & Destructor Documentation

◆ ADRCController()

yams::math::ADRCController::ADRCController ( std::shared_ptr< ADRCConfig > config)
explicit

Create a controller that reads its settings from a config, with the config's system order.

Parameters
configConfig this controller runs.
Exceptions
std::invalid_argumentif the config is null or has no system order.

Member Function Documentation

◆ Calculate() [1/3]

wpi::units::volt_t yams::math::ADRCController::Calculate ( double measurement,
double setpoint )

Run one control step, as Step(setpoint, measurement).

Parameters
measurementMeasured output.
setpointReference.
Returns
Controller output.
Exceptions
std::logic_errorif the controller has not been initialized.

◆ Calculate() [2/3]

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.

Parameters
measurementMeasured output.
setpointReference.
appliedOutputOutput the actuator applied, such as the motor controller's voltage. A non-finite value is ignored.
Returns
Controller output.
Exceptions
std::logic_errorif the controller has not been initialized.

◆ Calculate() [3/3]

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.

Parameters
measurementMeasured output.
nowThe setpoint now, and its rate.
nextThe setpoint one control period from now, and its rate.
appliedOutputOutput the actuator applied, such as the motor controller's voltage. A non-finite value is ignored.
Returns
Controller output.
Exceptions
std::logic_errorif the controller has not been initialized.

◆ GetB0()

double yams::math::ADRCController::GetB0 ( ) const
Returns
The input gain the gains were worked out with. Throws std::logic_error until initialized.

◆ GetBandwidth()

wpi::units::radians_per_second_t yams::math::ADRCController::GetBandwidth ( ) const
Returns
The closed-loop bandwidth the config's settling time and bandwidth scale give.

◆ GetBandwidthScale()

double yams::math::ADRCController::GetBandwidthScale ( ) const
Returns
The config's bandwidth scale, or its default.

◆ GetConfig()

const std::shared_ptr< ADRCConfig > & yams::math::ADRCController::GetConfig ( ) const
inline
Returns
The config this controller reads its settings and gains from.

◆ GetControllerOrder()

int yams::math::ADRCController::GetControllerOrder ( ) const
inline
Returns
The observer order, systemOrder + 1.

◆ GetEstimatedDisturbance()

double yams::math::ADRCController::GetEstimatedDisturbance ( ) const
Returns
The estimated total disturbance, in the plant's highest derivative per second. Divide by b0 to see it as an input. Throws until initialized.

◆ GetEstimatedStates()

std::vector< double > yams::math::ADRCController::GetEstimatedStates ( ) const
Returns
The observer's state: the measurement, its derivatives, then the disturbance. Throws until initialized.

◆ GetLastOutput()

wpi::units::volt_t yams::math::ADRCController::GetLastOutput ( ) const
Returns
The last output.

◆ GetObserverBandwidth()

wpi::units::radians_per_second_t yams::math::ADRCController::GetObserverBandwidth ( ) const
Returns
The observer bandwidth, four times the closed-loop bandwidth.

◆ GetSettlingTime()

wpi::units::second_t yams::math::ADRCController::GetSettlingTime ( ) const
Returns
The config's settling time, or its default.

◆ GetSystemOrder()

int yams::math::ADRCController::GetSystemOrder ( ) const
inline
Returns
The plant order.

◆ GetTrackingDifferentiator()

const TrackingDifferentiator * yams::math::ADRCController::GetTrackingDifferentiator ( ) const
Returns
The tracking differentiator, or null when it is off.

◆ Initialize()

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.

Parameters
b0Input gain used while the config leaves b0 unset, such as the one ADRCConfig::ResolveB0 derives. Must be finite and non-zero when used.
periodControl period. Must be finite and positive.
Exceptions
std::invalid_argumentif a value is invalid, or the tracking differentiator cannot run at this period.

◆ IsInitialized()

bool yams::math::ADRCController::IsInitialized ( ) const
inline
Returns
Whether Initialize has completed.

◆ Reset() [1/2]

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.

Parameters
xhatInitObserver state, systemOrder + 1 long; empty starts at zero.
uInitOutput.
Exceptions
std::logic_errorif the controller has not been initialized.
std::invalid_argumentif the state has the wrong length or the output is not finite.

◆ Reset() [2/2]

void yams::math::ADRCController::Reset ( double measurement)

Start the observer at a measurement, with the other states, the disturbance and the output at zero.

Parameters
measurementMeasured output, in the units the controller is stepped with.
Exceptions
std::logic_errorif the controller has not been initialized.
std::invalid_argumentif the measurement is not finite.

◆ Step()

wpi::units::volt_t yams::math::ADRCController::Step ( double reference,
double output,
const std::vector< double > & refDerivatives = {} )

Run one control step.

Parameters
referenceReference.
outputMeasured output.
refDerivativesReference derivatives; only the first systemOrder entries are used.
Returns
Controller output.
Exceptions
std::logic_errorif the controller has not been initialized.

◆ ADRCConfig

friend class ADRCConfig
friend

The documentation for this class was generated from the following file: