Combinaison du profilage de mouvement et du PID dans les commandes

Note

Pour une description des fonctionnalités de contrôle PID WPILib utilisées par ces wrappers basés sur des commandes, voir: Contrôle PID dans WPILib.

A common FRC® controls solution is to pair a trapezoidal motion profile for setpoint generation with a PID controller for setpoint tracking. To facilitate this, WPILib includes its own ProfiledPIDController class. To further aid teams in integrating this functionality into their robots, the command-based framework contains two convenience wrappers for the ProfiledPIDController class: ProfiledPIDSubsystem, which integrates the controller into a subsystem, and ProfiledPIDCommand, which integrates the controller into a command.

ProfiledPIDSubsystem

Note

En C ++, la classe ProfiledPIDSubsystem 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++.

The ProfiledPIDSubsystem class (Java, C++) allows users to conveniently create a subsystem with a built-in PIDController. In order to use the ProfiledPIDSubsystem class, users must create a subclass of it.

Création d’un sous-système ProfiledPIDS

Note

If periodic is overridden when inheriting from ProfiledPIDSubsystem, make sure to call super.periodic()! Otherwise, control functionality will not work properly.

Dans la sous-classe ProfiledPIDSubsystem, les utilisateurs doivent remplacer deux méthodes abstraites pour fournir des fonctionnalités que la classe utilisera dans son fonctionnement normal:

getMeasurement ()

  protected abstract double getMeasurement();
  virtual Distance_t GetMeasurement() = 0;

La méthode getMeasurement() renvoie la mesure actuelle de la variable du processus en cours. PIDSubsystem appellera automatiquement cette méthode depuis son bloc periodic(), et passera sa valeur à la boucle de contrôle.

Les utilisateurs doivent remplacer cette méthode pour renvoyer la lecture du capteur qu’ils souhaitent utiliser comme mesure de variable de processus.

useOutput ()

  protected abstract void useOutput(double output, State setpoint);
  virtual void UseOutput(double output, State setpoint) = 0;

The useOutput() method consumes the output of the Profiled PID controller, and the current setpoint state (which is often useful for computing a feedforward). The PIDSubsystem will automatically call this method from its periodic() block, and pass it the computed output of the control loop.

Les utilisateurs doivent remplacer cette méthode pour transmettre la sortie de contrôle calculée finale aux moteurs de leur sous-système.

Passer le contrôleur

Les utilisateurs doivent également transmettre un ProfiledPIDController à la classe de base ProfiledPIDSubsystem via l’appel du constructeur de superclasse de leur sous-classe. Cela permet de spécifier les gains PID, les contraintes du profil de mouvement et la période (si l’utilisateur utilise une période de boucle de robot principal non standard).

Des modifications supplémentaires (par exemple l’activation d’une entrée continue) peuvent être apportées au contrôleur dans le corps du constructeur en appelant getController().

Utilisation d’un sous-système ProfiledPIDS

Une fois qu’une instance d’une sous-classe PIDSubsystem a été créée, elle peut être utilisée par des commandes via les méthodes suivantes:

setGoal()

Note

Si vous souhaitez définir l’objectif à une distance simple avec une vitesse cible implicite de zéro, il existe une version de setGoal() qui prend une seule valeur de distance, plutôt qu’un état de profil de mouvement complet.

La méthode setGoal() peut être utilisée pour définir le point de consigne du PIDSubsystem. Le sous-système suivra automatiquement le point de consigne à l’aide de la sortie définie:

// The subsystem will track to a goal of 5 meters and velocity of 3 meters per second.
examplePIDSubsystem.setGoal(5, 3);
// The subsystem will track to a goal of 5 meters and velocity of 3 meters per second.
examplePIDSubsystem.SetGoal({5_m, 3_mps});

enable() et disable()

Les méthodes enable() et disable() activent et désactivent le contrôle automatique du ProfiledPIDSubsystem. Lorsque le sous-système est activé, il exécutera automatiquement le profil de mouvement et la boucle de contrôle et suivra jusqu’au but. Lorsqu’il est désactivé, aucun contrôle n’est effectué.

De plus, la méthode enable() réinitialise le ProfiledPIDController interne, et la méthode disable() appelle la méthode définie par l’utilisateur useOutput() avec à la fois la sortie et le point de consigne définis sur 0.

Exemple de sous-système ProfiledPIDS complet

À quoi ressemble un PIDSubsystem lorsqu’il est utilisé dans la pratique? Les exemples suivants sont pris à partir du projet d’exemple ArmBot (Java, C++):

 5package edu.wpi.first.wpilibj.examples.armbot.subsystems;
 6
 7import edu.wpi.first.math.controller.ArmFeedforward;
 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.examples.armbot.Constants.ArmConstants;
12import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
13import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
14
15/** A robot arm subsystem that moves with a motion profile. */
16public class ArmSubsystem extends ProfiledPIDSubsystem {
17  private final PWMSparkMax m_motor = new PWMSparkMax(ArmConstants.kMotorPort);
18  private final Encoder m_encoder =
19      new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]);
20  private final ArmFeedforward m_feedforward =
21      new ArmFeedforward(
22          ArmConstants.kSVolts, ArmConstants.kGVolts,
23          ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
24
25  /** Create a new ArmSubsystem. */
26  public ArmSubsystem() {
27    super(
28        new ProfiledPIDController(
29            ArmConstants.kP,
30            0,
31            0,
32            new TrapezoidProfile.Constraints(
33                ArmConstants.kMaxVelocityRadPerSecond,
34                ArmConstants.kMaxAccelerationRadPerSecSquared)),
35        0);
36    m_encoder.setDistancePerPulse(ArmConstants.kEncoderDistancePerPulse);
37    // Start arm at rest in neutral position
38    setGoal(ArmConstants.kArmOffsetRads);
39  }
40
41  @Override
42  public void useOutput(double output, TrapezoidProfile.State setpoint) {
43    // Calculate the feedforward from the sepoint
44    double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
45    // Add the feedforward to the PID output to get the motor output
46    m_motor.setVoltage(output + feedforward);
47  }
48
49  @Override
50  public double getMeasurement() {
51    return m_encoder.getDistance() + ArmConstants.kArmOffsetRads;
52  }
53}
 5#pragma once
 6
 7#include <frc/Encoder.h>
 8#include <frc/controller/ArmFeedforward.h>
 9#include <frc/motorcontrol/PWMSparkMax.h>
10#include <frc2/command/ProfiledPIDSubsystem.h>
11#include <units/angle.h>
12
13/**
14 * A robot arm subsystem that moves with a motion profile.
15 */
16class ArmSubsystem : public frc2::ProfiledPIDSubsystem<units::radians> {
17  using State = frc::TrapezoidProfile<units::radians>::State;
18
19 public:
20  ArmSubsystem();
21
22  void UseOutput(double output, State setpoint) override;
23
24  units::radian_t GetMeasurement() override;
25
26 private:
27  frc::PWMSparkMax m_motor;
28  frc::Encoder m_encoder;
29  frc::ArmFeedforward m_feedforward;
30};
 5#include "subsystems/ArmSubsystem.h"
 6
 7#include "Constants.h"
 8
 9using namespace ArmConstants;
10using State = frc::TrapezoidProfile<units::radians>::State;
11
12ArmSubsystem::ArmSubsystem()
13    : frc2::ProfiledPIDSubsystem<units::radians>(
14          frc::ProfiledPIDController<units::radians>(
15              kP, 0, 0, {kMaxVelocity, kMaxAcceleration})),
16      m_motor(kMotorPort),
17      m_encoder(kEncoderPorts[0], kEncoderPorts[1]),
18      m_feedforward(kS, kG, kV, kA) {
19  m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
20  // Start arm in neutral position
21  SetGoal(State{kArmOffset, 0_rad_per_s});
22}
23
24void ArmSubsystem::UseOutput(double output, State setpoint) {
25  // Calculate the feedforward from the sepoint
26  units::volt_t feedforward =
27      m_feedforward.Calculate(setpoint.position, setpoint.velocity);
28  // Add the feedforward to the PID output to get the motor output
29  m_motor.SetVoltage(units::volt_t{output} + feedforward);
30}
31
32units::radian_t ArmSubsystem::GetMeasurement() {
33  return units::radian_t{m_encoder.GetDistance()} + kArmOffset;
34}

L’utilisation d’un ProfiledPIDSubsystem avec des commandes peut être très simple:

55    // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
56    m_driverController
57        .a()
58        .onTrue(
59            Commands.runOnce(
60                () -> {
61                  m_robotArm.setGoal(2);
62                  m_robotArm.enable();
63                },
64                m_robotArm));
32  // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
33  m_driverController.A().OnTrue(frc2::cmd::RunOnce(
34      [this] {
35        m_arm.SetGoal(2_rad);
36        m_arm.Enable();
37      },
38      {&m_arm}));

ProfiledPIDCommand

Note

En C++, la classe ProfiledPIDCommand 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++.

The ProfiledPIDCommand class (Java, C++) allows users to easily create commands with a built-in ProfiledPIDController.

Création d’une commande PID

A ProfiledPIDCommand can be created two ways - by subclassing the ProfiledPIDCommand class, or by defining the command inline. Both methods ultimately extremely similar, and ultimately the choice of which to use comes down to where the user desires that the relevant code be located.

Note

If subclassing ProfiledPIDCommand and overriding any methods, make sure to call the super version of those methods! Otherwise, control functionality will not work properly.

Dans les deux cas, un ProfiledPIDCommand est créé en passant les paramètres nécessaires à son constructeur (si vous définissez une sous-classe, cela peut être fait avec un appel super()):

29  /**
30   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
31   * velocity is specified.
32   *
33   * @param controller the controller that controls the output.
34   * @param measurementSource the measurement of the process variable
35   * @param goalSource the controller's goal
36   * @param useOutput the controller's output
37   * @param requirements the subsystems required by this command
38   */
39  public ProfiledPIDCommand(
40      ProfiledPIDController controller,
41      DoubleSupplier measurementSource,
42      Supplier<State> goalSource,
43      BiConsumer<Double, State> useOutput,
44      Subsystem... requirements) {
38  /**
39   * Creates a new PIDCommand, which controls the given output with a
40   * ProfiledPIDController.
41   *
42   * @param controller        the controller that controls the output.
43   * @param measurementSource the measurement of the process variable
44   * @param goalSource   the controller's goal
45   * @param useOutput         the controller's output
46   * @param requirements      the subsystems required by this command
47   */
48  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
49                     std::function<Distance_t()> measurementSource,
50                     std::function<State()> goalSource,
51                     std::function<void(double, State)> useOutput,
52                     Requirements requirements = {})

Le paramètre controller

Le paramètre controller est l’objet ProfiledPIDController qui sera utilisé par la commande. En transmettant cela, les utilisateurs peuvent spécifier les gains PID, les contraintes de profil de mouvement et la période pour le contrôleur (si l’utilisateur utilise une période de boucle de robot principal non standard).

Lors de la sous-classification de ProfiledPIDCommand, des modifications supplémentaires (par exemple, l’activation d’une entrée continue) peuvent être apportées au contrôleur dans le corps du constructeur en appelant getController().

Le paramètre measurementSource

The measurementSource parameter is a function (usually passed as a lambda) that returns the measurement of the process variable. Passing in the measurementSource function in ProfiledPIDCommand is functionally analogous to overriding the getMeasurement() function in ProfiledPIDSubsystem.

Lors de la sous-classe de ProfiledPIDCommand, les utilisateurs avancés peuvent modifier davantage les sources de mesures en modifiant le champ m_measurement de la classe.

Le paramètre goalSource

The goalSource parameter is a function (usually passed as a lambda) that returns the current goal state for the mechanism. If only a constant goal is needed, an overload exists that takes a constant goal rather than a supplier. Additionally, if goal velocities are desired to be zero, overloads exist that take a constant distance rather than a full profile state.

Lors de la sous-classe de ProfiledPIDCommand, les utilisateurs avancés peuvent modifier davantage le fournisseur de point de consigne en modifiant le champ m_goal de la classe.

Le paramètre useOutput

The useOutput parameter is a function (usually passed as a lambda) that consumes the output and setpoint state of the control loop. Passing in the useOutput function in ProfiledPIDCommand is functionally analogous to overriding the useOutput() function in ProfiledPIDSubsystem.

Lors de la sous-classe de ProfiledPIDCommand, les utilisateurs avancés peuvent modifier davantage l’état de sortie en modifiant le champ m_useOutput de la classe.

Les exigences

Comme toutes les commandes en ligne, ProfiledPIDCommand permet à l’utilisateur de spécifier ses exigences de sous-système en tant que paramètre constructeur.

Exemple complet de ProfiledPIDCommand

What does a ProfiledPIDCommand look like when used in practice? The following examples are from the GyroDriveCommands example project (Java, C++):

 5package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;
 6
 7import edu.wpi.first.math.controller.ProfiledPIDController;
 8import edu.wpi.first.math.trajectory.TrapezoidProfile;
 9import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
10import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
11import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
12
13/** A command that will turn the robot to the specified angle using a motion profile. */
14public class TurnToAngleProfiled extends ProfiledPIDCommand {
15  /**
16   * Turns to robot to the specified angle using a motion profile.
17   *
18   * @param targetAngleDegrees The angle to turn to
19   * @param drive The drive subsystem to use
20   */
21  public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) {
22    super(
23        new ProfiledPIDController(
24            DriveConstants.kTurnP,
25            DriveConstants.kTurnI,
26            DriveConstants.kTurnD,
27            new TrapezoidProfile.Constraints(
28                DriveConstants.kMaxTurnRateDegPerS,
29                DriveConstants.kMaxTurnAccelerationDegPerSSquared)),
30        // Close loop on heading
31        drive::getHeading,
32        // Set reference to target
33        targetAngleDegrees,
34        // Pipe output to turn robot
35        (output, setpoint) -> drive.arcadeDrive(0, output),
36        // Require the drive
37        drive);
38
39    // Set the controller to be continuous (because it is an angle controller)
40    getController().enableContinuousInput(-180, 180);
41    // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
42    // setpoint before it is considered as having reached the reference
43    getController()
44        .setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
45  }
46
47  @Override
48  public boolean isFinished() {
49    // End when the controller is at the reference.
50    return getController().atGoal();
51  }
52}
 5#pragma once
 6
 7#include <frc2/command/CommandHelper.h>
 8#include <frc2/command/ProfiledPIDCommand.h>
 9
10#include "subsystems/DriveSubsystem.h"
11
12/**
13 * A command that will turn the robot to the specified angle using a motion
14 * profile.
15 */
16class TurnToAngleProfiled
17    : public frc2::CommandHelper<frc2::ProfiledPIDCommand<units::radians>,
18                                 TurnToAngleProfiled> {
19 public:
20  /**
21   * Turns to robot to the specified angle using a motion profile.
22   *
23   * @param targetAngleDegrees The angle to turn to
24   * @param drive              The drive subsystem to use
25   */
26  TurnToAngleProfiled(units::degree_t targetAngleDegrees,
27                      DriveSubsystem* drive);
28
29  bool IsFinished() override;
30};
 5#include "commands/TurnToAngleProfiled.h"
 6
 7#include <frc/controller/ProfiledPIDController.h>
 8
 9using namespace DriveConstants;
10
11TurnToAngleProfiled::TurnToAngleProfiled(units::degree_t target,
12                                         DriveSubsystem* drive)
13    : CommandHelper{
14          frc::ProfiledPIDController<units::radians>{
15              kTurnP, kTurnI, kTurnD, {kMaxTurnRate, kMaxTurnAcceleration}},
16          // Close loop on heading
17          [drive] { return drive->GetHeading(); },
18          // Set reference to target
19          target,
20          // Pipe output to turn robot
21          [drive](double output, auto setpointState) {
22            drive->ArcadeDrive(0, output);
23          },
24          // Require the drive
25          {drive}} {
26  // Set the controller to be continuous (because it is an angle controller)
27  GetController().EnableContinuousInput(-180_deg, 180_deg);
28  // Set the controller tolerance - the delta tolerance ensures the robot is
29  // stationary at the setpoint before it is considered as having reached the
30  // reference
31  GetController().SetTolerance(kTurnTolerance, kTurnRateTolerance);
32
33  AddRequirements(drive);
34}
35
36bool TurnToAngleProfiled::IsFinished() {
37  return GetController().AtGoal();
38}