Combinaison du profilage de mouvement et du contrôle PID avec ProfiledPIDController

Note

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

Dans l’article précédent, nous avons vu comment utiliser la classe TrapezoidProfile pour créer et utiliser un profil de mouvement trapézoïdal. L’exemple de code de cet article montre comment créer manuellement la classe TrapezoidProfile avec la fonction de contrôle PID externe d’un contrôleur de moteur « intelligent ».

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.

L’utilisation de la classe ProfiledPIDController

Note

En C ++, la classe ProfiledPIDController est basée sur le type d’unité utilisé pour les mesures de distance, qui peut être angulaire ou linéaire. Les valeurs transmises doivent avoir des unités cohérentes avec les unités de distance, sinon une erreur de compilation sera levée. Pour plus d’informations sur les unités C++, voir La librairie d’unités C++.

Note

La plupart des fonctionnalités de ProfiledPIDController sont relativement identiques à celle de PIDController. Par conséquent, cet article ne couvrira que les fonctionnalités qui ont été substantiellement modifiées pour s’adapter à la fonctionnalité de profilage de mouvement. Pour plus d’informations sur les fonctionnalités standard de PIDController, voir Contrôle PID dans WPILib.

La construction d’un objet ProfiledPIDController

Note

C ++ est souvent capable d’inférer le type des classes internes, et donc une simple liste d’initialisation (sans le nom de la classe) peut être envoyée en tant que paramètre. Le nom complet de la classe est inclus dans l’exemple ci-dessous pour plus de clarté.

La création d’un ProfiledPIDController est presque identique à creating a PIDController. La seule différence est la nécessité de fournir un ensemble de trapezoidal profile constraints, qui seront automatiquement transmises aux instances du « TrapezoidProfile » :

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

Objectif vs point de consigne

Une différence majeure entre un PIDController normal et un ProfiledPIDController est que le point de consigne réel de la boucle de régulation n’est pas directement spécifié par l’utilisateur. Au contraire, l’utilisateur spécifie une position ou un état souhaité, ou objectif, et le point de consigne pour le contrôleur est calculé automatiquement à partir du profil de mouvement généré entre l’état actuel et l’état objectif. Alors que la commande pour appeler cette classe semble essentiellement identique:

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

La valeur objectif spécifiée (qui peut être une valeur de position ou un TrapezoidProfile.State, si une vitesse non nulle est souhaitée) n’est pas nécessairement le point de consigne actuel de la boucle - plutôt, elle est le point de consigne éventuel une fois le profil généré terminé.

L’obtention / l’utilisation du point de consigne

Étant donné que l’objectif ProfiledPIDController diffère du point de consigne, il est souvent souhaitable d’interroger le contrôleur pour obtenir le point de consigne actuel (par exemple, pour obtenir des valeurs à utiliser avec feedforward). Cela peut être fait avec la méthode getSetpoint().

Le point de consigne renvoyé peut alors être utilisé comme dans l’exemple suivant.

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

Un exemple complet d’utilisation

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        )