Tutorial 04: How commands and subsystems really work
This is probably the most important page on this Wiki. Every time you want to add any capabilities to the robot, you need to think about this stuff. Misunderstanding how WPILib handles commands is a very common source of bugs and frustration. If you're ever in a situation where multiple commands aren't stopping/starting when you expect, two command are interacting in a strange way, or your autonomous mode refuses to do what you want, come back to this page and think carefully about what's happening.
If anything here is not clear, please tell me. This is important, so I will re-write it until it is.
As we've seen in the previous tutorial, Commands let the robot perform
multiple tasks simultaneously (controlling a lifter and driving). I
encourage you to think about how you could code that if I just handed
you a main() method to fill in.
One solutoin would be to have a big loop, where you check every possible thing the robot might need to do, and work a bit on it. For example:
while the robot is enabled:
if the drive button is down:
set the drive motors according to joysticks
else:
stop the drive motors
if the liftUp button is down:
set the lifter to go up
else if the liftDown button is down:
set the lifter to go down
else:
stop the lifter
... and so on, for every possible thing the robot might do ...
So far, our robot only does a few simple things, so this isn't unreasonable. However, this list could quickly get hard to maintain, as the list of our robot's cababilities grows.
(Aside: If you know what a thread is, you might wonder if the solution is to run independent parts of the code on different threads. If you know a bit more about threads, you know that communicating between threads can be a messy business. While there are a few places where threads get used in WPILib/HYPERLib, we do not use threads for running multiple commands simultaneously. All commands run on a single thread.)
Here's an idea: maintain a list of tasks we're currently running. Then we just repeatedly loop through the list, and do a little bit of work on each one. We also need to check periodically if we need to add any new tasks (e.g. because a button got pressed) or if we need to remove any old tasks (either because it finished, or because some other "conflicting" command kicked it off, as we'll discuss later).
In order to make this work, we need a class for "task". This class needs to have a method for "do a little bit of work", and it also needs a method to check if it's done. To code a specific task, we should be able to extend this class, and override these methods to define what "doing a little bit of work" means, and how we check if we're done.
WPILib has such a class, and it is called Command. It looks
something like this:
public abstract class Command {
// Do a little bit of work (by default: do nothing)
protected void execute() {};
// Check if we're done
protected abstract boolean isFinished();
// Lots of other stuff that doesn't matter for now.
}(If you're not familiar with the abstract keyword: it just means
that there is no implementation for isFinished by default. It's
impossible to make a generic command with new Command(). You have
to extend it, and override isFinished.)
For every action you want the robot to take, you write a subclass of
Command. WPILib maintains a list of commands, just as I described.
(For the details, check out the source of the Scheduler class in
WPILib. The HYPERRobot class calls Scheduler.run() periodically,
which in turn runs all execute() on all the commands in the list,
and updates the list if needed.) If you want to add a command to this
list manually, you can use the start() method in the Command
class. Usually though, you want to set a command to run either as a
default command (using setDefaultCommand in a subsystem, see
below) or when a button is pressed/released/held. From the previous
tutorials, you should already know how to do this.
In the "old days" (2015 and before), this is exactly what we would do.
For every command we wanted to code, we would define a new subclass of
Command. The problem is, you get stuff like this:
You read that right. This is after deleting files. Most of these
commands boil down to one line of code each for execute and
isFinished.
What you've been doing so far is to use the QuickCommand.continuous
function. Its signature looks like this:
public Command continuous(Subsystem req, Runnable body);The first argument is the subsystem the command requires, which we
will discuss when we get to subsystems. The second argument is a
Runnable, representing what to do in the execute() method.
(Runnable is an interface with just one method: void run(). This
is also the type of an expression () -> { /* your code here */ }.)
This method returns a command which runs body.run() in the
execute() method, and always returns false for isFinished().
That is, a command that runs continuously, until it needs to stop for
some other external reason (e.g. it was bound to WhileHeld, and you
released the button).
There is also QuickCommand.oneShot(), which is similar, except it
runs the body only once, and finishes immediately. This isn't as
useful for things like motors, where you want to keep updating the
power as frequently as possible, but it does have other uses. For
example, if we want pressing a button to update some variable, we
could use oneShot.
Stop and think carefully about what's happening here, and when each
bit of code is getting called. In the examples, the functions
driveCmd(), stopCmd(), etc. all get called when the program
starts up. They return commands, which get scheduled at some later
time, depending on where they get passed. Try this, and look at the
console on the driver station:
public Command driveCmd() {
DriverStation.reportError("driveCmd() called", false);
return QuickCommand.continuous(this, () -> {
DriverStation.reportError("Now I'm actually driving", false);
robotDrive.tankDrive(Robot.oi.leftDriver(), Robot.oi.rightDriver());
});
}Why write functions to return commands? Why not just declare commands as constants or variables? Let's suppose we stuck an encoder on our lifter, and we read ahead and got a PID loop working. We want to write a command that can move the lifter to a given position. Should we write a new command for each position we could move to? We don't have to:
public static double TOP = 0.0;
public static double BOTTOM = 100.0;
public Command moveToLevel(double level) {
return QuickCommand.continuous(this, () -> {
// move lifter to level
});
}Now, we can call moveToLevel() multiple times with different inputs,
and it will give us back multiple Command objects. For example:
@WhenPressed(3) public final Command lifterToTop = Robot.lifter.moveToLevel(Lifter.TOP);
@WhenPressed(4) public final Command lifterToBot = Robot.lifter.moveToLevel(Lifter.Bottom);Remember, these function calls are happening at program startup. The information of what level to move to is saved in the Command object.
When you first made two commands for the drive train, driveCmd() and
stopCmd(), running driveCmd() would stop stopCmd(). When
driveCmd() was done, stopCmd() started back up again. A similar
thing happened for the lifter commands. However, moving the lifter
didn't stop us from driving. This is what we want: it makes no sense
to move the lifter up and down at the same time, but it really would
be nice if unrelated parts of the robot could operate simultaneously.
For our purposes, this is definition of a subsystem. A subsystem is the smallest collection of parts that it makes sense to operate independently. This is likely to differ from what mechanical calls a subsystem. Usually, your subsystems will be much smaller. A few examlpes from previous years are listed below.
WPILib is designed so that commands can specify that they require one or more subsystems. When a new command is scheduled, WPILib checks if it requires a subsystem that is currently in use. If it does, the previous command using it gets kicked off before the new one can start.
If no command is currently using a given subsystem, WPILib checks for a default command. If there is one, it runs that. This is usually something like "stop all motors", but it very well could be something more advanced. For example, if our lifter isn't strong enough to stay in place by itself, we might want the default command to be to hold it in place using a PID controller.
The commands we've coded all require a single subsystem, which also
happened to correspond to the file they were declared in. This is the
most common case. In the QuickCommand.continuous function, the
first argument is the subsystem to require. In all of our cases, we
have passed this.
You should declare simple commands inside of their subsystems, and
make more complicated commands out of simple ones (using
CommandBuilder, something for another tutorial). This way, you can
declare the internals of a subsystem as private, and return commands
in public methods. Then you know that at any given time, exactly
one of the commands in that file will be running, and no other code
will modify the subsystem.
If your simplest command really cannot use just one subsystem,
consider merging the subsystems together. If you really want to run
two commands at once, but can't because they're part of the same
subsystem, consider splitting the subsystem up. If you've considered
both of these and have a really really good reason not to do either,
subclass Command directly, and you can do whatever you want. Call
requires() in the constructor to specify which subsystems (if any)
it requires.
These examples hopefully help illustrate how important and useful it is to keep subsystems and commands small, and build more complex commands from simple ones.
- In the 2015 game (Recycle Rush), our initial bot design had a very complicated tote lifter, which had a moving plate, two latches controlled by pistons, and then two additional pistons to help support the tote. There were also sensors to detect if we had successfully latched onto a tote. To operate, all the parts had to move in the right sequence, or we could possibly damage the robot. Thus, it was a good idea to make the entire lifter a single subsystem.
- In the same year, we had a container lifter (containers are different from totes!) that used suction cups. There were two valves: one to create a vacuum, and another to seal it, once a vacuum was established. To hold a container, we had to toggle these on and off, depending on the value of a sensor. Meanwhile, the operator needs to be able to move containers around. We made the mistake of putting these all on one subsystem, so we needed an ugly hack written at competition time to get things to work right. In a redesign, we split it into three subsystems: the lifter, the tilt control, and the vacuum control.
- In the 2016 game (Stronghold), we had a shooter that could tilt up and down. Learning from the previous year, we split this into three subsystems as well: the tilt, the wheels, and the trigger. This way, we could hold a particular tilt using PID, and spin the wheels up to a fixed speed with PID again, and when everything was ready, we could fire the trigger, without interrupting anything. The trigger consisted of two servo motors, one to hold the ball back, and the other to push. Since these always worked in tandem, they were kept together. Similarly, the shooter itself was two sets of wheels, which we always wanted to sync to the same speed, so we kept those together.
- In the 2017 game (Steamworks), we had another shooter, which consisted of a high speed wheel, as well as a feeder (the "flippidy-dippider") that would push balls into the shooter. As you could guess, these were two seperate subsystems, to let us spin up the wheel without shooting. Even though they were seperate subsystems, the command to feed balls would still periodically check that the wheel was up to speed, and that the vision tracking was on-target, so we wouldn't waste any balls if we knew we would miss. Remember, a command in one subsystem can still access public methods from other subsystems, in this case to read sensors.