Perfil de movimiento trapezoidal en WPILib

Nota

Este artículo cubre la generación en código de perfil de movimiento trapezoidal. A continuación se presentan más detalles que describen los conceptos involucrados.

Nota

Para obtener una guía sobre la implementación de la clase TrapezoidProfile` en command-based framework framework, ver Creación de perfiles de movimiento a través de los subsistemas TrapezoidProfile y los comandos TrapezoidProfile.

Nota

La clase TrapezoidProfile, utilizada por su cuenta, es más útil cuando se compone con un controlador personalizado (tales como un controlador de motor «inteligente» con una funcionalidad PID integrada). Para integrarlo con un WPILib ControladorPID, vea Combinando perfiles de movimiento y control de PID con ProfiledPIDController.

Mientras el avance y el control de la retroalimentación ofrecen maneras convenientes de lograr un punto fijo, estamos constantemente enfrentándonos al problema de la generación de un punto fijo para nuestros mecanismos. Si bien el enfoque ingenuo de comandar inmediatamente un mecanismo a su estado deseado puede funcionar, usualmente no es óptimo. Para mejorar el manejo de nuestros mecanismos, normalmente deseamos ordenar mecanismos a una secuencia de puntos fijos que interpolan suavemente entre su estado actual, y su estado deseado.

To help users do this, WPILib provides a TrapezoidProfile class (Java, C++).

Creando un Perfil Trapezoidal

Nota

En C++, la clase TrapezoidProfile está basada en el tipo de unidad utilizada para medidas de distancia, las cuales pueden ser angulares o lineales. Los valores pasados deben tener unidades consistentes con las unidades de distancia, o se producirá un error de compilación de tiempo. Para más información de las unidades C++, vea Biblioteca de unidades de C++.

Restricciones

Nota

Los diversos feedforward helper classes proporcionan métodos para calcular la máxima velocidad simultáneamente alcanzable y la aceleración de un mecanismo. Esto puede ser muy útil para calcular restricciones de movimientos adecuadas para ``Perfil Trapezoidal`.

In order to create a trapezoidal motion profile, we must first impose some constraints on the desired motion. Namely, we must specify a maximum velocity and acceleration that the mechanism will be expected to achieve during the motion. To do this, we create an instance of the TrapezoidProfile.Constraints class (Java, C++):

JavaC++

// Creates a new set of trapezoidal motion profile constraints
// Max velocity of 10 meters per second
// Max acceleration of 20 meters per second squared
new TrapezoidProfile.Constraints(10, 20);

Estados de Inicio y Fin

Next, we must specify the desired starting and ending states for our mechanisms using the TrapezoidProfile.State class (Java, C++). Each state has a position and a velocity:

JavaC++

// Creates a new state with a position of 5 meters
// and a velocity of 0 meters per second
new TrapezoidProfile.State(5, 0);

Poniéndolo todo junto

Nota

C++ normalmente es capaz de inferir el tipo de las clases internas y, por lo tanto, se puede enviar una lista de inicializadores simple (sin el nombre de clase) como parámetro. Los nombres completos de la clase están incluidos en el siguiente ejemplo.

Ahora que sabemos cómo crear un conjunto de restricciones y los estados de inicio/fin deseados, estamos listos para crear nuestro perfil de movimiento. El constructor “”PerfilTrapezoidal”” toma 3 parámetros, para: las restricciones, el estado del objetivo y el estado inicial.

JavaC++

// Creates a new TrapezoidProfile
// Profile will have a max vel of 5 meters per second
// Profile will have a max acceleration of 10 meters per second squared
// Profile will end stationary at 5 meters
// Profile will start stationary at zero position
TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(5, 10),
                                                new TrapezoidProfile.State(5, 0),
                                                new TrapezoidProfile.State(0, 0));

Usando un Perfil Trapezoidal

Muestreo del Perfil

Una vez que hemos creado un Perfil Trapezoide, su uso es muy simple: para obtener el estado del perfil en el momento dado después de que el perfil ha comenzado, llame al método calculate():

JavaC++

// Returns the motion profile state after 5 seconds of motion
profile.calculate(5);

Usando el Estado

El método calculate devuelve una clase TrapezoidProfile.State (la misma que se usó para especificar los estados iniciales/finales al construir el perfil). Para usar esto para el control real, simplemente pasa los valores de posición y velocidad contenidos a cualquier controlador que desees (por ejemplo, un ControladorPID):

JavaC++

var setpoint = profile.calculate(elapsedTime);
controller.calculate(encoder.getDistance(), setpoint.position);

Ejemplo de Uso Completo

Nota

In this example, the profile is re-computed every timestep. This is a somewhat different usage technique than is detailed above, but works according to the same principles - the profile is sampled at a time corresponding to the loop period to get the setpoint for the next loop iteration.

Un ejemplo más completo del uso de TrapezoidProfile se proporciona en el proyecto de ejemplo ElevatorTrapezoidProfile (Java, C++):

JavaC++

package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;

import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;

public class Robot extends TimedRobot {
  private static double kDt = 0.02;

  private final Joystick m_joystick = new Joystick(1);
  private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1);
  // Note: These gains are fake, and will have to be tuned for your robot.
  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1.5);

  private final TrapezoidProfile.Constraints m_constraints =
      new TrapezoidProfile.Constraints(1.75, 0.75);
  private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
  private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();

  @Override
  public void robotInit() {
    // Note: These gains are fake, and will have to be tuned for your robot.
    m_motor.setPID(1.3, 0.0, 0.7);
  }

  @Override
  public void teleopPeriodic() {
    if (m_joystick.getRawButtonPressed(2)) {
      m_goal = new TrapezoidProfile.State(5, 0);
    } else if (m_joystick.getRawButtonPressed(3)) {
      m_goal = new TrapezoidProfile.State(0, 0);
    }

    // Create a motion profile with the given maximum velocity and maximum
    // acceleration constraints for the next setpoint, the desired goal, and the
    // current setpoint.
    var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);

    // Retrieve the profiled setpoint for the next timestep. This setpoint moves
    // toward the goal while obeying the constraints.
    m_setpoint = profile.calculate(kDt);

    // Send setpoint to offboard controller PID
    m_motor.setSetpoint(
        ExampleSmartMotorController.PIDMode.kPosition,
        m_setpoint.position,
        m_feedforward.calculate(m_setpoint.velocity) / 12.0);
  }
}