Profilage de mouvement via TrapezoidProfileSubsystems et TrapezoidProfileCommands
Note
Pour une description des fonctionnalités de profilage de mouvement WPILib utilisées par ces wrappers basés sur des commandes, voir Profils de mouvement trapézoïdal dans WPILib.
Note
Les wrappers de commande TrapezoidProfile
sont généralement destinés pour usage avec des contrôleurs personnalisés ou externes. Pour combiner le profilage de mouvement trapézoïdal avec le PIDController
de WPILib, voir Combinaison du profilage de mouvement et du PID dans les commandes.
When controlling a mechanism, is often desirable to move it smoothly between two positions, rather than to abruptly change its setpoint. This is called « motion-profiling, » and is supported in WPILib through the TrapezoidProfile
class (Java, C++).
Pour aider davantage les équipes à intégrer le profilage de mouvement dans leurs projets de robot basés sur des commandes, WPILib comprend deux wrappers pratiques pour la classe TrapezoidProfile
: TrapezoidProfileSubsystem
, qui génère et exécute automatiquement des profils de mouvement dans la méthode periodic()
, et TrapezoidProfileCommand
, qui exécute un seul TrapezoidProfile
fourni par l’utilisateur.
TrapezoidProfileSubsystem
Note
En C ++, la classe TrapezoidProfileSubsystem
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 indiquée. Pour plus d’informations sur les unités C ++, voir La librairie d’unités C++.
The TrapezoidProfileSubsystem
class (Java, C++) will automatically create and execute trapezoidal motion profiles to reach the user-provided goal state. To use the TrapezoidProfileSubsystem
class, users must create a subclass of it.
Création d’un sous-système TrapezoidProfile
Note
If periodic
is overridden when inheriting from TrapezoidProfileSubsystem
, make sure to call super.periodic()
! Otherwise, motion profiling functionality will not work properly.
Lors de la sous-classification de TrapezoidProfileSubsystem
, les utilisateurs doivent remplacer une seule méthode abstraite pour fournir des fonctionnalités que la classe utilisera dans son fonctionnement ordinaire:
useState()
protected abstract void useState(TrapezoidProfile.State state);
virtual void UseState(State state) = 0;
La méthode useState()
utilise l’état actuel du profil de mouvement pour les calculs. Le TrapezoidProfileSubsystem
appellera automatiquement cette méthode à partir de son segment periodic()
, et lui passera l’état du profil de mouvement correspondant à la progression actuelle du sous-système à travers le profil de mouvement.
Les utilisateurs peuvent faire ce qu’ils veulent avec cet état; un cas d’utilisation typique (comme indiqué dans Full TrapezoidProfileSubsystem Example) consiste à utiliser l’état pour obtenir un point de consigne et une action directe pour un contrôleur de moteur externe « intelligent ».
Paramètres du constructeur
Les utilisateurs doivent transmettre un ensemble de valeurs-contraintes, soit TrapezoidProfile.Constraints
à la classe de base TrapezoidProfileSubsystem
via l’appel de constructeur de superclasse de leur sous-classe. Cela permet de contraindre les profils générés automatiquement à une vitesse et une accélération maximales données.
Les utilisateurs doivent également définir et transmettre une position initiale pour le mécanisme.
Les utilisateurs avancés peuvent transmettre une autre valeur pour la période de boucle, si la période de boucle principale est différente de la valeur standard.
Utilisation d’un sous-système TrapezoidProfile
Une fois qu’une instance d’une sous-classe TrapezoidProfileSubsystem
a été créée, elle peut être utilisée par les commandes via les méthodes suivantes:
setGoal()
Note
Si vous souhaitez définir l’objectif à atteindre comme étant une distance fixe, avec une vitesse de zéro, il existe une version simplifiée 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 l’état de l’objectif du TrapezoidProfileSubsystem
. Le sous-système exécutera automatiquement un profil vers l’objectif, en passant l’état actuel à chaque itération à la méthode useState() fournie.
// The subsystem will execute a profile to a position of 5 and a velocity of 3.
examplePIDSubsystem.setGoal(new TrapezoidProfile.State(5, 3);
// The subsystem will execute a profile to a position of 5 meters and a velocity of 3 mps.
examplePIDSubsyste.SetGoal({5_m, 3_mps});
enable() and disable()
The enable()
and disable()
methods enable and disable the motion profiling control of the TrapezoidProfileSubsystem
. When the subsystem is enabled, it will automatically run the control loop and call useState()
periodically. When it is disabled, no control is performed.
Un exemple complet de sous-système TrapezoidProfile
À quoi ressemble un TrapezoidProfileSubsystem
lorsqu’il est utilisé dans la pratique? Les exemples suivants proviennent du projet d’exemple ArmbotOffobard (Java, C++):
5package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
6
7import edu.wpi.first.math.controller.ArmFeedforward;
8import edu.wpi.first.math.trajectory.TrapezoidProfile;
9import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants;
10import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController;
11import edu.wpi.first.wpilibj2.command.Command;
12import edu.wpi.first.wpilibj2.command.Commands;
13import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
14
15/** A robot arm subsystem that moves with a motion profile. */
16public class ArmSubsystem extends TrapezoidProfileSubsystem {
17 private final ExampleSmartMotorController m_motor =
18 new ExampleSmartMotorController(ArmConstants.kMotorPort);
19 private final ArmFeedforward m_feedforward =
20 new ArmFeedforward(
21 ArmConstants.kSVolts, ArmConstants.kGVolts,
22 ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
23
24 /** Create a new ArmSubsystem. */
25 public ArmSubsystem() {
26 super(
27 new TrapezoidProfile.Constraints(
28 ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared),
29 ArmConstants.kArmOffsetRads);
30 m_motor.setPID(ArmConstants.kP, 0, 0);
31 }
32
33 @Override
34 public void useState(TrapezoidProfile.State setpoint) {
35 // Calculate the feedforward from the sepoint
36 double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
37 // Add the feedforward to the PID output to get the motor output
38 m_motor.setSetpoint(
39 ExampleSmartMotorController.PIDMode.kPosition, setpoint.position, feedforward / 12.0);
40 }
41
42 public Command setArmGoalCommand(double kArmOffsetRads) {
43 return Commands.runOnce(() -> setGoal(kArmOffsetRads), this);
44 }
45}
5#pragma once
6
7#include <frc/controller/ArmFeedforward.h>
8#include <frc2/command/Commands.h>
9#include <frc2/command/TrapezoidProfileSubsystem.h>
10#include <units/angle.h>
11
12#include "ExampleSmartMotorController.h"
13
14/**
15 * A robot arm subsystem that moves with a motion profile.
16 */
17class ArmSubsystem : public frc2::TrapezoidProfileSubsystem<units::radians> {
18 using State = frc::TrapezoidProfile<units::radians>::State;
19
20 public:
21 ArmSubsystem();
22
23 void UseState(State setpoint) override;
24
25 frc2::CommandPtr SetArmGoalCommand(units::radian_t goal);
26
27 private:
28 ExampleSmartMotorController m_motor;
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::TrapezoidProfileSubsystem<units::radians>(
14 {kMaxVelocity, kMaxAcceleration}, kArmOffset),
15 m_motor(kMotorPort),
16 m_feedforward(kS, kG, kV, kA) {
17 m_motor.SetPID(kP, 0, 0);
18}
19
20void ArmSubsystem::UseState(State setpoint) {
21 // Calculate the feedforward from the sepoint
22 units::volt_t feedforward =
23 m_feedforward.Calculate(setpoint.position, setpoint.velocity);
24 // Add the feedforward to the PID output to get the motor output
25 m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
26 setpoint.position.value(), feedforward / 12_V);
27}
28
29frc2::CommandPtr ArmSubsystem::SetArmGoalCommand(units::radian_t goal) {
30 return frc2::cmd::RunOnce([this, goal] { this->SetGoal(goal); }, {this});
31}
L’utilisation d’un TrapezoidProfileSubsystem
avec des commandes peut être assez simple:
52 // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
53 m_driverController.a().onTrue(m_robotArm.setArmGoalCommand(2));
54
55 // Move the arm to neutral position when the 'B' button is pressed.
56 m_driverController
57 .b()
58 .onTrue(m_robotArm.setArmGoalCommand(Constants.ArmConstants.kArmOffsetRads));
25 // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
26 m_driverController.A().OnTrue(m_arm.SetArmGoalCommand(2_rad));
27
28 // Move the arm to neutral position when the 'B' button is pressed.
29 m_driverController.B().OnTrue(
30 m_arm.SetArmGoalCommand(ArmConstants::kArmOffset));
TrapezoidProfileCommand
Note
En C ++, la classe TrapezoidProfileCommand
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 indiquée. Pour plus d’informations sur les unités C++, voir La librairie d’unités C++.
The TrapezoidProfileCommand
class (Java, C++) allows users to create a command that will execute a single TrapezoidProfile
, passing its current state at each iteration to a user-defined function.
Création d’un TrapezoidProfileCommand
A TrapezoidProfileCommand
can be created two ways - by subclassing the TrapezoidProfileCommand
class, or by defining the command inline. Both methods are 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 TrapezoidProfileCommand
and overriding any methods, make sure to call the super
version of those methods! Otherwise, motion profiling functionality will not work properly.
Dans les deux cas, un TrapezoidProfileCommand
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() ):
28 * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
29 * Output will be piped to the provided consumer function.
30 *
31 * @param profile The motion profile to execute.
32 * @param output The consumer for the profile output.
33 * @param goal The supplier for the desired state
34 * @param currentState The current state
35 * @param requirements The subsystems required by this command.
36 */
37 @SuppressWarnings("this-escape")
38 public TrapezoidProfileCommand(
39 TrapezoidProfile profile,
40 Consumer<State> output,
41 Supplier<State> goal,
42 Supplier<State> currentState,
43 Subsystem... requirements) {
35 /**
36 * Creates a new TrapezoidProfileCommand that will execute the given
37 * TrapezoidalProfile. Output will be piped to the provided consumer function.
38 *
39 * @param profile The motion profile to execute.
40 * @param output The consumer for the profile output.
41 * @param goal The supplier for the desired state
42 * @param currentState The current state
43 * @param requirements The list of requirements.
44 */
45 TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
46 std::function<void(State)> output,
47 std::function<State()> goal,
48 std::function<State()> currentState,
49 Requirements requirements = {})
Le paramètre Profile (Profil)
The profile
parameter is the TrapezoidProfile
object that will be executed by the command. By passing this in, users specify the motion constraints of the profile that the command will use.
Le paramètre Output (Sortie)
The output
parameter is a function (usually passed as a lambda) that consumes the output and setpoint of the control loop. Passing in the useOutput
function in PIDCommand
is functionally analogous to overriding the useState() function in PIDSubsystem
.
goal
The goal
parameter is a function that supplies the desired state of the motion profile. This can be used to change the goal at runtime if desired.
currentState
The currentState
parameter is a function that supplies the starting state of the motion profile. Combined with goal
, this can be used to dynamically generate and follow any motion profile at runtime.
Les exigences
Comme toutes les commandes en ligne, TrapezoidProfileCommand
permet à l’utilisateur de spécifier ses exigences de sous-système en tant que paramètre constructeur.
Exemple de commande TrapezoidProfileCommand complète
What does a TrapezoidProfileSubsystem
look like when used in practice? The following examples are taking from the DriveDistanceOffboard example project (Java, C++):
5package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands;
6
7import edu.wpi.first.math.trajectory.TrapezoidProfile;
8import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
9import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
10import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
11
12/** Drives a set distance using a motion profile. */
13public class DriveDistanceProfiled extends TrapezoidProfileCommand {
14 /**
15 * Creates a new DriveDistanceProfiled command.
16 *
17 * @param meters The distance to drive.
18 * @param drive The drive subsystem to use.
19 */
20 public DriveDistanceProfiled(double meters, DriveSubsystem drive) {
21 super(
22 new TrapezoidProfile(
23 // Limit the max acceleration and velocity
24 new TrapezoidProfile.Constraints(
25 DriveConstants.kMaxSpeedMetersPerSecond,
26 DriveConstants.kMaxAccelerationMetersPerSecondSquared)),
27 // Pipe the profile state to the drive
28 setpointState -> drive.setDriveStates(setpointState, setpointState),
29 // End at desired position in meters; implicitly starts at 0
30 () -> new TrapezoidProfile.State(meters, 0),
31 // Current position
32 TrapezoidProfile.State::new,
33 // Require the drive
34 drive);
35 // Reset drive encoders since we're starting at 0
36 drive.resetEncoders();
37 }
38}
5#pragma once
6
7#include <frc2/command/CommandHelper.h>
8#include <frc2/command/TrapezoidProfileCommand.h>
9
10#include "subsystems/DriveSubsystem.h"
11
12class DriveDistanceProfiled
13 : public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meters>,
14 DriveDistanceProfiled> {
15 public:
16 DriveDistanceProfiled(units::meter_t distance, DriveSubsystem* drive);
17};
5#include "commands/DriveDistanceProfiled.h"
6
7#include "Constants.h"
8
9using namespace DriveConstants;
10
11DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance,
12 DriveSubsystem* drive)
13 : CommandHelper{
14 frc::TrapezoidProfile<units::meters>{
15 // Limit the max acceleration and velocity
16 {kMaxSpeed, kMaxAcceleration}},
17 // Pipe the profile state to the drive
18 [drive](auto setpointState) {
19 drive->SetDriveStates(setpointState, setpointState);
20 },
21 // End at desired position in meters; implicitly starts at 0
22 [distance] {
23 return frc::TrapezoidProfile<units::meters>::State{distance, 0_mps};
24 },
25 [] { return frc::TrapezoidProfile<units::meters>::State{}; },
26 // Require the drive
27 {drive}} {
28 // Reset drive encoders since we're starting at 0
29 drive->ResetEncoders();
30}
And, for an inlined example:
66 // Do the same thing as above when the 'B' button is pressed, but defined inline
67 m_driverController
68 .b()
69 .onTrue(
70 new TrapezoidProfileCommand(
71 new TrapezoidProfile(
72 // Limit the max acceleration and velocity
73 new TrapezoidProfile.Constraints(
74 DriveConstants.kMaxSpeedMetersPerSecond,
75 DriveConstants.kMaxAccelerationMetersPerSecondSquared)),
76 // Pipe the profile state to the drive
77 setpointState -> m_robotDrive.setDriveStates(setpointState, setpointState),
78 // End at desired position in meters; implicitly starts at 0
79 () -> new TrapezoidProfile.State(3, 0),
80 // Current position
81 TrapezoidProfile.State::new,
82 // Require the drive
83 m_robotDrive)
84 .beforeStarting(m_robotDrive::resetEncoders)
85 .withTimeout(10));
37 DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s));
38
39 // Do the same thing as above when the 'B' button is pressed, but defined
40 // inline
41 m_driverController.B().OnTrue(
42 frc2::TrapezoidProfileCommand<units::meters>(
43 frc::TrapezoidProfile<units::meters>(
44 // Limit the max acceleration and velocity
45 {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}),
46 // Pipe the profile state to the drive
47 [this](auto setpointState) {
48 m_drive.SetDriveStates(setpointState, setpointState);
49 },
50 // End at desired position in meters; implicitly starts at 0
51 [] {
52 return frc::TrapezoidProfile<units::meters>::State{3_m, 0_mps};
53 },
54 // Current position
55 [] { return frc::TrapezoidProfile<units::meters>::State{}; },
56 // Require the drive
57 {&m_drive})
58 // Convert to CommandPtr
59 .ToPtr()
60 .BeforeStarting(