PID Control in Command-based
Nota
Para obtener una descripción de las funciones de control de WPILib PID utilizadas por estos contenedores basados en comandos, consulte Control PID en 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.