Class ADRCConfig

java.lang.Object
yams.core.math.ADRCConfig
All Implemented Interfaces:
Cloneable

public class ADRCConfig extends Object implements Cloneable
Configuration for an active disturbance rejection controller.

Written by FRC Team 8331, who contributed active disturbance rejection control to YAMS.

An active disturbance rejection controller lumps everything the mechanism model does not capture (friction, back-EMF, game-piece loads, dropped battery voltage, unmodeled dynamics) into a single "total disturbance" and estimates it live with an extended state observer. The estimate is then cancelled out of the control law, so the closed loop behaves like a clean chain of integrators that settles in the time you ask for, without hand-tuning gains. There is no proportional, integral, or derivative gain to tune: you describe the plant (order and input gain) and the response you want (settling time), and the controller derives its own gains.

This is the drop-in equivalent of withClosedLoopController(...): pass it to SmartMotorControllerConfig.withADRCController(ADRCConfig) and the motor controller runs the active disturbance rejection controller on its own closed-loop thread. Mechanisms need no extra code; they keep calling setVelocity(...) and setPosition(...) exactly as they would with a PIDController.

Getting started

Like every other YAMS config class, this is a plain builder: teams set the values they tune with the withX(...) methods, usually from their Constants class. A complete velocity-loop configuration for a shooter wheel looks like this:

new ADRCConfig()
    .withSystemOrder(SystemOrder.FIRST)
    .withSettlingTime(Seconds.of(0.2))

Every value starts empty. The getters return that empty Optional, and buildController(DCMotor, SmartMotorControllerConfig, Time) fills it with the default documented on the withX(...) method. The one exception is systemOrder, which has no default: buildController(DCMotor, SmartMotorControllerConfig, Time) throws until it is set.

Choosing the system order

The system order is the number of integrators between the motor voltage and the quantity the closed loop commands and measures. Pick the lowest order that describes your loop; the observer adds one more state on top of the plant order, so higher orders are more sensitive to sensor noise.

Velocity loops and position loops differ only in the order; every other option in this class works the same for both.

Tuning the response

  • withSettlingTime(Time) sets how quickly the closed loop settles after a setpoint change. The default, 0.2 seconds, is a good starting point. Lower is faster and more aggressive but demands more voltage and stresses the mechanism; higher is gentler.
  • withBandwidthScale(double) scales both the closed-loop bandwidth and the observer bandwidth together. Above 1.0 is more aggressive, below 1.0 is more conservative. Leave it at the default of 1.0 until the settling time is dialed in.
  • withB0(double) sets the approximate input gain, meaning how strongly one volt accelerates the measured quantity. It only sets the controller's operating point, so being off by 20 to 50 percent usually still works. Leave it unset to derive it from the motor, gearing, and moment of inertia (see "What is never assumed" below); set it explicitly when the mechanism has no meaningful inertia, such as a mechanism driven against a hard stop.
  • withInputDelay(Time) compensates a known delay between commanding the output and seeing its effect, such as a deliberately slowed sensor. Zero (the default) disables it.
  • withCascaded(boolean) runs the observer in cascaded form, which some higher-order plants track more accurately. Leave it disabled unless you have a reason.

The output limits and ramp come from the motor config, as for every closed loop controller: withClosedLoopControllerMaximumVoltage(...) limits the output, and withClosedLoopRampRate(...) limits how quickly it changes.

The withX(...) methods only accept values the controller can use: an invalid one, such as a settling time that is not positive or a zero b0, throws IllegalArgumentException and leaves the config as it was. The controller reads its settings from this config instead of keeping copies, so the config never holds a value it cannot run.

Smoothing the reference with a tracking differentiator

A step setpoint asks the controller for an instantaneous change, which spikes the output and excites unmodeled dynamics. A tracking differentiator replaces the raw reference with a smoothed reference plus its derivatives, so the controller follows a feasible trajectory instead. Enable one with withTrackingDifferentiator(TrackingDifferentiator.Method, double...), for example:

new ADRCConfig()
    .withSystemOrder(SystemOrder.FIRST)
    .withTrackingDifferentiator(TrackingDifferentiator.Method.LD, 0.08)

Five methods are available; see TrackingDifferentiator for the full parameter reference, and ADRCGuide.md for how to choose and tune one. Each entry below lists its parameters in order, in the loop's units (mechanism rotations, meters for a linear loop, or rotations per second for a velocity loop):

  • TrackingDifferentiator.Method.EULER, first-order exponential smoothing. One parameter, alpha in [0, 1] (default 0.9): higher is smoother and slower. The mildest option, but its yd jumps on a step.
  • TrackingDifferentiator.Method.TOD, time-optimal tracking. One parameter, r greater than 0 (default 1.0), the acceleration bound: higher tracks faster. Bang-bang, so yd chatters at rest at a 20 ms period.
  • TrackingDifferentiator.Method.LD, the linear differentiator. One parameter, lambda greater than 0 (default 1.0), a time constant in seconds; the sample period must be below 2 * lambda. No overshoot, and a step is followed in about 5.8 * lambda. The best default.
  • TrackingDifferentiator.Method.RED, the nonlinear differentiator. Two parameters, lambda1 and lambda2, both greater than 0 (default 1.0 each). A super-twisting filter that converges in finite time, but overshoots and chatters at a 20 ms period.
  • TrackingDifferentiator.Method.INTD, the integral differentiator. Four parameters: alpha in [0, 1) (default 0.5), and beta, gamma, r greater than 0 (default 1.0 each). Saturating: cruises at r * beta / (1 - alpha), then lands like LD; gamma = 4 * beta gives no overshoot.

Parameters may be omitted entirely to use the method defaults, and the differentiator is off by default. If the reference only ever ramps (a joystick-driven setpoint, for example), a differentiator buys little; it earns its keep on step changes.

What is never assumed

  • systemOrder must be set with withSystemOrder(SystemOrder). There is no safe default, so buildController(DCMotor, SmartMotorControllerConfig, Time) throws while it is unset.
  • b0 may be set with withB0(double). Otherwise it is derived from the motor, gearing, and the mechanism's moment of inertia, but only when the mechanism explicitly configured that inertia with withMomentOfInertia(...) or withWheelMomentOfInertia(...). The library's placeholder inertia is never used.
  • Output saturation comes from the motor config's withClosedLoopControllerMaximumVoltage(...), which the motor controller applies to every closed loop. The controller itself does not limit its output.
  • The control period comes from withClosedLoopControlPeriod(...), so the controller and the loop cannot disagree.
See Also:
  • Field Details

    • DEFAULT_SETTLING_TIME

      public static final org.wpilib.units.measure.Time DEFAULT_SETTLING_TIME
      Closed-loop settling time used when none is set.
    • DEFAULT_BANDWIDTH_SCALE

      public static final double DEFAULT_BANDWIDTH_SCALE
      Bandwidth scale used when none is set.
      See Also:
    • DEFAULT_INPUT_DELAY

      public static final org.wpilib.units.measure.Time DEFAULT_INPUT_DELAY
      Input delay used when none is set; zero disables it.
    • SPEED_PER_VOLT_TIME_CONSTANT_PERIODS

      public static final double SPEED_PER_VOLT_TIME_CONSTANT_PERIODS
      A position loop's back-EMF time constant below which it is first order, with its speed per volt as b0, in control periods. See ADRCConfig.SystemOrder.POSITION.
      See Also:
  • Constructor Details

    • ADRCConfig

      public ADRCConfig()
      Create a configuration. It belongs to the motor controller it is given to, whose telemetry name its errors and alerts use.
  • Method Details

    • clone

      public ADRCConfig clone()
      Overrides:
      clone in class Object
    • withSystemOrder

      public ADRCConfig withSystemOrder(ADRCConfig.SystemOrder systemOrder)
      Set the plant order. There is no default; the controller cannot be built without it.
      Parameters:
      systemOrder - Plant order.
      Returns:
      This config for chaining.
      Throws:
      IllegalStateException - if a controller was built from this config with a different order, or a different kind of b0.
    • withSettlingTime

      public ADRCConfig withSettlingTime(org.wpilib.units.measure.Time settlingTime)
      Set the desired closed-loop settling time. Defaults to DEFAULT_SETTLING_TIME.
      Parameters:
      settlingTime - Settling time; must be finite and positive.
      Returns:
      This config for chaining.
      Throws:
      IllegalArgumentException - if the settling time is not finite and positive.
    • withBandwidthScale

      public ADRCConfig withBandwidthScale(double bandwidthScale)
      Set the closed-loop bandwidth scale. Defaults to DEFAULT_BANDWIDTH_SCALE.
      Parameters:
      bandwidthScale - Bandwidth scale; must be finite and positive. Larger is more aggressive.
      Returns:
      This config for chaining.
      Throws:
      IllegalArgumentException - if the bandwidth scale is not finite and positive.
    • withB0

      public ADRCConfig withB0(double b0)
      Set the input gain explicitly instead of deriving it.
      Parameters:
      b0 - Input gain in mechanism units per second squared per volt; must be finite and non-zero.
      Returns:
      This config for chaining.
      Throws:
      IllegalArgumentException - if b0 is not finite or is zero.
    • withInputDelay

      public ADRCConfig withInputDelay(org.wpilib.units.measure.Time inputDelay)
      Set the input delay. Defaults to DEFAULT_INPUT_DELAY, which disables it.
      Parameters:
      inputDelay - Input delay; zero disables it. Must be finite and not negative.
      Returns:
      This config for chaining.
      Throws:
      IllegalArgumentException - if the delay is not finite or is negative.
    • withCascaded

      public ADRCConfig withCascaded(boolean cascaded)
      Select the cascaded extended-state-observer form. Disabled by default.
      Parameters:
      cascaded - Whether to run the cascaded observer.
      Returns:
      This config for chaining.
    • withBackEmfModel

      public ADRCConfig withBackEmfModel(boolean backEmfModel)
      Choose whether the observer models the motor's back-EMF. On by default.

      A motor's back-EMF brakes the mechanism in proportion to its speed. Left out of the observer's model, it is lumped into the disturbance the observer estimates, which then changes as fast as the speed does: the observer lags it whenever the mechanism speeds up or slows down, and a mechanism following a motion profile overshoots. Modeled, the disturbance only holds what the model misses, such as gravity and friction.

      Measured in simulation, a 12:1 pivot following a trapezoidal profile overshot by at most 1.1 degrees with it on the motor controllers simulated in step with the robot, and by up to 8.3 without, where the SPARKs oscillated. Held at a setpoint in a Pivot mechanism on a TalonFXS, whose simulation runs on CTRE's own real-time thread, it sometimes oscillated instead. To cancel the modeled back-EMF, the control law pushes with the observer's estimated speed, which cancels the damping the back-EMF gives the motor, so a loop whose timing is far from what it was built for can be better off without it.

      It applies to a position loop that chose second order when built from a motor config (see ADRCConfig.SystemOrder.POSITION), whose back-EMF time constant comes from the motor, the gearing and the moment of inertia. A first order position loop's b0, a speed per volt, already includes the back-EMF. A ADRCConfig.SystemOrder.FLYWHEEL still lumps it into the disturbance: modeled, a flywheel settled up to twice as fast at a settling time of 0.3 s or more, but overshot by up to 7% at 0.15 s on a loop with a period of delay. ADRCConfig.SystemOrder.FIRST to ADRCConfig.SystemOrder.FOURTH leave the model to you. Changing it on a built config works the gains out again.

      Parameters:
      backEmfModel - Whether to model the back-EMF.
      Returns:
      This config for chaining.
    • withTrackingDifferentiator

      public ADRCConfig withTrackingDifferentiator(TrackingDifferentiator.Method method, double... params)
      Enable a tracking differentiator on the reference. Disabled by default.

      The differentiator smooths the reference and supplies its derivatives, so a step setpoint no longer spikes the controller output. See the class documentation for a guide to the five methods and their parameters, and TrackingDifferentiator for the full reference.

      A motion profile on the motor config also shapes the setpoint, so a config with both is refused when the controller is built.

      Parameters:
      method - Differentiation method.
      params - Method parameters in order; leave them out to use the method defaults.
      Returns:
      This config for chaining.
    • withoutTrackingDifferentiator

      public ADRCConfig withoutTrackingDifferentiator()
      Disable the tracking differentiator on the reference.
      Returns:
      This config for chaining.
    • getSystemOrder

      public Optional<ADRCConfig.SystemOrder> getSystemOrder()
      Get the configured plant order.
      Returns:
      Plant order, or empty when it has not been configured.
    • getSettlingTime

      public Optional<org.wpilib.units.measure.Time> getSettlingTime()
      Get the configured settling time.
      Returns:
      Settling time, or empty for DEFAULT_SETTLING_TIME.
    • getBandwidthScale

      public OptionalDouble getBandwidthScale()
      Get the configured bandwidth scale.
      Returns:
      Bandwidth scale, or empty for DEFAULT_BANDWIDTH_SCALE.
    • getB0

      public OptionalDouble getB0()
      Get the configured input gain.
      Returns:
      Input gain, or empty when it should be derived.
    • getInputDelay

      public Optional<org.wpilib.units.measure.Time> getInputDelay()
      Get the configured input delay.
      Returns:
      Input delay, or empty for DEFAULT_INPUT_DELAY.
    • isCascaded

      public boolean isCascaded()
      Whether the cascaded extended-state-observer form is enabled.
      Returns:
      true when the cascaded observer is enabled.
    • isBackEmfModeled

      public boolean isBackEmfModeled()
      Whether the observer may model the motor's back-EMF. See withBackEmfModel(boolean).
      Returns:
      true unless it was turned off.
    • getModeledBackEmfTimeConstant

      public Optional<org.wpilib.units.measure.Time> getModeledBackEmfTimeConstant()
      The back-EMF time constant the built controller's observer models.
      Returns:
      The time constant, or empty when the back-EMF is left in the disturbance: it was turned off, the plant does not model it, or no controller has been built.
    • getTdMethod

      public Optional<TrackingDifferentiator.Method> getTdMethod()
      Get the configured tracking differentiator method.
      Returns:
      Differentiation method, or empty when the differentiator is disabled.
    • getTdParams

      public Optional<double[]> getTdParams()
      Get the tracking differentiator parameters.
      Returns:
      Copy of the parameters, or empty for the method defaults.
    • buildController

      public ADRCController buildController(org.wpilib.math.system.DCMotor motor, SmartMotorControllerConfig<?> config, org.wpilib.units.measure.Time period)
      Build a controller from this config, filling every unset value with its default.

      The controller reads its settings from this config every step, and its gains too: the config keeps the control period, the b0 used while it is unset, and the gains worked out from them, and works the gains out again whenever its settling time, bandwidth scale or b0 changes. So a change, as live tuning makes, takes effect on the next step without resetting the state estimate. The system order, input delay and tracking differentiator only take effect when a controller is built, and a built config refuses a different order.

      The controller does not limit or ramp its output: the motor controller does, with withClosedLoopControllerMaximumVoltage(...) and withClosedLoopRampRate(...).

      Parameters:
      motor - Motor model used for b0 derivation.
      config - Motor config that carries gearing, inertia, circumference, and the loop period.
      period - Closed-loop control period.
      Returns:
      Initialized ADRCController.
      Throws:
      SmartMotorControllerConfigurationException - if the order is missing or b0 cannot be derived.
      SmartMotorControllerConfigurationException - if the config has a tracking differentiator and the motor config has a motion profile: both shape the setpoint.
      IllegalArgumentException - if ADRCController.initialize(double, Time) rejects the derived b0 or the tracking differentiator.
    • resolveB0

      public double resolveB0(org.wpilib.math.system.DCMotor motor, SmartMotorControllerConfig<?> config)
      Resolve the input gain: the one set, or one derived in the loop's units. For an ADRCConfig.SystemOrder.ELEVATOR it is the speed per volt, from the motor, gearing and mechanism circumference. Otherwise it is the acceleration per volt, derived only when the mechanism explicitly configured its inertia.
      Parameters:
      motor - Motor model.
      config - Motor config that carries gearing, inertia, and circumference.
      Returns:
      Input gain in the loop's units per volt per second to the plant's order.
      Throws:
      SmartMotorControllerConfigurationException - if the order is an elevator's and the closed loop is not linear, or b0 is not set and cannot be derived safely.
    • reportRejected

      public void reportRejected(Optional<String> reason)
      Report a value this config refused, as live tuning does when a tuned value throws. The alert lives on the config, so it outlasts a controller rebuilt from it.
      Parameters:
      reason - Why the value was refused, or empty once a tuned value is accepted.
    • toString

      public String toString()
      Overrides:
      toString in class Object