Commands – V3 – Robotics Programming

Now that we have a DriveMechanism 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.

We are going to create a function in the DriveMechanism that will return a Command that will perform this task. After the stop() function in DriveMechanism.java add the following function:

    public Command driveForTime() {
        
    }

public Command driveForTime() {

}

When you do this, Command should come up undefined. You need to use the quick fix to import the command but you need to be sure that you import the Command3 version as follows:

Once you have done this, your DriveMechanism.java file should look like:

package first.robot.mechanisms;

import org.wpilib.command3.Command;
import org.wpilib.command3.Mechanism;

import robotCore.Logger;
import robotCore.Minibot;
import robotCore.PWMMotor;

public class DriveMechanism implements Mechanism {
    private PWMMotor m_leftMotor = new PWMMotor(Minibot.LeftMotorPWM, Minibot.LeftMotorDir);
    private PWMMotor m_rightMotor = new PWMMotor(Minibot.RightMotorPWM, Minibot.RightMotorDir);

    public DriveMechanism() {
        Logger.log("DriveMechanism", 1, "DriveMechanism()");
    }

    public void setPower(double leftPower, double rightPower) {
        m_rightMotor.set(rightPower);
        m_leftMotor.set(leftPower);
    }

    public void stop() {
        setPower(0, 0);
    }

    public Command driveForTime() {
        
    }
}

package first.robot.mechanisms;

import org.wpilib.command3.Command;

import org.wpilib.command3.Mechanism;

import robotCore.Logger;

import robotCore.Minibot;

import robotCore.PWMMotor;

public class DriveMechanism implements Mechanism {

private PWMMotor m_leftMotor = new PWMMotor(Minibot.LeftMotorPWM, Minibot.LeftMotorDir);

private PWMMotor m_rightMotor = new PWMMotor(Minibot.RightMotorPWM, Minibot.RightMotorDir);

public DriveMechanism() {

Logger.log("DriveMechanism", 1, "DriveMechanism()");

}

public void setPower(double leftPower, double rightPower) {

m_rightMotor.set(rightPower);

m_leftMotor.set(leftPower);

}

public void stop() {

setPower(0, 0);

}

public Command driveForTime() {

}

Note the correct import for Command3 at the top:

import org.wpilib.command3.Command;

1	import org.wpilib.command3.Command;

You will now notice that driveForTime is underlined in red. If you hover over it you will find that the error is “This method must return a result of type Command“. This means that this function should be returning a value and it is not.

What we need to do now is to construct a command that will drive the robot forward at 1/2 power for 3 seconds. To do this, add the following code inside the body of driveForTime:

    public Command driveForTime() {
        return run(coroutine -> {
            // Your command code goes here
        }).named("DriveForTime");
    }

public Command driveForTime() {

return run(coroutine -> {

// Your command code goes here

}).named("DriveForTime");

}

You may be wondering what the run(…) in our command actually does. run is a function that every Mechanism has, and it does two things. First, it builds a command whose body is the code we put between the braces. Second, and just as important, it tells the scheduler that this command requires the DriveMechanism.

Requirements are how the scheduler keeps commands from fighting over the same hardware. Only one running command can require a particular mechanism at a time. If a new command starts that requires a mechanism that is already in use, the scheduler cancels the command that was using it.

Because we create our commands with the Mechanism’s run(…) function, the requirement is declared for us automatically. We will see it in action in the next chapter.

The space between the curly brackets with the comment “Your command code goes here” is where we are going to write the code to make the robot drive. Note the .named(“driveForTime”) call. All commands need to be named and you should name them according to their function.

Finally we need to create the code that will be executed for this command. What we need to do is to turn the motors on at 0.5 power, wait 3 seconds and then turn the motors off. We can do this as follows:

    public Command driveForTime() {
        return run(coroutine -> {
            Logger.log("DriveMechanism", 1, "run starts");
            setPower(0.5, 0.5);
            coroutine.wait(Units.Seconds.of(3));
            stop();
            Logger.log("DriveMechanism", 1, "run ends");
        }).named("driveForTime");
    }

public Command driveForTime() {

return run(coroutine -> {

Logger.log("DriveMechanism", 1, "run starts");

setPower(0.5, 0.5);

coroutine.wait(Units.Seconds.of(3));

stop();

Logger.log("DriveMechanism", 1, "run ends");

}).named("driveForTime");

}

You may need to import Units but make sure you import the org.wpilib.units.Units version. Also note that we are logging the start of the command and the end. This is useful for later debugging if something is not going as planned.

Also note the coroutine.wait. This causes the command to wait for the specified amount of time. You might wonder why we don’t just simply use the Java function Thread.sleep(…) here. The reason is that the schedular needs to be able to run other command while this one is waiting and using the coroutine.wait method will allow it to do that. Using sleep would lock out all other command for the entire time it is sleeping.

There is one more thing that we must do before we are done. Normally this command will end after 3 seconds. However if, during those 3 seconds, another command which uses the DriveMechanism is started, this command will be prematurely stopped. When that happens, the stop() function to turn the motors off will not be called. To make sure that stop() is called in all cases we can add a whenCanceled as follows:

    public Command driveForTime() {
        return run(coroutine -> {
            Logger.log("DriveMechanism", 1, "run starts");
            setPower(0.5, 0.5);
            coroutine.wait(Units.Seconds.of(3));
            stop();
            Logger.log("DriveMechanism", 1, "run ends");
        }).whenCanceled(this::stop).named("driveForTime");
    }

public Command driveForTime() {

return run(coroutine -> {

Logger.log("DriveMechanism", 1, "run starts");

setPower(0.5, 0.5);

coroutine.wait(Units.Seconds.of(3));

stop();

Logger.log("DriveMechanism", 1, "run ends");

}).whenCanceled(this::stop).named("driveForTime");

}

Now that we have created the DriveMechanisim complete with a command to drive the robot we need to create an instance of it somewhere. The place to do it is in Robot.java where we add the following line at the beginning of the class:

  final DriveMechanism m_driveMechanism = new DriveMechanism();

1	final DriveMechanism m_driveMechanism = new DriveMechanism();

We are also going to need an instance of the CommandJoystick class so we should also instantiate that here as well.

  final CommandJoystick m_cmdJoystick = new CommandJoystick(0);

1	final CommandJoystick m_cmdJoystick = new CommandJoystick(0);

So your Robot.java would now look like:

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package first.robot;

import org.wpilib.command3.Scheduler;
import org.wpilib.command3.button.CommandJoystick;
import org.wpilib.framework.OpModeRobot;

import first.robot.mechanisms.DriveMechanism;
import robotCore.Logger;

public class Robot extends OpModeRobot {
  final DriveMechanism m_driveMechanism = new DriveMechanism();
  final CommandJoystick m_cmdJoystick = new CommandJoystick(0);

  public Robot() {
    Logger.log("Robot", 2, "Robot()");
  }

  @Override
  public void robotPeriodic() {
    Scheduler.getDefault().run();
  }
}

// Copyright (c) FIRST and other WPILib contributors.

// Open Source Software; you can modify and/or share it under the terms of

// the WPILib BSD license file in the root directory of this project.

package first.robot;

import org.wpilib.command3.Scheduler;

import org.wpilib.command3.button.CommandJoystick;

import org.wpilib.framework.OpModeRobot;

import first.robot.mechanisms.DriveMechanism;

import robotCore.Logger;

public class Robot extends OpModeRobot {

final DriveMechanism m_driveMechanism = new DriveMechanism();

final CommandJoystick m_cmdJoystick = new CommandJoystick(0);

public Robot() {

Logger.log("Robot", 2, "Robot()");

}

@Override

public void robotPeriodic() {

Scheduler.getDefault().run();

}

Now we need to tie the driveForTime command we created to one of the buttons on the joystick. If you look at the file list for your project you will see that there are two other files that we have not discussed, TeleopOpMode.java and AutoOpMode.java. The robot is able to operate in one of two modes: Teleoperated, where a human controls the robot with a joystick or game controller and Autonomous, where the robot operates without human input. The TeleopOpMode class will be used when operating in Teleoperated mode and the AutoOpMode class is used during Autonomous.

Since we want to tie our driveForTime command to joystick button #1, this connection belongs to the TeleopOpMode class. Let’s take a look at that file:

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package first.robot;

import org.wpilib.opmode.OpMode;
import org.wpilib.opmode.Teleop;

import robotCore.Logger;

@Teleop
public class TeleopOpMode implements OpMode {
  public TeleopOpMode(Robot robot) {
  }

  @Override 
  public void start() {
    Logger.log("TeleopOpMode", 2, "start()");
  }
}

// Copyright (c) FIRST and other WPILib contributors.

// Open Source Software; you can modify and/or share it under the terms of

// the WPILib BSD license file in the root directory of this project.

package first.robot;

import org.wpilib.opmode.OpMode;

import org.wpilib.opmode.Teleop;

import robotCore.Logger;

@Teleop

public class TeleopOpMode implements OpMode {

public TeleopOpMode(Robot robot) {

}

@Override

public void start() {

Logger.log("TeleopOpMode", 2, "start()");

}

We see a constructor that will be called when this class is instantiated and a start() function that is called when the robot enters teleop.

The constructor is the place where we are going to connect our driveForTime command to the joystick by adding the following line:

    robot.m_cmdJoystick.getHID().button(0).onTrue(robot.m_driveMechanism.driveForTime());

1	robot.m_cmdJoystick.getHID().button(0).onTrue(robot.m_driveMechanism.driveForTime());

Note that we are using .button(0) not .button(1). This is because, in WPILib, the buttons are numbered starting with zero not one. So the button marked ‘1’ on the joystick will be .button(0).

Your TeleopOpMode.java file should now look like

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package first.robot;

import org.wpilib.opmode.OpMode;
import org.wpilib.opmode.Teleop;

import robotCore.Logger;

@Teleop
public class TeleopOpMode implements OpMode {
  public TeleopOpMode(Robot robot) {
    robot.m_cmdJoystick.getHID().button(0).onTrue(robot.m_driveMechanism.driveForTime());
  }

  @Override
  public void start() {
    Logger.log("TeleopOpMode", 2, "start()");
  }
}

// Copyright (c) FIRST and other WPILib contributors.

// Open Source Software; you can modify and/or share it under the terms of

// the WPILib BSD license file in the root directory of this project.

package first.robot;

import org.wpilib.opmode.OpMode;

import org.wpilib.opmode.Teleop;

import robotCore.Logger;

@Teleop

public class TeleopOpMode implements OpMode {

public TeleopOpMode(Robot robot) {

robot.m_cmdJoystick.getHID().button(0).onTrue(robot.m_driveMechanism.driveForTime());

}

@Override

public void start() {

Logger.log("TeleopOpMode", 2, "start()");

}

Now run your program (remember that if your program is already running, you need to stop it using the red stop button , or Ctl-C if you are using the newer version of VSCode) 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, press the B1 button and your robot should drive forward for about 3 seconds.

Before we move on, let’s try and make this command more useful. Right now it is always driving the robot at 50% power, and always for 3 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 driveForTime function so we add two parameters, power and time to the function and then use those values to set the power and wait time later in the code.

Your driveForTime(…) function in DriveMechanisim.java should now look like:

    public Command driveForTime(double power, double time) {
        return run(coroutine -> {
            Logger.log("DriveMechanism", 1, "run starts");
            setPower(power, power);
            coroutine.wait(Units.Seconds.of(time));
            stop();
            Logger.log("DriveMechanism", 1, "run ends");
        }).whenCanceled(this::stop).named("driveForTime");
    }

public Command driveForTime(double power, double time) {

return run(coroutine -> {

Logger.log("DriveMechanism", 1, "run starts");

setPower(power, power);

coroutine.wait(Units.Seconds.of(time));

stop();

Logger.log("DriveMechanism", 1, "run ends");

}).whenCanceled(this::stop).named("driveForTime");

}

You will no notice that your TeleopOpMode class now has an error on the line where we make the call to driveForTime(). Hovering over the error marker it tells us:

The method driveForTime(double, double) in the type DriveMechanism is not applicable for the arguments ()

1	The method driveForTime(double, double) in the type DriveMechanism is not applicable for the arguments ()

The reason it is telling us this is that there is no longer a version of driveForTime which has no arguments (remember that we changed the function to take two arguments). So what we must do is provide the proper arguments for the updated function. Let’s have our robot drive forward at 0.75 power for 2 seconds:

public class TeleopOpMode implements OpMode {
  public TeleopOpMode(Robot robot) {
    Logger.log("TeleopOpMode", 2, "TeleopOpMode");

    robot.m_driveMechanism.setDefaultCommand(robot.m_driveMechanism.arcadeDrive(
        () -> robot.m_cmdJoystick.getX(),
        () -> robot.m_cmdJoystick.getY()));

    robot.m_cmdJoystick.getHID().button(0).onTrue(robot.m_driveMechanism.driveForTime(0.75, 2));

public class TeleopOpMode implements OpMode {

public TeleopOpMode(Robot robot) {

Logger.log("TeleopOpMode", 2, "TeleopOpMode");

robot.m_driveMechanism.setDefaultCommand(robot.m_driveMechanism.arcadeDrive(

() -> robot.m_cmdJoystick.getX(),

() -> robot.m_cmdJoystick.getY()));

robot.m_cmdJoystick.getHID().button(0).onTrue(robot.m_driveMechanism.driveForTime(0.75, 2));

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

Note that you can create multiple instances of driveForTime connected to different buttons. For example you could make it so that button 2 would drive the robot at full speed for 4 seconds. Try it!

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

Next: Arcade Drive