Combining Commands

Now that we have commands to drive and turn let’s see how we can combine them to create more complex commands. Let’s use this class to create a set of commands that will drive the robot around a certain path.  The first step is to create a new command called DriveCourseCommand. This time instead of copying the ExampleCommand we are going to make this new class inherit from CommandGroup instead of Command. Instead of copying this command, right click on the commands folder and choose New/Class. Then enter DriveCourseCommand as the class name:

NewJavaClass

Now copy the following code to form the basis of this new class:

package commands;

import robotCore.Logger;
import robotWpi.command.CommandGroup;

/**
 *
 */
public class DriveCourseCommand extends CommandGroup 
{
    public DriveCourseCommand() 
    {
    	Logger.Log("DriveCourseCommand", 3, "DriveCourseCommand()");
    }
}

Note that for this type of command, we inherit from CommandGroup. Also we do not have the normal command functions (e.g. initialize, execute, etc). This is because this command will be composed of a set of composite commands.

For example, if we want to drive forward 20 inches and then turn right by 90 degrees, we would add the following lines to the constructor:

package commands;

import robotCore.Logger;
import robotWpi.command.CommandGroup;

/**
 *
 */
public class DriveCourseCommand extends CommandGroup 
{
    public DriveCourseCommand() 
    {
    	Logger.Log("DriveCourseCommand", 3, "DriveCourseCommand()");
    	
        int turn90 = 70;
        double speed = 0.5;
    	
    	addSequential(new DriveForDistanceCommand(speed, 20));
        addSequential(new PauseCommand(speed));
        addSequential(new TurnCommand(speed, turn90));
    }
}

Notice how I placed a PauseCommand between the DriveForDistanceCommand and TurnCommand. Doing so will increase the accuracy of the turns. Transitioning directly from driving to turning can cause errors like wheel slippage which will throw of our turn calculations. I am also using variables turn90 and speed to store the turn angle and speed. I am doing this since we will want to add multiple of these commands to get the robot to drive our course, and I want to be able to adjust the turn angle and speed for all of the commands in one place.

We can then modify our OI.java file so that we run this new command when we press our test button 2.

package robot;

import robotCore.Joystick;
import commands.DriveCourseCommand;
import commands.DriveForTimeCommand;
import robotWpi.buttons.Button;
import robotWpi.buttons.JoystickButton;;

/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OI {
    //// CREATING BUTTONS
    // One type of button is a joystick button which is any button on a joystick.
    // You create one by telling it which joystick it's on and which button
    // number it is.
     Joystick m_stick = new Joystick(0);
     Button m_trigger = new JoystickButton(m_stick, 1);
     Button m_testButton = new JoystickButton(m_stick, 2);
     
    // There are a few additional built in buttons you can use. Additionally,
    // by subclassing Button you can create custom triggers and bind those to
    // commands the same as any other Button.
    
    //// TRIGGERING COMMANDS WITH BUTTONS
    // Once you have a button, it's trivial to bind it to a button in one of
    // three ways:
    
    // Start the command when the button is pressed and let it run the command
    // until it is finished as determined by it's isFinished method.
    public OI()
    {
    	m_trigger.whenPressed(new DriveForTimeCommand(0.75, 2));
    	m_testButton.whenPressed(new DriveCourseCommand());
    }
}

Now deploy and run your program and verify that it drives forward 20 inches, turns right and then stops.

Now let’s change your program so that the robot will drive in a rectangular pattern 20 inches by 10 inches. In principle this should bring your robot back to the starting position. After you have completed and tested your code, compare it to my solution below.

.

.

.

.

.

.

package commands;

import robotCore.Logger;
import robotWpi.command.CommandGroup;

/**
 *
 */
public class DriveCourseCommand extends CommandGroup 
{
    public DriveCourseCommand() 
    {
    	Logger.Log("DriveCourseCommand", 3, "DriveCourseCommand()");
    	
        int turn90 = 70;
        double speed = 0.5;
    	
    	addSequential(new DriveForDistanceCommand(speed, 20));
        addSequential(new PauseCommand(speed));
        addSequential(new TurnCommand(speed, turn90));
        addSequential(new PauseCommand(speed));
        addSequential(new DriveForDistanceCommand(speed, 10));
        addSequential(new PauseCommand(speed));
        addSequential(new TurnCommand(speed, turn90));
        addSequential(new PauseCommand(speed));
        addSequential(new DriveForDistanceCommand(speed, 20));
        addSequential(new PauseCommand(speed));
        addSequential(new TurnCommand(speed, turn90));
        addSequential(new PauseCommand(speed));
        addSequential(new DriveForDistanceCommand(speed, 10));
        addSequential(new PauseCommand(speed));
        addSequential(new TurnCommand(speed, turn90));
    }
}

You will probably find that it was very hard to set the speed and turn parameters correctly to bring the robot back to the exact starting position. In fact, you probably saw that running the same program twice in a row actually produced a slightly different outcome. This is because there are a number of things that go on which introduce slight errors (such as wheel slippage), and these errors accumulate over time and are different every time. You might find that if you slow the robot down, you can get more reproducible results, but you can never reach the point where it is always spot on.

The important thing to take away from this is that this kind of ‘dead reckoning’ navigation is fraught with problems. The only way to overcome these problems is to use sensors (e.g. gyro, distance sensor, camera, etc.) on the robot that give you a way to determine it’s absolute position and orientation by observing and using the environment around it.

Next: Using Sensors