PID Control in Command-based
Nota
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.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}
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.