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

  5#pragma once
  6
  7#include "Constants.hpp"
  8#include "ExampleSmartMotorController.hpp"
  9#include "wpi/commands2/CommandPtr.hpp"
 10#include "wpi/commands2/SubsystemBase.hpp"
 11#include "wpi/drive/DifferentialDrive.hpp"
 12#include "wpi/hardware/rotation/Encoder.hpp"
 13#include "wpi/math/controller/SimpleMotorFeedforward.hpp"
 14#include "wpi/math/trajectory/TrapezoidProfile.hpp"
 15#include "wpi/system/Timer.hpp"
 16#include "wpi/units/length.hpp"
 17
 18class DriveSubsystem : public wpi::cmd::SubsystemBase {
 19 public:
 20  DriveSubsystem();
 21
 22  /**
 23   * Will be called periodically whenever the CommandScheduler runs.
 24   */
 25  void Periodic() override;
 26
 27  // Subsystem methods go here.
 28
 29  /**
 30   * Attempts to follow the given drive states using offboard PID.
 31   *
 32   * @param currentLeft The current left wheel state.
 33   * @param currentRight The current right wheel state.
 34   * @param nextLeft The next left wheel state.
 35   * @param nextRight The next right wheel state.
 36   */
 37  void SetDriveStates(
 38      wpi::math::TrapezoidProfile<wpi::units::meters>::State currentLeft,
 39      wpi::math::TrapezoidProfile<wpi::units::meters>::State currentRight,
 40      wpi::math::TrapezoidProfile<wpi::units::meters>::State nextLeft,
 41      wpi::math::TrapezoidProfile<wpi::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  wpi::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  wpi::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  wpi::cmd::CommandPtr ProfiledDriveDistance(wpi::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  wpi::cmd::CommandPtr DynamicProfiledDriveDistance(
 95      wpi::units::meter_t distance);
 96
 97 private:
 98  wpi::math::TrapezoidProfile<wpi::units::meters> profile{
 99      {DriveConstants::MAX_VELOCITY, DriveConstants::MAX_ACCELERATION}};
100  wpi::Timer timer;
101  wpi::units::meter_t initialLeftDistance;
102  wpi::units::meter_t initialRightDistance;
103  // Components (e.g. motor controllers and sensors) should generally be
104  // declared private and exposed only through public methods.
105
106  // The motor controllers
107  ExampleSmartMotorController leftLeader;
108  ExampleSmartMotorController leftFollower;
109  ExampleSmartMotorController rightLeader;
110  ExampleSmartMotorController rightFollower;
111
112  // A feedforward component for the drive
113  wpi::math::SimpleMotorFeedforward<wpi::units::meters> feedforward;
114
115  // The robot's drive
116  wpi::DifferentialDrive drive{[&](double output) { leftLeader.Set(output); },
117                               [&](double output) { rightLeader.Set(output); }};
118};

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

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.