Combinando perfiles de movimiento y control de PID con ProfiledPIDController

Nota

Para una guia implementando la clase ProfiledPIDController en el command-based framework entorno de trabajo, vea Combinando Motion Profiling y PID Basado en Comandos.

En el artículo anterior, vimos cómo usar la clase TrapezoidProfile para crear y usar un perfil de movimiento trapezoidal. El código de ejemplo de ese artículo demuestra la composición manual de la clase TrapezoidProfile con la función de control PID externo de un controlador de motor «smart».

This combination of functionality (a motion profile for generating setpoints combined with a PID controller for following them) is extremely common. To facilitate this, WPILib comes with a ProfiledPIDController class (Java, C++) that does most of the work of combining these two functionalities. The API of the ProfiledPIDController is very similar to that of the PIDController, allowing users to add motion profiling to a PID-controlled mechanism with very few changes to their code.

Usando la clase ProfiledPIDController

Nota

En C ++, la clase ProfiledPIDController 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++.

Nota

Gran parte de la funcionalidad de ProfiledPIDController es efectivamente idéntica a la de PIDController. En consecuencia, este artículo solo cubrirá las características que han cambiado sustancialmente para adaptarse a la funcionalidad de creación de perfiles de movimiento. Para obtener información sobre las características estándar del PIDController, consulte Control PID en WPILib.

Construyendo un ProfiledPIDController

Nota

C++ a menudo puede inferir el tipo de las clases internas y, por lo tanto, se puede enviar una lista de inicializadores simple (sin el nombre de la clase) como parámetro. El nombre completo de la clase se incluye en el siguiente ejemplo para mayor claridad.

Crear un ProfiledPIDController es casi idéntico a creating a PIDController. La única diferencia es la necesidad de suministrar un conjunto de trapezoidal profile constraints, que se reenviará automáticamente a las instancias TrapezoidProfile generadas internamente:

JavaC++

// Creates a ProfiledPIDController
// Max velocity is 5 meters per second
// Max acceleration is 10 meters per second
ProfiledPIDController controller = new ProfiledPIDController(
  kP, kI, kD,
  new TrapezoidProfile.Constraints(5, 10));

Objetivo vs punto de ajuste

Una diferencia importante entre un PIDController estándar y un ProfiledPIDController es que el usuario no especifica directamente el setpoint real del lazo de control. Más bien, el usuario especifica una posición o estado meta, y el punto de ajuste para el controlador se calcula automáticamente a partir del perfil de movimiento generado entre el estado actual y el objetivo. Entonces, mientras que la llamada del lado del usuario parece casi idéntica:

JavaC++

// Calculates the output of the PID algorithm based on the sensor reading
// and sends it to a motor
motor.set(controller.calculate(encoder.getDistance(), goal));

El valor de meta especificado (que puede ser un valor de posición o un TrapezoidProfile.State, si se desea una velocidad distinta de cero) no es necesariamente el actual del bucle, sino que es el eventual punto de ajuste una vez que finaliza el perfil generado.

Obtención / uso del punto de ajuste

Dado que el objetivo del ProfiledPIDController difiere del punto de ajuste, si a menudo es deseable sondear el punto de ajuste actual del controlador (por ejemplo, para obtener valores para usar con:ref:feedforward <docs/software/advanced-controls/controllers/combining-feedforward-feedback:Using Feedforward Components with PID>). Esto se puede hacer con el método getSetpoint().

El punto de ajuste devuelto podría usarse como en el siguiente ejemplo:

JavaC++

double lastSpeed = 0;
double lastTime = Timer.getFPGATimestamp();

// Controls a simple motor's position using a SimpleMotorFeedforward
// and a ProfiledPIDController
public void goToPosition(double goalPosition) {
  double pidVal = controller.calculate(encoder.getDistance(), goalPosition);
  double acceleration = (controller.getSetpoint().velocity - lastSpeed) / (Timer.getFPGATimestamp() - lastTime);
  motor.setVoltage(
      pidVal
      + feedforward.calculate(controller.getSetpoint().velocity, acceleration));
  lastSpeed = controller.getSetpoint().velocity;
  lastTime = Timer.getFPGATimestamp();
}

Ejemplo de uso completo

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

JavaC++

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

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.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;

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

  private final Joystick m_joystick = new Joystick(1);
  private final Encoder m_encoder = new Encoder(1, 2);
  private final MotorController m_motor = new PWMSparkMax(1);

  // Create a PID controller whose setpoint's change is subject to maximum
  // velocity and acceleration constraints.
  private final TrapezoidProfile.Constraints m_constraints =
      new TrapezoidProfile.Constraints(1.75, 0.75);
  private final ProfiledPIDController m_controller =
      new ProfiledPIDController(1.3, 0.0, 0.7, m_constraints, kDt);

  @Override
  public void robotInit() {
    m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
  }

  @Override
  public void teleopPeriodic() {
    if (m_joystick.getRawButtonPressed(2)) {
      m_controller.setGoal(5);
    } else if (m_joystick.getRawButtonPressed(3)) {
      m_controller.setGoal(0);
    }

    // Run controller and update motor output
    m_motor.set(m_controller.calculate(m_encoder.getDistance()));
  }
}