Combinando perfiles de movimiento y control de PID con ProfiledPIDController

Nota

For a guide on implementing the ProfiledPIDController class in the Commands v2 framework framework, see Combining Motion Profiling and PID in Command-Based.

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
wpi::math::ProfiledPIDController<units::meters> controller(
  kP, kI, kD,
  wpi::math::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.get_distance(), 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:

37  public void goToPosition(double goalPosition) {
38    double pidVal = controller.calculate(encoder.getDistance(), goalPosition);
39    motor.setVoltage(
40        pidVal + feedforward.calculate(lastVelocity, controller.getSetpoint().velocity));
41    lastVelocity = controller.getSetpoint().velocity;
42  }

25  void GoToPosition(wpi::units::meter_t goalPosition) {
26    auto pidVal = controller.Calculate(
27        wpi::units::meter_t{encoder.GetDistance()}, goalPosition);
28    motor.SetVoltage(
29        wpi::units::volt_t{pidVal} +
30        feedforward.Calculate(lastVelocity, controller.GetSetpoint().velocity));
31    lastVelocity = controller.GetSetpoint().velocity;
32  }

from wpilib.controller import ProfiledPIDController
from wpilib.controller import SimpleMotorFeedforward
def __init__(self):
    # Assuming encoder, motor, controller are already defined
    self.lastSpeed = 0
    # 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.get_distance(), goalPosition)
    self.motor.set_voltage(
        pidVal
        + self.feedforward.calculate(self.lastSpeed, self.controller.get_setpoint().velocity))
    self.lastSpeed = self.controller.get_setpoint().velocity

Ejemplo de uso completo

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

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

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

 8import math
 9
10import wpilib
11import wpilib_drivers
12import wpimath
13
14
15class MyRobot(wpilib.TimedRobot):
16    DT = 0.02
17    MAX_VELOCITY = 1.75
18    MAX_ACCELERATION = 0.75
19    P = 1.3
20    I = 0.0
21    D = 0.7
22    S = 1.1
23    G = 1.2
24    V = 1.3
25
26    def __init__(self) -> None:
27        super().__init__()
28        self.joystick = wpilib.Joystick(1)
29        self.encoder = wpilib.Encoder(2, 3)
30        self.motor = wpilib_drivers.PWMSparkMax(1)
31
32        # Create a PID controller whose setpoint's change is subject to maximum
33        # velocity and acceleration constraints.
34        self.constraints = wpimath.TrapezoidProfile.Constraints(
35            self.MAX_VELOCITY, self.MAX_ACCELERATION
36        )
37        self.controller = wpimath.ProfiledPIDController(
38            self.P, self.I, self.D, self.constraints, self.DT
39        )
40        self.feedforward = wpimath.ElevatorFeedforward(self.S, self.G, self.V)
41
42        self.encoder.set_distance_per_pulse(1 / 360 * 2 * math.pi * 1.5)
43
44    def teleop_periodic(self) -> None:
45        if self.joystick.get_raw_button_pressed(2):
46            self.controller.set_goal(5)
47        elif self.joystick.get_raw_button_pressed(3):
48            self.controller.set_goal(0)
49
50        # Run controller and update motor output
51        self.motor.set_voltage(
52            self.controller.calculate(self.encoder.get_distance())
53            + self.feedforward.calculate(self.controller.get_setpoint().velocity)
54        )