Escape!

Our final 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 its 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. Remember that we are going to need access to the IR Sensor so be sure to include that as one of the arguments for the constructor.

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. We also want to declare a variable to hold the current 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:

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. We also define the constant k_driveSpeed for the speed so that we can easily change this later if need be:

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:

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.

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. We are also going to need a timer that we can start so that we can time the backup phase so we declare a Timer variable at the top of our class:

Since we will be using the Timer throughout this command we want to start it in the constructor:

Now we can finish the DrivingForward() function:

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():

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.4 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_backingTimek_turnSpeedk_minTurnTimem_random, and 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().

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:

Now in RobotContainer, connect this new command to button your on the joystick. You RobotContainer.java file should now look like:

Now deploy and run your program and watch your robot escape from the box. Note that we currently 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.

It would be nice if we added some code to automatically stop the robot once it has escaped. If we were to ensure that there was sufficient white space after the exit (e.g. by putting some white sheets of paper after the exit) then we might be able to use a timer to determine that it had escaped. If we started the timer when we enter the DriveForward state, then in the DrivingForward() function we could check the timer and if it exceeded a certain value we would know that the robot had escaped. We would have to choose a time that it is longer than the longest time the robot would drive if it were still contained (i.e. the time it would take to go from corner to corner). If the time expired while we were driving we would then know the robot was out of the box.

Implementation of this idea is left as an exercise to the student.

This concludes the Minibot tutorial.