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:

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