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

// 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));

// Creates a ProfiledPIDController
// Max velocity is 5 meters per second
// Max acceleration is 10 meters per second
frc::ProfiledPIDController<units::meters> controller(
  kP, kI, kD,
  frc::TrapezoidProfile<units::meters>::Constraints{5_mps, 10_mps_sq});

from wpimath.controller import ProfiledPIDController
from wpimath.trajectory import TrapezoidProfile
  # Creates a ProfiledPIDController
# Max velocity is 5 meters per second
# Max acceleration is 10 meters per second
controller = ProfiledPIDController(
  kP, kI, kD,
  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:

// 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));

// 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));

# 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:

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

units::meters_per_second_t lastSpeed = 0_mps;
units::second_t lastTime = frc2::Timer::GetFPGATimestamp();
  // Controls a simple motor's position using a SimpleMotorFeedforward
// and a ProfiledPIDController
void GoToPosition(units::meter_t goalPosition) {
  auto pidVal = controller.Calculate(units::meter_t{encoder.GetDistance()}, goalPosition);
  auto acceleration = (controller.GetSetpoint().velocity - lastSpeed) /
      (frc2::Timer::GetFPGATimestamp() - lastTime);
  motor.SetVoltage(
       pidVal +
      feedforward.Calculate(controller.GetSetpoint().velocity, acceleration));
  lastSpeed = controller.GetSetpoint().velocity;
  lastTime = frc2::Timer::GetFPGATimestamp();
}

from wpilib import Timer
from wpilib.controller import ProfiledPIDController
from wpilib.controller import SimpleMotorFeedforward
def __init__(self):
    # Assuming encoder, motor, controller are already defined
    self.lastSpeed = 0
    self.lastTime = Timer.getFPGATimestamp()
    # Assuming feedforward is a SimpleMotorFeedforward object
    self.feedforward = SimpleMotorFeedforward(ks=0.0, kv=0.0, ka=0.0)
def goToPosition(self, goalPosition: float):
    pidVal = self.controller.calculate(self.encoder.getDistance(), goalPosition)
    acceleration = (self.controller.getSetpoint().velocity - self.lastSpeed) / (Timer.getFPGATimestamp() - self.lastTime)
    self.motor.setVoltage(
        pidVal
        + self.feedforward.calculate(self.controller.getSetpoint().velocity, acceleration))
    self.lastSpeed = controller.getSetpoint().velocity
    self.lastTime = Timer.getFPGATimestamp()

Ejemplo de uso completo

A more complete example of ProfiledPIDController usage is provided in the ElevatorProfilePID example project (Java, C++, Python):

 5package edu.wpi.first.wpilibj.examples.elevatorprofiledpid;
 6
 7import edu.wpi.first.math.controller.ElevatorFeedforward;
 8import edu.wpi.first.math.controller.ProfiledPIDController;
 9import edu.wpi.first.math.trajectory.TrapezoidProfile;
10import edu.wpi.first.wpilibj.Encoder;
11import edu.wpi.first.wpilibj.Joystick;
12import edu.wpi.first.wpilibj.TimedRobot;
13import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
14
15@SuppressWarnings("PMD.RedundantFieldInitializer")
16public class Robot extends TimedRobot {
17  private static double kDt = 0.02;
18  private static double kMaxVelocity = 1.75;
19  private static double kMaxAcceleration = 0.75;
20  private static double kP = 1.3;
21  private static double kI = 0.0;
22  private static double kD = 0.7;
23  private static double kS = 1.1;
24  private static double kG = 1.2;
25  private static double kV = 1.3;
26
27  private final Joystick m_joystick = new Joystick(1);
28  private final Encoder m_encoder = new Encoder(1, 2);
29  private final PWMSparkMax m_motor = new PWMSparkMax(1);
30
31  // Create a PID controller whose setpoint's change is subject to maximum
32  // velocity and acceleration constraints.
33  private final TrapezoidProfile.Constraints m_constraints =
34      new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration);
35  private final ProfiledPIDController m_controller =
36      new ProfiledPIDController(kP, kI, kD, m_constraints, kDt);
37  private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward(kS, kG, kV);
38
39  public Robot() {
40    m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
41  }
42
43  @Override
44  public void teleopPeriodic() {
45    if (m_joystick.getRawButtonPressed(2)) {
46      m_controller.setGoal(5);
47    } else if (m_joystick.getRawButtonPressed(3)) {
48      m_controller.setGoal(0);
49    }
50
51    // Run controller and update motor output
52    m_motor.setVoltage(
53        m_controller.calculate(m_encoder.getDistance())
54            + m_feedforward.calculate(m_controller.getSetpoint().velocity));
55  }
56}

 5#include <numbers>
 6
 7#include <frc/Encoder.h>
 8#include <frc/Joystick.h>
 9#include <frc/TimedRobot.h>
10#include <frc/controller/ElevatorFeedforward.h>
11#include <frc/controller/ProfiledPIDController.h>
12#include <frc/motorcontrol/PWMSparkMax.h>
13#include <frc/trajectory/TrapezoidProfile.h>
14#include <units/acceleration.h>
15#include <units/length.h>
16#include <units/time.h>
17#include <units/velocity.h>
18#include <units/voltage.h>
19
20class Robot : public frc::TimedRobot {
21 public:
22  static constexpr units::second_t kDt = 20_ms;
23
24  Robot() {
25    m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * std::numbers::pi * 1.5);
26  }
27
28  void TeleopPeriodic() override {
29    if (m_joystick.GetRawButtonPressed(2)) {
30      m_controller.SetGoal(5_m);
31    } else if (m_joystick.GetRawButtonPressed(3)) {
32      m_controller.SetGoal(0_m);
33    }
34
35    // Run controller and update motor output
36    m_motor.SetVoltage(
37        units::volt_t{
38            m_controller.Calculate(units::meter_t{m_encoder.GetDistance()})} +
39        m_feedforward.Calculate(m_controller.GetSetpoint().velocity));
40  }
41
42 private:
43  static constexpr units::meters_per_second_t kMaxVelocity = 1.75_mps;
44  static constexpr units::meters_per_second_squared_t kMaxAcceleration =
45      0.75_mps_sq;
46  static constexpr double kP = 1.3;
47  static constexpr double kI = 0.0;
48  static constexpr double kD = 0.7;
49  static constexpr units::volt_t kS = 1.1_V;
50  static constexpr units::volt_t kG = 1.2_V;
51  static constexpr auto kV = 1.3_V / 1_mps;
52
53  frc::Joystick m_joystick{1};
54  frc::Encoder m_encoder{1, 2};
55  frc::PWMSparkMax m_motor{1};
56
57  // Create a PID controller whose setpoint's change is subject to maximum
58  // velocity and acceleration constraints.
59  frc::TrapezoidProfile<units::meters>::Constraints m_constraints{
60      kMaxVelocity, kMaxAcceleration};
61  frc::ProfiledPIDController<units::meters> m_controller{kP, kI, kD,
62                                                         m_constraints, kDt};
63  frc::ElevatorFeedforward m_feedforward{kS, kG, kV};
64};
65
66#ifndef RUNNING_FRC_TESTS
67int main() {
68  return frc::StartRobot<Robot>();
69}
70#endif

 8import wpilib
 9import wpimath.controller
10import wpimath.trajectory
11import math
12
13
14class MyRobot(wpilib.TimedRobot):
15    kDt = 0.02
16
17    def robotInit(self) -> None:
18        self.joystick = wpilib.Joystick(1)
19        self.encoder = wpilib.Encoder(1, 2)
20        self.motor = wpilib.PWMSparkMax(1)
21
22        # Create a PID controller whose setpoint's change is subject to maximum
23        # velocity and acceleration constraints.
24        self.constraints = wpimath.trajectory.TrapezoidProfile.Constraints(1.75, 0.75)
25        self.controller = wpimath.controller.ProfiledPIDController(
26            1.3, 0, 0.7, self.constraints, self.kDt
27        )
28
29        self.encoder.setDistancePerPulse(1 / 360 * 2 * math.pi * 1.5)
30
31    def teleopPeriodic(self) -> None:
32        if self.joystick.getRawButtonPressed(2):
33            self.controller.setGoal(5)
34        elif self.joystick.getRawButtonPressed(3):
35            self.controller.setGoal(0)
36
37        # Run controller and update motor output
38        self.motor.set(self.controller.calculate(self.encoder.getDistance()))