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.

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++.

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

Creación de un subsistema ProfiledPIDS

Nota

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

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

Java

  protected abstract double getMeasurement();

C++

  virtual Distance_t 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()

Java

  protected abstract void useOutput(double output, State setpoint);

C++

  virtual void UseOutput(double output, State setpoint) = 0;

The useOutput() method consumes the output of the Profiled PID controller, and the current setpoint state (which is often useful for computing a feedforward). The PIDSubsystem will automatically call this method from its periodic() block, and pass it the computed output of the control loop.

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:

JAVA

// The subsystem will track to a goal of 5 meters and velocity of 3 meters per second.
examplePIDSubsystem.setGoal(5, 3);

C++

// The subsystem will track to a goal of 5 meters and velocity of 3 meters per second.
examplePIDSubsystem.SetGoal({5_m, 3_mps});

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

Java

package edu.wpi.first.wpilibj.examples.armbot.subsystems;

import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;

/** A robot arm subsystem that moves with a motion profile. */
public class ArmSubsystem extends ProfiledPIDSubsystem {
  private final PWMSparkMax m_motor = new PWMSparkMax(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.kGVolts,
          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;
  }
}

C++ (Header)

#pragma once

#include <frc/Encoder.h>
#include <frc/controller/ArmFeedforward.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <frc2/command/ProfiledPIDSubsystem.h>
#include <units/angle.h>

/**
 * A robot arm subsystem that moves with a motion profile.
 */
class ArmSubsystem : public frc2::ProfiledPIDSubsystem<units::radians> {
  using State = frc::TrapezoidProfile<units::radians>::State;

 public:
  ArmSubsystem();

  void UseOutput(double output, State setpoint) override;

  units::radian_t GetMeasurement() override;

 private:
  frc::PWMSparkMax m_motor;
  frc::Encoder m_encoder;
  frc::ArmFeedforward m_feedforward;
};

C++ (Source)

#include "subsystems/ArmSubsystem.h"

#include "Constants.h"

using namespace ArmConstants;
using State = frc::TrapezoidProfile<units::radians>::State;

ArmSubsystem::ArmSubsystem()
    : frc2::ProfiledPIDSubsystem<units::radians>(
          frc::ProfiledPIDController<units::radians>(
              kP, 0, 0, {kMaxVelocity, kMaxAcceleration})),
      m_motor(kMotorPort),
      m_encoder(kEncoderPorts[0], kEncoderPorts[1]),
      m_feedforward(kS, kG, kV, kA) {
  m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
  // Start arm in neutral position
  SetGoal(State{kArmOffset, 0_rad_per_s});
}

void ArmSubsystem::UseOutput(double output, State setpoint) {
  // Calculate the feedforward from the sepoint
  units::volt_t feedforward =
      m_feedforward.Calculate(setpoint.position, setpoint.velocity);
  // Add the feedforward to the PID output to get the motor output
  m_motor.SetVoltage(units::volt_t{output} + feedforward);
}

units::radian_t ArmSubsystem::GetMeasurement() {
  return units::radian_t{m_encoder.GetDistance()} + kArmOffset;
}

Usar un ProfiledPIDSubsystem con comandos puede ser muy simple:

Java

    // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
    m_driverController
        .a()
        .onTrue(
            Commands.runOnce(
                () -> {
                  m_robotArm.setGoal(2);
                  m_robotArm.enable();
                },
                m_robotArm));

C++

  // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
  m_driverController.A().OnTrue(frc2::cmd::RunOnce(
      [this] {
        m_arm.SetGoal(2_rad);
        m_arm.Enable();
      },
      {&m_arm}));

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++.

The ProfiledPIDCommand class (Java, C++) allows users to easily create commands with a built-in ProfiledPIDController.

Creación de un comando PID

A ProfiledPIDCommand can be created two ways - by subclassing the ProfiledPIDCommand 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 ProfiledPIDCommand and overriding any methods, make sure to call the super version of those methods! Otherwise, control functionality will not work properly.

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

Java

  /**
   * 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) {

C++

  /**
   * Creates a new PIDCommand, which controls the given output with a
   * ProfiledPIDController.
   *
   * @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
   */
  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
                     std::function<Distance_t()> measurementSource,
                     std::function<State()> goalSource,
                     std::function<void(double, State)> useOutput,
                     Requirements 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

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 ProfiledPIDCommand is functionally analogous to overriding the getMeasurement() function in 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

The goalSource parameter is a function (usually passed as a lambda) that returns the current goal state for the mechanism. If only a constant goal is needed, an overload exists that takes a constant goal rather than a supplier. Additionally, if goal velocities are desired to be zero, overloads exist that take a constant distance rather than a full profile state.

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

The useOutput parameter is a function (usually passed as a lambda) that consumes the output and setpoint state of the control loop. Passing in the useOutput function in ProfiledPIDCommand is functionally analogous to overriding the useOutput() function in 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++):

Java

package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;

import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;

/** 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();
  }
}

C++ (Header)

#pragma once

#include <frc2/command/CommandHelper.h>
#include <frc2/command/ProfiledPIDCommand.h>

#include "subsystems/DriveSubsystem.h"

/**
 * A command that will turn the robot to the specified angle using a motion
 * profile.
 */
class TurnToAngleProfiled
    : public frc2::CommandHelper<frc2::ProfiledPIDCommand<units::radians>,
                                 TurnToAngleProfiled> {
 public:
  /**
   * 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
   */
  TurnToAngleProfiled(units::degree_t targetAngleDegrees,
                      DriveSubsystem* drive);

  bool IsFinished() override;
};

C++ (Source)

#include "commands/TurnToAngleProfiled.h"

#include <frc/controller/ProfiledPIDController.h>

using namespace DriveConstants;

TurnToAngleProfiled::TurnToAngleProfiled(units::degree_t target,
                                         DriveSubsystem* drive)
    : CommandHelper{
          frc::ProfiledPIDController<units::radians>{
              kTurnP, kTurnI, kTurnD, {kMaxTurnRate, kMaxTurnAcceleration}},
          // Close loop on heading
          [drive] { return drive->GetHeading(); },
          // Set reference to target
          target,
          // Pipe output to turn robot
          [drive](double output, auto setpointState) {
            drive->ArcadeDrive(0, output);
          },
          // Require the drive
          {drive}} {
  // Set the controller to be continuous (because it is an angle controller)
  GetController().EnableContinuousInput(-180_deg, 180_deg);
  // 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(kTurnTolerance, kTurnRateTolerance);

  AddRequirements(drive);
}

bool TurnToAngleProfiled::IsFinished() {
  return GetController().AtGoal();
}