Command Subsystem Framework - MatthewMArnold/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 they are doing it. Commands and subsystems are the two abstractions that make command-based programming possible.

Subsystem

A subsystem can be thought of as an organizational unit that encapsulates robot hardware into an interface that can be used. One example of a subsytem would be a wrist mechanism. 'WristSubsystem' would contain the code necessary for controlling the wrist motors. The subsystem would provide a convenient API to control the motors. This level of encapsulation allows developers to easily modify and debug one portion of the robot code without impacting other parts.

A simple block diagram of a wrist mechanism's subsystem is illustrated below.

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

API details

In addition to the description above, the Subsystem class has a number of other features to be aware of.

  • A virtual initialize function is called once when a subsystem is added to the command scheduler (see below)
  • A default command may be attached to the subsystem via setDefaultCommand, which will automatically be scheduled whenever the subsystem is not being used by any other command.
  • A virtual refresh function is called once each time the main command scheduler runs. This function can be used to run control loops or poll related I/O devices that the subsystem owns.

Command

A command defines an action that the robot should perform. To interact with the robot, the command will request access to an active subsystem and tell it what to do. Alternatively, a comprised command contains multiple commands and will attempt to connect to multiple active subsystems, scheduling the commands that it is comprised of in some fashion. An example command would be a "move wrist to grabbing position" command, which tells the wrist subsystem to rotate the wrist into a grabbing position.

A simple block diagram of a wrist mechanism's "extend wrist command" is illustrated below.

API Details

In addition to the description above, the command class has a number of other features to be aware of.

  • An addSubsystemRequirement function allows you to specify which subsystem the command requires. Multiple subsystems may be added via this function, and getRequirements and hasRequirement functions serve as getters used by the command scheduler to check subsystem requirements when adding a command.
  • A pure virtual getName function may be used when debugging to provide a command identifier.
  • A pure virtual initialize function is called once when the command is initially scheduled by a command scheduler.
  • A pure virtual execute function is called repeatedly while the command is scheduled by a command scheduler at the same frequency as the subsystem's refresh function.
  • A pure virtual end function is called when the command ends. It is called either when the command finishes normally, or if it has been interrupted by another command being added to the scheduler.
  • A pure virtual isFinished function is used by the scheduler to determine if a command has finished running.

Command Scheduler

The command scheduler is the central location where commands and subsystems are scheduled and run. 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 scheduled. This scheduler also removes commands when they are done being run. The command scheduler's run function is where all subsystems/commands are refreshed and updated. 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, which uses it to internally schedule the commands it is comprised of.

Below is a flow diagram of the command scheduler's run function. The scheduler is comprised of a map of subsystems to commands.

The other large portion of the command scheduler's job is deciding if a command scheduler can be added, outside of just adding default commands. Below is another flow diagram, this time of the command scheduler's addCommand function.

Command Mapper and Control Operator Interface

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 is met, 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.

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 it does not make sense for a command to be either running or not. 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 API documentation on the framework itself, see the aruwlib::control API documentation or for comments in code, check out aruw-mcb-project/src/aruwilb/control/