Escape!
Our next task is going to be to write a program which will give the robot a way to escape from the following box:
As you can see, there is a black line which surrounds a orange field and this line has a break on one side. Our goal is to be able to place the robot anywhere within the box, pointing at any angle, and have it find it’s way out without crossing the black line.
Now there are a number of ways that we could approach solving this problem. The one we are going to choose first is relatively simple and, although not the most efficient, involves the use of a state machine which makes it a good exercise.
Basically what we are going to do is to program the robot to drive forward until it ‘sees’ a black line. Then we will have it drive backwards a short distance, turn left or right for a random amount of time and then repeat. Eventually, the robot will be pointed at the exit from which it will make it’s escape.
When implementing a state machine, it is useful to draw a state diagram. The state diagram for our machine looks like:
Now we want to create a command that we will use to implement this state machine. Let’s call that command EscapeCommand. Go ahead and create the framework for the new command:
package commands;
import robot.Robot;
import robotCore.Logger;
import robotWpi.command.Command;
/**
*
*/
public class EscapeCommand extends Command
{
public EscapeCommand()
{
Logger.Log("EscapeCommand", 3, "EscapeCommand()");
// 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("EscapeCommand", 2, "initialize()");
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("EscapeCommand", -1, "execute()");
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("EscapeCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("EscapeCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("EscapeCommand", 2, "interrupted()");
}
}
The first thing we need to do is to create an enum to name our states. We have three states, so our enum will have three values:
private enum State
{
DriveForward,
BackUp,
Turn
}
And we need a variable to store the current state:
private State m_state;
In the initialize() function, we need to set the state to our starting state DriveForward and start the robot driving forward. Since we will be switching to this state in multiple places we will create a function to do this which we will call from our initialize() function:
private void DriveForward()
{
Logger.Log("EscapeCommand", 2, "DriveForward()");
m_state = State.DriveForward;
Robot.m_driveSubsystem.SetSpeed(k_driveSpeed, k_driveSpeed);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("EscapeCommand", 2, "initialize()");
DriveForward();
}
Note that we have added a Logger call to our DriveForward() function. This will enable us to monitor the progress of our state machine which will be useful if we have any problems with it. Also note that we have defined a constant k_driveSpeed for the speed so that we can easily change this later if need be. We must, of course, define this constant:
private static final double k_driveSpeed = 0.5;
Now in our execute() function we need to add the logic that will switch the robot from one state to another when the time come. We will use a switch statement and create functions to handle each of the three states. First let’s just create the switch with each of the states enumerated:
protected void execute()
{
Logger.Log("EscapeCommand", -1, "execute()");
switch (m_state)
{
case DriveForward:
break;
case BackUp:
break;
case Turn:
break;
}
}
Now let’s start with the DriveForward state. Rather than handle the state logic in-line in the switch statement, we are going to create a function to do it for us. It is always good practice to break your program into nice small pieces rather than have one long function that handles a lot of complex tasks.
We will call the function that handles the DriveForward state DrivingForward.
private void DrivingForward()
{
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("EscapeCommand", -1, "execute()");
switch (m_state)
{
case DriveForward:
DrivingForward();
break;
case BackUp:
break;
case Turn:
break;
}
}
Now in our DrivingForward() function we need to check to see if the robot has encountered a black line which is the condition for switching out of that state. To do this, we are going to need a variable that allows us to read the digital light sensor like we did in the previous chapter:
private static final int k_leftSensorPin = 10; private DigitalInput m_leftSensor = new DigitalInput(k_leftSensorPin);
Since we will be backing up for a short time, we will also need a timer to use for that purpose:
private Timer m_timer = new Timer();
Now we can finish the DrivingForward() function:
private void DrivingForward()
{
if (m_leftSensor.get())
{
Logger.Log("EscapeCommand", 2, "BackUp");
m_state = State.BackUp;
Robot.m_driveSubsystem.SetSpeed(-k_driveSpeed, -k_driveSpeed);
m_timer.reset();
}
}
First we check to see if the robot ‘sees’ the black line. If so, we need to set the state to BackUp, start the robot moving backwards, and reset the timer we will use to time the backup. We also add a Logger call so that we can, once again, monitor the progress of our state machine in case anything goes wrong:
Next we will handle the BackUp state by adding a function Backing():
private void Backing()
{
if (m_timer.get() >= k_backingTime)
{
Logger.Log("EsapeCommand", 2, "Turn");
m_state = State.Turn;
if (m_random.nextInt(2) == 1)
{
Robot.m_driveSubsystem.SetSpeed(k_turnSpeed, -k_turnSpeed); // Turn right
}
else
{
Robot.m_driveSubsystem.SetSpeed(-k_turnSpeed, k_turnSpeed); // Turn left
}
m_turnTime = k_minTurnTime + m_random.nextDouble();
m_timer.reset();
}
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("EscapeCommand", -1, "execute()");
switch (m_state)
{
case DriveForward:
DrivingForward();
break;
case BackUp:
Backing();
break;
case Turn:
break;
}
}
First we check to see if the time we have been backing up exceeds the backing time limit (which we will define as the constant k_backingTime). If the time has expired then we want enter the Turn state and we want to randomly set the motors to turn the robot either left or right. For this we will need a random number generator and Java provides us with the Random class for this purpose.
Looking at the documentation for the Random class we see that if we call the nextInt(int n) function it will return a random number between zero (inclusive) and n (exclusive). So if we call nextInt(2) we will get either a zero or one. If the result is one, we start the robot turning right, and if it is zero we start it turning left.
We also want to turn for a random amount of time. We see that calling the nextDouble() function of the random number generator will return a value between 0.0 and 1.0. We can add this to a k_minTurnTime to create a turn time between 0.5 and 1.5 seconds. We store this value in a member variable m_turnTime and will use this time to determine when the turn is complete.
Finally, we reset the timer, and log the state change.
We, of course, must declare k_backingTime, k_turnSpeed, k_minTurnTime, m_random, and m_turnTime:
private static final double k_backingTime = 0.5; private static final double k_turnSpeed = 0.5; private static final double k_minTurnTime = 0.5; private Random m_random = new Random(System.currentTimeMillis()); private double m_turnTime;
Note that the constructor for Random takes a long parameter to act as a seed for the random number generator. If we pass in the same number every time, we will get the same string of random numbers (yes, random number generators do not really generate random numbers). By passing in the current time, we can ensure that we get a different sequence of random number each time we run our program.
The final state we need to handle is the Turn state. For this we create the function Turning().
protected void Turning()
{
if (m_timer.get() >= m_turnTime)
{
DriveForward();
}
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("EscapeCommand", -1, "execute()");
switch (m_state)
{
case DriveForward:
DrivingForward();
break;
case BackUp:
Backing();
break;
case Turn:
Turning();
break;
}
}
Here we simply wait for the calculated time to expire and then switch back to the DriveForward state by calling our previously created DriveForward() function.
At this point your EscapeCommand.java file should look like:
package commands;
import robot.Robot;
import robotCore.DigitalInput;
import robotCore.Logger;
import java.util.Random;
import robotCore.Timer;
import robotWpi.command.Command;
/**
*
*/
public class EscapeCommand extends Command
{
private enum State
{
DriveForward,
BackUp,
Turn
}
private static final double k_driveSpeed = 0.5;
private static final int k_leftSensorPin = 10;
private static final double k_backingTime = 0.5;
private static final double k_turnSpeed = 0.5;
private static final double k_minTurnTime = 0.5;
private State m_state;
private DigitalInput m_leftSensor = new DigitalInput(k_leftSensorPin);
private Timer m_timer = new Timer();
private Random m_random = new Random(System.currentTimeMillis());
private double m_turnTime;
public EscapeCommand()
{
Logger.Log("EscapeCommand", 3, "EscapeCommand()");
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
private void DriveForward()
{
Logger.Log("EscapeCommand", 2, "DriveForward()");
m_state = State.DriveForward;
Robot.m_driveSubsystem.SetSpeed(k_driveSpeed, k_driveSpeed);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("EscapeCommand", 2, "initialize()");
DriveForward();
}
private void DrivingForward()
{
if (m_leftSensor.get())
{
Logger.Log("EscapeCommand", 2, "BackUp");
m_state = State.BackUp;
Robot.m_driveSubsystem.SetSpeed(-k_driveSpeed, -k_driveSpeed);
m_timer.reset();
}
}
private void Backing()
{
if (m_timer.get() >= k_backingTime)
{
Logger.Log("EsapeCommand", 2, "Turn");
m_state = State.Turn;
if (m_random.nextInt(2) == 1)
{
Robot.m_driveSubsystem.SetSpeed(k_turnSpeed, -k_turnSpeed); // Turn right
}
else
{
Robot.m_driveSubsystem.SetSpeed(-k_turnSpeed, k_turnSpeed); // Turn left
}
m_turnTime = k_minTurnTime + m_random.nextDouble();
m_timer.reset();
}
}
protected void Turning()
{
if (m_timer.get() >= m_turnTime)
{
DriveForward();
}
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("EscapeCommand", -1, "execute()");
switch (m_state)
{
case DriveForward:
DrivingForward();
break;
case BackUp:
Backing();
break;
case Turn:
Turning();
break;
}
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("EscapeCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("EscapeCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("EscapeCommand", 2, "interrupted()");
}
}
Now connect this new command to button your #2 test button on the joystick. You OI.java file should now look like:
package robot;
import robotCore.Joystick;
import commands.DriveForTimeCommand;
import commands.EscapeCommand;
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 EscapeCommand());
}
}
Now deploy and run your program and watch your robot escape from the box. Note that we have no way to detect that the robot has actually escaped, so once it is out, you will need to hit the Disable button on the driver station to stop it.
This concludes the Java Robot tutorial.