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

The first step is 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. Then change the name of the copy to DriveForTimeCommand.java and 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. Also you should 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:

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package robot.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import robot.subsystems.DriveSubsystem;
import robotCore.Logger;

/**
 * This command will drive the robot forward for a specified period of time
 */
public class DriveForTimeCommand extends CommandBase {
  private final DriveSubsystem m_subsystem;

  /**
   * Creates a new DriveForTimeCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public DriveForTimeCommand(DriveSubsystem subsystem) {
    Logger.Log("DriveForTimeCommand", 3, "DriveForTimeCommand()");

    m_subsystem = subsystem;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(m_subsystem);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    Logger.Log("DriveForTimeCommand", 2, "initialize()");
  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    Logger.Log("DriveForTimeCommand", -1, "execute()");
  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
    Logger.Log("DriveForTimeCommand", 2, String.format("end(%b)", interrupted));
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    Logger.Log("DriveForTimeCommand", -1, "isFinished()");
    return false;
  }
}

Let’s take a look at the constructor:

  public DriveForTimeCommand(DriveSubsystem subsystem) {
    Logger.Log("DriveForTimeCommand", 3, "DriveForTimeCommand()");

    m_subsystem = subsystem;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(m_subsystem);
  }

The first thing we need to do is to save a copy of the DriveSubsystem instance that we are passing into a member variable so that we have access to 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 this 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.

  public void initialize() {
    Logger.Log("DriveForTimeCommand", 2, "initialize()");

    m_subsystem.setPower(0.5, 0.5);
    m_timer.reset();
  }

Of course we must also declare and initialize the variable m_timer at the top of our class.

  private Timer m_timer = new Timer();

Now you will also need to import the Timer class. This time when you click on Timer and press CTRL + ., you will see multiple possible choices. We want to choose the version from RobotCore:

ImportTimer

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:

  public void end(boolean interrupted) {
    Logger.Log("DriveForTimeCommand", 2, String.format("end(%b)", interrupted));

    m_subsystem.setPower(0, 0);
  }

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 2 seconds:

  public boolean isFinished() {
    Logger.Log("DriveForTimeCommand", -1, "isFinished()");

    return(m_timer.get() >= 2);
  }

Your DriveForTimeCommand.java file should now look like:

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package robot.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import robot.subsystems.DriveSubsystem;
import robotCore.Logger;
import robotCore.Timer;

/**
 * This command will drive the robot forward for a specified period of time
 */
public class DriveForTimeCommand extends CommandBase {
  private final DriveSubsystem m_subsystem;
  private Timer m_timer = new Timer();

  /**
   * Creates a new DriveForTimeCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public DriveForTimeCommand(DriveSubsystem subsystem) {
    Logger.Log("DriveForTimeCommand", 3, "DriveForTimeCommand()");

    m_subsystem = subsystem;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(m_subsystem);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    Logger.Log("DriveForTimeCommand", 2, "initialize()");

    m_subsystem.setPower(0.5, 0.5);
    m_timer.reset();
  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    Logger.Log("DriveForTimeCommand", -1, "execute()");
  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
    Logger.Log("DriveForTimeCommand", 2, String.format("end(%b)", interrupted));

    m_subsystem.setPower(0, 0);
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    Logger.Log("DriveForTimeCommand", -1, "isFinished()");

    return(m_timer.get() >= 2);
  }
}

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. 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 as well as an instance of the JoystickButton 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:

public class RobotContainer {
  // The robot's subsystems and commands are defined here...
  @SuppressWarnings("unused")
  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
  private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
  private final Joystick m_joystick = new Joystick(0);
  private final JoystickButton m_button1 = new JoystickButton(m_joystick, 1);

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

The declaration of m_joystick creates an instance Joystick number 0. Our implementation of the Joystick 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.

The declaration of m_button1 creates an instance of JoystickButton which ties it to the number 1 button on the joystick.

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

  private void configureButtonBindings() {
    m_button1.whenPressed(new DriveForTimeCommand(m_driveSubsystem));
  }

Here we create a instance of our DriveForTimeCommand, passing in our instance of the DriveSubsystem, and tell the JoystickButton class to call that instance when the button is pressed.

Your RobotContainer.java file should now look like:

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package robot;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import robot.commands.DriveForTimeCommand;
import robot.commands.ExampleCommand;
import robot.subsystems.DriveSubsystem;
import robot.subsystems.ExampleSubsystem;
import robotCore.Joystick;

/**
 * This class is where the bulk of the robot should be declared. Since
 * Command-based is a "declarative" paradigm, very little robot logic should
 * actually be handled in the {@link Robot} periodic methods (other than the
 * scheduler calls). Instead, the structure of the robot (including subsystems,
 * commands, and button mappings) should be declared here.
 */
public class RobotContainer {
  // The robot's subsystems and commands are defined here...
  @SuppressWarnings("unused")
  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
  private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
  private final Joystick m_joystick = new Joystick(0);
  private final JoystickButton m_button1 = new JoystickButton(m_joystick, 1);

  private final ExampleCommand m_autoCommand = null; // new ExampleCommand(m_exampleSubsystem);

  /**
   * The container for the robot. Contains subsystems, OI devices, and commands.
   */
  public RobotContainer() {
    // Configure the button bindings
    configureButtonBindings();
  }

  /**
   * Use this method to define your button->command mappings. Buttons can be
   * created by instantiating a {@link GenericHID} or one of its subclasses
   * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
   * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
   */
  private void configureButtonBindings() {
    m_button1.whenPressed(new DriveForTimeCommand(m_driveSubsystem));
  }

  /**
   * Use this to pass the autonomous command to the main {@link Robot} class.
   *
   * @return the command to run in autonomous
   */
  public Command getAutonomousCommand() {
    // An ExampleCommand will run in autonomous
    return m_autoCommand;
  }
}

Now run your program (remember that if your program is already running, you need to stop it using the red stop button  Stop) 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

DriverStationJoystick

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 half speed, 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 constructor for the class. We add two parameters power and time to the constructor and add two member variables m_power and m_time to store these values:

public class DriveForTimeCommand extends CommandBase {
  private final DriveSubsystem m_subsystem;
  private Timer m_timer = new Timer();
  private double m_power;
  private double m_time;

  /**
   * Creates a new DriveForTimeCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public DriveForTimeCommand(DriveSubsystem subsystem, double power, double time) {
    Logger.Log("DriveForTimeCommand", 3, "DriveForTimeCommand()");

    m_subsystem = subsystem;
    m_power = power;
    m_time = time;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(m_subsystem);
  }

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

  public void initialize() {
    Logger.Log("DriveForTimeCommand", 2, "initialize()");

    m_subsystem.setPower(m_power, m_power);
    m_timer.reset();
  }

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

  public boolean isFinished() {
    Logger.Log("DriveForTimeCommand", -1, "isFinished()");

    return(m_timer.get() >= m_time);
  }

Your DriveForTimeCommand.java file should now look like:

/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package robot.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import robot.subsystems.DriveSubsystem;
import robotCore.Logger;
import robotCore.Timer;

/**
 * An example command that uses an example subsystem.
 */
public class DriveForTimeCommand extends CommandBase {
  private final DriveSubsystem m_subsystem;
  private Timer m_timer = new Timer();
  private double m_power;
  private double m_time;

  /**
   * Creates a new DriveForTimeCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public DriveForTimeCommand(DriveSubsystem subsystem, double power, double time) {
    Logger.Log("DriveForTimeCommand", 3, "DriveForTimeCommand()");

    m_subsystem = subsystem;
    m_power = power;
    m_time = time;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(m_subsystem);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    Logger.Log("DriveForTimeCommand", 2, "initialize()");

    m_subsystem.setPower(m_power, m_power);
    m_timer.reset();
  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    Logger.Log("DriveForTimeCommand", -1, "execute()");
  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
    Logger.Log("DriveForTimeCommand", 2, String.format("end(%b)", interrupted));

    m_subsystem.setPower(0, 0);
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    Logger.Log("DriveForTimeCommand", -1, "isFinished()");

    return(m_timer.get() >= m_time);
  }
}

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 constructor DriveForTimeCommand(DriveSubsystem) is undefined

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 new constructor. Lets have our robot drive forward at 0.75 power for 3 seconds:

  private void configureButtonBindings() {
    m_button1.whenPressed(new DriveForTimeCommand(m_driveSubsystem, 0.75, 3.0));
  }

Now deploy and run your program that the robot not drives at three quarters 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