- FRC Game Manual
- FRC Game Q&A

# PID Control in Command-based

Note

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

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. The following example is from the RapidReactCommandBot example project (Java, C++) and shows how PIDControllers can be used within the command-based framework:

Java

     5package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
     6
     7import static edu.wpi.first.wpilibj2.command.Commands.parallel;
     8import static edu.wpi.first.wpilibj2.command.Commands.waitUntil;
     9
    10import edu.wpi.first.epilogue.Logged;
    11import edu.wpi.first.math.controller.PIDController;
    12import edu.wpi.first.math.controller.SimpleMotorFeedforward;
    13import edu.wpi.first.wpilibj.Encoder;
    14import edu.wpi.first.wpilibj.examples.rapidreactcommandbot.Constants.ShooterConstants;
    15import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
    16import edu.wpi.first.wpilibj2.command.Command;
    17import edu.wpi.first.wpilibj2.command.SubsystemBase;
    18
    19@Logged
    20public class Shooter extends SubsystemBase {
    21  private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
    22  private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
    23  private final Encoder m_shooterEncoder =
    24      new Encoder(
    25          ShooterConstants.kEncoderPorts[0],
    26          ShooterConstants.kEncoderPorts[1],
    27          ShooterConstants.kEncoderReversed);
    28  private final SimpleMotorFeedforward m_shooterFeedforward =
    29      new SimpleMotorFeedforward(
    30          ShooterConstants.kSVolts, ShooterConstants.kVVoltSecondsPerRotation);
    31  private final PIDController m_shooterFeedback = new PIDController(ShooterConstants.kP, 0.0, 0.0);
    32
    33  /** The shooter subsystem for the robot. */
    34  public Shooter() {
    35    m_shooterFeedback.setTolerance(ShooterConstants.kShooterToleranceRPS);
    36    m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
    37
    38    // Set default command to turn off both the shooter and feeder motors, and then idle
    39    setDefaultCommand(
    40        runOnce(
    41                () -> {
    42                  m_shooterMotor.disable();
    43                  m_feederMotor.disable();
    44                })
    45            .andThen(run(() -> {}))
    46            .withName("Idle"));
    47  }
    48
    49  /**
    50   * Returns a command to shoot the balls currently stored in the robot. Spins the shooter flywheel
    51   * up to the specified setpoint, and then runs the feeder motor.
    52   *
    53   * @param setpointRotationsPerSecond The desired shooter velocity
    54   */
    55  public Command shootCommand(double setpointRotationsPerSecond) {
    56    return parallel(
    57            // Run the shooter flywheel at the desired setpoint using feedforward and feedback
    58            run(
    59                () -> {
    60                  m_shooterMotor.set(
    61                      m_shooterFeedforward.calculate(setpointRotationsPerSecond)
    62                          + m_shooterFeedback.calculate(
    63                              m_shooterEncoder.getRate(), setpointRotationsPerSecond));
    64                }),
    65
    66            // Wait until the shooter has reached the setpoint, and then run the feeder
    67            waitUntil(m_shooterFeedback::atSetpoint).andThen(() -> m_feederMotor.set(1)))
    68        .withName("Shoot");
    69  }
    70}

C++

     5#pragma once
     6
     7#include <functional>
     8
     9#include <frc/Encoder.h>
    10#include <frc/controller/PIDController.h>
    11#include <frc/controller/SimpleMotorFeedforward.h>
    12#include <frc/motorcontrol/PWMSparkMax.h>
    13#include <frc2/command/CommandPtr.h>
    14#include <frc2/command/SubsystemBase.h>
    15#include <units/angle.h>
    16#include <units/angular_velocity.h>
    17
    18#include "Constants.h"
    19
    20class Shooter : public frc2::SubsystemBase {
    21 public:
    22  Shooter();
    23
    24  /**
    25   * Returns a command to shoot the balls currently stored in the robot. Spins
    26   * the shooter flywheel up to the specified setpoint, and then runs the feeder
    27   * motor.
    28   *
    29   * @param setpointRotationsPerSecond The desired shooter velocity
    30   */
    31  frc2::CommandPtr ShootCommand(units::turns_per_second_t setpoint);
    32
    33 private:
    34  frc::PWMSparkMax m_shooterMotor{ShooterConstants::kShooterMotorPort};
    35  frc::PWMSparkMax m_feederMotor{ShooterConstants::kFeederMotorPort};
    36
    37  frc::Encoder m_shooterEncoder{ShooterConstants::kEncoderPorts[0],
    38                                ShooterConstants::kEncoderPorts[1],
    39                                ShooterConstants::kEncoderReversed};
    40  frc::SimpleMotorFeedforward<units::radians> m_shooterFeedforward{
    41      ShooterConstants::kS, ShooterConstants::kV};
    42  frc::PIDController m_shooterFeedback{ShooterConstants::kP, 0.0, 0.0};
    43};

C++ (Source)

     5#include "subsystems/Shooter.h"
     6
     7#include <frc2/command/Commands.h>
     8
     9Shooter::Shooter() {
    10  m_shooterFeedback.SetTolerance(ShooterConstants::kShooterTolerance.value());
    11  m_shooterEncoder.SetDistancePerPulse(
    12      ShooterConstants::kEncoderDistancePerPulse);
    13
    14  SetDefaultCommand(RunOnce([this] {
    15                      m_shooterMotor.Disable();
    16                      m_feederMotor.Disable();
    17                    })
    18                        .AndThen(Run([] {}))
    19                        .WithName("Idle"));
    20}
    21
    22frc2::CommandPtr Shooter::ShootCommand(units::turns_per_second_t setpoint) {
    23  return frc2::cmd::Parallel(
    24             // Run the shooter flywheel at the desired setpoint using
    25             // feedforward and feedback
    26             Run([this, setpoint] {
    27               m_shooterMotor.SetVoltage(
    28                   m_shooterFeedforward.Calculate(setpoint) +
    29                   units::volt_t(m_shooterFeedback.Calculate(
    30                       m_shooterEncoder.GetRate(), setpoint.value())));
    31             }),
    32             // Wait until the shooter has reached the setpoint, and then
    33             // run the feeder
    34             frc2::cmd::WaitUntil([this] {
    35               return m_shooterFeedback.AtSetpoint();
    36             }).AndThen([this] { m_feederMotor.Set(1.0); }))
    37      .WithName("Shoot");
    38}

A `PIDController` is declared inside the `Shooter` subsystem. It is used by `ShootCommand` alongside a feedforward to spin the shooter flywheel to the specified velocity. Once the `PIDController` reaches the specified velocity, the `ShootCommand` runs the feeder.