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.

Nota

A diferencia de la versión anterior de PIDController, la clase 2020 PIDController se ejecuta síncronamente y no se maneja en su propio hilo. En consecuencia, cambiar su parámetro de period no cambiará la frecuencia real a la que se ejecuta en ninguna de estas clases de envoltura. Los usuarios nunca deben modificar el parámetro period a menos que estén seguros de lo que están haciendo.

Uno de los algoritmos de control más comunes utilizados en FRC® es el PID controller. WPILib ofrece su propia clase PIDController para ayudar a los equipos a implementar esta funcionalidad en sus robots. Para ayudar aún más a los equipos a integrar el control PID en un proyecto de robot basado en comandos, la biblioteca basada en comandos incluye dos envoltorios de conveniencia para la clase PIDController: PIDSubsystem, que integra el controlador PID en un subsistema, y PIDCommand, que integra el controlador PID en un comando.

Subsistemas PIDS

La clase PIDSubsystem (Java, C++) permite a los usuarios crear convenientemente un subsistema con un PIDController. Para utilizar la clase PIDSubsystem, los usuarios deben crear una subclase de la misma.

Creación de un subsistema PIDS

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();

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);

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);

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++):

 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;

import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;

public class ShooterSubsystem extends PIDSubsystem {
  private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(ShooterConstants.kShooterMotorPort);
  private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(ShooterConstants.kFeederMotorPort);
  private final Encoder m_shooterEncoder =
      new Encoder(ShooterConstants.kEncoderPorts[0], ShooterConstants.kEncoderPorts[1],
                  ShooterConstants.kEncoderReversed);
  private final SimpleMotorFeedforward m_shooterFeedforward =
      new SimpleMotorFeedforward(ShooterConstants.kSVolts,
                                 ShooterConstants.kVVoltSecondsPerRotation);

  /**
   * The shooter subsystem for the robot.
   */
  public ShooterSubsystem() {
    super(new PIDController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD));
    getController().setTolerance(ShooterConstants.kShooterToleranceRPS);
    m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
    setSetpoint(ShooterConstants.kShooterTargetRPS);
  }

  @Override
  public void useOutput(double output, double setpoint) {
    m_shooterMotor.setVoltage(output + m_shooterFeedforward.calculate(setpoint));
  }

  @Override
  public double getMeasurement() {
    return m_shooterEncoder.getRate();
  }

  public boolean atSetpoint() {
    return m_controller.atSetpoint();
  }

  public void runFeeder() {
    m_feederMotor.set(ShooterConstants.kFeederSpeed);
  }

  public void stopFeeder() {
    m_feederMotor.set(0);
  }
}

Usar un PIDSubsystem con comandos puede ser muy simple:

85
86
87
88
89
90
91
    // Spin up the shooter when the 'A' button is pressed
    new JoystickButton(m_driverController, Button.kA.value)
        .whenPressed(new InstantCommand(m_shooter::enable, m_shooter));

    // Turn off the shooter when the 'B' button is pressed
    new JoystickButton(m_driverController, Button.kB.value)
        .whenPressed(new InstantCommand(m_shooter::disable, m_shooter));

PIDCommand

La clase PIDCommand permite a los usuarios crear fácilmente comandos con un PIDController incorporado. Al igual que con el subsistema PIDS, los usuarios pueden crear un PIDCommand subclasificando la clase PIDCommand. Sin embargo, al igual que con muchas de las otras clases de comandos en la biblioteca basada en comandos, los usuarios pueden querer guardar el código definiendo un PIDCommand inline.

Crear un comando PID

Se puede crear un PIDCommand de dos maneras: subclasificando la clase `PIDCommand o definiendo el comando inline. En última instancia, ambos métodos son extremadamente similares y, en última instancia, la elección de cuál usar se reduce al lugar donde el usuario desea que se ubique el código relevante.

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):

29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
  /**
   * Creates a new PIDCommand, which controls the given output with a PIDController.
   *
   * @param controller        the controller that controls the output.
   * @param measurementSource the measurement of the process variable
   * @param setpointSource    the controller's setpoint
   * @param useOutput         the controller's output
   * @param requirements      the subsystems required by this command
   */
  public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
                    DoubleSupplier setpointSource, DoubleConsumer useOutput,
                    Subsystem... requirements) {
    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
    requireNonNullParam(setpointSource, "setpointSource", "SynchronousPIDCommand");
    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");

    m_controller = controller;
    m_useOutput = useOutput;
    m_measurement = measurementSource;
    m_setpoint = setpointSource;
    m_requirements.addAll(Set.of(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

El parámetro measurementSource es una función (generalmente pasada como lambda) que devuelve la medición de la variable de proceso. Pasar la función measurementSource en PIDCommand es funcionalmente análogo a anular la función getMeasurement() en 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

El parámetro setpointSource es una función (generalmente pasada como lambda) que devuelve el punto de ajuste actual para el bucle de control. Si solo se necesita un punto de ajuste constante, existe una sobrecarga que toma un punto de ajuste constante en lugar de un proveedor.

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

El parámetro useOutput es una función (generalmente pasada como lambda) que consume la salida y el punto de ajuste del bucle de control. Pasar la función useOutput en PIDCommand es funcionalmente análogo a anular la función useOutput() en 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++):

 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;

import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj2.command.PIDCommand;

import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;

/**
 * A command that will turn the robot to the specified angle.
 */
public class TurnToAngle extends PIDCommand {
  /**
   * Turns to robot to the specified angle.
   *
   * @param targetAngleDegrees The angle to turn to
   * @param drive              The drive subsystem to use
   */
  public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
    super(
        new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD),
        // Close loop on heading
        drive::getHeading,
        // Set reference to target
        targetAngleDegrees,
        // Pipe output to turn robot
        output -> drive.arcadeDrive(0, output),
        // Require the drive
        drive);

    // Set the controller to be continuous (because it is an angle controller)
    getController().enableContinuousInput(-180, 180);
    // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
    // setpoint before it is considered as having reached the reference
    getController()
        .setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
  }

  @Override
  public boolean isFinished() {
    // End when the controller is at the reference.
    return getController().atSetpoint();
  }
}

Y, para un ejemplo inlined.

70
71
72
73
74
75
76
77
78
79
80
81
    // Stabilize robot to drive straight with gyro when left bumper is held
    new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld(new PIDCommand(
        new PIDController(DriveConstants.kStabilizationP, DriveConstants.kStabilizationI,
                          DriveConstants.kStabilizationD),
        // Close the loop on the turn rate
        m_robotDrive::getTurnRate,
        // Setpoint is 0
        0,
        // Pipe the output to the turning controls
        output -> m_robotDrive.arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output),
        // Require the robot drive
        m_robotDrive));