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

  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::kMaxVelocity, DriveConstants::kMaxAcceleration}};
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{kLeftMotor1Port},
 13      leftFollower{kLeftMotor2Port},
 14      rightLeader{kRightMotor1Port},
 15      rightFollower{kRightMotor2Port},
 16      feedforward{ks, kv, ka} {
 17  wpi::util::SendableRegistry::AddChild(&drive, &leftLeader);
 18  wpi::util::SendableRegistry::AddChild(&drive, &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  rightLeader.SetInverted(true);
 24
 25  leftFollower.Follow(leftLeader);
 26  rightFollower.Follow(rightLeader);
 27
 28  leftLeader.SetPID(kp, 0, 0);
 29  rightLeader.SetPID(kp, 0, 0);
 30}
 31
 32void DriveSubsystem::Periodic() {
 33  // Implementation of subsystem periodic method goes here.
 34}
 35
 36void DriveSubsystem::SetDriveStates(
 37    wpi::math::TrapezoidProfile<wpi::units::meters>::State currentLeft,
 38    wpi::math::TrapezoidProfile<wpi::units::meters>::State currentRight,
 39    wpi::math::TrapezoidProfile<wpi::units::meters>::State nextLeft,
 40    wpi::math::TrapezoidProfile<wpi::units::meters>::State nextRight) {
 41  // Feedforward is divided by battery voltage to normalize it to [-1, 1]
 42  leftLeader.SetSetpoint(
 43      ExampleSmartMotorController::PIDMode::kPosition,
 44      currentLeft.position.value(),
 45      feedforward.Calculate(currentLeft.velocity, nextLeft.velocity) /
 46          wpi::RobotController::GetBatteryVoltage());
 47  rightLeader.SetSetpoint(
 48      ExampleSmartMotorController::PIDMode::kPosition,
 49      currentRight.position.value(),
 50      feedforward.Calculate(currentRight.velocity, nextRight.velocity) /
 51          wpi::RobotController::GetBatteryVoltage());
 52}
 53
 54void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
 55  drive.ArcadeDrive(fwd, rot);
 56}
 57
 58void DriveSubsystem::ResetEncoders() {
 59  leftLeader.ResetEncoder();
 60  rightLeader.ResetEncoder();
 61}
 62
 63wpi::units::meter_t DriveSubsystem::GetLeftEncoderDistance() {
 64  return wpi::units::meter_t{leftLeader.GetEncoderDistance()};
 65}
 66
 67wpi::units::meter_t DriveSubsystem::GetRightEncoderDistance() {
 68  return wpi::units::meter_t{rightLeader.GetEncoderDistance()};
 69}
 70
 71void DriveSubsystem::SetMaxOutput(double maxOutput) {
 72  drive.SetMaxOutput(maxOutput);
 73}
 74
 75wpi::cmd::CommandPtr DriveSubsystem::ProfiledDriveDistance(
 76    wpi::units::meter_t distance) {
 77  return StartRun(
 78             [&] {
 79               // Restart timer so profile setpoints start at the beginning
 80               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 = timer.Get();
 87               auto currentSetpoint =
 88                   profile.Calculate(currentTime, {}, {distance, 0_mps});
 89               auto nextSetpoint =
 90                   profile.Calculate(currentTime + kDt, {}, {distance, 0_mps});
 91               SetDriveStates(currentSetpoint, currentSetpoint, nextSetpoint,
 92                              nextSetpoint);
 93             })
 94      .Until([&] { return profile.IsFinished(0_s); });
 95}
 96
 97wpi::cmd::CommandPtr DriveSubsystem::DynamicProfiledDriveDistance(
 98    wpi::units::meter_t distance) {
 99  return StartRun(
100             [&] {
101               // Restart timer so profile setpoints start at the beginning
102               timer.Restart();
103               // Store distance so we know the target distance for each encoder
104               initialLeftDistance = GetLeftEncoderDistance();
105               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 = timer.Get();
112
113               auto currentLeftSetpoint =
114                   profile.Calculate(currentTime, {initialLeftDistance, 0_mps},
115                                     {initialLeftDistance + distance, 0_mps});
116               auto currentRightSetpoint =
117                   profile.Calculate(currentTime, {initialRightDistance, 0_mps},
118                                     {initialRightDistance + distance, 0_mps});
119
120               auto nextLeftSetpoint = profile.Calculate(
121                   currentTime + kDt, {initialLeftDistance, 0_mps},
122                   {initialLeftDistance + distance, 0_mps});
123               auto nextRightSetpoint = profile.Calculate(
124                   currentTime + kDt, {initialRightDistance, 0_mps},
125                   {initialRightDistance + distance, 0_mps});
126               SetDriveStates(currentLeftSetpoint, currentRightSetpoint,
127                              nextLeftSetpoint, nextRightSetpoint);
128             })
129      .Until([&] { return 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.