Note
For a description of the WPILib motion profiling features used by these command-based wrappers, see :ref:`docs/software/controls/controllers/trapezoidal-profiles:Trapezoidal Motion Profiles in WPILib`.
Note
The TrapezoidProfile command wrappers are generally intended for composition with custom or external controllers. For combining trapezoidal motion profiling with WPILib's PIDController, see :doc:`profilepid-subsystems-commands`.
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++).
To further help teams integrate motion profiling into their command-based robot projects, WPILib includes two convenience wrappers for the TrapezoidProfile class: TrapezoidProfileSubsystem, which automatically generates and executes motion profiles in its periodic() method, and the TrapezoidProfileCommand, which executes a single user-provided TrapezoidProfile.
Note
In C++, the TrapezoidProfileSubsystem class is templated on the unit type used for distance measurements, which may be angular or linear. The passed-in values must have units consistent with the distance units, or a compile-time error will be thrown. For more information on C++ units, see :ref:`docs/software/basic-programming/cpp-units:The C++ Units Library`.
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.
Note
If periodic is overridden when inheriting from TrapezoidProfileSubsystem, make sure to call super.periodic()! Otherwise, motion profiling functionality will not work properly.
When subclassing TrapezoidProfileSubsystem, users must override a single abstract method to provide functionality that the class will use in its ordinary operation:
.. tab-set::
.. tab-item:: Java
:sync: Java
.. rli:: https://github.com/wpilibsuite/allwpilib/raw/v2024.1.1/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
:language: java
:lines: 105-105
.. tab-item:: C++
:sync: C++
.. rli:: https://github.com/wpilibsuite/allwpilib/raw/v2024.1.1/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
:language: c++
:lines: 75-75
The useState() method consumes the current state of the motion profile. The TrapezoidProfileSubsystem will automatically call this method from its periodic() block, and pass it the motion profile state corresponding to the subsystem's current progress through the motion profile.
Users may do whatever they want with this state; a typical use case (as shown in the Full TrapezoidProfileSubsystem Example) is to use the state to obtain a setpoint and a feedforward for an external "smart" motor controller.
Users must pass in a set of TrapezoidProfile.Constraints to the TrapezoidProfileSubsystem base class through the superclass constructor call of their subclass. This serves to constrain the automatically-generated profiles to a given maximum velocity and acceleration.
Users must also pass in an initial position for the mechanism.
Advanced users may pass in an alternate value for the loop period, if a non-standard main loop period is being used.
Once an instance of a TrapezoidProfileSubsystem subclass has been created, it can be used by commands through the following methods:
Note
If you wish to set the goal to a simple distance with an implicit target velocity of zero, an overload of setGoal() exists that takes a single distance value, rather than a full motion profile state.
The setGoal() method can be used to set the goal state of the TrapezoidProfileSubsystem. The subsystem will automatically execute a profile to the goal, passing the current state at each iteration to the provided useState() method.
.. tab-set-code::
.. code-block:: java
// The subsystem will execute a profile to a position of 5 and a velocity of 3.
examplePIDSubsystem.setGoal(new TrapezoidProfile.State(5, 3);
.. code-block:: c++
// The subsystem will execute a profile to a position of 5 meters and a velocity of 3 mps.
examplePIDSubsyste.SetGoal({5_m, 3_mps});
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.
What does a TrapezoidProfileSubsystem look like when used in practice? The following examples are taking from the ArmbotOffobard example project (Java, C++):
.. tab-set::
.. tab-item:: Java
:sync: Java
.. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/v2024.1.1/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
:language: java
:lines: 5-
:linenos:
:lineno-start: 5
.. tab-item:: C++ (Header)
:sync: C++ (Header)
.. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/v2024.1.1/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
:language: c++
:lines: 5-
:linenos:
:lineno-start: 5
.. tab-item:: C++ (Source)
:sync: C++ (Source)
.. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/v2024.1.1/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp
:language: c++
:lines: 5-
:linenos:
:lineno-start: 5
Using a TrapezoidProfileSubsystem with commands can be quite simple:
.. tab-set::
.. tab-item:: Java
:sync: Java
.. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/v2024.1.1/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
:language: java
:lines: 52-58
:linenos:
:lineno-start: 52
.. tab-item:: C++
:sync: C++
.. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/v2024.1.1/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
:language: c++
:lines: 25-30
:linenos:
:lineno-start: 25
Note
In C++, the TrapezoidProfileCommand class is templated on the unit type used for distance measurements, which may be angular or linear. The passed-in values must have units consistent with the distance units, or a compile-time error will be thrown. For more information on C++ units, see :ref:`docs/software/basic-programming/cpp-units:The C++ Units Library`.
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.
A TrapezoidProfileCommand can be created two ways - by subclassing the TrapezoidProfileCommand class, or by defining the command :ref:`inline <docs/software/commandbased/organizing-command-based:Inline Commands>`. 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.
In either case, a TrapezoidProfileCommand is created by passing the necessary parameters to its constructor (if defining a subclass, this can be done with a super() call):
.. tab-set::
.. tab-item:: Java
:sync: Java
.. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/main/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
:language: java
:lines: 28-43
:linenos:
:lineno-start: 28
.. tab-item:: C++
:sync: C++
.. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/main/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
:language: c++
:lines: 35-49
:linenos:
:lineno-start: 35
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.
The output parameter is a function (usually passed as a :ref:`lambda <docs/software/commandbased/index:Lambda Expressions (Java)>`) 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.
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.
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.
Like all inlineable commands, TrapezoidProfileCommand allows the user to specify its subsystem requirements as a constructor parameter.
What does a TrapezoidProfileSubsystem look like when used in practice? The following examples are taking from the DriveDistanceOffboard example project (Java, C++):
.. tab-set::
.. tab-item:: Java
:sync: Java
.. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
:language: java
:lines: 5-
:linenos:
:lineno-start: 5
.. tab-item:: C++ (Header)
:sync: C++ (Header)
.. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/main/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h
:language: c++
:lines: 5-
:linenos:
:lineno-start: 5
.. tab-item:: C++ (Source)
:sync: C++ (Source)
.. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/main/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp
:language: c++
:lines: 5-
:linenos:
:lineno-start: 5
And, for an :ref:`inlined <docs/software/commandbased/organizing-command-based:Inline Commands>` example:
.. tab-set::
.. tab-item:: Java
:sync: Java
.. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
:language: java
:lines: 66-85
:linenos:
:lineno-start: 66
.. tab-item:: C++
:sync: C++
.. remoteliteralinclude:: https://raw.githubusercontent.com/wpilibsuite/allwpilib/main/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
:language: c++
:lines: 37-60
:linenos:
:lineno-start: 37