PID Control through PIDSubsystems and PIDCommands

Note

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

Note

Unlike the earlier version of PIDController, the 2020 PIDController class runs synchronously, and is not handled in its own thread. Accordingly, changing its period parameter will not change the actual frequency at which it runs in any of these wrapper classes. Users should never modify the period parameter unless they are certain of what they are doing.

One of the most common control algorithms used in FRC is the PID controller. WPILib offers its own PIDController class to help teams implement this functionality on their robots. To further help teams integrate PID control into a command-based robot project, the command-based library includes two convenience wrappers for the PIDController class: PIDSubsystem, which integrates the PID controller into a subsystem, and PIDCommand, which integrates the PID controller into a command.

PIDSubsystems

The PIDSubsystem class (Java, C++) allows users to conveniently create a subsystem with a built-in PIDController. In order to use the PIDSubsystem class, users must create a subclass of it.

Creating a PIDSubsystem

When subclassing PIDSubsystem, users must override two abstract methods to provide functionality that the class will use in its ordinary operation:

getMeasurement()

protected abstract double getMeasurement();
virtual double GetMeasurement() = 0;

The getMeasurement method returns the current measurement of the process variable. The PIDSubsystem will automatically call this method from its periodic() block, and pass its value to the control loop.

Users should override this method to return whatever sensor reading they wish to use as their process variable measurement.

useOutput()

protected abstract void useOutput(double output, double setpoint);
virtual void UseOutput(double output, double setpoint) = 0;

The useOutput() method consumes the output of the PID controller, and the current setpoint (which is often useful for computing a feedforward). The PIDSubsystem will automatically call this method from its periodic() block, and pass it the computed output of the control loop.

Users should override this method to pass the final computed control output to their subsystem’s motors.

Passing In the Controller

Users must also pass in a PIDController to the PIDSubsystem base class through the superclass constructor call of their subclass. This serves to specify the PID gains, as well as the period (if the user is using a non-standard main robot loop period).

Additional modifications (e.g. enabling continuous input) can be made to the controller in the constructor body by calling getController().

Using a PIDSubsystem

Once an instance of a PIDSubsystem subclass has been created, it can be used by commands through the following methods:

setSetpoint()

The setSetpoint() method can be used to set the setpoint of the PIDSubsystem. The subsystem will automatically track to the setpoint using the defined output:

// The subsystem will track to a setpoint of 5.
examplePIDSubsystem.setSetpoint(5);
// The subsystem will track to a setpoint of 5.
examplePIDSubsystem.SetSetpoint(5);

enable() and disable()

The enable() and disable() methods enable and disable the PID control of the PIDSubsystem. When the subsystem is enabled, it will automatically run the control loop and track the setpoint. When it is disabled, no control is performed.

Additionally, the enable() method resets the internal PIDController, and the disable() method calls the user-defined useOutput() method with both output and setpoint set to 0.

Full PIDSubsystem Example

What does a PIDSubsystem look like when used in practice? The following examples are taken from the FrisbeeBot example project (Java, C++):

 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;

import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;

public class ShooterSubsystem extends PIDSubsystem {
  private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(ShooterConstants.kShooterMotorPort);
  private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(ShooterConstants.kFeederMotorPort);
  private final Encoder m_shooterEncoder =
      new Encoder(ShooterConstants.kEncoderPorts[0], ShooterConstants.kEncoderPorts[1],
                  ShooterConstants.kEncoderReversed);
  private final SimpleMotorFeedforward m_shooterFeedforward =
      new SimpleMotorFeedforward(ShooterConstants.kSVolts,
                                 ShooterConstants.kVVoltSecondsPerRotation);

  /**
   * The shooter subsystem for the robot.
   */
  public ShooterSubsystem() {
    super(new PIDController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD));
    getController().setTolerance(ShooterConstants.kShooterToleranceRPS);
    m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
    setSetpoint(ShooterConstants.kShooterTargetRPS);
  }

  @Override
  public void useOutput(double output, double setpoint) {
    m_shooterMotor.setVoltage(output + m_shooterFeedforward.calculate(setpoint));
  }

  @Override
  public double getMeasurement() {
    return m_shooterEncoder.getRate();
  }

  public boolean atSetpoint() {
    return m_controller.atSetpoint();
  }

  public void runFeeder() {
    m_feederMotor.set(ShooterConstants.kFeederSpeed);
  }

  public void stopFeeder() {
    m_feederMotor.set(0);
  }
}
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
#pragma once

#include <frc/Encoder.h>
#include <frc/PWMVictorSPX.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc2/command/PIDSubsystem.h>
#include <units/angle.h>

class ShooterSubsystem : public frc2::PIDSubsystem {
 public:
  ShooterSubsystem();

  void UseOutput(double output, double setpoint) override;

  double GetMeasurement() override;

  bool AtSetpoint();

  void RunFeeder();

  void StopFeeder();

 private:
  frc::PWMVictorSPX m_shooterMotor;
  frc::PWMVictorSPX m_feederMotor;
  frc::Encoder m_shooterEncoder;
  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward;
};
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
#include "subsystems/ShooterSubsystem.h"

#include <frc/controller/PIDController.h>

#include "Constants.h"

using namespace ShooterConstants;

ShooterSubsystem::ShooterSubsystem()
    : PIDSubsystem(frc2::PIDController(kP, kI, kD)),
      m_shooterMotor(kShooterMotorPort),
      m_feederMotor(kFeederMotorPort),
      m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]),
      m_shooterFeedforward(kS, kV) {
  m_controller.SetTolerance(kShooterToleranceRPS.to<double>());
  m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
  SetSetpoint(kShooterTargetRPS.to<double>());
}

void ShooterSubsystem::UseOutput(double output, double setpoint) {
  m_shooterMotor.SetVoltage(units::volt_t(output) +
                            m_shooterFeedforward.Calculate(kShooterTargetRPS));
}

bool ShooterSubsystem::AtSetpoint() { return m_controller.AtSetpoint(); }

double ShooterSubsystem::GetMeasurement() { return m_shooterEncoder.GetRate(); }

void ShooterSubsystem::RunFeeder() { m_feederMotor.Set(kFeederSpeed); }

void ShooterSubsystem::StopFeeder() { m_feederMotor.Set(0); }

Using a PIDSubsystem with commands can be very simple:

85
86
87
88
89
90
91
    // Spin up the shooter when the 'A' button is pressed
    new JoystickButton(m_driverController, Button.kA.value)
        .whenPressed(new InstantCommand(m_shooter::enable, m_shooter));

    // Turn off the shooter when the 'B' button is pressed
    new JoystickButton(m_driverController, Button.kB.value)
        .whenPressed(new InstantCommand(m_shooter::disable, m_shooter));
73
74
75
76
77
  frc2::InstantCommand m_spinUpShooter{[this] { m_shooter.Enable(); },
                                       {&m_shooter}};

  frc2::InstantCommand m_stopShooter{[this] { m_shooter.Disable(); },
                                     {&m_shooter}};
32
33
34
35
36
  // Spin up the shooter when the 'A' button is pressed
  frc2::JoystickButton(&m_driverController, 1).WhenPressed(&m_spinUpShooter);

  // Turn off the shooter when the 'B' button is pressed
  frc2::JoystickButton(&m_driverController, 2).WhenPressed(&m_stopShooter);

PIDCommand

The PIDCommand class allows users to easily create commands with a built-in PIDController. As with PIDSubsystem, users can create a PIDCommand by subclassing the PIDCommand class. However, as with many of the other command classes in the command-based library, users may want to save code by defining a PIDCommand inline.

Creating a PIDCommand

A PIDCommand can be created two ways - by subclassing the PIDCommand class, or by defining the command inline. Both methods ultimately extremely similar, and ultimately the choice of which to use comes down to where the user desires that the relevant code be located.

In either case, a PIDCommand is created by passing the necessary parameters to its constructor (if defining a subclass, this can be done with a super() call):

29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
  /**
   * Creates a new PIDCommand, which controls the given output with a PIDController.
   *
   * @param controller        the controller that controls the output.
   * @param measurementSource the measurement of the process variable
   * @param setpointSource    the controller's setpoint
   * @param useOutput         the controller's output
   * @param requirements      the subsystems required by this command
   */
  public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
                    DoubleSupplier setpointSource, DoubleConsumer useOutput,
                    Subsystem... requirements) {
    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
    requireNonNullParam(setpointSource, "setpointSource", "SynchronousPIDCommand");
    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");

    m_controller = controller;
    m_useOutput = useOutput;
    m_measurement = measurementSource;
    m_setpoint = setpointSource;
    m_requirements.addAll(Set.of(requirements));
  }
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
  /**
   * Creates a new PIDCommand, which controls the given output with a
   * PIDController.
   *
   * @param controller        the controller that controls the output.
   * @param measurementSource the measurement of the process variable
   * @param setpointSource   the controller's reference (aka setpoint)
   * @param useOutput         the controller's output
   * @param requirements      the subsystems required by this command
   */
  PIDCommand(PIDController controller,
             std::function<double()> measurementSource,
             std::function<double()> setpointSource,
             std::function<void(double)> useOutput,
             std::initializer_list<Subsystem*> requirements);

controller

The controller parameter is the PIDController object that will be used by the command. By passing this in, users can specify the PID gains and the period for the controller (if the user is using a nonstandard main robot loop period).

When subclassing PIDCommand, additional modifications (e.g. enabling continuous input) can be made to the controller in the constructor body by calling getController().

measurementSource

The measurementSource parameter is a function (usually passed as a lambda) that returns the measurement of the process variable. Passing in the measurementSource function in PIDCommand is functionally analogous to overriding the getMeasurement() function in PIDSubsystem.

When subclassing PIDCommand, advanced users may further modify the measurement supplier by modifying the class’s m_measurement field.

setpointSource

The setpointSource parameter is a function (usually passed as a lambda) that returns the current setpoint for the control loop. If only a constant setpoint is needed, an overload exists that takes a constant setpoint rather than a supplier.

When subclassing PIDCommand, advanced users may further modify the setpoint supplier by modifying the class’s m_setpoint field.

useOutput

The useOutput parameter is a function (usually passed as a lambda) that consumes the output and setpoint of the control loop. Passing in the useOutput function in PIDCommand is functionally analogous to overriding the useOutput() function in PIDSubsystem.

When subclassing PIDCommand, advanced users may further modify the output consumer by modifying the class’s m_useOutput field.

requirements

Like all inlineable commands, PIDCommand allows the user to specify its subsystem requirements as a constructor parameter.

Full PIDCommand Example

What does a PIDCommand look like when used in practice? The following examples are from the GyroDriveCommands example project (Java, C++):

 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;

import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj2.command.PIDCommand;

import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;

/**
 * A command that will turn the robot to the specified angle.
 */
public class TurnToAngle extends PIDCommand {
  /**
   * Turns to robot to the specified angle.
   *
   * @param targetAngleDegrees The angle to turn to
   * @param drive              The drive subsystem to use
   */
  public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
    super(
        new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD),
        // Close loop on heading
        drive::getHeading,
        // Set reference to target
        targetAngleDegrees,
        // Pipe output to turn robot
        output -> drive.arcadeDrive(0, output),
        // Require the drive
        drive);

    // Set the controller to be continuous (because it is an angle controller)
    getController().enableContinuousInput(-180, 180);
    // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
    // setpoint before it is considered as having reached the reference
    getController()
        .setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
  }

  @Override
  public boolean isFinished() {
    // End when the controller is at the reference.
    return getController().atSetpoint();
  }
}
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
#pragma once

#include <frc2/command/CommandHelper.h>
#include <frc2/command/PIDCommand.h>

#include "subsystems/DriveSubsystem.h"

/**
 * A command that will turn the robot to the specified angle.
 */
class TurnToAngle : public frc2::CommandHelper<frc2::PIDCommand, TurnToAngle> {
 public:
  /**
   * Turns to robot to the specified angle.
   *
   * @param targetAngleDegrees The angle to turn to
   * @param drive              The drive subsystem to use
   */
  TurnToAngle(units::degree_t target, DriveSubsystem* drive);

  bool IsFinished() override;
};
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
#include "commands/TurnToAngle.h"

#include <frc/controller/PIDController.h>

using namespace DriveConstants;

TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive)
    : CommandHelper(
          frc2::PIDController(kTurnP, kTurnI, kTurnD),
          // Close loop on heading
          [drive] { return drive->GetHeading().to<double>(); },
          // Set reference to target
          target.to<double>(),
          // Pipe output to turn robot
          [drive](double output) { drive->ArcadeDrive(0, output); },
          // Require the drive
          {drive}) {
  // Set the controller to be continuous (because it is an angle controller)
  m_controller.EnableContinuousInput(-180, 180);
  // Set the controller tolerance - the delta tolerance ensures the robot is
  // stationary at the setpoint before it is considered as having reached the
  // reference
  m_controller.SetTolerance(kTurnTolerance.to<double>(),
                            kTurnRateTolerance.to<double>());

  AddRequirements({drive});
}

bool TurnToAngle::IsFinished() { return GetController().AtSetpoint(); }

And, for an inlined example:

70
71
72
73
74
75
76
77
78
79
80
81
    // Stabilize robot to drive straight with gyro when left bumper is held
    new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld(new PIDCommand(
        new PIDController(DriveConstants.kStabilizationP, DriveConstants.kStabilizationI,
                          DriveConstants.kStabilizationD),
        // Close the loop on the turn rate
        m_robotDrive::getTurnRate,
        // Setpoint is 0
        0,
        // Pipe the output to the turning controls
        output -> m_robotDrive.arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output),
        // Require the robot drive
        m_robotDrive));
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
  // Stabilize robot to drive straight with gyro when left bumper is held
  frc2::JoystickButton(&m_driverController, 5)
      .WhenHeld(frc2::PIDCommand{
          frc2::PIDController{dc::kStabilizationP, dc::kStabilizationI,
                              dc::kStabilizationD},
          // Close the loop on the turn rate
          [this] { return m_drive.GetTurnRate(); },
          // Setpoint is 0
          0,
          // Pipe the output to the turning controls
          [this](double output) {
            m_drive.ArcadeDrive(m_driverController.GetY(
                                    frc::GenericHID::JoystickHand::kLeftHand),
                                output);
          },
          // Require the robot drive
          {&m_drive}});