- FRC Game Manual
- FRC Game Q&A

# 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:

Java

      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}

C++ (Header)

     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  frc2::CommandPtr ArcadeDriveCommand(std::function<double()> fwd,
    32                                      std::function<double()> rot);
    33
    34  /**
    35   * Returns a command that drives the robot forward a specified distance at a
    36   * specified speed.
    37   *
    38   * @param distance The distance to drive forward in meters
    39   * @param speed The fraction of max speed at which to drive
    40   */
    41  frc2::CommandPtr DriveDistanceCommand(units::meter_t distance, double speed);
    42
    43  /**
    44   * Returns a command that turns to robot to the specified angle using a motion
    45   * profile and PID controller.
    46   *
    47   * @param angle The angle to turn to
    48   */
    49  frc2::CommandPtr TurnToAngleCommand(units::degree_t angle);
    50
    51 private:
    52  frc::PWMSparkMax m_leftLeader{DriveConstants::kLeftMotor1Port};
    53  frc::PWMSparkMax m_leftFollower{DriveConstants::kLeftMotor2Port};
    54  frc::PWMSparkMax m_rightLeader{DriveConstants::kRightMotor1Port};
    55  frc::PWMSparkMax m_rightFollower{DriveConstants::kRightMotor2Port};
    56
    57  frc::DifferentialDrive m_drive{
    58      [&](double output) { m_leftLeader.Set(output); },
    59      [&](double output) { m_rightLeader.Set(output); }};
    60
    61  frc::Encoder m_leftEncoder{DriveConstants::kLeftEncoderPorts[0],
    62                             DriveConstants::kLeftEncoderPorts[1],
    63                             DriveConstants::kLeftEncoderReversed};
    64  frc::Encoder m_rightEncoder{DriveConstants::kRightEncoderPorts[0],
    65                              DriveConstants::kRightEncoderPorts[1],
    66                              DriveConstants::kRightEncoderReversed};
    67
    68  frc::ADXRS450_Gyro m_gyro;
    69
    70  frc::ProfiledPIDController<units::radians> m_controller{
    71      DriveConstants::kTurnP,
    72      DriveConstants::kTurnI,
    73      DriveConstants::kTurnD,
    74      {DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}};
    75  frc::SimpleMotorFeedforward<units::radians> m_feedforward{
    76      DriveConstants::ks, DriveConstants::kv, DriveConstants::ka};
    77};

C++ (Source)

     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.