Combining Motion Profiling and PID in Command-Based

Note

For a description of the WPILib PID control features used by these command-based wrappers, see PID Control in WPILib.

A common FRC® controls solution is to pair a trapezoidal motion profile for setpoint generation with a PID controller for setpoint tracking. To facilitate this, WPILib includes its own ProfiledPIDController class. The following example is from the RapidReactCommandBot example project (Java, C++) and shows how ProfiledPIDController can be used within the command-based framework to turn a drivetrain to a specified angle:

  5package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
  6
  7import edu.wpi.first.epilogue.Logged;
  8import edu.wpi.first.epilogue.NotLogged;
  9import edu.wpi.first.math.controller.ProfiledPIDController;
 10import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 11import edu.wpi.first.math.trajectory.TrapezoidProfile;
 12import edu.wpi.first.util.sendable.SendableRegistry;
 13import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 14import edu.wpi.first.wpilibj.Encoder;
 15import edu.wpi.first.wpilibj.RobotController;
 16import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 17import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.DriveConstants;
 18import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 19import edu.wpi.first.wpilibj2.command.Command;
 20import edu.wpi.first.wpilibj2.command.SubsystemBase;
 21import java.util.function.DoubleSupplier;
 22
 23@Logged
 24public class Drive extends SubsystemBase {
 25  // The motors on the left side of the drive.
 26  private final PWMSparkMax m_leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
 27  private final PWMSparkMax m_leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
 28
 29  // The motors on the right side of the drive.
 30  private final PWMSparkMax m_rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
 31  private final PWMSparkMax m_rightFollower = new PWMSparkMax(DriveConstants.kRightMotor2Port);
 32
 33  // The robot's drive
 34  @NotLogged // Would duplicate motor data, there's no point sending it twice
 35  private final DifferentialDrive m_drive =
 36      new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
 37
 38  // The left-side drive encoder
 39  private final Encoder m_leftEncoder =
 40      new Encoder(
 41          DriveConstants.kLeftEncoderPorts[0],
 42          DriveConstants.kLeftEncoderPorts[1],
 43          DriveConstants.kLeftEncoderReversed);
 44
 45  // The right-side drive encoder
 46  private final Encoder m_rightEncoder =
 47      new Encoder(
 48          DriveConstants.kRightEncoderPorts[0],
 49          DriveConstants.kRightEncoderPorts[1],
 50          DriveConstants.kRightEncoderReversed);
 51
 52  private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
 53  private final ProfiledPIDController m_controller =
 54      new ProfiledPIDController(
 55          DriveConstants.kTurnP,
 56          DriveConstants.kTurnI,
 57          DriveConstants.kTurnD,
 58          new TrapezoidProfile.Constraints(
 59              DriveConstants.kMaxTurnRateDegPerS,
 60              DriveConstants.kMaxTurnAccelerationDegPerSSquared));
 61  private final SimpleMotorFeedforward m_feedforward =
 62      new SimpleMotorFeedforward(
 63          DriveConstants.ksVolts,
 64          DriveConstants.kvVoltSecondsPerDegree,
 65          DriveConstants.kaVoltSecondsSquaredPerDegree);
 66
 67  /** Creates a new Drive subsystem. */
 68  public Drive() {
 69    SendableRegistry.addChild(m_drive, m_leftLeader);
 70    SendableRegistry.addChild(m_drive, m_rightLeader);
 71
 72    m_leftLeader.addFollower(m_leftFollower);
 73    m_rightLeader.addFollower(m_rightFollower);
 74
 75    // We need to invert one side of the drivetrain so that positive voltages
 76    // result in both sides moving forward. Depending on how your robot's
 77    // gearbox is constructed, you might have to invert the left side instead.
 78    m_rightLeader.setInverted(true);
 79
 80    // Sets the distance per pulse for the encoders
 81    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
 82    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
 83
 84    // Set the controller to be continuous (because it is an angle controller)
 85    m_controller.enableContinuousInput(-180, 180);
 86    // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
 87    // setpoint before it is considered as having reached the reference
 88    m_controller.setTolerance(
 89        DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
 90  }
 91
 92  /**
 93   * Returns a command that drives the robot with arcade controls.
 94   *
 95   * @param fwd the commanded forward movement
 96   * @param rot the commanded rotation
 97   */
 98  public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
 99    // A split-stick arcade command, with forward/backward controlled by the left
100    // hand, and turning controlled by the right.
101    return run(() -> m_drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()))
102        .withName("arcadeDrive");
103  }
104
105  /**
106   * Returns a command that drives the robot forward a specified distance at a specified speed.
107   *
108   * @param distanceMeters The distance to drive forward in meters
109   * @param speed The fraction of max speed at which to drive
110   */
111  public Command driveDistanceCommand(double distanceMeters, double speed) {
112    return runOnce(
113            () -> {
114              // Reset encoders at the start of the command
115              m_leftEncoder.reset();
116              m_rightEncoder.reset();
117            })
118        // Drive forward at specified speed
119        .andThen(run(() -> m_drive.arcadeDrive(speed, 0)))
120        // End command when we've traveled the specified distance
121        .until(
122            () ->
123                Math.max(m_leftEncoder.getDistance(), m_rightEncoder.getDistance())
124                    >= distanceMeters)
125        // Stop the drive when the command ends
126        .finallyDo(interrupted -> m_drive.stopMotor());
127  }
128
129  /**
130   * Returns a command that turns to robot to the specified angle using a motion profile and PID
131   * controller.
132   *
133   * @param angleDeg The angle to turn to
134   */
135  public Command turnToAngleCommand(double angleDeg) {
136    return startRun(
137            () -> m_controller.reset(m_gyro.getRotation2d().getDegrees()),
138            () ->
139                m_drive.arcadeDrive(
140                    0,
141                    m_controller.calculate(m_gyro.getRotation2d().getDegrees(), angleDeg)
142                        // Divide feedforward voltage by battery voltage to normalize it to [-1, 1]
143                        + m_feedforward.calculate(m_controller.getSetpoint().velocity)
144                            / RobotController.getBatteryVoltage()))
145        .until(m_controller::atGoal)
146        .finallyDo(() -> m_drive.arcadeDrive(0, 0));
147  }
148}

 5#pragma once
 6
 7#include <functional>
 8
 9#include <frc/ADXRS450_Gyro.h>
10#include <frc/Encoder.h>
11#include <frc/controller/ProfiledPIDController.h>
12#include <frc/controller/SimpleMotorFeedforward.h>
13#include <frc/drive/DifferentialDrive.h>
14#include <frc/motorcontrol/PWMSparkMax.h>
15#include <frc2/command/CommandPtr.h>
16#include <frc2/command/SubsystemBase.h>
17#include <units/angle.h>
18#include <units/length.h>
19
20#include "Constants.h"
21
22class Drive : public frc2::SubsystemBase {
23 public:
24  Drive();
25  /**
26   * Returns a command that drives the robot with arcade controls.
27   *
28   * @param fwd the commanded forward movement
29   * @param rot the commanded rotation
30   */
31  [[nodiscard]]
32  frc2::CommandPtr ArcadeDriveCommand(std::function<double()> fwd,
33                                      std::function<double()> rot);
34
35  /**
36   * Returns a command that drives the robot forward a specified distance at a
37   * specified speed.
38   *
39   * @param distance The distance to drive forward in meters
40   * @param speed The fraction of max speed at which to drive
41   */
42  [[nodiscard]]
43  frc2::CommandPtr DriveDistanceCommand(units::meter_t distance, double speed);
44
45  /**
46   * Returns a command that turns to robot to the specified angle using a motion
47   * profile and PID controller.
48   *
49   * @param angle The angle to turn to
50   */
51  [[nodiscard]]
52  frc2::CommandPtr TurnToAngleCommand(units::degree_t angle);
53
54 private:
55  frc::PWMSparkMax m_leftLeader{DriveConstants::kLeftMotor1Port};
56  frc::PWMSparkMax m_leftFollower{DriveConstants::kLeftMotor2Port};
57  frc::PWMSparkMax m_rightLeader{DriveConstants::kRightMotor1Port};
58  frc::PWMSparkMax m_rightFollower{DriveConstants::kRightMotor2Port};
59
60  frc::DifferentialDrive m_drive{
61      [&](double output) { m_leftLeader.Set(output); },
62      [&](double output) { m_rightLeader.Set(output); }};
63
64  frc::Encoder m_leftEncoder{DriveConstants::kLeftEncoderPorts[0],
65                             DriveConstants::kLeftEncoderPorts[1],
66                             DriveConstants::kLeftEncoderReversed};
67  frc::Encoder m_rightEncoder{DriveConstants::kRightEncoderPorts[0],
68                              DriveConstants::kRightEncoderPorts[1],
69                              DriveConstants::kRightEncoderReversed};
70
71  frc::ADXRS450_Gyro m_gyro;
72
73  frc::ProfiledPIDController<units::radians> m_controller{
74      DriveConstants::kTurnP,
75      DriveConstants::kTurnI,
76      DriveConstants::kTurnD,
77      {DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}};
78  frc::SimpleMotorFeedforward<units::radians> m_feedforward{
79      DriveConstants::ks, DriveConstants::kv, DriveConstants::ka};
80};

 5#include "subsystems/Drive.h"
 6
 7#include <utility>
 8
 9#include <frc/RobotController.h>
10#include <frc2/command/Commands.h>
11
12Drive::Drive() {
13  wpi::SendableRegistry::AddChild(&m_drive, &m_leftLeader);
14  wpi::SendableRegistry::AddChild(&m_drive, &m_rightLeader);
15
16  m_leftLeader.AddFollower(m_leftFollower);
17  m_rightLeader.AddFollower(m_rightFollower);
18
19  // We need to invert one side of the drivetrain so that positive voltages
20  // result in both sides moving forward. Depending on how your robot's
21  // gearbox is constructed, you might have to invert the left side instead.
22  m_rightLeader.SetInverted(true);
23
24  // Sets the distance per pulse for the encoders
25  m_leftEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
26  m_rightEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
27
28  // Set the controller to be continuous (because it is an angle controller)
29  m_controller.EnableContinuousInput(-180_deg, 180_deg);
30  // Set the controller tolerance - the delta tolerance ensures the robot is
31  // stationary at the setpoint before it is considered as having reached the
32  // reference
33  m_controller.SetTolerance(DriveConstants::kTurnTolerance,
34                            DriveConstants::kTurnRateTolerance);
35}
36
37frc2::CommandPtr Drive::ArcadeDriveCommand(std::function<double()> fwd,
38                                           std::function<double()> rot) {
39  return Run([this, fwd = std::move(fwd), rot = std::move(rot)] {
40           m_drive.ArcadeDrive(fwd(), rot());
41         })
42      .WithName("ArcadeDrive");
43}
44
45frc2::CommandPtr Drive::DriveDistanceCommand(units::meter_t distance,
46                                             double speed) {
47  return RunOnce([this] {
48           // Reset encoders at the start of the command
49           m_leftEncoder.Reset();
50           m_rightEncoder.Reset();
51         })
52      // Drive forward at specified speed
53      .AndThen(Run([this, speed] { m_drive.ArcadeDrive(speed, 0.0); }))
54      .Until([this, distance] {
55        return units::math::max(units::meter_t(m_leftEncoder.GetDistance()),
56                                units::meter_t(m_rightEncoder.GetDistance())) >=
57               distance;
58      })
59      // Stop the drive when the command ends
60      .FinallyDo([this](bool interrupted) { m_drive.StopMotor(); });
61}
62
63frc2::CommandPtr Drive::TurnToAngleCommand(units::degree_t angle) {
64  return StartRun(
65             [this] { m_controller.Reset(m_gyro.GetRotation2d().Degrees()); },
66             [this, angle] {
67               m_drive.ArcadeDrive(
68                   0, m_controller.Calculate(m_gyro.GetRotation2d().Degrees(),
69                                             angle) +
70                          // Divide feedforward voltage by battery voltage to
71                          // normalize it to [-1, 1]
72                          m_feedforward.Calculate(
73                              m_controller.GetSetpoint().velocity) /
74                              frc::RobotController::GetBatteryVoltage());
75             })
76      .Until([this] { return m_controller.AtGoal(); })
77      .FinallyDo([this] { m_drive.ArcadeDrive(0, 0); });
78}

turnToAngleCommand uses a ProfiledPIDController to smoothly turn the drivetrain. The startRun command factory is used to reset the ProfiledPIDController when the command is scheduled to avoid unwanted behavior, and to calculate PID and feedforward outputs to pass into the arcadeDrive method in order to drive the robot. The command is decorated using the until decorator to end the command when the ProfiledPIDController is finished with the profile. To ensure the drivetrain stops when the command ends, the finallyDo decorator is used to stop the drivetrain by setting the speed to zero.