PID Control in Command-based
Note
Pour une description des fonctionnalités de contrôle PID WPILib utilisées par ces wrappers basés sur des commandes, voir Contrôle PID dans 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.