Control PID a través de subsistemas PID y comandos PID

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. To further help teams integrate PID control into a command-based robot project, the command-based library includes two convenience wrappers for the PIDController class: PIDSubsystem, which integrates the PID controller into a subsystem, and PIDCommand, which integrates the PID controller into a command.

Subsistemas PIDS

The PIDSubsystem class (Java, C++) allows users to conveniently create a subsystem with a built-in PIDController. In order to use the PIDSubsystem class, users must create a subclass of it.

Creación de un subsistema PIDS

Nota

If periodic is overridden when inheriting from PIDSubsystem, make sure to call super.periodic()! Otherwise, PID functionality will not work properly.

Al subclasificar PIDSubsystem, los usuarios deben anular dos métodos abstractos para proporcionar la funcionalidad que la clase usará en su operación ordinaria:

getMeasurement()

  protected abstract double getMeasurement();
  virtual double GetMeasurement() = 0;

El método getMeasurement devuelve la medición actual de la variable de proceso. El PIDSubsystem llamará automáticamente a este método desde su bloque periodic() y pasará su valor al bucle de control.

Los usuarios deben anular este método para devolver cualquier lectura del sensor que deseen utilizar como medida de la variable de proceso.

useOutput()

  protected abstract void useOutput(double output, double setpoint);
  virtual void UseOutput(double output, double setpoint) = 0;

El método useOutput() consume la salida del controlador PID y el punto de ajuste actual (que a menudo es útil para calcular un feedforward). El PIDSubsystem llamará automáticamente a este método desde su bloque periodic() y le pasará la salida calculada del bucle de control.

Los usuarios deben anular este método para pasar la salida de control calculada final a los motores de su subsistema.

Pasando el controlador

Los usuarios también deben pasar un PIDController a la clase base PIDSubsystem a través de la llamada al constructor de superclase de su subclase. Esto sirve para especificar las ganancias de PID, así como el período (si el usuario está utilizando un período de bucle de robot principal no estándar).

Se pueden realizar modificaciones adicionales (por ejemplo, habilitar la entrada continua) al controlador en el cuerpo del constructor llamando a getController().

Usando un subsistema PIDS

Una vez que se ha creado una instancia de una subclase PIDSubsystem, los comandos pueden usarla a través de los siguientes métodos:

setSetpoint()

El método setSetpoint() se puede utilizar para establecer el punto de ajuste del PIDSubsystem. El subsistema rastreará automáticamente el punto de ajuste usando la salida definida:

// The subsystem will track to a setpoint of 5.
examplePIDSubsystem.setSetpoint(5);
// The subsystem will track to a setpoint of 5.
examplePIDSubsystem.SetSetpoint(5);

enable() y disable()

Los métodos enable() y disable() habilitan y deshabilitan el control PID del PIDSubsystem.Cuando el subsistema está habilitado, automáticamente ejecutará el circuito de control y rastreará el punto de ajuste. Cuando está deshabilitado, no se realiza ningún control.

Además, el método enable() restablece el PIDController interno, y el método disable() llama al método useOutput() definido por el usuario con la salida y el punto de ajuste establecidos en 0.

Ejemplo completo de subsistema PIDS

¿Cómo se ve un PIDSubsystem cuando se usa en la práctica? Los siguientes ejemplos se han extraído del proyecto de ejemplo de FrisbeeBot (Java, C++):

 5package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
 6
 7import edu.wpi.first.math.controller.PIDController;
 8import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 9import edu.wpi.first.wpilibj.Encoder;
10import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;
11import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
12import edu.wpi.first.wpilibj2.command.PIDSubsystem;
13
14public class ShooterSubsystem extends PIDSubsystem {
15  private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
16  private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
17  private final Encoder m_shooterEncoder =
18      new Encoder(
19          ShooterConstants.kEncoderPorts[0],
20          ShooterConstants.kEncoderPorts[1],
21          ShooterConstants.kEncoderReversed);
22  private final SimpleMotorFeedforward m_shooterFeedforward =
23      new SimpleMotorFeedforward(
24          ShooterConstants.kSVolts, ShooterConstants.kVVoltSecondsPerRotation);
25
26  /** The shooter subsystem for the robot. */
27  public ShooterSubsystem() {
28    super(new PIDController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD));
29    getController().setTolerance(ShooterConstants.kShooterToleranceRPS);
30    m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
31    setSetpoint(ShooterConstants.kShooterTargetRPS);
32  }
33
34  @Override
35  public void useOutput(double output, double setpoint) {
36    m_shooterMotor.setVoltage(output + m_shooterFeedforward.calculate(setpoint));
37  }
38
39  @Override
40  public double getMeasurement() {
41    return m_shooterEncoder.getRate();
42  }
43
44  public boolean atSetpoint() {
45    return m_controller.atSetpoint();
46  }
47
48  public void runFeeder() {
49    m_feederMotor.set(ShooterConstants.kFeederSpeed);
50  }
51
52  public void stopFeeder() {
53    m_feederMotor.set(0);
54  }
55}
 5#pragma once
 6
 7#include <frc/Encoder.h>
 8#include <frc/controller/SimpleMotorFeedforward.h>
 9#include <frc/motorcontrol/PWMSparkMax.h>
10#include <frc2/command/PIDSubsystem.h>
11#include <units/angle.h>
12
13class ShooterSubsystem : public frc2::PIDSubsystem {
14 public:
15  ShooterSubsystem();
16
17  void UseOutput(double output, double setpoint) override;
18
19  double GetMeasurement() override;
20
21  bool AtSetpoint();
22
23  void RunFeeder();
24
25  void StopFeeder();
26
27 private:
28  frc::PWMSparkMax m_shooterMotor;
29  frc::PWMSparkMax m_feederMotor;
30  frc::Encoder m_shooterEncoder;
31  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward;
32};
 5#include "subsystems/ShooterSubsystem.h"
 6
 7#include <frc/controller/PIDController.h>
 8
 9#include "Constants.h"
10
11using namespace ShooterConstants;
12
13ShooterSubsystem::ShooterSubsystem()
14    : PIDSubsystem{frc::PIDController{kP, kI, kD}},
15      m_shooterMotor(kShooterMotorPort),
16      m_feederMotor(kFeederMotorPort),
17      m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]),
18      m_shooterFeedforward(kS, kV) {
19  m_controller.SetTolerance(kShooterToleranceRPS.value());
20  m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
21  SetSetpoint(kShooterTargetRPS.value());
22}
23
24void ShooterSubsystem::UseOutput(double output, double setpoint) {
25  m_shooterMotor.SetVoltage(units::volt_t{output} +
26                            m_shooterFeedforward.Calculate(kShooterTargetRPS));
27}
28
29bool ShooterSubsystem::AtSetpoint() {
30  return m_controller.AtSetpoint();
31}
32
33double ShooterSubsystem::GetMeasurement() {
34  return m_shooterEncoder.GetRate();
35}
36
37void ShooterSubsystem::RunFeeder() {
38  m_feederMotor.Set(kFeederSpeed);
39}
40
41void ShooterSubsystem::StopFeeder() {
42  m_feederMotor.Set(0);
43}

Usar un PIDSubsystem con comandos puede ser muy simple:

  private final Command m_spinUpShooter = Commands.runOnce(m_shooter::enable, m_shooter);
  private final Command m_stopShooter = Commands.runOnce(m_shooter::disable, m_shooter);

    // We can bind commands while retaining references to them in RobotContainer

    // Spin up the shooter when the 'A' button is pressed
    m_driverController.a().onTrue(m_spinUpShooter);

    // Turn off the shooter when the 'B' button is pressed
    m_driverController.b().onTrue(m_stopShooter);
45  frc2::CommandPtr m_spinUpShooter =
46      frc2::cmd::RunOnce([this] { m_shooter.Enable(); }, {&m_shooter});
47
48  frc2::CommandPtr m_stopShooter =
49      frc2::cmd::RunOnce([this] { m_shooter.Disable(); }, {&m_shooter});
25  // We can bind commands while keeping their ownership in RobotContainer
26
27  // Spin up the shooter when the 'A' button is pressed
28  m_driverController.A().OnTrue(m_spinUpShooter.get());
29
30  // Turn off the shooter when the 'B' button is pressed
31  m_driverController.B().OnTrue(m_stopShooter.get());

PIDCommand

The PIDCommand class allows users to easily create commands with a built-in PIDController.

Crear un comando PID

A PIDCommand can be created two ways - by subclassing the PIDCommand class, or by defining the command inline. Both methods ultimately extremely similar, and ultimately the choice of which to use comes down to where the user desires that the relevant code be located.

Nota

If subclassing PIDCommand and overriding any methods, make sure to call the super version of those methods! Otherwise, PID functionality will not work properly.

En cualquier caso, se crea un PIDCommand pasando los parámetros necesarios a su constructor (si se define una subclase, esto se puede hacer con una llamada super() call):

27  /**
28   * Creates a new PIDCommand, which controls the given output with a PIDController.
29   *
30   * @param controller the controller that controls the output.
31   * @param measurementSource the measurement of the process variable
32   * @param setpointSource the controller's setpoint
33   * @param useOutput the controller's output
34   * @param requirements the subsystems required by this command
35   */
36  public PIDCommand(
37      PIDController controller,
38      DoubleSupplier measurementSource,
39      DoubleSupplier setpointSource,
40      DoubleConsumer useOutput,
41      Subsystem... requirements) {
28  /**
29   * Creates a new PIDCommand, which controls the given output with a
30   * PIDController.
31   *
32   * @param controller        the controller that controls the output.
33   * @param measurementSource the measurement of the process variable
34   * @param setpointSource   the controller's reference (aka setpoint)
35   * @param useOutput         the controller's output
36   * @param requirements      the subsystems required by this command
37   */
38  PIDCommand(frc::PIDController controller,
39             std::function<double()> measurementSource,
40             std::function<double()> setpointSource,
41             std::function<void(double)> useOutput,
42             Requirements requirements = {});

controller

El parámetro controller es el objeto PIDController que será utilizado por el comando. Al pasar esto, los usuarios pueden especificar las ganancias de PID y el período para el controlador (si el usuario está utilizando un período de bucle de robot principal no estándar).

Al subclasificar PIDCommand, se pueden realizar modificaciones adicionales (por ejemplo, habilitar la entrada continua) al controlador en el cuerpo del constructor llamando a getController().

measurementSource

The measurementSource parameter is a function (usually passed as a lambda) that returns the measurement of the process variable. Passing in the measurementSource function in PIDCommand is functionally analogous to overriding the getMeasurement() function in PIDSubsystem.

Al realizar una subclasificación de PIDCommand, los usuarios avanzados pueden modificar aún más el proveedor de medición modificando el campo m_measurement de la clase.

setpointSource

The setpointSource parameter is a function (usually passed as a lambda) that returns the current setpoint for the control loop. If only a constant setpoint is needed, an overload exists that takes a constant setpoint rather than a supplier.

Al subclasificar PIDCommand, los usuarios avanzados pueden modificar aún más el proveedor del punto de ajuste modificando el campo``m_setpoint`` de la clase.

useOutput

The useOutput parameter is a function (usually passed as a lambda) that consumes the output and setpoint of the control loop. Passing in the useOutput function in PIDCommand is functionally analogous to overriding the useOutput() function in PIDSubsystem.

Al subclasificar PIDCommand, los usuarios avanzados pueden modificar aún más el consumidor de salida modificando el campo m_useOutput de la clase.

requisitos

Como todos los comandos en línea, PIDCommand permite al usuario especificar los requisitos de su subsistema como un parámetro de constructor.

Ejemplo completo de comando PID

¿Qué aspecto tiene un PIDCommand cuando se utiliza en la práctica? Los siguientes ejemplos proceden del proyecto de ejemplo GyroDriveCommands (Java, C++):

 5package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;
 6
 7import edu.wpi.first.math.controller.PIDController;
 8import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
 9import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
10import edu.wpi.first.wpilibj2.command.PIDCommand;
11
12/** A command that will turn the robot to the specified angle. */
13public class TurnToAngle extends PIDCommand {
14  /**
15   * Turns to robot to the specified angle.
16   *
17   * @param targetAngleDegrees The angle to turn to
18   * @param drive The drive subsystem to use
19   */
20  public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
21    super(
22        new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD),
23        // Close loop on heading
24        drive::getHeading,
25        // Set reference to target
26        targetAngleDegrees,
27        // Pipe output to turn robot
28        output -> drive.arcadeDrive(0, output),
29        // Require the drive
30        drive);
31
32    // Set the controller to be continuous (because it is an angle controller)
33    getController().enableContinuousInput(-180, 180);
34    // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
35    // setpoint before it is considered as having reached the reference
36    getController()
37        .setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
38  }
39
40  @Override
41  public boolean isFinished() {
42    // End when the controller is at the reference.
43    return getController().atSetpoint();
44  }
45}
 5#pragma once
 6
 7#include <frc2/command/CommandHelper.h>
 8#include <frc2/command/PIDCommand.h>
 9
10#include "subsystems/DriveSubsystem.h"
11
12/**
13 * A command that will turn the robot to the specified angle.
14 */
15class TurnToAngle : public frc2::CommandHelper<frc2::PIDCommand, TurnToAngle> {
16 public:
17  /**
18   * Turns to robot to the specified angle.
19   *
20   * @param targetAngleDegrees The angle to turn to
21   * @param drive              The drive subsystem to use
22   */
23  TurnToAngle(units::degree_t target, DriveSubsystem* drive);
24
25  bool IsFinished() override;
26};
 5#include "commands/TurnToAngle.h"
 6
 7#include <frc/controller/PIDController.h>
 8
 9using namespace DriveConstants;
10
11TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive)
12    : CommandHelper{frc::PIDController{kTurnP, kTurnI, kTurnD},
13                    // Close loop on heading
14                    [drive] { return drive->GetHeading().value(); },
15                    // Set reference to target
16                    target.value(),
17                    // Pipe output to turn robot
18                    [drive](double output) { drive->ArcadeDrive(0, output); },
19                    // Require the drive
20                    {drive}} {
21  // Set the controller to be continuous (because it is an angle controller)
22  m_controller.EnableContinuousInput(-180, 180);
23  // Set the controller tolerance - the delta tolerance ensures the robot is
24  // stationary at the setpoint before it is considered as having reached the
25  // reference
26  m_controller.SetTolerance(kTurnTolerance.value(), kTurnRateTolerance.value());
27
28  AddRequirements(drive);
29}
30
31bool TurnToAngle::IsFinished() {
32  return GetController().AtSetpoint();
33}

And, for an inlined example:

64    // Stabilize robot to drive straight with gyro when left bumper is held
65    new JoystickButton(m_driverController, Button.kL1.value)
66        .whileTrue(
67            new PIDCommand(
68                new PIDController(
69                    DriveConstants.kStabilizationP,
70                    DriveConstants.kStabilizationI,
71                    DriveConstants.kStabilizationD),
72                // Close the loop on the turn rate
73                m_robotDrive::getTurnRate,
74                // Setpoint is 0
75                0,
76                // Pipe the output to the turning controls
77                output -> m_robotDrive.arcadeDrive(-m_driverController.getLeftY(), output),
78                // Require the robot drive
79                m_robotDrive));
34  // Stabilize robot to drive straight with gyro when L1 is held
35  frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kL1)
36      .WhileTrue(
37          frc2::PIDCommand(
38              frc::PIDController{dc::kStabilizationP, dc::kStabilizationI,
39                                 dc::kStabilizationD},
40              // Close the loop on the turn rate
41              [this] { return m_drive.GetTurnRate(); },
42              // Setpoint is 0
43              0,
44              // Pipe the output to the turning controls
45              [this](double output) {
46                m_drive.ArcadeDrive(m_driverController.GetLeftY(), output);
47              },
48              // Require the robot drive
49              {&m_drive})