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++):

  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}

  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  [[nodiscard]]
 86  frc2::CommandPtr ProfiledDriveDistance(units::meter_t distance);
 87
 88  /**
 89   * Creates a command to drive forward a specified distance using a motion
 90   * profile without resetting the encoders.
 91   *
 92   * @param distance The distance to drive forward.
 93   * @return A command.
 94   */
 95  [[nodiscard]]
 96  frc2::CommandPtr DynamicProfiledDriveDistance(units::meter_t distance);
 97
 98 private:
 99  frc::TrapezoidProfile<units::meters> m_profile{
100      {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}};
101  frc::Timer m_timer;
102  units::meter_t m_initialLeftDistance;
103  units::meter_t m_initialRightDistance;
104  // Components (e.g. motor controllers and sensors) should generally be
105  // declared private and exposed only through public methods.
106
107  // The motor controllers
108  ExampleSmartMotorController m_leftLeader;
109  ExampleSmartMotorController m_leftFollower;
110  ExampleSmartMotorController m_rightLeader;
111  ExampleSmartMotorController m_rightFollower;
112
113  // A feedforward component for the drive
114  frc::SimpleMotorFeedforward<units::meters> m_feedforward;
115
116  // The robot's drive
117  frc::DifferentialDrive m_drive{
118      [&](double output) { m_leftLeader.Set(output); },
119      [&](double output) { m_rightLeader.Set(output); }};
120};

  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.