Commands
We have created a program that will drive the robot forward for a short distance, but we have placed our code directly in our teleopInit() and teleopPeriodic() functions. While this works for simple programs like this, you will find as your program gets more complex, doing it this way is not ideal.
What we want to do is create separate pieces of code, each of which can handle specific tasks, and then control when those pieces of code get executed. Fortunately a mechanism to accomplish this exists.
We can organize our code into Commands and then specify exactly when each command will be executed. So let’s change our drive program to take advantage of this.
We are going to create a new command called DriveForTimeCommand which will cause the robot to drive forward for a specific amount of time. As with the DriveSubsystem that we created, we are going to make a copy of the existing ExampleCommand to use as a starting point. Right click on the ExampleCommand.java file under the commands folder and then paste that into the commands folder giving it the name DriveForTimeCommand. Then open up the newly created DriveForTimeCommand.java file and replace all instances of ExampleCommand with DriveForTimeCommand. Once you have done this, your DriveForTimeCommand.java file should look like:
package commands;
import robotCore.Logger;
import robotWpi.command.Command;
/**
*
*/
public class DriveForTimeCommand extends Command
{
public DriveForTimeCommand()
{
Logger.Log("DriveForTimeCommand", 3, "DriveForTimeCommand()");
// Use requires() here to declare subsystem dependencies
// requires(Robot.m_exampleSubsystem);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("DriveForTimeCommand", 2, "initialize()");
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("DriveForTimeCommand", -1, "execute()");
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("DriveForTimeCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("DriveForTimeCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("DriveForTimeCommand", 2, "interrupted()");
}
}
Let’s take a look at what we have here.
First we have the constructor DriveForTimeCommand(). It is here that we will specify which Subsystems this command will require. We do this by calling the requires(…) function with the needed subsystems. It is important that 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.
Since this command will be using the DriveSubsystem we change our DriverForTimeCommand() constructor to reflect this.
public DriveForTimeCommand()
{
Logger.Log("DriveForTimeCommand", 3, "DriveForTimeCommand()");
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
Note that in order to access Robot.m_driveSubsystem we will need to import our Robot class. When you do this, make sure you choose our Robot class, not the Java one:
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.
protected void initialize()
{
Logger.Log("DriveForTimeCommand", 2, "initialize()");
Robot.m_driveSubsystem.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. Remember to import the Timer class and be sure to use the one from robotCore as opposed to the others offered.
private Timer m_timer = new Timer();
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.
Then we come to 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:
protected boolean isFinished()
{
Logger.Log("DriveForTimeCommand", -1, "isFinished()");
return(m_timer.get() >= 2);
}
Finally there is the end() and interrupted() function. The end() function is called when a command ends normally (i.e. your isFinished() function returns true). The interrupted() function is called if this command is ending because it has been interrupted by another command that needs the same resources that this command is using. In either case, we want to turn the motors back off:
// Called once after isFinished returns true
protected void end()
{
Logger.Log("DriveForTimeCommand", 2, "end()");
Robot.m_driveSubsystem.SetPower(0, 0);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("DriveForTimeCommand", 2, "interrupted()");
end();
}
Your DriveForTimeCommand.java file should now look like:
package commands;
import robotCore.Logger;
import robotCore.Timer;
import robotWpi.command.Command;
import robot.Robot;
/**
*
*/
public class DriveForTimeCommand extends Command
{
private Timer m_timer = new Timer();
public DriveForTimeCommand()
{
Logger.Log("DriveForTimeCommand", 3, "DriveForTimeCommand()");
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("DriveForTimeCommand", 2, "initialize()");
Robot.m_driveSubsystem.SetPower(0.5, 0.5);
m_timer.reset();
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("DriveForTimeCommand", -1, "execute()");
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("DriveForTimeCommand", -1, "isFinished()");
return(m_timer.get() >= 2);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("DriveForTimeCommand", 2, "end()");
Robot.m_driveSubsystem.SetPower(0, 0);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("DriveForTimeCommand", 2, "interrupted()");
end();
}
}
Now we need to find a place to cause this command to execute. We will do this in the teleopInit() function of our Robot class. First we remove the lines to set the power and reset the time (since this will be done by our new command) and replace them with a line that will create a new DriveForTimeCommand and start it running:
public void teleopInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "teleopInit()");
new DriveForTimeCommand().start();
}
Since we are now handling the driving via this command, we want to remove the remaining references to the timer and m_driveSubsystem.SetPower(…). After you have done this, your Robot.java file should look like:
package robot;
import commands.DriveForTimeCommand;
import robotWpi.command.Scheduler;
import robotCore.IterativeRobot;
import robotCore.Logger;
import subsystem.DriveSubsystem;
import subsystem.ExampleSubsystem;
public class Robot extends IterativeRobot
{
public static ExampleSubsystem m_exampleSubsystem;
public static OI m_OI;
public static DriveSubsystem m_driveSubsystem;
Robot()
{
Logger.Log("Robot", 2, "Robot()");
}
/**
* Called once to initialize the robot
*/
@Override
public void robotInit()
{
Logger.Log("Robot", 2, "robotiInit()");
m_driveSubsystem = new DriveSubsystem();
m_exampleSubsystem = new ExampleSubsystem();
m_OI = new OI();
}
/*
* Called at the start of autonomous mode
*/
@Override
public void autonomousInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "autonomousInit()");
}
/**
* Called periodically during autonomous
*/
@Override
public void autonomousPeriodic()
{
Logger.Log("Robot", -1, "autonomousPeriodic()");
Scheduler.getInstance().run();
Sleep(10);
}
/**
* Called at the start of teleop mode
*/
@Override
public void teleopInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "teleopInit()");
new DriveForTimeCommand().start();
}
/**
* Called periodically during operator control
*/
@Override
public void teleopPeriodic()
{
Logger.Log("Robot", -1, "teleopPeriodic()");
Scheduler.getInstance().run();
Sleep(10);
}
/**
* Called a the start of test mode
*/
@Override
public void testInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "testInit()");
}
/**
* Called periodically during test mode
*/
@Override
public void testPeriodic()
{
Logger.Log("Robot", 0, "testPeriodic()");
Sleep(10);
}
/**
* Main program entry point
*
*/
public static void main(String args[])
{
Robot Robot = new Robot();
Robot.Start(args);
}
}
Now deploy and run your program. When you enable the robot in Teleop mode, it should, once again, drive forward for two seconds.
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:
private double m_power;
private double m_time;
public DriveForTimeCommand(double power, double time)
{
Logger.Log("DriveForTimeCommand", 3, "DriveForTimeCommand()");
m_power = power;
m_time = time;
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
Now in our initialize() function we need to run the robot at the specified m_power:
protected void initialize()
{
Logger.Log("DriveForTimeCommand", 2, "initialize()");
Robot.m_driveSubsystem.SetPower(m_power, m_power);
m_timer.reset();
}
Finally, we need to change our isFinished() function to use the specified m_time:
protected boolean isFinished()
{
Logger.Log("DriveForTimeCommand", -1, "isFinished()");
return(m_timer.get() >= m_time);
}
Your DriveForTimeCommand.java file should now look like:
package commands;
import robot.Robot;
import robotCore.Logger;
import robotCore.Timer;
import robotWpi.command.Command;
/**
*
*/
public class DriveForTimeCommand extends Command
{
private Timer m_timer = new Timer();
private double m_power;
private double m_time;
public DriveForTimeCommand(double power, double time)
{
Logger.Log("DriveForTimeCommand", 3, "DriveForTimeCommand()");
m_power = power;
m_time = time;
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("DriveForTimeCommand", 2, "initialize()");
Robot.m_driveSubsystem.SetPower(m_power, m_power);
m_timer.reset();
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("DriveForTimeCommand", -1, "execute()");
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("DriveForTimeCommand", -1, "isFinished()");
return(m_timer.get() >= m_time);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("DriveForTimeCommand", 2, "end()");
Robot.m_driveSubsystem.SetPower(0, 0);
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("DriveForTimeCommand", 2, "interrupted()");
Robot.m_driveSubsystem.SetPower(0, 0);
}
}
You will no notice that your Robot 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() is undefined
The reason it is telling us this is that there is no longer a constructor for this class that takes no arguments (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:
public void teleopInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "teleopInit()");
new DriveForTimeCommand(0.75, 3).start();
}
Your Robot.java file should now look like:
package robot;
import commands.DriveForTimeCommand;
import robotWpi.command.Scheduler;
import robotCore.IterativeRobot;
import robotCore.Logger;
import subsystem.DriveSubsystem;
import subsystem.ExampleSubsystem;
public class Robot extends IterativeRobot
{
public static ExampleSubsystem m_exampleSubsystem;
public static OI m_OI;
public static DriveSubsystem m_driveSubsystem;
Robot()
{
Logger.Log("Robot", 2, "Robot()");
}
/**
* Called once to initialize the robot
*/
@Override
public void robotInit()
{
Logger.Log("Robot", 2, "robotiInit()");
m_driveSubsystem = new DriveSubsystem();
m_exampleSubsystem = new ExampleSubsystem();
m_OI = new OI();
}
/*
* Called at the start of autonomous mode
*/
@Override
public void autonomousInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "autonomousInit()");
}
/**
* Called periodically during autonomous
*/
@Override
public void autonomousPeriodic()
{
Logger.Log("Robot", -1, "autonomousPeriodic()");
Scheduler.getInstance().run();
Sleep(10);
}
/**
* Called at the start of teleop mode
*/
@Override
public void teleopInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "teleopInit()");
new DriveForTimeCommand(0.75, 3).start();
}
/**
* Called periodically during operator control
*/
@Override
public void teleopPeriodic()
{
Logger.Log("Robot", -1, "teleopPeriodic()");
Scheduler.getInstance().run();
Sleep(10);
}
/**
* Called a the start of test mode
*/
@Override
public void testInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "testInit()");
}
/**
* Called periodically during test mode
*/
@Override
public void testPeriodic()
{
Logger.Log("Robot", 0, "testPeriodic()");
Sleep(10);
}
/**
* Main program entry point
*
*/
public static void main(String args[])
{
Robot Robot = new Robot();
Robot.Start(args);
}
}
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.