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 org.wpilib.examples.rapidreactcommandbot.subsystems;
  6
  7import java.util.function.DoubleSupplier;
  8import org.wpilib.command2.Command;
  9import org.wpilib.command2.SubsystemBase;
 10import org.wpilib.drive.DifferentialDrive;
 11import org.wpilib.epilogue.Logged;
 12import org.wpilib.epilogue.NotLogged;
 13import org.wpilib.examples.rapidreactcommandbot.Constants.DriveConstants;
 14import org.wpilib.hardware.imu.OnboardIMU;
 15import org.wpilib.hardware.motor.PWMSparkMax;
 16import org.wpilib.hardware.rotation.Encoder;
 17import org.wpilib.math.controller.ProfiledPIDController;
 18import org.wpilib.math.controller.SimpleMotorFeedforward;
 19import org.wpilib.math.trajectory.TrapezoidProfile;
 20import org.wpilib.system.RobotController;
 21import org.wpilib.util.sendable.SendableRegistry;
 22
 23@Logged
 24public class Drive extends SubsystemBase {
 25  // The motors on the left side of the drive.
 26  private final PWMSparkMax leftLeader = new PWMSparkMax(DriveConstants.kLeftMotor1Port);
 27  private final PWMSparkMax leftFollower = new PWMSparkMax(DriveConstants.kLeftMotor2Port);
 28
 29  // The motors on the right side of the drive.
 30  private final PWMSparkMax rightLeader = new PWMSparkMax(DriveConstants.kRightMotor1Port);
 31  private final PWMSparkMax 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 drive =
 36      new DifferentialDrive(leftLeader::setThrottle, rightLeader::setThrottle);
 37
 38  // The left-side drive encoder
 39  private final Encoder 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 rightEncoder =
 47      new Encoder(
 48          DriveConstants.kRightEncoderPorts[0],
 49          DriveConstants.kRightEncoderPorts[1],
 50          DriveConstants.kRightEncoderReversed);
 51
 52  private final OnboardIMU imu = new OnboardIMU(OnboardIMU.MountOrientation.FLAT);
 53  private final ProfiledPIDController 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 feedforward =
 62      new SimpleMotorFeedforward(DriveConstants.ks, DriveConstants.kv, DriveConstants.ka);
 63
 64  /** Creates a new Drive subsystem. */
 65  public Drive() {
 66    SendableRegistry.addChild(drive, leftLeader);
 67    SendableRegistry.addChild(drive, rightLeader);
 68
 69    leftLeader.addFollower(leftFollower);
 70    rightLeader.addFollower(rightFollower);
 71
 72    // We need to invert one side of the drivetrain so that positive voltages
 73    // result in both sides moving forward. Depending on how your robot's
 74    // gearbox is constructed, you might have to invert the left side instead.
 75    rightLeader.setInverted(true);
 76
 77    // Sets the distance per pulse for the encoders
 78    leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
 79    rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
 80
 81    // Set the controller to be continuous (because it is an angle controller)
 82    controller.enableContinuousInput(-180, 180);
 83    // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
 84    // setpoint before it is considered as having reached the reference
 85    controller.setTolerance(
 86        DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
 87  }
 88
 89  /**
 90   * Returns a command that drives the robot with arcade controls.
 91   *
 92   * @param fwd the commanded forward movement
 93   * @param rot the commanded rotation
 94   */
 95  public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
 96    // A split-stick arcade command, with forward/backward controlled by the left
 97    // hand, and turning controlled by the right.
 98    return run(() -> drive.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble()))
 99        .withName("arcadeDrive");
100  }
101
102  /**
103   * Returns a command that drives the robot forward a specified distance at a specified velocity.
104   *
105   * @param distance The distance to drive forward in meters
106   * @param velocity The fraction of max velocity at which to drive
107   */
108  public Command driveDistanceCommand(double distance, double velocity) {
109    return runOnce(
110            () -> {
111              // Reset encoders at the start of the command
112              leftEncoder.reset();
113              rightEncoder.reset();
114            })
115        // Drive forward at specified velocity
116        .andThen(run(() -> drive.arcadeDrive(velocity, 0)))
117        // End command when we've traveled the specified distance
118        .until(() -> Math.max(leftEncoder.getDistance(), rightEncoder.getDistance()) >= distance)
119        // Stop the drive when the command ends
120        .finallyDo(interrupted -> drive.stopMotor());
121  }
122
123  /**
124   * Returns a command that turns to robot to the specified angle using a motion profile and PID
125   * controller.
126   *
127   * @param angleDeg The angle to turn to
128   */
129  public Command turnToAngleCommand(double angleDeg) {
130    return startRun(
131            () -> controller.reset(imu.getRotation2d().getDegrees()),
132            () ->
133                drive.arcadeDrive(
134                    0,
135                    controller.calculate(imu.getRotation2d().getDegrees(), angleDeg)
136                        // Divide feedforward voltage by battery voltage to normalize it to [-1, 1]
137                        + feedforward.calculate(controller.getSetpoint().velocity)
138                            / RobotController.getBatteryVoltage()))
139        .until(controller::atGoal)
140        .finallyDo(() -> drive.arcadeDrive(0, 0));
141  }
142}

 5#pragma once
 6
 7#include <functional>
 8
 9#include "Constants.hpp"
10#include "wpi/commands2/CommandPtr.hpp"
11#include "wpi/commands2/SubsystemBase.hpp"
12#include "wpi/drive/DifferentialDrive.hpp"
13#include "wpi/hardware/imu/OnboardIMU.hpp"
14#include "wpi/hardware/motor/PWMSparkMax.hpp"
15#include "wpi/hardware/rotation/Encoder.hpp"
16#include "wpi/math/controller/ProfiledPIDController.hpp"
17#include "wpi/math/controller/SimpleMotorFeedforward.hpp"
18#include "wpi/units/angle.hpp"
19#include "wpi/units/length.hpp"
20
21class Drive : public wpi::cmd::SubsystemBase {
22 public:
23  Drive();
24  /**
25   * Returns a command that drives the robot with arcade controls.
26   *
27   * @param fwd the commanded forward movement
28   * @param rot the commanded rotation
29   */
30  wpi::cmd::CommandPtr ArcadeDriveCommand(std::function<double()> fwd,
31                                          std::function<double()> rot);
32
33  /**
34   * Returns a command that drives the robot forward a specified distance at a
35   * specified velocity.
36   *
37   * @param distance The distance to drive forward in meters
38   * @param velocity The fraction of max velocity at which to drive
39   */
40  wpi::cmd::CommandPtr DriveDistanceCommand(wpi::units::meter_t distance,
41                                            double velocity);
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  wpi::cmd::CommandPtr TurnToAngleCommand(wpi::units::degree_t angle);
50
51 private:
52  wpi::PWMSparkMax leftLeader{DriveConstants::kLeftMotor1Port};
53  wpi::PWMSparkMax leftFollower{DriveConstants::kLeftMotor2Port};
54  wpi::PWMSparkMax rightLeader{DriveConstants::kRightMotor1Port};
55  wpi::PWMSparkMax rightFollower{DriveConstants::kRightMotor2Port};
56
57  wpi::DifferentialDrive drive{
58      [&](double output) { leftLeader.SetThrottle(output); },
59      [&](double output) { rightLeader.SetThrottle(output); }};
60
61  wpi::Encoder leftEncoder{DriveConstants::kLeftEncoderPorts[0],
62                           DriveConstants::kLeftEncoderPorts[1],
63                           DriveConstants::kLeftEncoderReversed};
64  wpi::Encoder rightEncoder{DriveConstants::kRightEncoderPorts[0],
65                            DriveConstants::kRightEncoderPorts[1],
66                            DriveConstants::kRightEncoderReversed};
67
68  wpi::OnboardIMU imu{wpi::OnboardIMU::FLAT};
69
70  wpi::math::ProfiledPIDController<wpi::units::radians> controller{
71      DriveConstants::kTurnP,
72      DriveConstants::kTurnI,
73      DriveConstants::kTurnD,
74      {DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}};
75  wpi::math::SimpleMotorFeedforward<wpi::units::radians> feedforward{
76      DriveConstants::ks, DriveConstants::kv, DriveConstants::ka};
77};

 5#include "subsystems/Drive.hpp"
 6
 7#include <utility>
 8
 9#include "wpi/commands2/Commands.hpp"
10#include "wpi/system/RobotController.hpp"
11
12Drive::Drive() {
13  wpi::util::SendableRegistry::AddChild(&drive, &leftLeader);
14  wpi::util::SendableRegistry::AddChild(&drive, &rightLeader);
15
16  leftLeader.AddFollower(leftFollower);
17  rightLeader.AddFollower(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  rightLeader.SetInverted(true);
23
24  // Sets the distance per pulse for the encoders
25  leftEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
26  rightEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
27
28  // Set the controller to be continuous (because it is an angle controller)
29  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  controller.SetTolerance(DriveConstants::kTurnTolerance,
34                          DriveConstants::kTurnRateTolerance);
35}
36
37wpi::cmd::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           drive.ArcadeDrive(fwd(), rot());
41         })
42      .WithName("ArcadeDrive");
43}
44
45wpi::cmd::CommandPtr Drive::DriveDistanceCommand(wpi::units::meter_t distance,
46                                                 double velocity) {
47  return RunOnce([this] {
48           // Reset encoders at the start of the command
49           leftEncoder.Reset();
50           rightEncoder.Reset();
51         })
52      // Drive forward at specified velocity
53      .AndThen(Run([this, velocity] { drive.ArcadeDrive(velocity, 0.0); }))
54      .Until([this, distance] {
55        return wpi::units::math::max(
56                   wpi::units::meter_t(leftEncoder.GetDistance()),
57                   wpi::units::meter_t(rightEncoder.GetDistance())) >= distance;
58      })
59      // Stop the drive when the command ends
60      .FinallyDo([this](bool interrupted) { drive.StopMotor(); });
61}
62
63wpi::cmd::CommandPtr Drive::TurnToAngleCommand(wpi::units::degree_t angle) {
64  return StartRun([this] { controller.Reset(imu.GetRotation2d().Degrees()); },
65                  [this, angle] {
66                    drive.ArcadeDrive(
67                        0, controller.Calculate(imu.GetRotation2d().Degrees(),
68                                                angle) +
69                               // Divide feedforward voltage by battery voltage
70                               // to normalize it to [-1, 1]
71                               feedforward.Calculate(
72                                   controller.GetSetpoint().velocity) /
73                                   wpi::RobotController::GetBatteryVoltage());
74                  })
75      .Until([this] { return controller.AtGoal(); })
76      .FinallyDo([this] { drive.ArcadeDrive(0, 0); });
77}

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.