- FRC Game Manual
- FRC Game Q&A

# Motion Profiling in Command-based

Note

For a description of the WPILib motion profiling features used by these command-based wrappers, see Trapezoidal Motion Profiles in WPILib.

Note

The `TrapezoidProfile` class, used on its own, is most useful when composed with external controllers, such as a “smart” motor controller with a built-in PID functionality. For combining trapezoidal motion profiling with WPILib’s `PIDController`, see Combining Motion Profiling and PID in Command-Based.

When controlling a mechanism, is often desirable to move it smoothly between two positions, rather than to abruptly change its setpoint. This is called “motion-profiling,” and is supported in WPILib through the `TrapezoidProfile` class (Java, C++).

Note

In C++, the `TrapezoidProfile` class is templated on the unit type used for distance measurements, which may be angular or linear. The passed-in values must have units consistent with the distance units, or a compile-time error will be thrown. For more information on C++ units, see The C++ Units Library.

The following examples are taken from the DriveDistanceOffboard example project (Java, C++):

Java

      5package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems;
      6
      7import edu.wpi.first.math.controller.SimpleMotorFeedforward;
      8import edu.wpi.first.math.trajectory.TrapezoidProfile;
      9import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
     10import edu.wpi.first.util.sendable.SendableRegistry;
     11import edu.wpi.first.wpilibj.RobotController;
     12import edu.wpi.first.wpilibj.Timer;
     13import edu.wpi.first.wpilibj.drive.DifferentialDrive;
     14import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
     15import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController;
     16import edu.wpi.first.wpilibj2.command.Command;
     17import edu.wpi.first.wpilibj2.command.SubsystemBase;
     18
     19public class DriveSubsystem extends SubsystemBase {
     20  // The motors on the left side of the drive.
     21  private final ExampleSmartMotorController m_leftLeader =
     22      new ExampleSmartMotorController(DriveConstants.kLeftMotor1Port);
     23
     24  private final ExampleSmartMotorController m_leftFollower =
     25      new ExampleSmartMotorController(DriveConstants.kLeftMotor2Port);
     26
     27  // The motors on the right side of the drive.
     28  private final ExampleSmartMotorController m_rightLeader =
     29      new ExampleSmartMotorController(DriveConstants.kRightMotor1Port);
     30
     31  private final ExampleSmartMotorController m_rightFollower =
     32      new ExampleSmartMotorController(DriveConstants.kRightMotor2Port);
     33
     34  // The feedforward controller.
     35  private final SimpleMotorFeedforward m_feedforward =
     36      new SimpleMotorFeedforward(
     37          DriveConstants.ksVolts,
     38          DriveConstants.kvVoltSecondsPerMeter,
     39          DriveConstants.kaVoltSecondsSquaredPerMeter);
     40
     41  // The robot's drive
     42  private final DifferentialDrive m_drive =
     43      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
     44
     45  // The trapezoid profile
     46  private final TrapezoidProfile m_profile =
     47      new TrapezoidProfile(
     48          new TrapezoidProfile.Constraints(
     49              DriveConstants.kMaxSpeedMetersPerSecond,
     50              DriveConstants.kMaxAccelerationMetersPerSecondSquared));
     51
     52  // The timer
     53  private final Timer m_timer = new Timer();
     54
     55  /** Creates a new DriveSubsystem. */
     56  public DriveSubsystem() {
     57    SendableRegistry.addChild(m_drive, m_leftLeader);
     58    SendableRegistry.addChild(m_drive, m_rightLeader);
     59
     60    // We need to invert one side of the drivetrain so that positive voltages
     61    // result in both sides moving forward. Depending on how your robot's
     62    // gearbox is constructed, you might have to invert the left side instead.
     63    m_rightLeader.setInverted(true);
     64
     65    m_leftFollower.follow(m_leftLeader);
     66    m_rightFollower.follow(m_rightLeader);
     67
     68    m_leftLeader.setPID(DriveConstants.kp, 0, 0);
     69    m_rightLeader.setPID(DriveConstants.kp, 0, 0);
     70  }
     71
     72  /**
     73   * Drives the robot using arcade controls.
     74   *
     75   * @param fwd the commanded forward movement
     76   * @param rot the commanded rotation
     77   */
     78  public void arcadeDrive(double fwd, double rot) {
     79    m_drive.arcadeDrive(fwd, rot);
     80  }
     81
     82  /**
     83   * Attempts to follow the given drive states using offboard PID.
     84   *
     85   * @param currentLeft The current left wheel state.
     86   * @param currentRight The current right wheel state.
     87   * @param nextLeft The next left wheel state.
     88   * @param nextRight The next right wheel state.
     89   */
     90  public void setDriveStates(
     91      TrapezoidProfile.State currentLeft,
     92      TrapezoidProfile.State currentRight,
     93      TrapezoidProfile.State nextLeft,
     94      TrapezoidProfile.State nextRight) {
     95    // Feedforward is divided by battery voltage to normalize it to [-1, 1]
     96    m_leftLeader.setSetpoint(
     97        ExampleSmartMotorController.PIDMode.kPosition,
     98        currentLeft.position,
     99        m_feedforward.calculateWithVelocities(currentLeft.velocity, nextLeft.velocity)
    100            / RobotController.getBatteryVoltage());
    101    m_rightLeader.setSetpoint(
    102        ExampleSmartMotorController.PIDMode.kPosition,
    103        currentRight.position,
    104        m_feedforward.calculateWithVelocities(currentLeft.velocity, nextLeft.velocity)
    105            / RobotController.getBatteryVoltage());
    106  }
    107
    108  /**
    109   * Returns the left encoder distance.
    110   *
    111   * @return the left encoder distance
    112   */
    113  public double getLeftEncoderDistance() {
    114    return m_leftLeader.getEncoderDistance();
    115  }
    116
    117  /**
    118   * Returns the right encoder distance.
    119   *
    120   * @return the right encoder distance
    121   */
    122  public double getRightEncoderDistance() {
    123    return m_rightLeader.getEncoderDistance();
    124  }
    125
    126  /** Resets the drive encoders. */
    127  public void resetEncoders() {
    128    m_leftLeader.resetEncoder();
    129    m_rightLeader.resetEncoder();
    130  }
    131
    132  /**
    133   * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
    134   *
    135   * @param maxOutput the maximum output to which the drive will be constrained
    136   */
    137  public void setMaxOutput(double maxOutput) {
    138    m_drive.setMaxOutput(maxOutput);
    139  }
    140
    141  /**
    142   * Creates a command to drive forward a specified distance using a motion profile.
    143   *
    144   * @param distance The distance to drive forward.
    145   * @return A command.
    146   */
    147  public Command profiledDriveDistance(double distance) {
    148    return startRun(
    149            () -> {
    150              // Restart timer so profile setpoints start at the beginning
    151              m_timer.restart();
    152              resetEncoders();
    153            },
    154            () -> {
    155              // Current state never changes, so we need to use a timer to get the setpoints we need
    156              // to be at
    157              var currentTime = m_timer.get();
    158              var currentSetpoint =
    159                  m_profile.calculate(currentTime, new State(), new State(distance, 0));
    160              var nextSetpoint =
    161                  m_profile.calculate(
    162                      currentTime + DriveConstants.kDt, new State(), new State(distance, 0));
    163              setDriveStates(currentSetpoint, currentSetpoint, nextSetpoint, nextSetpoint);
    164            })
    165        .until(() -> m_profile.isFinished(0));
    166  }
    167
    168  private double m_initialLeftDistance;
    169  private double m_initialRightDistance;
    170
    171  /**
    172   * Creates a command to drive forward a specified distance using a motion profile without
    173   * resetting the encoders.
    174   *
    175   * @param distance The distance to drive forward.
    176   * @return A command.
    177   */
    178  public Command dynamicProfiledDriveDistance(double distance) {
    179    return startRun(
    180            () -> {
    181              // Restart timer so profile setpoints start at the beginning
    182              m_timer.restart();
    183              // Store distance so we know the target distance for each encoder
    184              m_initialLeftDistance = getLeftEncoderDistance();
    185              m_initialRightDistance = getRightEncoderDistance();
    186            },
    187            () -> {
    188              // Current state never changes for the duration of the command, so we need to use a
    189              // timer to get the setpoints we need to be at
    190              var currentTime = m_timer.get();
    191              var currentLeftSetpoint =
    192                  m_profile.calculate(
    193                      currentTime,
    194                      new State(m_initialLeftDistance, 0),
    195                      new State(m_initialLeftDistance + distance, 0));
    196              var currentRightSetpoint =
    197                  m_profile.calculate(
    198                      currentTime,
    199                      new State(m_initialRightDistance, 0),
    200                      new State(m_initialRightDistance + distance, 0));
    201              var nextLeftSetpoint =
    202                  m_profile.calculate(
    203                      currentTime + DriveConstants.kDt,
    204                      new State(m_initialLeftDistance, 0),
    205                      new State(m_initialLeftDistance + distance, 0));
    206              var nextRightSetpoint =
    207                  m_profile.calculate(
    208                      currentTime + DriveConstants.kDt,
    209                      new State(m_initialRightDistance, 0),
    210                      new State(m_initialRightDistance + distance, 0));
    211              setDriveStates(
    212                  currentLeftSetpoint, currentRightSetpoint, nextLeftSetpoint, nextRightSetpoint);
    213            })
    214        .until(() -> m_profile.isFinished(0));
    215  }
    216}

C++ (Header)

      5#pragma once
      6
      7#include <frc/Encoder.h>
      8#include <frc/Timer.h>
      9#include <frc/controller/SimpleMotorFeedforward.h>
     10#include <frc/drive/DifferentialDrive.h>
     11#include <frc/trajectory/TrapezoidProfile.h>
     12#include <frc2/command/CommandPtr.h>
     13#include <frc2/command/SubsystemBase.h>
     14#include <units/length.h>
     15
     16#include "Constants.h"
     17#include "ExampleSmartMotorController.h"
     18
     19class DriveSubsystem : public frc2::SubsystemBase {
     20 public:
     21  DriveSubsystem();
     22
     23  /**
     24   * Will be called periodically whenever the CommandScheduler runs.
     25   */
     26  void Periodic() override;
     27
     28  // Subsystem methods go here.
     29
     30  /**
     31   * Attempts to follow the given drive states using offboard PID.
     32   *
     33   * @param currentLeft The current left wheel state.
     34   * @param currentRight The current right wheel state.
     35   * @param nextLeft The next left wheel state.
     36   * @param nextRight The next right wheel state.
     37   */
     38  void SetDriveStates(frc::TrapezoidProfile<units::meters>::State currentLeft,
     39                      frc::TrapezoidProfile<units::meters>::State currentRight,
     40                      frc::TrapezoidProfile<units::meters>::State nextLeft,
     41                      frc::TrapezoidProfile<units::meters>::State nextRight);
     42
     43  /**
     44   * Drives the robot using arcade controls.
     45   *
     46   * @param fwd the commanded forward movement
     47   * @param rot the commanded rotation
     48   */
     49  void ArcadeDrive(double fwd, double rot);
     50
     51  /**
     52   * Resets the drive encoders to currently read a position of 0.
     53   */
     54  void ResetEncoders();
     55
     56  /**
     57   * Gets the distance of the left encoder.
     58   *
     59   * @return the average of the TWO encoder readings
     60   */
     61  units::meter_t GetLeftEncoderDistance();
     62
     63  /**
     64   * Gets the distance of the right encoder.
     65   *
     66   * @return the average of the TWO encoder readings
     67   */
     68  units::meter_t GetRightEncoderDistance();
     69
     70  /**
     71   * Sets the max output of the drive.  Useful for scaling the drive to drive
     72   * more slowly.
     73   *
     74   * @param maxOutput the maximum output to which the drive will be constrained
     75   */
     76  void SetMaxOutput(double maxOutput);
     77
     78  /**
     79   * Creates a command to drive forward a specified distance using a motion
     80   * profile.
     81   *
     82   * @param distance The distance to drive forward.
     83   * @return A command.
     84   */
     85  frc2::CommandPtr ProfiledDriveDistance(units::meter_t distance);
     86
     87  /**
     88   * Creates a command to drive forward a specified distance using a motion
     89   * profile without resetting the encoders.
     90   *
     91   * @param distance The distance to drive forward.
     92   * @return A command.
     93   */
     94  frc2::CommandPtr DynamicProfiledDriveDistance(units::meter_t distance);
     95
     96 private:
     97  frc::TrapezoidProfile<units::meters> m_profile{
     98      {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}};
     99  frc::Timer m_timer;
    100  units::meter_t m_initialLeftDistance;
    101  units::meter_t m_initialRightDistance;
    102  // Components (e.g. motor controllers and sensors) should generally be
    103  // declared private and exposed only through public methods.
    104
    105  // The motor controllers
    106  ExampleSmartMotorController m_leftLeader;
    107  ExampleSmartMotorController m_leftFollower;
    108  ExampleSmartMotorController m_rightLeader;
    109  ExampleSmartMotorController m_rightFollower;
    110
    111  // A feedforward component for the drive
    112  frc::SimpleMotorFeedforward<units::meters> m_feedforward;
    113
    114  // The robot's drive
    115  frc::DifferentialDrive m_drive{
    116      [&](double output) { m_leftLeader.Set(output); },
    117      [&](double output) { m_rightLeader.Set(output); }};
    118};

C++ (Source)

      5#include "subsystems/DriveSubsystem.h"
      6
      7#include <frc/RobotController.h>
      8
      9using namespace DriveConstants;
     10
     11DriveSubsystem::DriveSubsystem()
     12    : m_leftLeader{kLeftMotor1Port},
     13      m_leftFollower{kLeftMotor2Port},
     14      m_rightLeader{kRightMotor1Port},
     15      m_rightFollower{kRightMotor2Port},
     16      m_feedforward{ks, kv, ka} {
     17  wpi::SendableRegistry::AddChild(&m_drive, &m_leftLeader);
     18  wpi::SendableRegistry::AddChild(&m_drive, &m_rightLeader);
     19
     20  // We need to invert one side of the drivetrain so that positive voltages
     21  // result in both sides moving forward. Depending on how your robot's
     22  // gearbox is constructed, you might have to invert the left side instead.
     23  m_rightLeader.SetInverted(true);
     24
     25  m_leftFollower.Follow(m_leftLeader);
     26  m_rightFollower.Follow(m_rightLeader);
     27
     28  m_leftLeader.SetPID(kp, 0, 0);
     29  m_rightLeader.SetPID(kp, 0, 0);
     30}
     31
     32void DriveSubsystem::Periodic() {
     33  // Implementation of subsystem periodic method goes here.
     34}
     35
     36void DriveSubsystem::SetDriveStates(
     37    frc::TrapezoidProfile<units::meters>::State currentLeft,
     38    frc::TrapezoidProfile<units::meters>::State currentRight,
     39    frc::TrapezoidProfile<units::meters>::State nextLeft,
     40    frc::TrapezoidProfile<units::meters>::State nextRight) {
     41  // Feedforward is divided by battery voltage to normalize it to [-1, 1]
     42  m_leftLeader.SetSetpoint(
     43      ExampleSmartMotorController::PIDMode::kPosition,
     44      currentLeft.position.value(),
     45      m_feedforward.Calculate(currentLeft.velocity, nextLeft.velocity) /
     46          frc::RobotController::GetBatteryVoltage());
     47  m_rightLeader.SetSetpoint(
     48      ExampleSmartMotorController::PIDMode::kPosition,
     49      currentRight.position.value(),
     50      m_feedforward.Calculate(currentRight.velocity, nextRight.velocity) /
     51          frc::RobotController::GetBatteryVoltage());
     52}
     53
     54void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
     55  m_drive.ArcadeDrive(fwd, rot);
     56}
     57
     58void DriveSubsystem::ResetEncoders() {
     59  m_leftLeader.ResetEncoder();
     60  m_rightLeader.ResetEncoder();
     61}
     62
     63units::meter_t DriveSubsystem::GetLeftEncoderDistance() {
     64  return units::meter_t{m_leftLeader.GetEncoderDistance()};
     65}
     66
     67units::meter_t DriveSubsystem::GetRightEncoderDistance() {
     68  return units::meter_t{m_rightLeader.GetEncoderDistance()};
     69}
     70
     71void DriveSubsystem::SetMaxOutput(double maxOutput) {
     72  m_drive.SetMaxOutput(maxOutput);
     73}
     74
     75frc2::CommandPtr DriveSubsystem::ProfiledDriveDistance(
     76    units::meter_t distance) {
     77  return StartRun(
     78             [&] {
     79               // Restart timer so profile setpoints start at the beginning
     80               m_timer.Restart();
     81               ResetEncoders();
     82             },
     83             [&] {
     84               // Current state never changes, so we need to use a timer to get
     85               // the setpoints we need to be at
     86               auto currentTime = m_timer.Get();
     87               auto currentSetpoint =
     88                   m_profile.Calculate(currentTime, {}, {distance, 0_mps});
     89               auto nextSetpoint = m_profile.Calculate(currentTime + kDt, {},
     90                                                       {distance, 0_mps});
     91               SetDriveStates(currentSetpoint, currentSetpoint, nextSetpoint,
     92                              nextSetpoint);
     93             })
     94      .Until([&] { return m_profile.IsFinished(0_s); });
     95}
     96
     97frc2::CommandPtr DriveSubsystem::DynamicProfiledDriveDistance(
     98    units::meter_t distance) {
     99  return StartRun(
    100             [&] {
    101               // Restart timer so profile setpoints start at the beginning
    102               m_timer.Restart();
    103               // Store distance so we know the target distance for each encoder
    104               m_initialLeftDistance = GetLeftEncoderDistance();
    105               m_initialRightDistance = GetRightEncoderDistance();
    106             },
    107             [&] {
    108               // Current state never changes for the duration of the command,
    109               // so we need to use a timer to get the setpoints we need to be
    110               // at
    111               auto currentTime = m_timer.Get();
    112
    113               auto currentLeftSetpoint = m_profile.Calculate(
    114                   currentTime, {m_initialLeftDistance, 0_mps},
    115                   {m_initialLeftDistance + distance, 0_mps});
    116               auto currentRightSetpoint = m_profile.Calculate(
    117                   currentTime, {m_initialRightDistance, 0_mps},
    118                   {m_initialRightDistance + distance, 0_mps});
    119
    120               auto nextLeftSetpoint = m_profile.Calculate(
    121                   currentTime + kDt, {m_initialLeftDistance, 0_mps},
    122                   {m_initialLeftDistance + distance, 0_mps});
    123               auto nextRightSetpoint = m_profile.Calculate(
    124                   currentTime + kDt, {m_initialRightDistance, 0_mps},
    125                   {m_initialRightDistance + distance, 0_mps});
    126               SetDriveStates(currentLeftSetpoint, currentRightSetpoint,
    127                              nextLeftSetpoint, nextRightSetpoint);
    128             })
    129      .Until([&] { return m_profile.IsFinished(0_s); });
    130}

There are two commands in this example. They function very similarly, with the main difference being that one resets encoders, and the other doesn’t, which allows encoder data to be preserved.

The subsystem contains a `TrapezoidProfile` with a `Timer`. The timer is used along with a `kDt` constant of 0.02 seconds to calculate the current and next states from the `TrapezoidProfile`. The current state is fed to the “smart” motor controller for PID control, while the current and next state are used to calculate feedforward outputs. Both commands end when `isFinished(0)` returns true, which means that the profile has reached the goal state.