Commands

Commands represent actions the robot can take. Commands run when scheduled, until they are interrupted or their end condition is met. Commands are represented in the command-based library by the Command class (Java, C++).

The Structure of a Command

Commands specify what the command will do in each of its possible states. This is done by overriding the initialize(), execute(), and end() methods. Additionally, a command must be able to tell the scheduler when (if ever) it has finished execution - this is done by overriding the isFinished() method. All of these methods are defaulted to reduce clutter in user code: initialize(), execute(), and end() are defaulted to simply do nothing, while isFinished() is defaulted to return false (resulting in a command that never finishes naturally, and will run until interrupted).

Initialization

The initialize() method (Java, C++) marks the command start, and is called exactly once per time a command is scheduled. The initialize() method should be used to place the command in a known starting state for execution. Command objects may be reused and scheduled multiple times, so any state or resources needed for the command’s functionality should be initialized or opened in initialize (which will be called at the start of each use) rather than the constructor (which is invoked only once on object allocation). It is also useful for performing tasks that only need to be performed once per time scheduled, such as setting motors to run at a constant speed or setting the state of a solenoid actuator.

Execution

The execute() method (Java, C++) is called repeatedly while the command is scheduled; this is when the scheduler’s run() method is called (this is generally done in the main robot periodic method, which runs every 20ms by default). The execute block should be used for any task that needs to be done continually while the command is scheduled, such as updating motor outputs to match joystick inputs, or using the output of a control loop.

Ending

The end(bool interrupted) method (Java, C++) is called once when the command ends, whether it finishes normally (i.e. isFinished() returned true) or it was interrupted (either by another command or by being explicitly canceled). The method argument specifies the manner in which the command ended; users can use this to differentiate the behavior of their command end accordingly. The end block should be used to “wrap up” command state in a neat way, such as setting motors back to zero or reverting a solenoid actuator to a “default” state. Any state or resources initialized in initialize() should be closed in end().

Specifying end conditions

The isFinished() method (Java, C++) is called repeatedly while the command is scheduled, whenever the scheduler’s run() method is called. As soon as it returns true, the command’s end() method is called and it ends. The isFinished() method is called after the execute() method, so the command will execute once on the same iteration that it ends.

Command Properties

In addition to the four lifecycle methods described above, each Command also has three properties, defined by getter methods that should always return the same value with no side affects.

getRequirements

Each command should declare any subsystems it controls as requirements. This backs the scheduler’s resource management mechanism, ensuring that no more than one command requires a given subsystem at the same time. This prevents situations such as two different pieces of code attempting to set the same motor controller to different output values.

Declaring requirements is done by overriding the getRequirements() method in the relevant command class, by calling addRequirements(), or by using the requirements vararg (Java) / Requirements struct (C++) parameter at the end of the parameter list of most command constructors and factories in the library:

Commands.run(intake::activate, intake);
frc2::cmd::Run([&intake] { intake.Activate(); }, {&intake});

As a rule, command compositions require all subsystems their components require.

runsWhenDisabled

The runsWhenDisabled() method (Java, C++) returns a boolean/bool specifying whether the command may run when the robot is disabled. With the default of returning false, the command will be canceled when the robot is disabled and attempts to schedule it will do nothing. Returning true will allow the command to run and be scheduled when the robot is disabled.

Important

When the robot is disabled, PWM outputs are disabled and CAN motor controllers may not apply voltage, regardless of runsWhenDisabled!

This property can be set either by overriding the runsWhenDisabled() method in the relevant command class, or by using the ignoringDisable decorator (Java, C++):

Command mayRunDuringDisabled = Commands.run(() -> updateTelemetry()).ignoringDisable(true);
frc2::CommandPtr mayRunDuringDisabled = frc2::cmd::Run([] { UpdateTelemetry(); }).IgnoringDisable(true);

As a rule, command compositions may run when disabled if all their component commands set runsWhenDisabled as true.

getInterruptionBehavior

The getInterruptionBehavior() method (Java, C++) defines what happens if another command sharing a requirement is scheduled while this one is running. In the default behavior, kCancelSelf, the current command will be canceled and the incoming command will be scheduled successfully. If kCancelIncoming is returned, the incoming command’s scheduling will be aborted and this command will continue running. Note that getInterruptionBehavior only affects resolution of requirement conflicts: all commands can be canceled, regardless of getInterruptionBehavior.

Note

This was previously controlled by the interruptible parameter passed when scheduling a command, and is now a property of the command object.

This property can be set either by overriding the getInterruptionBehavior method in the relevant command class, or by using the withInterruptBehavior() decorator (Java, C++):

Command noninteruptible = Commands.run(intake::activate, intake).withInterruptBehavior(Command.InterruptBehavior.kCancelIncoming);
frc2::CommandPtr noninterruptible = frc2::cmd::Run([&intake] { intake.Activate(); }, {&intake}).WithInterruptBehavior(Command::InterruptBehavior::kCancelIncoming);

As a rule, command compositions are kCancelIncoming if all their components are kCancelIncoming as well.

Included Command Types

The command-based library includes many pre-written command types. Through the use of lambdas, these commands can cover almost all use cases and teams should rarely need to write custom command classes. Many of these commands are provided via static factory functions in the Commands utility class (Java) or in the frc2::cmd namespace defined in the Commands.h header (C++). Classes inheriting from Subsystem also have instance methods that implicitly require this.

Running Actions

The most basic commands are actions the robot takes: setting voltage to a motor, changing a solenoid’s direction, etc. For these commands, which typically consist of a method call or two, the command-based library offers several factories to be construct commands inline with one or more lambdas to be executed.

The runOnce factory, backed by the InstantCommand (Java, C++) class, creates a command that calls a lambda once, and then finishes.

25  /** Grabs the hatch. */
26  public Command grabHatchCommand() {
27    // implicitly require `this`
28    return this.runOnce(() -> m_hatchSolenoid.set(kForward));
29  }
30
31  /** Releases the hatch. */
32  public Command releaseHatchCommand() {
33    // implicitly require `this`
34    return this.runOnce(() -> m_hatchSolenoid.set(kReverse));
35  }
20  /**
21   * Grabs the hatch.
22   */
23  frc2::CommandPtr GrabHatchCommand();
24
25  /**
26   * Releases the hatch.
27   */
28  frc2::CommandPtr ReleaseHatchCommand();
15frc2::CommandPtr HatchSubsystem::GrabHatchCommand() {
16  // implicitly require `this`
17  return this->RunOnce(
18      [this] { m_hatchSolenoid.Set(frc::DoubleSolenoid::kForward); });
19}
20
21frc2::CommandPtr HatchSubsystem::ReleaseHatchCommand() {
22  // implicitly require `this`
23  return this->RunOnce(
24      [this] { m_hatchSolenoid.Set(frc::DoubleSolenoid::kReverse); });
25}

The run factory, backed by the RunCommand (Java, C++) class, creates a command that calls a lambda repeatedly, until interrupted.

// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
new RunCommand(() -> m_robotDrive.arcadeDrive(
    -driverController.getLeftY(),
    driverController.getRightX()),
    m_robotDrive)
// A split-stick arcade command, with forward/backward controlled by the left
// hand, and turning controlled by the right.
frc2::RunCommand(
  [this] {
    m_drive.ArcadeDrive(
        -m_driverController.GetLeftY(),
        m_driverController.GetRightX());
  },
  {&m_drive}))

The startEnd factory, backed by the StartEndCommand (Java, C++) class, calls one lambda when scheduled, and then a second lambda when interrupted.

Commands.startEnd(
    // Start a flywheel spinning at 50% power
    () -> m_shooter.shooterSpeed(0.5),
    // Stop the flywheel at the end of the command
    () -> m_shooter.shooterSpeed(0.0),
    // Requires the shooter subsystem
    m_shooter
)
frc2::cmd::StartEnd(
  // Start a flywheel spinning at 50% power
  [this] { m_shooter.shooterSpeed(0.5); },
  // Stop the flywheel at the end of the command
  [this] { m_shooter.shooterSpeed(0.0); },
  // Requires the shooter subsystem
  {&m_shooter}
)

FunctionalCommand (Java, C++) accepts four lambdas that constitute the four command lifecycle methods: a Runnable/std::function<void()> for each of initialize() and execute(), a BooleanConsumer/std::function<void(bool)> for end(), and a BooleanSupplier/std::function<bool()> for isFinished().

new FunctionalCommand(
    // Reset encoders on command start
    m_robotDrive::resetEncoders,
    // Start driving forward at the start of the command
    () -> m_robotDrive.arcadeDrive(kAutoDriveSpeed, 0),
    // Stop driving at the end of the command
    interrupted -> m_robotDrive.arcadeDrive(0, 0),
    // End the command when the robot's driven distance exceeds the desired value
    () -> m_robotDrive.getAverageEncoderDistance() >= kAutoDriveDistanceInches,
    // Require the drive subsystem
    m_robotDrive
)
frc2::FunctionalCommand(
  // Reset encoders on command start
  [this] { m_drive.ResetEncoders(); },
  // Start driving forward at the start of the command
  [this] { m_drive.ArcadeDrive(ac::kAutoDriveSpeed, 0); },
  // Stop driving at the end of the command
  [this] (bool interrupted) { m_drive.ArcadeDrive(0, 0); },
  // End the command when the robot's driven distance exceeds the desired value
  [this] { return m_drive.GetAverageEncoderDistance() >= kAutoDriveDistanceInches; },
  // Requires the drive subsystem
  {&m_drive}
)

To print a string and ending immediately, the library offers the Commands.print(String)/frc2::cmd::Print(std::string_view) factory, backed by the PrintCommand (Java, C++) subclass of InstantCommand.

Waiting

Waiting for a certain condition to happen or adding a delay can be useful to synchronize between different commands in a command composition or between other robot actions.

To wait and end after a specified period of time elapses, the library offers the Commands.waitSeconds(double)/frc2::cmd::Wait(units::second_t) factory, backed by the WaitCommand (Java, C++) class.

// Ends 5 seconds after being scheduled
new WaitCommand(5.0)
// Ends 5 seconds after being scheduled
frc2::WaitCommand(5.0_s)

To wait until a certain condition becomes true, the library offers the Commands.waitUntil(BooleanSupplier)/frc2::cmd::WaitUntil(std::function<bool()>) factory, backed by the WaitUntilCommand class (Java, C++).

// Ends after m_limitSwitch.get() returns true
new WaitUntilCommand(m_limitSwitch::get)
// Ends after m_limitSwitch.Get() returns true
frc2::WaitUntilCommand([&m_limitSwitch] { return m_limitSwitch.Get(); })

Control Algorithm Commands

There are commands for various control setups:

  • PIDCommand uses a PID controller. For more info, see PIDCommand.

  • TrapezoidProfileCommand tracks a trapezoid motion profile. For more info, see TrapezoidProfileCommand.

  • ProfiledPIDCommand combines PID control with trapezoid motion profiles. For more info, see ProfiledPIDCommand.

  • MecanumControllerCommand (Java, C++) is useful for controlling mecanum drivetrains. See API docs and the MecanumControllerCommand (Java, C++) example project for more info.

  • SwerveControllerCommand (Java, C++) is useful for controlling swerve drivetrains. See API docs and the SwerveControllerCommand (Java, C++) example project for more info.

  • RamseteCommand (Java, C++) is useful for path following with differential drivetrains (“tank drive”). See API docs and the Trajectory Tutorial for more info.

Custom Command Classes

Users may also write custom command classes. As this is significantly more verbose, it’s recommended to use the more concise factories mentioned above.

Note

In the C++ API, a CRTP is used to allow certain Command methods to work with the object ownership model. Users should always extend the CommandHelper class when defining their own command classes, as is shown below.

To write a custom command class, subclass the abstract Command class (Java) or CommandHelper (C++), as seen in the command-based template (Java, C++):

 7import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem;
 8import edu.wpi.first.wpilibj2.command.Command;
 9
10/** An example command that uses an example subsystem. */
11public class ExampleCommand extends Command {
12  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
13  private final ExampleSubsystem m_subsystem;
14
15  /**
16   * Creates a new ExampleCommand.
17   *
18   * @param subsystem The subsystem used by this command.
19   */
20  public ExampleCommand(ExampleSubsystem subsystem) {
21    m_subsystem = subsystem;
22    // Use addRequirements() here to declare subsystem dependencies.
23    addRequirements(subsystem);
24  }
 5#pragma once
 6
 7#include <frc2/command/Command.h>
 8#include <frc2/command/CommandHelper.h>
 9
10#include "subsystems/ExampleSubsystem.h"
11
12/**
13 * An example command that uses an example subsystem.
14 *
15 * <p>Note that this extends CommandHelper, rather extending Command
16 * directly; this is crucially important, or else the decorator functions in
17 * Command will *not* work!
18 */
19class ExampleCommand
20    : public frc2::CommandHelper<frc2::Command, ExampleCommand> {
21 public:
22  /**
23   * Creates a new ExampleCommand.
24   *
25   * @param subsystem The subsystem used by this command.
26   */
27  explicit ExampleCommand(ExampleSubsystem* subsystem);
28
29 private:
30  ExampleSubsystem* m_subsystem;
31};

Simple Command Example

What might a functional command look like in practice? As before, below is a simple command from the HatchBot example project (Java, C++) that uses the HatchSubsystem:

 5package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
 6
 7import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
 8import edu.wpi.first.wpilibj2.command.Command;
 9
10/**
11 * A simple command that grabs a hatch with the {@link HatchSubsystem}. Written explicitly for
12 * pedagogical purposes. Actual code should inline a command this simple with {@link
13 * edu.wpi.first.wpilibj2.command.InstantCommand}.
14 */
15public class GrabHatch extends Command {
16  // The subsystem the command runs on
17  private final HatchSubsystem m_hatchSubsystem;
18
19  public GrabHatch(HatchSubsystem subsystem) {
20    m_hatchSubsystem = subsystem;
21    addRequirements(m_hatchSubsystem);
22  }
23
24  @Override
25  public void initialize() {
26    m_hatchSubsystem.grabHatch();
27  }
28
29  @Override
30  public boolean isFinished() {
31    return true;
32  }
33}
 5#pragma once
 6
 7#include <frc2/command/Command.h>
 8#include <frc2/command/CommandHelper.h>
 9
10#include "subsystems/HatchSubsystem.h"
11
12/**
13 * A simple command that grabs a hatch with the HatchSubsystem.  Written
14 * explicitly for pedagogical purposes.  Actual code should inline a command
15 * this simple with InstantCommand.
16 *
17 * @see InstantCommand
18 */
19class GrabHatch : public frc2::CommandHelper<frc2::Command, GrabHatch> {
20 public:
21  explicit GrabHatch(HatchSubsystem* subsystem);
22
23  void Initialize() override;
24
25  bool IsFinished() override;
26
27 private:
28  HatchSubsystem* m_hatch;
29};
 5#include "commands/GrabHatch.h"
 6
 7GrabHatch::GrabHatch(HatchSubsystem* subsystem) : m_hatch(subsystem) {
 8  AddRequirements(subsystem);
 9}
10
11void GrabHatch::Initialize() {
12  m_hatch->GrabHatch();
13}
14
15bool GrabHatch::IsFinished() {
16  return true;
17}

Notice that the hatch subsystem used by the command is passed into the command through the command’s constructor. This is a pattern called dependency injection, and allows users to avoid declaring their subsystems as global variables. This is widely accepted as a best-practice - the reasoning behind this is discussed in a later section.

Notice also that the above command calls the subsystem method once from initialize, and then immediately ends (as isFinished() simply returns true). This is typical for commands that toggle the states of subsystems, and as such it would be more succinct to write this command using the factories described above.

What about a more complicated case? Below is a drive command, from the same example project:

 5package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
 6
 7import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
 8import edu.wpi.first.wpilibj2.command.Command;
 9import java.util.function.DoubleSupplier;
10
11/**
12 * A command to drive the robot with joystick input (passed in as {@link DoubleSupplier}s). Written
13 * explicitly for pedagogical purposes - actual code should inline a command this simple with {@link
14 * edu.wpi.first.wpilibj2.command.RunCommand}.
15 */
16public class DefaultDrive extends Command {
17  private final DriveSubsystem m_drive;
18  private final DoubleSupplier m_forward;
19  private final DoubleSupplier m_rotation;
20
21  /**
22   * Creates a new DefaultDrive.
23   *
24   * @param subsystem The drive subsystem this command wil run on.
25   * @param forward The control input for driving forwards/backwards
26   * @param rotation The control input for turning
27   */
28  public DefaultDrive(DriveSubsystem subsystem, DoubleSupplier forward, DoubleSupplier rotation) {
29    m_drive = subsystem;
30    m_forward = forward;
31    m_rotation = rotation;
32    addRequirements(m_drive);
33  }
34
35  @Override
36  public void execute() {
37    m_drive.arcadeDrive(m_forward.getAsDouble(), m_rotation.getAsDouble());
38  }
39}
 5#pragma once
 6
 7#include <functional>
 8
 9#include <frc2/command/Command.h>
10#include <frc2/command/CommandHelper.h>
11
12#include "subsystems/DriveSubsystem.h"
13
14/**
15 * A command to drive the robot with joystick input passed in through lambdas.
16 * Written explicitly for pedagogical purposes - actual code should inline a
17 * command this simple with RunCommand.
18 *
19 * @see RunCommand
20 */
21class DefaultDrive : public frc2::CommandHelper<frc2::Command, DefaultDrive> {
22 public:
23  /**
24   * Creates a new DefaultDrive.
25   *
26   * @param subsystem The drive subsystem this command wil run on.
27   * @param forward The control input for driving forwards/backwards
28   * @param rotation The control input for turning
29   */
30  DefaultDrive(DriveSubsystem* subsystem, std::function<double()> forward,
31               std::function<double()> rotation);
32
33  void Execute() override;
34
35 private:
36  DriveSubsystem* m_drive;
37  std::function<double()> m_forward;
38  std::function<double()> m_rotation;
39};
 5#include "commands/DefaultDrive.h"
 6
 7#include <utility>
 8
 9DefaultDrive::DefaultDrive(DriveSubsystem* subsystem,
10                           std::function<double()> forward,
11                           std::function<double()> rotation)
12    : m_drive{subsystem},
13      m_forward{std::move(forward)},
14      m_rotation{std::move(rotation)} {
15  AddRequirements(subsystem);
16}
17
18void DefaultDrive::Execute() {
19  m_drive->ArcadeDrive(m_forward(), m_rotation());
20}

And then usage:

59    // Configure default commands
60    // Set the default drive command to split-stick arcade drive
61    m_robotDrive.setDefaultCommand(
62        // A split-stick arcade command, with forward/backward controlled by the left
63        // hand, and turning controlled by the right.
64        new DefaultDrive(
65            m_robotDrive,
66            () -> -m_driverController.getLeftY(),
67            () -> -m_driverController.getRightX()));
57  // Set up default drive command
58  m_drive.SetDefaultCommand(DefaultDrive(
59      &m_drive, [this] { return -m_driverController.GetLeftY(); },
60      [this] { return -m_driverController.GetRightX(); }));

Notice that this command does not override isFinished(), and thus will never end; this is the norm for commands that are intended to be used as default commands. Once more, this command is rather simple and calls the subsystem method only from one place, and as such, could be more concisely written using factories:

51    // Configure default commands
52    // Set the default drive command to split-stick arcade drive
53    m_robotDrive.setDefaultCommand(
54        // A split-stick arcade command, with forward/backward controlled by the left
55        // hand, and turning controlled by the right.
56        Commands.run(
57            () ->
58                m_robotDrive.arcadeDrive(
59                    -m_driverController.getLeftY(), -m_driverController.getRightX()),
60            m_robotDrive));
52  // Set up default drive command
53  m_drive.SetDefaultCommand(frc2::cmd::Run(
54      [this] {
55        m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
56                            -m_driverController.GetRightX());
57      },
58      {&m_drive}));