Combinando Motion Profiling y PID Basado en Comandos

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 ProfiledPIDController se ejecuta sincrónicamente y no se maneja en su propio hilo. En consecuencia, cambiar su parámetro de período 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 período a menos que estén seguros de lo que están haciendo.

Una solución común de controles FRC® es emparejar un perfil de movimiento trapezoidal para la generación de puntos de ajuste con un controlador PID para el seguimiento de puntos de ajuste. Para facilitar esto, WPILib incluye su propia clase ProfiledPIDController. Para ayudar aún más a los equipos a integrar esta funcionalidad en sus robots, el marco basado en comandos contiene dos envoltorios de conveniencia para la clase ProfiledPIDController: ProfiledPIDSubsystem, que integra el controlador en un subsistema, y ProfiledPIDCommand, que integra el controlador en un comando.

Subsistema ProfiledPIDS

Nota

En C++, la clase ProfiledPIDSubsystem se basa en el tipo de unidad utilizada para las mediciones de distancia, que puede ser angular o lineal. Los valores pasados deben tener unidades consistentes con las unidades de distancia, o se lanzará un error en tiempo de compilación. Para obtener más información sobre las unidades C++, consulte Biblioteca de unidades de C++.

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

Creación de un subsistema ProfiledPIDS

Al subclasificar ProfiledPIDSubsystem, los usuarios deben anular dos métodos abstractos para proporcionar la funcionalidad que la clase utilizará 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, TrapezoidProfile.State setpoint);

El método useOutput() consume la salida del controlador PID y el estado del 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 ProfiledPIDController a la clase base ProfiledPIDSubsystem a través de la llamada al constructor de superclase de su subclase. Esto sirve para especificar las ganancias de PID, las restricciones del perfil de movimiento y 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().

Uso de un subsistema ProfiledPIDS

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

setGoal()

Nota

Si desea establecer el objetivo en una distancia simple con una velocidad objetivo implícita de cero, existe una sobrecarga de setGoal() que toma un valor de distancia único, en lugar de un estado de perfil de movimiento completo.

El método setGoal() 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 goal of 5 meters and velocity of 3 meters per second.
examplePIDSubsystem.setGoal(5, 3);

enable() y disable()

Los métodos enable() y disable() habilitan y deshabilitan el control automático del ProfiledPIDSubsystem. Cuando el subsistema está habilitado, ejecutará automáticamente el perfil de movimiento y el bucle de control y seguirá hasta la meta. Cuando está deshabilitado, no se realiza ningún control.

Además, el método enable() restablece el ProfiledPIDController 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 ProfiledPIDS

¿Qué aspecto tiene un subsistema PIDS cuando se utiliza en la práctica? Los siguientes ejemplos están tomados del proyecto de ejemplo ArmBot (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
package edu.wpi.first.wpilibj.examples.armbot.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.controller.ArmFeedforward;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;

import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants;

/**
 * A robot arm subsystem that moves with a motion profile.
 */
public class ArmSubsystem extends ProfiledPIDSubsystem {
  private final PWMVictorSPX m_motor = new PWMVictorSPX(ArmConstants.kMotorPort);
  private final Encoder m_encoder =
      new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]);
  private final ArmFeedforward m_feedforward =
      new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts,
                         ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);

  /**
   * Create a new ArmSubsystem.
   */
  public ArmSubsystem() {
    super(new ProfiledPIDController(ArmConstants.kP, 0, 0, new TrapezoidProfile.Constraints(
        ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared)), 0);
    m_encoder.setDistancePerPulse(ArmConstants.kEncoderDistancePerPulse);
    // Start arm at rest in neutral position
    setGoal(ArmConstants.kArmOffsetRads);
  }

  @Override
  public void useOutput(double output, TrapezoidProfile.State setpoint) {
    // Calculate the feedforward from the sepoint
    double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
    // Add the feedforward to the PID output to get the motor output
    m_motor.setVoltage(output + feedforward);
  }

  @Override
  public double getMeasurement() {
    return m_encoder.getDistance() + ArmConstants.kArmOffsetRads;
  }
}

Usar un ProfiledPIDSubsystem con comandos puede ser muy simple:

63
64
65
66
67
68
    // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
    new JoystickButton(m_driverController, Button.kA.value)
        .whenPressed(() -> {
          m_robotArm.setGoal(2);
          m_robotArm.enable();
        }, m_robotArm);

Comando ProfiledPID

Nota

En C++, la clase ProfiledPIDCommand se basa en el tipo de unidad utilizado para las mediciones de distancia, que puede ser angular o lineal. Los valores pasados deben tener unidades consistentes con las unidades de distancia, o se lanzará un error en tiempo de compilación. Para obtener más información sobre las unidades C++, consulte Biblioteca de unidades de C++.

La clase ProfiledPIDCommand (Java, C++) permite a los usuarios crear fácilmente comandos con un ProfiledPIDController incorporado. Al igual que con ProfiledPIDSubsystem, los usuarios pueden crear un ProfiledPIDCommmand subclasificando la clase ProfiledPIDCommand. Sin embargo, como ocurre con muchas otras clases de comandos de la biblioteca basada en comandos, los usuarios pueden querer ahorrar código definiéndolo inline.

Creación de un comando PID

Un ProfiledPIDCommand se puede crear de dos formas: subclasificando la clase ProfiledPIDCommand 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 ProfiledPIDCommand pasando los parámetros necesarios a su constructor (si se define una subclase, esto se puede hacer con una llamada super()):

32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
  /**
   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
   * Goal velocity is specified.
   *
   * @param controller        the controller that controls the output.
   * @param measurementSource the measurement of the process variable
   * @param goalSource        the controller's goal
   * @param useOutput         the controller's output
   * @param requirements      the subsystems required by this command
   */
  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
                            Supplier<State> goalSource, BiConsumer<Double, State> useOutput,
                            Subsystem... requirements) {
    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
    requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");

    m_controller = controller;
    m_useOutput = useOutput;
    m_measurement = measurementSource;
    m_goal = goalSource;
    m_requirements.addAll(Set.of(requirements));
  }

controlador

El parámetro controller es el objeto ProfiledPIDController que será utilizado por el comando. Al pasar esto, los usuarios pueden especificar las ganancias de PID, las restricciones del perfil de movimiento 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 ProfiledPIDCommand, 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 (normalmente pasada como lambda) que devuelve la medida de la variable de proceso. Pasar la función measurementSource en ProfiledPIDCommand es funcionalmente análogo a anular la función getMeasurement() en ProfiledPIDSubsystem.

Al subclasificar ProfiledPIDCommand, los usuarios avanzados pueden modificar aún más el proveedor de medidas modificando el campo m_measurement de la clase.

goalSource

El parámetro goalSource es una función (generalmente pasada como lambda) que devuelve el estado objetivo actual para el mecanismo. Si solo se necesita un objetivo constante, existe una sobrecarga que requiere un objetivo constante en lugar de un proveedor. Además, si se desea que las velocidades objetivo sean cero, existen sobrecargas que toman una distancia constante en lugar de un estado de perfil completo.

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

useOutput

El parámetro useOutput es una función (generalmente pasada como lambda) que consume la salida y el estado del punto de ajuste del lazo de control. Pasar la función useOutput en ProfiledPIDCommand es funcionalmente análogo a anular la función useOutput() en ProfiledPIDSubsystem.

Al subclasificar ProfiledPIDCommand, 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, ProfiledPIDCommand permite al usuario especificar los requisitos de su subsistema como un parámetro de constructor.

Ejemplo completo de ProfiledPIDCommand

¿Qué aspecto tiene un ProfiledPIDCommand 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
52
53
54
55
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;

import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;

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 using a motion profile.
 */
public class TurnToAngleProfiled extends ProfiledPIDCommand {
  /**
   * Turns to robot to the specified angle using a motion profile.
   *
   * @param targetAngleDegrees The angle to turn to
   * @param drive              The drive subsystem to use
   */
  public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) {
    super(
        new ProfiledPIDController(DriveConstants.kTurnP, DriveConstants.kTurnI,
                                  DriveConstants.kTurnD, new TrapezoidProfile.Constraints(
            DriveConstants.kMaxTurnRateDegPerS,
            DriveConstants.kMaxTurnAccelerationDegPerSSquared)),
        // Close loop on heading
        drive::getHeading,
        // Set reference to target
        targetAngleDegrees,
        // Pipe output to turn robot
        (output, setpoint) -> 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().atGoal();
  }
}