Motion Profiling in Command-based

Nota

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

Nota

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

Nota

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

  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.