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

Note

Pour un guide sur l’implémentation de la classe ProfiledPIDController dans le cadre du command-based framework framework, see Combinaison du profilage de mouvement et du PID dans les commandes.

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

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.getDistance(), 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.

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

Un exemple complet d’utilisation

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  @Override
40  public void robotInit() {
41    m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
42  }
43
44  @Override
45  public void teleopPeriodic() {
46    if (m_joystick.getRawButtonPressed(2)) {
47      m_controller.setGoal(5);
48    } else if (m_joystick.getRawButtonPressed(3)) {
49      m_controller.setGoal(0);
50    }
51
52    // Run controller and update motor output
53    m_motor.setVoltage(
54        m_controller.calculate(m_encoder.getDistance())
55            + m_feedforward.calculate(m_controller.getSetpoint().velocity));
56  }
57}
 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()))