Intro to CBP_Commands

Disclaimer: The majority of text content on this page is pulled almost verbatim from WPILib documentation, with light edits, and the examples have been reworked to correspond to their Taproot equivalents.

Commands

Commands are simple state machines that perform high-level robot functions using the methods defined by subsystems. Commands can be either idle, in which they do nothing, or scheduled, in which the scheduler will execute a specific set of the command’s code depending on the state of the command. The CommandScheduler recognizes scheduled commands as being in one of three states: initializing, executing, or ending. Commands specify what is done in each of these states through the initialize(), execute() and end() methods. Commands are represented in Taproot by the Command interface.

Creating Commands

Similarly to subsystems, the recommended method for most users to create a command is to subclass the abstract Command class, as seen in the command-based template:

#include "tap/control/command.hpp"

class MyCommand: public tap::control::Command
{
public:

    MyCommand()

    ~MyCommand() = default;

    /**
     * Called once when the subsystem is added to the scheduler.
     */
    void initialize() override;

    /**
     * Returns the command name. Used by the CommandScheduler and for debugging purposes.
     */
    const char *getName() const { return "Command Name"; }

    /**
     * Will be called periodically whenever the CommandScheduler runs.
     */
    void execute() override;

    /**
     * Will be called once when IsFinished() returns true or the command is interrupted
     */
    void end(bool) override;

    /**
     * Called periodically whenever the CommandScheduler runs. If it returns true, the end() method is called and the
     * command is removed from the CommandScheduler.
     */
    bool isFinished() const override;

};

The Structure of a Command

While subsystems are fairly freeform, and may generally look like whatever the user wishes them to, commands are quite a bit more constrained. Command code must 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.

Initialization

The initialize() method is run exactly once per time a command is scheduled, as part of the scheduler’s addCommand() method. The scheduler’s run() method does not need to be called for the initialize() method to run. The initialize block should be used to place the command in a known starting state for execution. 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 is called repeatedly while the command is scheduled, whenever the scheduler’s run() method is called (this is done in the main robot periodic method). 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() method 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.

Specifying end conditions

The isFinished() method 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 is un-scheduled. The isFinished() method is called after the execute() method, so the command will execute once on the same iteration that it is un-scheduled.

Simple command example

Below is an example of a command that sets the feeder motor to turn at a constant speed. In our case, the command will be mapped to a switch on the remote, and removed when the switch is no longer in the correct position.

#include "feeder_feed_command.hpp"
#include "feeder_constants.hpp"

#include "tap/algorithms/math_user_utils.hpp"
#include "tap/errors/create_errors.hpp"

namespace control
{
namespace feeder
{
FeederFeedCommand::FeederFeedCommand(
    FeederSubsystem *const feeder,
    src::Drivers *drivers)
    : feeder(feeder),
      drivers(drivers)
{
    if (feeder == nullptr)
    {
        return;
    }
    this->addSubsystemRequirement(dynamic_cast<tap::control::Subsystem *>(feeder));
}

void  FeederFeedCommand::initialize() { feeder->setDesiredOutput(FEEDER_RPM); }

void  FeederFeedCommand::execute() {}

void  FeederFeedCommand::end(bool) { feeder->setDesiredOutput(0); }

bool  FeederFeedCommand::isFinished() const { return false; }
}  // namespace feeder
}  // namespace control

When the command is added to the command scheduler, the initialize() method is called and the feeder is set to a the desired RPM. When the command ends, the end() method is called and the feeder rotation is set back to zero.

Notice that the feeder subsystem used by the command (as well as the drivers object) 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.