Command Subsystem Framework - uw-advanced-robotics/taproot GitHub Wiki

The command/subsystem framework follows the design of command-based programming. The key idea surrounding this design is that one should focus on what the program should be doing instead of how it is being done. Commands and subsystems are the two abstractions that make command-based programming possible.

A subsystem is a core organizational unit that encapsulates a group of related inputs and/or outputs. One example of a subsystem would be a wrist mechanism. WristSubsystem would contain the code necessary for controlling the wrist motors. The APIs that a Subsystem exposes are intended to describe the meaningful behaviors of the robot component: think "open the claw", not "activate piston 4". Often, one externally-visible behavior of a subsystem is actually many internal steps. This level of encapsulation allows developers to easily modify and debug one portion of the robot code without impacting other parts.

A simple class diagram showing the basic, packaged functionality of the WristSubsystem is shown below.

classDiagram
    class WristSubsystem {
        DjiMotor wristMotor
        Pid wristPositionPid
        LimitSwitch recalibrationLimitSwitch
        ExtendWrist()
        RetractWrist()
        CalibrateWrist()
    }

The top section of the diagram illustrates what this subsystem encapsulates (internal variables), while the bottom section illustrates the public API (methods) that a wrist subsystem may have for a command to interact with.

The description above was only an overview and not a complete description of the Subsystem's functionality. I highly advise you to read the Sphinx documentation to fully understand how the Subsystem works.

A command defines an action that the robot should perform. The idea is that command-based programming should allow one writing a Command class to focus on what hardware should do instead of how. While the subsystem takes care of how a robot should accomplish some goal, at the command level we only care about requesting that the robot to do some task.

To interact with the robot, the command will request access to an "active" subsystem (more on this below) and tell it what to do. Building on the example of the WristSubsystem described in the above section, a command to "move the wrist to a grabbing position" would be responsible for calling the subsystem's ExtendWrist() function when appropriate.

Note that command instances are re-used: a single command could be initialized, run, finished, then later initialized again. Ensure that initialize() resets any state stored in the command!

In addition to the description above, the Command class has a number of other features to be aware of. For details about the complete functionality of this class, refer to our Sphinx documentation

The command scheduler is the central entity in charge of scheduling and running commands.

The scheduler ensures that commands are not attempting to use the same subsystem, which would lead to undefined behavior. No two commands that require the same subsystem will ever be simultaneously scheduled. This scheduler also removes commands when they report completion. The command scheduler's run function is where all subsystems/commands are refreshed and updated. In our codebase, the singleton command scheduler's run function is called at a frequency of 500 Hz.

Note: A singleton command scheduler is declared in the Drivers class. A separate command scheduler is used in each comprised command object (read more on this below).

The other large portion of the command scheduler's job is deciding if a command can be added, outside of adding default commands. The following considerations must be made while attempting to add a command:

• The scheduler must currently have registered every Subsystem in the Command's list of Subsystem requirements.
• After the addition of a Command to the scheduler, all other commands that remain in the scheduler must have disjoint subsystem requirement sets. This means that if a command in the scheduler shares some subsystem with the command to be added, that command should be completely removed from the scheduler during the addition of the new command.

In addition to the description above, the CommandScheduler class has a number of other features to be aware of. For details about the complete functionality of this class, refer to our Sphinx documentation

The command mapper is used to schedule commands based on the state of the remote. A remote mapping and associated command can be added to the mapper. When the remote mapping's preconditions are met (e.g., particular buttons are pressed), the command is scheduled.

Currently the following types of remote maps are supported:

• Press mappings: The command associated with the mapping is added exactly once when the remote's state matches the mapped state.
• Hold mappings: The command associated with the mapping is added once when the remote's state matches the mapped state and removed when the state no longer matches.
• Hold repeat mappings: The command associated with the mapping is added when the remote's state matches the mapped state and is added again every time the command ends in its own. The command is removed when the remote mapping no longer matches the mapping.
• Toggle mappings: The command associated with the mapping is added when the state matches the correct state and removed the next time you re-enter the state.

The command mapper is designed such that when a command is mapped, instead of creating a new command and then deleting it when it is finished running, the same command is re-used when the remote mapping is met multiple times to avoid dynamic allocation. It is therefore very important that state is properly reset in every Command's initialize and end functions.

For safety reasons, the command mapper will only allow you to use an instance of a command in a single mapping object. If you have multiple inputs which you'd like to map the same command, create two different instances (separate variables) and use one in each mapping.

Some concrete examples of remote mappings are as follows:

• A press mapping is met and a command scheduled when the left switch is in the up position. When the left switch is no longer in the up position, the command is removed from the scheduler.
• A hold repeat mapping is met and a command scheduled when the A and Shift keys are pressed. As long as this combination of keys is still pressed, whenever the command naturally finishes, the command mapper reschedules the mapping. Once the A and Shift keys are no longer pressed, the command is removed from the scheduler.

In addition to the description above, you can find specific details about how to add mapping correctly to the CommandMapper via the Sphinx documentation

The control operator interface is an interface used to interpret remote stick and key values to be used by commands. This is useful for cases where commands need to accept user input in addition to the scheduler's start/stop command mappings. A chassis command, for example, could be running continuously and then interact with the control operator interface to receive remote input to tell the chassis to move.

For more details about the ControlOperatorInterface class, see our Doxygen documentation.

The comprised command is a layer built on top of the Command class. The key idea is that a comprised command is an encapsulation of multiple commands. Interacting with multiple commands can be done easily because each comprised command has access to its own unique command scheduler that it may use to add/remove instances of the commands that it uses. As a very small example, take the following pair of subsystems and associated commands.

classDiagram
    class WristSubsystem {
        DjiMotor wristMotor
        Pid wristPositionPid
        LimitSwitch recalibrationLimitSwitch
        ExtendWrist()
        RetractWrist()
        CalibrateWrist()
    }
    class GrabberSubsystem {
        Solenoid pneumaticJawSolenoid
        Grab()
        Release()
    }
    class ExtendWristCommand {
        calls_RetractWrist
    }
    class GrabBinCommand {
        calls_Grab
    }
    Subsystem <|-- WristSubsystem
    Subsystem <|-- GrabberSubsystem
    Command <|-- ExtendWristCommand
    Command <|-- GrabBinCommand

Now suppose we want to be able to command the wrist to extend and then have the grabber grab a bin once the wrist is finished extending. To give you some idea of what the command should do, refer to the clip below:

One option is to create a command (not a comprised command) that handles the logic for interacting with the wrist and grabber subsystem directly. While this would work, it would mean we now have duplicated code that directly interacts with the wrist and grabber subsystems. In this example, since the subsystem API is very simple, a case could be made to directly interact with them; however, doing so becomes unmaintainable when working with more complex commands and subsystems and when the sheer volume of subsystems and commands increases.

Instead, one can create a comprised command that has instances of the grab bin and extend wrist commands. The ComprisedCommand is purely in charge of sequencing its child commands: first it runs the "extend" command, and once that has finished, it runs the "grab" command. ComprisedCommands are often structured like state machines, where it progresses from one "state" to another as commands terminate.

We want to grab directly following wrist extension. We can use an instance of the ExtendWristCommand and an instance of a GrabBinCommand in the comprised command to accomplish this goal. A partial example is shown below for what the command's initialize and refresh functions might look like. In this example, the comprised command is named ExtendAndGrabCommand, the ExtendWristCommand instance named extendWrist, and the GrabBinCommand instance named grabBin.

void ExtendAndGrabCommand::initialize()
{
    prevExtendWristFinished = false;
    comprisedCommandScheduler.addCommand(&extendWrist);
}

void ExtendAndGrabCommand::refresh()
{
    if (extendWrist.isFinished() && !prevExtendWristFinished)
    {
        prevExtendWristFinished = true;
        comprisedCommandScheduler.addCommand(&grabBin);
    }
    comprisedCommandScheduler.run();
}

This means if the API for any subsystem ever changes, only the first level of commands that interact directly with the subsystem will have to change, and all the comprised commands built on top of the base commands can stay the same. This also allows us a convenient way to create "macros". If we want a single key press to set in motion a number of complex robot events that span multiple subsystems, using a comprised command is usually the way to go.

For an alternative explanation of the ComprisedCommand class, see the Doxygen documentation.