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.