Creación de perfiles de movimiento a través de los subsistemas TrapezoidProfile y los comandos TrapezoidProfile

Nota

Para obtener una descripción de las funciones de creación de perfiles de movimiento de WPILib utilizadas por estos contenedores basados en comandos, consulte Perfil de movimiento trapezoidal en WPILib.

Nota

Los contenedores de comando TrapezoidProfile generalmente están destinados a la composición con controladores personalizados o externos. Para combinar la creación de perfiles de movimiento trapezoidal con el PIDController de WPILib, consulte Combinando Motion Profiling y PID Basado en Comandos.

When controlling a mechanism, is often desirable to move it smoothly between two positions, rather than to abruptly change its setpoint. This is called «motion-profiling,» and is supported in WPILib through the TrapezoidProfile class (Java, C++).

Para ayudar aún más a los equipos a integrar la creación de perfiles de movimiento en sus proyectos de robots basados en comandos, WPILib incluye dos envoltorios de conveniencia para la clase TrapezoidProfile: TrapezoidProfileSubsystem, que genera y ejecuta automáticamente perfiles de movimiento en su método periodic() , y el``TrapezoidProfileCommand``, que ejecuta un único TrapezoidProfile proporcionado por el usuario.

TrapezoideProfileSubsystem

Nota

En C ++, la clase TrapezoidProfileSubsystem 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 TrapezoidProfileSubsystem class (Java, C++) will automatically create and execute trapezoidal motion profiles to reach the user-provided goal state. To use the TrapezoidProfileSubsystem class, users must create a subclass of it.

Creación de un subsistema de perfil trapezoidal

Nota

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

Al subclasificar TrapezoidProfileSubsystem, los usuarios deben anular un único método abstracto para proporcionar la funcionalidad que la clase usará en su operación ordinaria:

useState()

Java

  protected abstract void useState(TrapezoidProfile.State state);

C++

  virtual void UseState(State state) = 0;

El método useState() consume el estado actual del perfil de movimiento. El TrapezoidProfileSubsystem llamará automáticamente a este método desde su bloque periodic() y le pasará el estado del perfil de movimiento correspondiente al progreso actual del subsistema a través del perfil de movimiento.

Los usuarios pueden hacer lo que quieran con este estado; un caso de uso típico (como se muestra en el Full TrapezoidProfileSubsystem Example) es usar el estado para obtener un punto de ajuste y un avance para un controlador de motor «inteligente» externo.

Parámetros de constructor

Los usuarios deben pasar un conjunto de TrapezoidProfile.Constraints a la clase base TrapezoidProfileSubsystem a través de la llamada al constructor de superclase de su subclase. Esto sirve para restringir los perfiles generados automáticamente a una velocidad y aceleración máximas determinadas.

Los usuarios también deben pasar en una posición inicial para el mecanismo.

Los usuarios avanzados pueden pasar un valor alternativo para el período de bucle, si se está utilizando un período de bucle principal no estándar.

Uso de un TrapezoidProfileSubsystem

Una vez que se ha creado una instancia de una subclase TrapezoidProfileSubsystem, 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 estado objetivo del TrapezoidProfileSubsystem. El subsistema ejecutará automáticamente un perfil para el objetivo, pasando el estado actual en cada iteración al método useState() proporcionado.

JAVA

// The subsystem will execute a profile to a position of 5 and a velocity of 3.
examplePIDSubsystem.setGoal(new TrapezoidProfile.State(5, 3);

C++

// The subsystem will execute a profile to a position of 5 meters and a velocity of 3 mps.
examplePIDSubsyste.SetGoal({5_m, 3_mps});

enable() and disable()

The enable() and disable() methods enable and disable the motion profiling control of the TrapezoidProfileSubsystem. When the subsystem is enabled, it will automatically run the control loop and call useState() periodically. When it is disabled, no control is performed.

Ejemplo completo de TrapezoidProfileSubsystem

¿Qué aspecto tiene un TrapezoidProfileSubsystem cuando se utiliza en la práctica? Los siguientes ejemplos están tomados del proyecto de ejemplo ArmbotOffobard (Java, C++):

Java

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

import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants;
import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;

/** A robot arm subsystem that moves with a motion profile. */
public class ArmSubsystem extends TrapezoidProfileSubsystem {
  private final ExampleSmartMotorController m_motor =
      new ExampleSmartMotorController(ArmConstants.kMotorPort);
  private final ArmFeedforward m_feedforward =
      new ArmFeedforward(
          ArmConstants.kSVolts, ArmConstants.kGVolts,
          ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);

  /** Create a new ArmSubsystem. */
  public ArmSubsystem() {
    super(
        new TrapezoidProfile.Constraints(
            ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared),
        ArmConstants.kArmOffsetRads);
    m_motor.setPID(ArmConstants.kP, 0, 0);
  }

  @Override
  public void useState(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.setSetpoint(
        ExampleSmartMotorController.PIDMode.kPosition, setpoint.position, feedforward / 12.0);
  }

  public Command setArmGoalCommand(double kArmOffsetRads) {
    return Commands.runOnce(() -> setGoal(kArmOffsetRads), this);
  }
}

C++ (Header)

#pragma once

#include <frc/controller/ArmFeedforward.h>
#include <frc2/command/Commands.h>
#include <frc2/command/TrapezoidProfileSubsystem.h>
#include <units/angle.h>

#include "ExampleSmartMotorController.h"

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

 public:
  ArmSubsystem();

  void UseState(State setpoint) override;

  frc2::CommandPtr SetArmGoalCommand(units::radian_t goal);

 private:
  ExampleSmartMotorController m_motor;
  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::TrapezoidProfileSubsystem<units::radians>(
          {kMaxVelocity, kMaxAcceleration}, kArmOffset),
      m_motor(kMotorPort),
      m_feedforward(kS, kG, kV, kA) {
  m_motor.SetPID(kP, 0, 0);
}

void ArmSubsystem::UseState(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.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
                      setpoint.position.value(), feedforward / 12_V);
}

frc2::CommandPtr ArmSubsystem::SetArmGoalCommand(units::radian_t goal) {
  return frc2::cmd::RunOnce([this, goal] { this->SetGoal(goal); }, {this});
}

Usar un TrapezoidProfileSubsystem con comandos puede ser bastante simple:

Java

    // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
    m_driverController.a().onTrue(m_robotArm.setArmGoalCommand(2));

    // Move the arm to neutral position when the 'B' button is pressed.
    m_driverController
        .b()
        .onTrue(m_robotArm.setArmGoalCommand(Constants.ArmConstants.kArmOffsetRads));

C++

  // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
  m_driverController.A().OnTrue(m_arm.SetArmGoalCommand(2_rad));

  // Move the arm to neutral position when the 'B' button is pressed.
  m_driverController.B().OnTrue(
      m_arm.SetArmGoalCommand(ArmConstants::kArmOffset));

Comando TrapezoideProfile

Nota

En C++, la clase TrapezoidProfileCommand 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 TrapezoidProfileCommand class (Java, C++) allows users to create a command that will execute a single TrapezoidProfile, passing its current state at each iteration to a user-defined function.

Creando un TrapezoidProfileCommand

A TrapezoidProfileCommand can be created two ways - by subclassing the TrapezoidProfileCommand class, or by defining the command inline. Both methods are 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 TrapezoidProfileCommand and overriding any methods, make sure to call the super version of those methods! Otherwise, motion profiling functionality will not work properly.

En cualquier caso, se crea un TrapezoidProfileCommand 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 TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
   * Output will be piped to the provided consumer function.
   *
   * @param profile The motion profile to execute.
   * @param output The consumer for the profile output.
   * @param goal The supplier for the desired state
   * @param currentState The current state
   * @param requirements The subsystems required by this command.
   */
  @SuppressWarnings("this-escape")
  public TrapezoidProfileCommand(
      TrapezoidProfile profile,
      Consumer<State> output,
      Supplier<State> goal,
      Supplier<State> currentState,

C++

  /**
   * Creates a new TrapezoidProfileCommand that will execute the given
   * TrapezoidalProfile. Output will be piped to the provided consumer function.
   *
   * @param profile      The motion profile to execute.
   * @param output       The consumer for the profile output.
   * @param goal The supplier for the desired state
   * @param currentState The current state
   * @param requirements The list of requirements.
   */
  TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
                          std::function<void(State)> output,
                          std::function<State()> goal,
                          std::function<State()> currentState,
                          Requirements requirements = {})

profile

The profile parameter is the TrapezoidProfile object that will be executed by the command. By passing this in, users specify the motion constraints of the profile that the command will use.

output

The output 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 useState() function in PIDSubsystem.

goal

The goal parameter is a function that supplies the desired state of the motion profile. This can be used to change the goal at runtime if desired.

currentState

The currentState parameter is a function that supplies the starting state of the motion profile. Combined with goal, this can be used to dynamically generate and follow any motion profile at runtime.

requisitos

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

Ejemplo completo de TrapezoidProfileCommand

¿Qué aspecto tiene un TrapezoidProfileSubsystem cuando se utiliza en la práctica? Los siguientes ejemplos están tomados del proyecto de ejemplo DriveDistanceOffboard (Java, C++):

Java

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

import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;

/** Drives a set distance using a motion profile. */
public class DriveDistanceProfiled extends TrapezoidProfileCommand {
  /**
   * Creates a new DriveDistanceProfiled command.
   *
   * @param meters The distance to drive.
   * @param drive The drive subsystem to use.
   */
  public DriveDistanceProfiled(double meters, DriveSubsystem drive) {
    super(
        new TrapezoidProfile(
            // Limit the max acceleration and velocity
            new TrapezoidProfile.Constraints(
                DriveConstants.kMaxSpeedMetersPerSecond,
                DriveConstants.kMaxAccelerationMetersPerSecondSquared)),
        // Pipe the profile state to the drive
        setpointState -> drive.setDriveStates(setpointState, setpointState),
        // End at desired position in meters; implicitly starts at 0
        () -> new TrapezoidProfile.State(meters, 0),
        // Current position
        TrapezoidProfile.State::new,
        // Require the drive
        drive);
    // Reset drive encoders since we're starting at 0
    drive.resetEncoders();
  }
}

C++ (Header)

#pragma once

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

#include "subsystems/DriveSubsystem.h"

class DriveDistanceProfiled
    : public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meters>,
                                 DriveDistanceProfiled> {
 public:
  DriveDistanceProfiled(units::meter_t distance, DriveSubsystem* drive);
};

C++ (Source)

#include "commands/DriveDistanceProfiled.h"

#include "Constants.h"

using namespace DriveConstants;

DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance,
                                             DriveSubsystem* drive)
    : CommandHelper{
          frc::TrapezoidProfile<units::meters>{
              // Limit the max acceleration and velocity
              {kMaxSpeed, kMaxAcceleration}},
          // Pipe the profile state to the drive
          [drive](auto setpointState) {
            drive->SetDriveStates(setpointState, setpointState);
          },
          // End at desired position in meters; implicitly starts at 0
          [distance] {
            return frc::TrapezoidProfile<units::meters>::State{distance, 0_mps};
          },
          [] { return frc::TrapezoidProfile<units::meters>::State{}; },
          // Require the drive
          {drive}} {
  // Reset drive encoders since we're starting at 0
  drive->ResetEncoders();
}

And, for an inlined example:

Java

    // Do the same thing as above when the 'B' button is pressed, but defined inline
    m_driverController
        .b()
        .onTrue(
            new TrapezoidProfileCommand(
                    new TrapezoidProfile(
                        // Limit the max acceleration and velocity
                        new TrapezoidProfile.Constraints(
                            DriveConstants.kMaxSpeedMetersPerSecond,
                            DriveConstants.kMaxAccelerationMetersPerSecondSquared)),
                    // Pipe the profile state to the drive
                    setpointState -> m_robotDrive.setDriveStates(setpointState, setpointState),
                    // End at desired position in meters; implicitly starts at 0
                    () -> new TrapezoidProfile.State(3, 0),
                    // Current position
                    TrapezoidProfile.State::new,
                    // Require the drive
                    m_robotDrive)
                .beforeStarting(m_robotDrive::resetEncoders)
                .withTimeout(10));

C++

      DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s));

  // Do the same thing as above when the 'B' button is pressed, but defined
  // inline
  m_driverController.B().OnTrue(
      frc2::TrapezoidProfileCommand<units::meters>(
          frc::TrapezoidProfile<units::meters>(
              // Limit the max acceleration and velocity
              {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}),
          // Pipe the profile state to the drive
          [this](auto setpointState) {
            m_drive.SetDriveStates(setpointState, setpointState);
          },
          // End at desired position in meters; implicitly starts at 0
          [] {
            return frc::TrapezoidProfile<units::meters>::State{3_m, 0_mps};
          },
          // Current position
          [] { return frc::TrapezoidProfile<units::meters>::State{}; },
          // Require the drive
          {&m_drive})
          // Convert to CommandPtr
          .ToPtr()
          .BeforeStarting(