Micromouse maze solving with Processing and the Right Hand Rule
In previous posts I’ve shown you the micromouse maze that I’m building, as well as the micromouse robot. Today I’m going to show you the maze solving code I’m using to find the center of a maze. Here’s a preview.
Maslow’s Pyramid
Stick with me here, this will make sense in a moment. Maslow’s Heirarchy of Needs is a theory about what motivates people. It’s drawn as a pyramid, because the things on the bottom are more important than the things on top.
The robot is similar. At the bottom robot I’ve got power and movement. In the middle level I’ve got sensors to keep it from smashing into things. On the top level I have Big Ideas like goals and how to achieve them. Thankfully I can design the top of the pyramid and put it on when the rest is in place.
Safety Third
A robot’s code are all the thoughts it will ever have. I want to be confident that when I upload the code into the robot it is likely to work right. I don’t want to damage the machine or the maze playing guessing games! What I need is a way to build a huge number of mazes and test them all in the virtual reality inside the computer. For that I turned to Processing, which makes drawing on a screen and talking to a robot really easy.
Making the virtual mazes
Anticipating the need for mazes, I wrote some maze-generating code in the Makelangelo software. The maze generator is based on the maze generating Wikipedia article. I then copied the code into Processing and modified it just enough to fit the rules of the Micromouse contest – no exits around the outside edge, and four open spaces in the center.
The micromouse contest says there are 16 * 16 cells, but I wrote the code to handle pretty much any size of maze. The structure of the maze started as MazeWalls between every pair of adjacent MazeCells, and then I strategically removed just enough MazeWalls to make every cell reachable from every other cell. The code is long and well commented, so I invite you to read the micromouse maze generator code on github.
I didn’t actually remove the walls from computer memory. I set a flag that said “this wall is removed.” That way I can easily check if a wall exists or not. The real robot would use it’s sensors to look for a wall.
/**
* Find the index of the wall between two cells
* returns -1 if no wall is found (asking the impossible)
*/
int findWallBetween(int currentCell, int nextCell) {
int i;
for (i = 0; i < walls.length; ++i) {
if (walls[i].cellA == currentCell || walls[i].cellA == nextCell) {
if (walls[i].cellB == currentCell || walls[i].cellB == nextCell)
return i;
}
}
return -1;
}
Making the virtual micromouse
I want to keep the code as simple as possible, so in this test I’m going to pretend that the robot’s movement is already completely under my control. That way I only need two questions and two commands to interface with the robot, whether it’s real or virtual:
- Is there a wall in front of you?
- Is there a wall on your right?
- Please turn 90 degrees to the right.
- Please move one cell forward.
I can combine three right turns to make one left turn, or right-right-forward-right-right to move backward one square. This interface will be the same for the virtual robot and the real robot – it’s the line between two parts of the pyramid. The real robot would use it’s sensors to look for a wall and motors to move, while the virtual micromouse will use code tricks to look at the virtual maze.
Is there a wall?
I know where the micromouse robot starts and which way it is facing in the contest, so I’ll start the virtual micrmouse in the same place. As long as I track where the robot goes and which way it faces I can test for walls around the robot.
// returns true if there is a wall between cells A and B.
boolean thereIsAWallBetween(int a,int b) {
int wi = findWallBetween(a,b);
return (wi==-1 || !walls[wi].removed);
}
boolean thereIsAWallToTheNorth(int c) { return thereIsAWallBetween(c, c+1 ); }
boolean thereIsAWallToTheSouth(int c) { return thereIsAWallBetween(c, c-1 ); }
boolean thereIsAWallToTheEast (int c) { return thereIsAWallBetween(c, c+columns); }
boolean thereIsAWallToTheWest (int c) { return thereIsAWallBetween(c, c-columns); }
boolean thereIsAWallToTheRight() {
int c = getCurrentCellNumber();
switch(turtle.dir) {
case NORTH: return thereIsAWallToTheEast (c);
case EAST: return thereIsAWallToTheSouth(c);
case SOUTH: return thereIsAWallToTheWest (c);
default : return thereIsAWallToTheNorth(c);
}
}
boolean thereIsAWallAhead() {
int c = getCurrentCellNumber();
switch(turtle.dir) {
case NORTH: return thereIsAWallToTheNorth(c);
case EAST: return thereIsAWallToTheEast (c);
case SOUTH: return thereIsAWallToTheSouth(c);
default : return thereIsAWallToTheWest (c);
}
}
int getCurrentCellNumber() {
return getCellNumberAt( turtle.cellX, turtle.cellY );
}
I called the class Turtle because it was giving me flashbacks to my youth on the Commodore 64 playing with the LOGO turtle to turn and move through a maze.
Searching the maze
The maze solving system the robot will use is to follow the right hand wall all the way until it finds the center.
void searchMaze() {
if(!thereIsAWallToTheRight()) {
print("No wall on the right. ");
turnRight();
}
if(!thereIsAWallAhead()) {
print("No wall ahead. ");
stepForward();
addToHistory();
} else {
print("Wall ahead. ");
turnLeft();
}
println();
// remove dead ends
pruneHistory(getCurrentCellNumber());
if( iAmInTheCenter() ) {
println("** CENTER FOUND. GOING HOME **");
turtleState = GOHOME;
walkCount = historyCount;
}
}
Remembering the path
The robot is going to search through the maze to find the center and then it’s going to drive between the center and the starting point. That means the robot has to remember the route from start to center. Every time the robot steps forward it will remember which direction it stepped and which way it was facing, in addToHistory().
void addToHistory() {
history[historyCount].cellX = turtle.cellX;
history[historyCount].cellY = turtle.cellY;
history[historyCount].dir = turtle.dir;
historyCount++;
}
I can tell from the history when the robot is walking back over it’s old steps, which only happens when it is coming back from a dead end. The robot can remove these dead branches from the path and shorten the home-to-center path.
void pruneHistory(int findCell) {
int i;
for(i = historyCount-2; i >= 0; --i) {
int cell = getCellNumberAt(history[i].cellX, history[i].cellY);
if(cell == findCell) {
historyCount = i+1;
return;
}
}
}
Micromouse, Go home!
Once the center of the maze has been found and the history is built, going home means following the history in reverse. Going back to the center means following the history start-to-finish.
void goHome() {
walkCount--;
// face the opposite of the history
turnToFace((history[walkCount].dir+2)%4);
stepForward();
println();
if(walkCount==0) {
println("** HOME FOUND. GOING TO CENTER **");
turtleState = GOCENTER;
walkCount=1;
}
}
void goToCenter() {
turnToFace(history[walkCount].dir); // face the opposite of the history
stepForward();
println();
walkCount++;
if(walkCount==historyCount) {
println("** CENTER FOUND. GOING HOME **");
turtleState = GOHOME;
}
}
Micromoue results
Next
The next step is to write the code in the real robot to move one cell forward and turn 90 degrees left or right.
Final thoughts
The fastest micromice (micromouses? micromeese!) can cut corners and drive diagonally. How would you do this?
