Contrôle PID via PIDSubsystems et PIDCommands

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

One of the most common control algorithms used in FRC® is the PID controller. WPILib offers its own PIDController class to help teams implement this functionality on their robots. To further help teams integrate PID control into a command-based robot project, the command-based library includes two convenience wrappers for the PIDController class: PIDSubsystem, which integrates the PID controller into a subsystem, and PIDCommand, which integrates the PID controller into a command.

Sous-systèmes PIDS

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 PIDS

Lors de la sous-classification de PIDSubsystem, les utilisateurs doivent remplacer deux méthodes abstraites pour fournir des fonctionnalités que la classe utilisera dans son fonctionnement ordinaire:

getMeasurement()

protected abstract double getMeasurement();

La méthode getMeasurement renvoie la mesure actuelle de la variable de processus. Le 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 plutôt la lecture du capteur qu’ils souhaitent utiliser comme mesure de variable de processus.

useOutput()

protected abstract void useOutput(double output, double setpoint);

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

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

La transmission d’un contrôleur

Les utilisateurs doivent également transmettre un PIDController à la classe de base PIDSubsystem via l’appel de constructeur de superclasse de leur sous-classe. Cela permet de spécifier les gains PID, ainsi que la période (si l’utilisateur utilise une période de boucle différente de celle utilisée dans le code du robot 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 PIDS

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:

setSetpoint()

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

// The subsystem will track to a setpoint of 5.
examplePIDSubsystem.setSetpoint(5);

enable() and disable()

Les méthodes enable() et disable() activent et désactivent respectivement le contrôle PID du PIDSubsystem. Lorsque le sous-système est activé, il exécute automatiquement la boucle de contrôle et suit le point de consigne. Lorsqu’il est désactivé, aucun contrôle n’est effectué.

De plus, la méthode enable() réinitialise le PIDController 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 la valeur 0.

Exemple complet avec PIDSubsystem

À quoi ressemble un PIDSubsystem lorsqu’il est utilisé dans la pratique? Les exemples suivants sont pris à partir du projet d’exemple FrisbeeBot(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
56
57
58
59
package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;

import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;

public class ShooterSubsystem extends PIDSubsystem {
  private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(ShooterConstants.kShooterMotorPort);
  private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(ShooterConstants.kFeederMotorPort);
  private final Encoder m_shooterEncoder =
      new Encoder(ShooterConstants.kEncoderPorts[0], ShooterConstants.kEncoderPorts[1],
                  ShooterConstants.kEncoderReversed);
  private final SimpleMotorFeedforward m_shooterFeedforward =
      new SimpleMotorFeedforward(ShooterConstants.kSVolts,
                                 ShooterConstants.kVVoltSecondsPerRotation);

  /**
   * The shooter subsystem for the robot.
   */
  public ShooterSubsystem() {
    super(new PIDController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD));
    getController().setTolerance(ShooterConstants.kShooterToleranceRPS);
    m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
    setSetpoint(ShooterConstants.kShooterTargetRPS);
  }

  @Override
  public void useOutput(double output, double setpoint) {
    m_shooterMotor.setVoltage(output + m_shooterFeedforward.calculate(setpoint));
  }

  @Override
  public double getMeasurement() {
    return m_shooterEncoder.getRate();
  }

  public boolean atSetpoint() {
    return m_controller.atSetpoint();
  }

  public void runFeeder() {
    m_feederMotor.set(ShooterConstants.kFeederSpeed);
  }

  public void stopFeeder() {
    m_feederMotor.set(0);
  }
}

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

85
86
87
88
89
90
91
    // Spin up the shooter when the 'A' button is pressed
    new JoystickButton(m_driverController, Button.kA.value)
        .whenPressed(new InstantCommand(m_shooter::enable, m_shooter));

    // Turn off the shooter when the 'B' button is pressed
    new JoystickButton(m_driverController, Button.kB.value)
        .whenPressed(new InstantCommand(m_shooter::disable, m_shooter));

La classe PIDCommand

La classe PIDCommand permet aux utilisateurs de créer facilement des commandes avec un PIDController intégré. Comme avec PIDSubsystem, les utilisateurs peuvent créer un PIDCommand en sous-classant la classe PIDCommand. Cependant, comme avec la plupart des autres classes de la librairie basée sur les commandes, les utilisateurs peuvent vouloir réduire la quantité de code/programme en définissant une version inline de PIDCommand.

Création d’une commande PID

Un PIDCommand peut être créé de deux manières - en sous-classant la classe PIDCommand, ou en définissant la commande de façon inline. Les deux méthodes sont extrêmement similaires et, finalement, le choix de la méthode à utiliser se résume à l’endroit où l’utilisateur souhaite que le code correspondant soit situé.

Dans les deux cas, un PIDCommand est créé en passant les paramètres nécessaires à son constructeur (lors de la définition d’une sous-classe, cela peut être fait avec un appel super() ):

29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
  /**
   * Creates a new PIDCommand, which controls the given output with a PIDController.
   *
   * @param controller        the controller that controls the output.
   * @param measurementSource the measurement of the process variable
   * @param setpointSource    the controller's setpoint
   * @param useOutput         the controller's output
   * @param requirements      the subsystems required by this command
   */
  public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
                    DoubleSupplier setpointSource, DoubleConsumer useOutput,
                    Subsystem... requirements) {
    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
    requireNonNullParam(setpointSource, "setpointSource", "SynchronousPIDCommand");
    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");

    m_controller = controller;
    m_useOutput = useOutput;
    m_measurement = measurementSource;
    m_setpoint = setpointSource;
    m_requirements.addAll(Set.of(requirements));
  }

manette

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

Lors de la sous-classification de PIDCommand, 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 PIDCommand est fonctionnellement similaire à la substitution de la fonction getMeasurement() dans PIDSubsystem.

Lors de la sous-classification de PIDCommand, les utilisateurs plus expérimentés peuvent modifier le système qui fournit les mesures en modifiant le champ m_measurement de la classe.

Le paramètre setpointSource

Le paramètre setpointSource est une fonction (généralement transmise sous la forme lambda) qui renvoie le point de consigne actuel pour la boucle de contrôle. Si seulement un point de consigne constant est nécessaire, il existe une version qui utilise ce seul point de consigne constant plutôt qu’une fonction-fournisseur.

Lors de la sous-classification de PIDCommand, les utilisateurs plus expérimentés peuvent modifier le système qui fournit les mesures en modifiant le champ m_measurement de la classe.

Le paramètre useOutput

Le paramètre useOutput est une fonction (généralement transmise sous la forme lambda) qui utilise la sortie et le point de consigne de la boucle de contrôle. Le passage de la fonction useOutput dans PIDCommand est fonctionnellement similaire à la substitution de la fonction useOutput () dans PIDSubsystem.

Lors de la sous-classification de PIDCommand, les utilisateurs plus expérimentés peuvent modifier le système qui fournit les mesures en modifiant le champ m_measurement de la classe.

exigences

Comme toutes les commandes en ligne, PIDCommand permet à l’utilisateur de spécifier des exigences de sous-système via ses paramètres de constructeur.

Exemple complet de PIDCommand

À quoi ressemble un PIDCommand lorsqu’il est utilisé dans la pratique? Les exemples suivants proviennent du projet d’exemple GyroDriveCommands (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
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;

import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj2.command.PIDCommand;

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.
 */
public class TurnToAngle extends PIDCommand {
  /**
   * Turns to robot to the specified angle.
   *
   * @param targetAngleDegrees The angle to turn to
   * @param drive              The drive subsystem to use
   */
  public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
    super(
        new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD),
        // Close loop on heading
        drive::getHeading,
        // Set reference to target
        targetAngleDegrees,
        // Pipe output to turn robot
        output -> 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().atSetpoint();
  }
}

Et, pour un exemple de type inlined :

70
71
72
73
74
75
76
77
78
79
80
81
    // Stabilize robot to drive straight with gyro when left bumper is held
    new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld(new PIDCommand(
        new PIDController(DriveConstants.kStabilizationP, DriveConstants.kStabilizationI,
                          DriveConstants.kStabilizationD),
        // Close the loop on the turn rate
        m_robotDrive::getTurnRate,
        // Setpoint is 0
        0,
        // Pipe the output to the turning controls
        output -> m_robotDrive.arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output),
        // Require the robot drive
        m_robotDrive));