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

 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/drivers/motor/PWMSparkMax.hpp"
14#include "wpi/hardware/imu/OnboardIMU.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::LEFT_MOTOR1_PORT};
53  wpi::PWMSparkMax leftFollower{DriveConstants::LEFT_MOTOR2_PORT};
54  wpi::PWMSparkMax rightLeader{DriveConstants::RIGHT_MOTOR1_PORT};
55  wpi::PWMSparkMax rightFollower{DriveConstants::RIGHT_MOTOR2_PORT};
56
57  wpi::DifferentialDrive drive{
58      [&](double output) { leftLeader.SetThrottle(output); },
59      [&](double output) { rightLeader.SetThrottle(output); }};
60
61  wpi::Encoder leftEncoder{DriveConstants::LEFT_ENCODER_PORTS[0],
62                           DriveConstants::LEFT_ENCODER_PORTS[1],
63                           DriveConstants::LEFT_ENCODER_REVERSED};
64  wpi::Encoder rightEncoder{DriveConstants::RIGHT_ENCODER_PORTS[0],
65                            DriveConstants::RIGHT_ENCODER_PORTS[1],
66                            DriveConstants::RIGHT_ENCODER_REVERSED};
67
68  wpi::OnboardIMU imu{wpi::OnboardIMU::FLAT};
69
70  wpi::math::ProfiledPIDController<wpi::units::radians> controller{
71      DriveConstants::TURN_P,
72      DriveConstants::TURN_I,
73      DriveConstants::TURN_D,
74      {DriveConstants::MAX_TURN_RATE, DriveConstants::MAX_TURN_ACCELERATION}};
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  leftLeader.AddFollower(leftFollower);
14  rightLeader.AddFollower(rightFollower);
15
16  // We need to invert one side of the drivetrain so that positive voltages
17  // result in both sides moving forward. Depending on how your robot's
18  // gearbox is constructed, you might have to invert the left side instead.
19  rightLeader.SetInverted(true);
20
21  // Sets the distance per pulse for the encoders
22  leftEncoder.SetDistancePerPulse(DriveConstants::ENCODER_DISTANCE_PER_PULSE);
23  rightEncoder.SetDistancePerPulse(DriveConstants::ENCODER_DISTANCE_PER_PULSE);
24
25  // Set the controller to be continuous (because it is an angle controller)
26  controller.EnableContinuousInput(-180_deg, 180_deg);
27  // Set the controller tolerance - the delta tolerance ensures the robot is
28  // stationary at the setpoint before it is considered as having reached the
29  // reference
30  controller.SetTolerance(DriveConstants::TURN_TOLERANCE,
31                          DriveConstants::TURN_RATE_TOLERANCE);
32}
33
34wpi::cmd::CommandPtr Drive::ArcadeDriveCommand(std::function<double()> fwd,
35                                               std::function<double()> rot) {
36  return Run([this, fwd = std::move(fwd), rot = std::move(rot)] {
37           drive.ArcadeDrive(fwd(), rot());
38         })
39      .WithName("ArcadeDrive");
40}
41
42wpi::cmd::CommandPtr Drive::DriveDistanceCommand(wpi::units::meter_t distance,
43                                                 double velocity) {
44  return RunOnce([this] {
45           // Reset encoders at the start of the command
46           leftEncoder.Reset();
47           rightEncoder.Reset();
48         })
49      // Drive forward at specified velocity
50      .AndThen(Run([this, velocity] { drive.ArcadeDrive(velocity, 0.0); }))
51      .Until([this, distance] {
52        return wpi::units::math::max(
53                   wpi::units::meter_t(leftEncoder.GetDistance()),
54                   wpi::units::meter_t(rightEncoder.GetDistance())) >= distance;
55      })
56      // Stop the drive when the command ends
57      .FinallyDo([this](bool interrupted) { drive.ArcadeDrive(0.0, 0.0); });
58}
59
60wpi::cmd::CommandPtr Drive::TurnToAngleCommand(wpi::units::degree_t angle) {
61  return StartRun([this] { controller.Reset(imu.GetRotation2d().Degrees()); },
62                  [this, angle] {
63                    drive.ArcadeDrive(
64                        0, controller.Calculate(imu.GetRotation2d().Degrees(),
65                                                angle) +
66                               // Divide feedforward voltage by battery voltage
67                               // to normalize it to [-1, 1]
68                               feedforward.Calculate(
69                                   controller.GetSetpoint().velocity) /
70                                   wpi::RobotController::GetBatteryVoltage());
71                  })
72      .Until([this] { return controller.AtGoal(); })
73      .FinallyDo([this] { drive.ArcadeDrive(0, 0); });
74}

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.