Commands

Now that we have a DriveSubsystem to control the motors we are going to want to do something with it. In this programming model we use commands to control actions. In this chapter we are going to create a command which will cause the robot to drive forward for a specific amount of time.

You can think of commands as being a little like the following loop:

In fact, it’s more complicated, because these calls will be interleaved with other periodic commands, but that’s the basic idea. We do things this way to allow many such loops to run at the same time.

Like with DriveSubsystem, we need to:

  • Create the new class called DriveForTimeCommand under the commands folder. Like we did with the DriveSubsystem, copy the ExampleCommand and paste it into the commands folder.
  • Change the name of the copy to DriveForTimeCommand.java.
  •  In that file, search and replace all instances of ExampleCommand with DriveForTimeCommand.
  • Also, since this command will be using the DriveSubsystem, replace all instances of ExampleSubsystem with DriveSubsystem.
  • Change the comment describing the purpose of this command (i.e. “An example command that uses an example subsystem”) to one that describes this command.

After you have done that you DriveForTimeCommand.java file should look like:

Let’s take a look at the constructor:

The first thing to do is save a copy of the incoming parameter “subsystem” (a DriveSubsystem object) to a member variable, “m_subsystem”.  This let’s us access it later.

Second, it is here that we will specify which Subsystems this command will require. We do this by calling the addRequirements(…) passing in the DriveSubsystem instance we just saved. It is important we do this because this is how the command system keeps track of which commands need which subsystems and makes sure that only one command at a time has access to a particular subsystem. If we get this wrong, we might end up with mysterious problems where two commands are fighting for control of the same motor.

The next function initialize() is called whenever this command starts. In our case, when the command runs we want to start the robot moving forward. Also we are going to want to have a timer to control how long the robot will drive and we will need to reset that timer when the command starts.

We need to:

  • Create a Timer object, m_timer
  • Import the Timer class (we could do this first, but VS helps us import it if we create it first).
  • Add timer commands to initialize().

First, declare and initialize the variable m_timer at the top of our class.

Java doesn’t know where to get the Timer class, so you need to import it. Click on Timer and press CTRL + ., you will see multiple possible choices. We want to choose the version from edu.wpi.first.wpilibj:

Finally, add the timer commands to initialize(). The commands are reset() to set the timer back to zero, and start() to start it for this run.

The next function in our class, execute(), is called repeatedly as long as this command is running. Since we are simply going to continue driving until the timer expires, we don’t need to do anything special here.

Next there is the end() function which is called when a command ends. When this happens we want to turn the motors off:

Finally there is the isFinished() function. This function is also called repeatedly as long as the command is running, and should return false if it wants the command to continue, and true if it wants it to end. In our case, we want the command to end when the timer exceeds (or equals) 2 seconds:

Your DriveForTimeCommand.java file should now look like:

Now we need to provide some way to run this command and we will do this by tying this command to a button on the joystick (or for now, an emulated joystick). The place to do this is in the RobotContainer class that we mentioned previously. Be sure to save your DriveForTimeCommand.java file and then open the RobotContainer.java file by clicking on it in the left pane.

Since we will be using the joystick, we need to create an instance of the Joystick class. We are also going to need an instance of the DriveSubsystem so we declare that here as well. We do this by adding the following to the RobotContainer class:

The declaration of m_driveSubsystem creates an instance of our DriveSubsystem class.

The declaration of m_joystick creates an instance CommandJoystick number 0. Our implementation of the CommandJoystick class really only supports a single joystick, but the joystick number is included in the constructor to be consistent with the FRC library which supports multiple joysticks.

Note that we need to import the packages for these new objects. Do this just like we did earlier: Hover over the class name, then select Quick Fix. Do this for both DriveSubsystem and CommandJoystick. The latter looks like this:

…and the import lines…

Now we need to connect a button to our DriveForTimeCommand. We will do this in the configureButtonBindings() function of the RobotContainer:

Here we create a instance of our DriveForTimeCommand, passing in our instance of the DriveSubsystem, and tell the CommandJoystick button(1) object to call that instance when the button is pressed. Make sure to get the import line for it.

This is an example of event-driven programming. We’re not checking the state of the button and then doing something; instead we’re defining something that should happen whenever the button is pressed.

Your RobotContainer.java file should now look like:

Now run your program (remember that if your program is already running, you need to stop it using the red stop button  ) and connect and enable your robot from the Driver Station. Since you don’t have an actual joystick attached to your computer, click the Enable Virtual Joystick checkbox which will allow you to use a simulated joystick.

With your robot enabled, click the B1 button and your robot should drive forward for 2 seconds (make sure you put your robot on the floor!)

Before we move on, let’s try and make this command more useful. Right now it is always driving the robot at 75% power, and always for 2 seconds. We would like to change this command so that we can control both the time and the speed.

The place to set these values is when we call the DriveForTimeCommand constructor. We add two parameters, power and time to the constructor and add two member variables m_power and m_time to store these values:

(Note that good programming practice avoids putting constants in your code. So creating variables for these, with meaningful variable names and comments, makes for better code.)

Now in our initialize() function we need to run the robot at the specified m_power:

Finally, we need to change our isFinished() function to use the specified m_time:

Your DriveForTimeCommand.java file should now look like:

You will no notice that your RobotContainer class now has an error on the line where we create an instance of DriveForTimeCommand. Hovering over the error marker it tells us:

The reason it is telling us this is that there is no longer a constructor for this class that takes only one argument (remember that we changed the constructor). So what we must do is provide the proper arguments for the updated constructor. Lets have our robot drive forward at 0.50 power for 3 seconds:

Now deploy and run your program. Verify the robot now drives at half power for three seconds.

You might notice that your robot is not driving quite straight but don’t worry, we will address this issue later.

Next: Arcade Drive