Class ExponentialProfilePIDController

java.lang.Object
yams.core.math.ExponentialProfilePIDController
public class ExponentialProfilePIDController extends Object
Exponential profile PID controller. Similar to PIDController or ProfiledPIDController, but uses an ExponentialProfile

This controller combines an ExponentialProfile motion profile with a PIDController to achieve smooth, continuous position control. On each iteration the profile generates a feasible intermediate setpoint (position + velocity) that the PID then tracks, preventing large step demands from commanding unreachable states. The exponential profile shape respects the plant's natural first-order dynamics, making it well-suited for flywheels, arms, and elevators.

Use the static factory helpers (createArmConstraints(Voltage, DCMotor, MomentOfInertia, MechanismGearing), createElevatorConstraints(Voltage, DCMotor, Mass, Distance, MechanismGearing), createFlywheelConstraints(Voltage, DCMotor, MomentOfInertia, MechanismGearing)) to derive physically accurate ExponentialProfile.Constraints directly from motor and mechanism parameters.

Example arm position control

import static org.wpilib.units.Units.*;

// Derive constraints from the physical arm model
ExponentialProfile.Constraints armConstraints =
    ExponentialProfilePIDController.createArmConstraints(
        Volts.of(12),
        DCMotor.getNEO(1),
        KilogramSquareMeters.of(0.25),
        MechanismGearing.ofReduction(60.0));

// kP=1.0, kI=0.0, kD=0.0 tune these for your mechanism
ExponentialProfilePIDController armPID =
    new ExponentialProfilePIDController(1.0, 0.0, 0.0, armConstraints);

// In robotInit, seed the profile at the current sensor state:
armPID.reset(encoder.getPosition(), 0.0);

// In periodic, calculate output (positions in rotations):
double output = armPID.calculate(
        encoder.getPosition(),   // measured position (rotations)
        targetPosition);          // desired position (rotations)
motor.set(output);

  • Constructor Summary

    Constructors
    Constructor
    Description
    ExponentialProfilePIDController(double kP, double kI, double kD, org.wpilib.math.trajectory.ExponentialProfile.Constraints constraints)
    Constructor.
    ExponentialProfilePIDController(org.wpilib.math.controller.PIDController controller, org.wpilib.math.trajectory.ExponentialProfile.Constraints constraints)
    Constructor.

  • Method Summary

    Modifier and Type
    Method
    Description
    boolean
    atSetpoint()
    Returns true if the error is within the tolerance of the setpoint.
    double
    calculate(double measurementPosition, double setpointPosition)
    Calculate the feedback, assuming no setpoint velocity.
    double
    calculate(double measurementPosition, double setpointVelocity, double setpointPosition)
    Calculate the feedback, assuming previous state velocity.
    static org.wpilib.math.trajectory.ExponentialProfile.Constraints
    createArmConstraints(org.wpilib.units.measure.Voltage maxVolts, org.wpilib.math.system.DCMotor motor, org.wpilib.units.measure.Mass mass, org.wpilib.units.measure.Distance length, MechanismGearing gearing)
    Get the ExponentialProfile.Constraints for an arm.
    static org.wpilib.math.trajectory.ExponentialProfile.Constraints
    createArmConstraints(org.wpilib.units.measure.Voltage maxVolts, org.wpilib.math.system.DCMotor motor, org.wpilib.units.measure.MomentOfInertia moi, MechanismGearing gearing)
    Get the ExponentialProfile.Constraints for an arm.
    static org.wpilib.math.trajectory.ExponentialProfile.Constraints
    createConstraints(org.wpilib.units.measure.Voltage maxVolts, org.wpilib.units.measure.AngularVelocity maxVelocity, org.wpilib.units.measure.AngularAcceleration maxAcceleration)
    Create a generic constraints object.
    static org.wpilib.math.trajectory.ExponentialProfile.Constraints
    createElevatorConstraints(org.wpilib.units.measure.Voltage maxVolts, org.wpilib.math.system.DCMotor motor, org.wpilib.units.measure.Mass mass, org.wpilib.units.measure.Distance drumRadius, MechanismGearing gearing)
    Get the ExponentialProfile.Constraints for an elevator.
    static org.wpilib.math.trajectory.ExponentialProfile.Constraints
    createFlywheelConstraints(org.wpilib.units.measure.Voltage maxVolts, org.wpilib.math.system.DCMotor motor, org.wpilib.units.measure.Mass mass, org.wpilib.units.measure.Distance radius, MechanismGearing gearing)
    Get the ExponentialProfile.Constraints for a flywheel.
    static org.wpilib.math.trajectory.ExponentialProfile.Constraints
    createFlywheelConstraints(org.wpilib.units.measure.Voltage maxVolts, org.wpilib.math.system.DCMotor motor, org.wpilib.units.measure.MomentOfInertia moi, MechanismGearing gearing)
    Get the ExponentialProfile.Constraints for a flywheel.
    void
    enableContinuousInput(double minimumInput, double maximumInput)
    Enables continuous input.
    Optional<org.wpilib.math.trajectory.ExponentialProfile.Constraints>
    getConstraints()
    Get the constraints.
    org.wpilib.units.measure.Angle
    getCurrentAngle()
    Get the current angle.
    org.wpilib.math.trajectory.ExponentialProfile.State
    getCurrentState()
    Get the current state of the ExponentialProfile.
    org.wpilib.units.measure.AngularVelocity
    getCurrentVelocitySetpoint()
    Get the current velocity from ExponentialProfile.
    double
    getD()
    Get the Differential coefficient.
    double
    getI()
    Get the Integral coefficient.
    org.wpilib.units.measure.AngularAcceleration
    getKa()
    Get the acceleration gain kA
    org.wpilib.units.measure.AngularVelocity
    getKv()
    Get the velocity gain as constant.
    org.wpilib.units.measure.Angle
    getNextAngle()
    Get the next angle from the ExponentialProfile
    Optional<org.wpilib.math.trajectory.ExponentialProfile.State>
    getNextState()
    Get the next state of the ExponentialProfile.
    org.wpilib.units.measure.AngularVelocity
    getNextVelocitySetpoint()
    Get the next velocity from ExponentialProfile
    double
    getP()
    Get the Proportional coefficient.
    double
    getPositionTolerance()
    Returns the error tolerance of this controller.
    double
    getSetpoint()
    Get the setpoint.
    void
    reset(double position, double velocity)
    Reset the PID and profile with the given position and velocity as the measured position and velocity.
    void
    reset(org.wpilib.math.trajectory.ExponentialProfile.State measurement)
    Reset the controller, set the next setpoint to empty.
    void
    setConstraints(org.wpilib.math.trajectory.ExponentialProfile.Constraints constraints)
    Sets the constraints for the ExponentialProfile.
    void
    setD(double kD)
    Sets the Differential coefficient of the PID controller gain.
    void
    setI(double kI)
    Sets the Integral coefficient of the PID controller gain.
    void
    setP(double kP)
    Sets the Proportional coefficient of the PID controller gain.
    void
    setTolerance(double tolerance)
    Sets the error which is considered tolerable for use with atSetpoint().

    Methods inherited from class Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Constructor Details

    • ExponentialProfilePIDController

      public ExponentialProfilePIDController(org.wpilib.math.controller.PIDController controller, org.wpilib.math.trajectory.ExponentialProfile.Constraints constraints)
      Constructor.
      Parameters:
      controller - The wrapped PID controller.
      constraints - The wrapped profile constraints.

    • ExponentialProfilePIDController

      public ExponentialProfilePIDController(double kP, double kI, double kD, org.wpilib.math.trajectory.ExponentialProfile.Constraints constraints)
      Constructor.
      Parameters:
      kP - kP value for the PIDController
      kI - kI value for the PIDController
      kD - kD value for the PIDController
      constraints - ExponentialProfile.Constraints for the ExponentialProfile

  • Method Details

    • createElevatorConstraints

      public static org.wpilib.math.trajectory.ExponentialProfile.Constraints createElevatorConstraints(org.wpilib.units.measure.Voltage maxVolts, org.wpilib.math.system.DCMotor motor, org.wpilib.units.measure.Mass mass, org.wpilib.units.measure.Distance drumRadius, MechanismGearing gearing)
      Get the ExponentialProfile.Constraints for an elevator.
      Parameters:
      maxVolts - Maximum input voltage for profile generation.
      motor - DCMotor of the elevator.
      mass - Mass of the elevator carriage.
      drumRadius - Distance of the elevator drum radius.
      gearing - MechanismGearing of the elevator from the drum to the rotor.
      Returns:
      ExponentialProfile.Constraints

    • createArmConstraints

      public static org.wpilib.math.trajectory.ExponentialProfile.Constraints createArmConstraints(org.wpilib.units.measure.Voltage maxVolts, org.wpilib.math.system.DCMotor motor, org.wpilib.units.measure.MomentOfInertia moi, MechanismGearing gearing)
      Get the ExponentialProfile.Constraints for an arm.
      Parameters:
      maxVolts - Maximum input voltage for profile generation.
      motor - DCMotor of the arm.
      moi - MomentOfInertia of the arm.
      gearing - MechanismGearing of the arm from the rotor to the drum. gearing.getMechanismToRotorRatio()
      Returns:
      ExponentialProfile.Constraints

    • createArmConstraints

      public static org.wpilib.math.trajectory.ExponentialProfile.Constraints createArmConstraints(org.wpilib.units.measure.Voltage maxVolts, org.wpilib.math.system.DCMotor motor, org.wpilib.units.measure.Mass mass, org.wpilib.units.measure.Distance length, MechanismGearing gearing)
      Get the ExponentialProfile.Constraints for an arm.
      Parameters:
      maxVolts - Maximum input voltage for profile generation.
      motor - DCMotor of the arm.
      mass - Mass of the arm.
      length - Distance of the arm length.
      gearing - MechanismGearing of the arm from the rotor to the drum. gearing.getMechanismToRotorRatio()
      Returns:
      ExponentialProfile.Constraints

    • createFlywheelConstraints

      public static org.wpilib.math.trajectory.ExponentialProfile.Constraints createFlywheelConstraints(org.wpilib.units.measure.Voltage maxVolts, org.wpilib.math.system.DCMotor motor, org.wpilib.units.measure.MomentOfInertia moi, MechanismGearing gearing)
      Get the ExponentialProfile.Constraints for a flywheel.
      Parameters:
      maxVolts - Maximum input voltage for profile generation.
      motor - DCMotor of the flywheel.
      moi - MomentOfInertia of the flywheel.
      gearing - MechanismGearing of the flywheel from the rotor to the drum.
      Returns:
      ExponentialProfile.Constraints

    • createFlywheelConstraints

      public static org.wpilib.math.trajectory.ExponentialProfile.Constraints createFlywheelConstraints(org.wpilib.units.measure.Voltage maxVolts, org.wpilib.math.system.DCMotor motor, org.wpilib.units.measure.Mass mass, org.wpilib.units.measure.Distance radius, MechanismGearing gearing)
      Get the ExponentialProfile.Constraints for a flywheel.
      Parameters:
      maxVolts - Maximum input voltage for profile generation.
      motor - DCMotor of the flywheel.
      mass - Mass of the flywheel.
      radius - Distance of the flywheel radius.
      gearing - MechanismGearing of the flywheel from the rotor to the drum.
      Returns:
      ExponentialProfile.Constraints

    • createConstraints

      public static org.wpilib.math.trajectory.ExponentialProfile.Constraints createConstraints(org.wpilib.units.measure.Voltage maxVolts, org.wpilib.units.measure.AngularVelocity maxVelocity, org.wpilib.units.measure.AngularAcceleration maxAcceleration)
      Create a generic constraints object.
      Parameters:
      maxVolts - Maximum input voltage for profile generation.
      maxVelocity - Maximum velocity.
      maxAcceleration - Maximum acceleration.
      Returns:
      ExponentialProfile.Constraints

    • getKv

      public org.wpilib.units.measure.AngularVelocity getKv()
      Get the velocity gain as constant.
      Returns:
      kV with (-A/B)
      Throws:
      IllegalStateException - if the profile constraints are null (never set or set to null).

    • getKa

      public org.wpilib.units.measure.AngularAcceleration getKa()
      Get the acceleration gain kA
      Returns:
      kA interpreted as (1.0/B)
      Throws:
      IllegalStateException - if the profile constraints are null (never set or set to null).

    • reset

      public void reset(org.wpilib.math.trajectory.ExponentialProfile.State measurement)
      Reset the controller, set the next setpoint to empty.
      Parameters:
      measurement - Measurement in Rotations, and Rotations per Second.

    • reset

      public void reset(double position, double velocity)
      Reset the PID and profile with the given position and velocity as the measured position and velocity.
      Parameters:
      position - Measured position
      velocity - Measured velocity.

    • getConstraints

      public Optional<org.wpilib.math.trajectory.ExponentialProfile.Constraints> getConstraints()
      Get the constraints.
      Returns:
      ExponentialProfile.Constraints

    • setConstraints

      public void setConstraints(org.wpilib.math.trajectory.ExponentialProfile.Constraints constraints)
      Sets the constraints for the ExponentialProfile.
      Parameters:
      constraints - The constraints for the ExponentialProfile.

    • setTolerance

      public void setTolerance(double tolerance)
      Sets the error which is considered tolerable for use with atSetpoint().
      Parameters:
      tolerance - – Error which is tolerable.

    • atSetpoint

      public boolean atSetpoint()
      Returns true if the error is within the tolerance of the setpoint. The error tolerance defaults to 0.05, and the error derivative tolerance defaults to ∞. This will return false until at least one input value has been computed.
      Returns:
      Whether the error is within the acceptable bounds

    • getSetpoint

      public double getSetpoint()
      Get the setpoint.
      Returns:
      setpoint.

    • getCurrentState

      public org.wpilib.math.trajectory.ExponentialProfile.State getCurrentState()
      Get the current state of the ExponentialProfile.
      Returns:
      ExponentialProfile.State given by the ExponentialProfile.

    • getCurrentAngle

      public org.wpilib.units.measure.Angle getCurrentAngle()
      Get the current angle.
      Returns:
      Angle from ExponentialProfile

    • getNextAngle

      public org.wpilib.units.measure.Angle getNextAngle()
      Get the next angle from the ExponentialProfile
      Returns:
      Angle from ExponentialProfile
      Throws:
      NoSuchElementException - if no next state exists because calculate(double, double, double) has not been called since construction or the last reset(State).

    • getCurrentVelocitySetpoint

      public org.wpilib.units.measure.AngularVelocity getCurrentVelocitySetpoint()
      Get the current velocity from ExponentialProfile.
      Returns:
      AngularVelocity from ExponentialProfile

    • getNextVelocitySetpoint

      public org.wpilib.units.measure.AngularVelocity getNextVelocitySetpoint()
      Get the next velocity from ExponentialProfile
      Returns:
      Next AngularVelocity from ExponentialProfile
      Throws:
      NoSuchElementException - if no next state exists because calculate(double, double, double) has not been called since construction or the last reset(State).

    • getNextState

      public Optional<org.wpilib.math.trajectory.ExponentialProfile.State> getNextState()
      Get the next state of the ExponentialProfile.
      Returns:
      ExponentialProfile.State given by the ExponentialProfile.

    • calculate

      public double calculate(double measurementPosition, double setpointVelocity, double setpointPosition)
      Calculate the feedback, assuming previous state velocity.
      Parameters:
      measurementPosition - Measurement position to set as the current state..
      setpointVelocity - Setpoint velocity.
      setpointPosition - Setpoint position.
      Returns:
      Profile calculation

    • calculate

      public double calculate(double measurementPosition, double setpointPosition)
      Calculate the feedback, assuming no setpoint velocity.
      Parameters:
      measurementPosition - Measurement position to set as the current state.
      setpointPosition - Setpoint position.
      Returns:
      Profile calculation where setpoint velocity is 0.

    • getPositionTolerance

      public double getPositionTolerance()
      Returns the error tolerance of this controller. Defaults to 0.05.
      Returns:
      the error tolerance of the controller

    • getP

      public double getP()
      Get the Proportional coefficient.
      Returns:
      proportional coefficient

    • setP

      public void setP(double kP)
      Sets the Proportional coefficient of the PID controller gain.
      Parameters:
      kP - The proportional coefficient. Must be >= 0.

    • getI

      public double getI()
      Get the Integral coefficient.
      Returns:
      integral coefficient

    • setI

      public void setI(double kI)
      Sets the Integral coefficient of the PID controller gain.
      Parameters:
      kI - The integral coefficient. Must be >= 0.

    • getD

      public double getD()
      Get the Differential coefficient.
      Returns:
      differential coefficient

    • setD

      public void setD(double kD)
      Sets the Differential coefficient of the PID controller gain.
      Parameters:
      kD - The differential coefficient. Must be >= 0.

    • enableContinuousInput

      public void enableContinuousInput(double minimumInput, double maximumInput)
      Enables continuous input.

      Rather then using the max and min input range as constraints, it considers them to be the same point and automatically calculates the shortest route to the setpoint.

      Parameters:
      minimumInput - The minimum value expected from the input.
      maximumInput - The maximum value expected from the input.