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 use is often called a “Random Walk”. What we are going to do is to drive forward until we hit a black line, then back up a short distance and turn randomly either left or right, then drive forward again and repeat. Eventually we will find the exit.
Here is some “pseudo code” of what our code should look like (this is just a framework, don’t go typing it into your program).
|
1 2 3 4 5 6 7 8 |
initialize random number generator while (true) { drive forward until a black line is detected drive backwards for a short distance generate a random number between 20 and 90 to get a turn angle generate a random number between 0 an 1 to get turn direction use these number to turn to the right or left for a random amount } |
To write this routine you are going to need the following information about generating random numbers in java:
|
1 2 3 |
Random random = new Random(System.currentTimeMillis()); // Initialize random number generator int rand1 = random.nextInt(2); // Generates a random number between 0 and 1 (inclusive) int rand2 = random.nextInt(20, 90); // Generates a random number between 20 and 89 (inclusive) |
Now take a crack at writing this command yourself and compare you solution to the one below.
.
.
.
.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 |
public Command escape() { return run(coroutine -> { Random random = new Random(System.currentTimeMillis()); // Initialize random number generator while (true) { coroutine.await(driveToLine()); coroutine.await(driveForDistance(-0.4, 3)); // Backup 3" // Turn in a random direstion for a random amount int angle = random.nextInt(20, 90); // Generate an angle between 10 and 89 degrees if (random.nextInt(2) == 1) { // random direction to turn // Turn right coroutine.await(turn(0.3, -angle)); } else { // Turn left coroutine.await(turn(0.3, angle)); } } }).whenCanceled(this::stop).named("escape"); } |
Attach this command to button #8 and then 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. As a final exercise try and come up with a way to do this. Here is a list of hints to help you if you are stymied. Each hint give you a little more insight into the problem.
Hint 1: define “escaped.”
Right now the robot never decides it’s done. What’s different about driving when it’s outside the box compared to inside?
Hint 2: make it measurable.
How long could the robot possibly drive straight inside the box before it hits a line? How could you work that out, or measure it?
Hint 3: something to measure with.
WPILib has a class for measuring elapsed time. Look up Timer. Where in your loop would you start it, and when would you check it?
Hint 4: stopping a command early.driveToLine() only ends when it sees a line. Commands have ways to add an extra condition that ends them early. Check the Command documentation for one that takes a condition (hint: until).
Hint 5: the result problem.
After await returns, driveToLine() has ended, but did it end because of the line or because of your condition? await doesn’t tell you. What do you already have that could answer that question?
Hint 6: getting out of the loop.
Your loop is while (true). Once you know you’ve escaped, how do you get out of it, and what happens to the command when you do?
This concludes the Minibot tutorial.