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.

Note

Contrairement à la version précédente de PIDController, la version 2020 de la classe ProfiledPIDController s’exécute de manière synchrone et n’est pas gérée dans son propre thread. Par conséquent, la modification de son paramètre period ne changera pas la fréquence réelle à laquelle elle s’exécute dans l’une de ces classes « wrapper ». Les utilisateurs ne doivent jamais modifier le paramètre period à moins d’être certains de ce qu’ils font.

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++.

La classe ProfiledPIDSubsystem (Java, C++) permet aux utilisateurs de créer facilement un sous-système avec un contrôleur PID intégré. Pour utiliser la classe ProfiledPIDSubsystem, les utilisateurs doivent d’abord créer une sous-classe de celle-ci.

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

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

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, TrapezoidProfile.State setpoint);

La méthode useOutput() utilise la sortie du contrôleur PID et l’état actuel du point de consigne (ce qui est souvent utile pour calculer un feedforward). Le PIDSubsystem appellera automatiquement cette méthode depuis son bloc periodic(), et lui passera la sortie calculée de la boucle de contrôle.

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

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

 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
package edu.wpi.first.wpilibj.examples.armbot.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.controller.ArmFeedforward;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;

import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants;

/**
 * A robot arm subsystem that moves with a motion profile.
 */
public class ArmSubsystem extends ProfiledPIDSubsystem {
  private final PWMVictorSPX m_motor = new PWMVictorSPX(ArmConstants.kMotorPort);
  private final Encoder m_encoder =
      new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]);
  private final ArmFeedforward m_feedforward =
      new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts,
                         ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);

  /**
   * Create a new ArmSubsystem.
   */
  public ArmSubsystem() {
    super(new ProfiledPIDController(ArmConstants.kP, 0, 0, new TrapezoidProfile.Constraints(
        ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared)), 0);
    m_encoder.setDistancePerPulse(ArmConstants.kEncoderDistancePerPulse);
    // Start arm at rest in neutral position
    setGoal(ArmConstants.kArmOffsetRads);
  }

  @Override
  public void useOutput(double output, TrapezoidProfile.State setpoint) {
    // Calculate the feedforward from the sepoint
    double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
    // Add the feedforward to the PID output to get the motor output
    m_motor.setVoltage(output + feedforward);
  }

  @Override
  public double getMeasurement() {
    return m_encoder.getDistance() + ArmConstants.kArmOffsetRads;
  }
}

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

63
64
65
66
67
68
    // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
    new JoystickButton(m_driverController, Button.kA.value)
        .whenPressed(() -> {
          m_robotArm.setGoal(2);
          m_robotArm.enable();
        }, m_robotArm);

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++.

La classe ProfiledPIDCommand (Java, C++) permet aux utilisateurs de créer facilement des commandes avec un ProfiledPIDController intégré. Comme pour ProfiledPIDSubsystem, les utilisateurs peuvent créer un ProfiledPIDCommmand en sous-classant la classe ProfiledPIDCommand. Cependant, comme avec la plupart des autres classes de commandes de la librairie basée sur les commandes, les utilisateurs peuvent vouloir enregistrer le code en le définissant en-ligne.

Création d’une commande PID

Un ProfiledPIDCommand peut être créé de deux manières - en sous-classant la classe ProfiledPIDCommand, ou en définissant la commande en-ligne. Les deux méthodes sont extrêmement similaires et, en fin de compte, le choix de l’utilisation se résume à l’endroit où l’utilisateur souhaite que le code correspondant soit situé.

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

32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
  /**
   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
   * Goal velocity is specified.
   *
   * @param controller        the controller that controls the output.
   * @param measurementSource the measurement of the process variable
   * @param goalSource        the controller's goal
   * @param useOutput         the controller's output
   * @param requirements      the subsystems required by this command
   */
  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
                            Supplier<State> goalSource, BiConsumer<Double, State> useOutput,
                            Subsystem... requirements) {
    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
    requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");

    m_controller = controller;
    m_useOutput = useOutput;
    m_measurement = measurementSource;
    m_goal = goalSource;
    m_requirements.addAll(Set.of(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

Le paramètre measurementSource est une fonction (généralement transmise sous la forme lambda) qui renvoie la mesure de la variable de processus. Le passage de la fonction measurementSource dans ProfiledPIDCommand est fonctionnellement analogue à la substitution de la fonction getMeasurement() dans 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

Le paramètre goalSource est une fonction (généralement passée sous la forme lambda) qui renvoie l’état actuel de l’objectif pour le mécanisme. Si seul un objectif constant est nécessaire, il existe une version qui prend un objectif constant plutôt qu’un fournisseur. De plus, si l’on veut que les vitesses d’objectif soient nulles, il existe des versions qui prennent une distance constante plutôt qu’un état de profil complet.

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

Le paramètre useOutput est une fonction (généralement transmise sous la forme lambda) qui utilise l’état de sortie et le point de consigne de la boucle de contrôle. Le passage de la fonction useOutput dans ProfiledPIDCommand est fonctionnellement analogue à la substitution de la fonction useOutput () dans 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++):

 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;

import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;

import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;

/**
 * A command that will turn the robot to the specified angle using a motion profile.
 */
public class TurnToAngleProfiled extends ProfiledPIDCommand {
  /**
   * Turns to robot to the specified angle using a motion profile.
   *
   * @param targetAngleDegrees The angle to turn to
   * @param drive              The drive subsystem to use
   */
  public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) {
    super(
        new ProfiledPIDController(DriveConstants.kTurnP, DriveConstants.kTurnI,
                                  DriveConstants.kTurnD, new TrapezoidProfile.Constraints(
            DriveConstants.kMaxTurnRateDegPerS,
            DriveConstants.kMaxTurnAccelerationDegPerSSquared)),
        // Close loop on heading
        drive::getHeading,
        // Set reference to target
        targetAngleDegrees,
        // Pipe output to turn robot
        (output, setpoint) -> drive.arcadeDrive(0, output),
        // Require the drive
        drive);

    // Set the controller to be continuous (because it is an angle controller)
    getController().enableContinuousInput(-180, 180);
    // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
    // setpoint before it is considered as having reached the reference
    getController()
        .setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
  }

  @Override
  public boolean isFinished() {
    // End when the controller is at the reference.
    return getController().atGoal();
  }
}