Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
Final Report
Module: 8096 BSc Computing Project
School of Technology
Design, implementation and testing of a maze
solving LEGO robot
Student: Jean-Luc Pasquier 10077923
Supervisor: Mr. Ken Brownsey
1
Jean-Luc Pasquier: 10077923
I.
Oxford Brookes University
U08096
Table of Contents
II.
Introduction ..................................................................................................................................... 4
A.
Background.................................................................................................................................. 4
B.
Objective...................................................................................................................................... 4
C.
Assumptions ................................................................................................................................ 4
III.
Literature Review of technologies............................................................................................... 5
A.
LEGO Mindstorms[1] ................................................................................................................... 5
B.
Coding platform[2] ...................................................................................................................... 6
1.
LEGOS NXT-G : ......................................................................................................................... 6
2.
LEJOS, Java for Lego Mindstorms ............................................................................................ 6
3.
C based programming platforms ............................................................................................. 6
4.
Others ...................................................................................................................................... 6
C.
Maze algorithms .......................................................................................................................... 7
1.
Wall follower (right-hand or left-hand) ................................................................................... 7
2.
Tremaux Algorithm .................................................................................................................. 7
3.
Recursive backtracker ............................................................................................................. 8
IV.
Specifications ............................................................................................................................... 9
A.
Objectives .................................................................................................................................... 9
B.
Requirements .............................................................................................................................. 9
C.
Choice of technologies ................................................................................................................ 9
1.
Maze solving algorithm ........................................................................................................... 9
2.
Coding platform..................................................................................................................... 10
D.
Robot specifications .................................................................................................................. 10
E.
Algorithm specification ............................................................................................................. 11
F.
Robot movement specification ................................................................................................. 12
V.
Design and Implementation .......................................................................................................... 13
A.
Design ........................................................................................................................................ 13
1.
Robot design .......................................................................................................................... 13
2.
Maze solving algorithm ......................................................................................................... 13
3.
Robot movement and navigation design .............................................................................. 14
4.
Wall finder ............................................................................................................................. 14
5.
Exit ......................................................................................................................................... 14
B.
Implementation ......................................................................................................................... 14
1.
Robot ..................................................................................................................................... 14
2
Jean-Luc Pasquier: 10077923
VI.
Oxford Brookes University
U08096
2.
Movement and Navigation .................................................................................................... 15
3.
Tremaux algorithm in Java .................................................................................................... 16
Testing ....................................................................................................................................... 20
A.
Testing protocol......................................................................................................................... 20
B.
Algorithm test ............................................................................................................................ 21
C.
Navigational test........................................................................................................................ 21
VII.
Results ....................................................................................................................................... 22
A.
Algorithm test ............................................................................................................................ 22
B.
Navigational test........................................................................................................................ 22
VIII.
Conclusion ................................................................................................................................. 23
IX.
References ................................................................................................................................. 23
X.
Appendices .................................................................................................................................... 24
A.
Program Code ............................................................................................................................ 24
B.
Algorithm test Code .................................................................................................................. 30
C.
Interim Report ........................................................................................................................... 37
3
Jean-Luc Pasquier: 10077923
II.
Oxford Brookes University
U08096
Introduction
A.
Background
“Robot: any machine or mechanical device that operates automatically with humanlike
skill.”(dictionary.com).
Robots were created to reduce the need of human manpower for repetitive and hard
jobs. At first, they were simple, heavy and very expensive, so they weren’t widely used to
replace human labour, but after the invention of electronics and the reduction of size and cost
they became smaller, cheaper and more complex. That made them perfect to replace people in
the factories because they can reproduce the same identical movement over and over again.
Now, the field of robotics has evolved from its initial use and has expanded out of the
factories and helped humankind doing jobs that humans don’t want or cannot do. Robots like
Honda’s ASIMO (world.honda.com/ASIMO) or Tosy’s TOPIO (www.tosy.com) have shown
the capabilities of robots and in that context, maze-solving robots have a huge amount of
possible uses in the real world. For example, it can be used to create automated robots for
search and rescue operations in disaster zones or in deep water automated submarines, which
are really difficult and dangerous task for humans to do.
B.
Objective
There is a good number of maze solving robots out there but they tend to use external
help, like having the layout of the map in their memory to get through the maze or using a
simple algorithm, like following only the left walls or randomly going through the maze,
which cannot solve every kind of maze available or are very time and resources consuming.
Those flaws make the robot less useful and not very practical in the real world. This project
intends to try and solve those problems with a simple robot, a robot built with a LEGO
Mindstorms set.
The overall objective of the project is to create a robot that can solve a maze without
any external help, using only its sensors to get around the maze and be able to retain the path
to the exit so it can go again directly to the exit for example. The robotics should be designed
carefully to get a balanced robot. In addition, the robot may not go straight or be parallel to
the walls, so a good control system should be built to overcome the problem.
C.
Assumptions
We assume that the maze is composed of fixed length and composed of “cells” which
are squares, so the turns are 90 degrees to the left or to the right. That means that the maze has
well defined passages. The exit is indicated by a red square in the floor and the maze has a
maximum of around 80*80 cells because of memory limitations.
4
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
III. Literature Review of technologies
A.
LEGO Mindstorms[1]
The Mindstorms set is a robotic set build around the famous Lego bricks, in this case
the LEGO technic (technic.lego.com). That allows the user to create almost any kind of robot,
from eight legged animals that crawl or jump to humanoid exoskeletons capable of walking.
That makes the Mindstorms set a very practical, flexible and simple way to enter in the world
of robotics.
The set comes with a range of sensors, motors and a brain to control the robot:
-The “brain” of the robot is a mini-computer called the “NXT brick”. It is built around
an ARM7 micro-controller and has 3 motor ports, 4 sensor ports, a loud speaker, buttons and
a USB port for the connection to the computer.
Figure 1 NXT brick
-In the set there’s also 3 servo motors so the robot can move, or to rotate something.
One of the main features of the servo motor is that is comes with a built-in rotation sensor, so
we can accurately rotate the motor with a precision of +/- 1 degree.
Figure 2 Mindstorms servo motor
-In this project, there are 2 types of sensors that will be used. The ultrasonic sensor can
see objects and gives us the distance between the sensor and the object with a precision of
around 3 centimetres and a maximum distance of around 170 centimetres. The touch sensor’s
main purpose is to detect the exit of the maze because it can detect when the sensor is
touched.
5
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
Figure 3 Ultrasonic sensor and touch sensor
B.
Coding platform[2]
There are a lot of different platforms in different languages, some are open source,
some are made for beginners and others are for more experienced programmers. Here we will
look at some of them:
1.
LEGOS NXT-G :
Is the base interface that comes with the Mindstorms set. It only includes the basic
programming elements that include controlling motors, different sensors inputs, simple math
calculations, etc… But only in version 2.1 arrays and integer support were added. So it’s
limited in complexity but does the job very good for simple programs. It works on Microsoft
Windows and Apple OS X.
2.
LEJOS, Java for Lego Mindstorms
“LeJOS (pronounced like the Spanish word "lejos" for "far") is a tiny Java Virtual
Machine.”(www.lejos.sourceforge.net). Lejos is a Java virtual machine mounted in the NXT
brick to run Java programs using the Lejos API [1]. The API consists of a series of functions
made to simplify the creation and use of the sensors and motors from the robot. It gives the
programmer more control and simpler use of the motors and sensor to be able to create very
powerful programs that the standard software bundle wouldn’t allow because it is made for
people that might not know how to code in Java, Pascal or C.
3.
C based programming platforms
They come in very different forms. For example, the Bricx Command
Center(http://bricxcc.sourceforge.net/) is a IDE[2] originally created to create programs for the
RCX, which is the older version of the NXT, and now has been ported to the NXT brick. “Not
eXactly C” is an open-source programming language, very close to C language. It is one of
the most popular alternatives to NXT-G.
4.
Others
Besides these more important and supported programming platforms, there are at least
10 other different programming platforms including Ada, Lua and Ruby. These platforms are
not interesting for this project.
6
Jean-Luc Pasquier: 10077923
C.
Oxford Brookes University
U08096
Maze algorithms
1.
Wall follower (right-hand or left-hand)
a)
Principle
The Wall follower method solves a big number off mazes, but not all of them. It
consists in a simple rule to apply. It states that, when possible, if you follow the left (or right)
and turn left (or right) whenever possible, you will find the exit.[3]
b)
c)
-
d)
-
2.
Advantages
You are very likely to find the exit
It’s really simple to apply, only one rule
There is no need to know about position and orientation
It only needs to follow a line, or a wall and recognize the exit
Disadvantages
It doesn’t work for every maze, especially those containing
loops and when the exit or the entrance aren’t in the external
walls
Implementation
It is really simple to implement because there is no need to
know any position or track where you’ve been. It’s simply
follow a line or a wall and being able to recognize the exit.
Tremaux Algorithm
a)
Principle
The Tremaux algorithm consists on storing the path we follow in the maze. In our
case, we can consider dividing the maze in cells. Each square is unvisited, marked once or
marked twice. Every time we advance in one direction we mark it. On arriving at a junction
that has not been visited before, we pick a random direction and mark the path. When arriving
at a marked junction and if our current path is marked only once we then turn around and
walk back and mark the path again. If this is not the case, pick the direction with the fewest
marks. When you finally reach the solution, paths marked exactly once will indicate a direct
way back to the start.[3]
b)
-
Advantages
We will always find the exit
We have stored a direct path to the exit
-
Disadvantages
It’s more complicated to implement
-
Implementation
The robot will need to know it’s relative position
The robot will need “memory” to store the maze
May be limited by the quantity of ram available on big mazes
c)
d)
7
Jean-Luc Pasquier: 10077923
3.
Oxford Brookes University
U08096
Recursive backtracker
a)
Principle
The Recursive backtracker is essentially a depth-first search. It tries to move in the
four directions. When we hit a wall, we return false, else if, we are at the exit, we return true.
We mark the spot when we try a new position and erase when we return false. That way, we
will get a single solution when we get to the exit.[3]
b)
-
Advantages
It is a fairly simply code, because it calls himself
We will always find the exit
-
Disadvantages
We don’t get necessarily the shortest solution
It’s more complicated to implement
c)
d)
-
Implementation
The implementation is more complicated since we have a 3D
maze, not 2D.
We are more likely to encounter a lot of collision problems.
We need to divide the maze into squares to be able to well
define the path.
8
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
IV. Specifications
A.
Objectives
The main objective of the robot is to exit the maze using only the information from the
sensors it’s connected to. It should not find the exit randomly; it has to follow an algorithm
suited for maze solving. It also needs to save the path followed for later use.
The other part of this project is the maze. The maze should be consisted of a series of
cells, which have a predefined length that is in proportion with the length of the robot, made
out of cardboard or any other material, as long as the floor is smooth, not like carpet for
example.
B.
Requirements
Using only 2 servo motors and 1 sensor someone could make a robot that full fills the
objectives but it would make the coding of the program hard and not very easy to understand.
To most elegant and simple way of creating the robot would be with 3 sensors to detect the
walls and a sensor to detect the exit, but due to lack of hardware in this project, only one
sensor to detect the walls will be used and another to detect the exit.
The other requirement is the saving of the path. Having assumed that the maze is
composed of “cells” the resulting path should be instructions to direct the robot to the exit.
For example, an array which contains numbers for which direction to go next so that we can
put the robot at the start and using that array it can safely arrive to the exit.
C.
Choice of technologies
1.
Maze solving algorithm
After carefully examination of the 3 different maze solving algorithms I think that the
best suited algorithm to implement is the Tremaux algorithm. The reasons behind this
decision are that I cannot use the wall follower because it cannot solve all the mazes. In fact,
if the exit of the maze is inside the maze, it will not find the exit. As for the Recursive
backtracker, the representation of the “hitting a wall” not suited for the objectives of the
project. I chose the Tremaux algorithm because it is designed to be able to be used by a
human in a maze. I find it very interesting because it represents our process to find the exit
with a maze. Even if, when asked by someone, we cannot give the details of how we try to
find the exit of a maze, this algorithm actually does what our brain does. It simply goes
through the maze, and when we hit a dead-end, we go back and go where we haven’t been
already.
9
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
2.
Coding platform
The decision to use the Lejos virtual machine and API was made before I even got the
set of legos. The set I got for this project was already mounted in the NXT brick but I still did
a bit of research of the possible ways to code programs for the NXT brick. The software
bundle that comes with the set lego set is the NXT-G, which is not adequate for this project
because it needs a fair amount of complexity, to maintain the robot parallel to the walls for
example. I’m used to work with object oriented languages like Java or C++ even if I didn’t
know how to code in Java at the start of this project. That’s why I’ve decided to stay with the
Lejos system, it made me learn a new programming language but I’m still familiarised with
the philosophy of the language.
D.
Robot specifications
The robot needs to fulfil a series of tasks in order to achieve the objectives. It must be
a robot capable of moving in 360 degrees. It also has to be able to detect walls and the exit. It
also should be as compact as possible with the pieces in our disposal.
To be able to rotate the robot precisely it will need to have 2 motors and they will
work just like a tank’s caterpillar tracks. They will rotate in the same direction to go forward
or backward. To rotate, the will rotate in different direction. That allows the robot to turn in
place with a good precision.
To mount the wall detector, which is the ultrasonic sensor, 2 choices are possible. One
possibility is that the sensor could be rotated in the directions needed with the help of a third
servo motor and the use of some gears. The other possibility is to leave the sensor facing in
front of the robot and make the whole robot rotate to the right and left to detect the walls.
The other sensor is the touch sensor; its position will depend on the choice of location
for the ultrasonic sensor and the remaining space in the robot. An obstacle will be put in the
exit of the maze that will touch the sensor and the program will know that it has arrived to the
exit.
In order to create a robot that can accomplish the objectives, it will need some
mandatory pieces. The resulting robot should contain all the hardware but still trying to be as
compact as possible because the size of the robot will directly impact the minimum size of a
cell.
The final specification requirements are:
- 1 NXT brick to control the robot
- 2 servo motors to power the wheels and be able to move and rotate the robot.
- 1 ultrasonic sensors, which will be mounted with a third servo motor to rotate or
just looking in front.
- 1 light sensor that should be mounted looking down, but the final decision will
depend on the choices made for the other parts of the robot.
10
Jean-Luc Pasquier: 10077923
E.
Oxford Brookes University
U08096
Algorithm specification
The algorithm used in this project is the Tremaux algorithm. Basically, it is a depthfirst search algorithm. It starts at the root, the entrance of the maze, and then explores the tree
as long as it doesn’t find a dead-end. When it finds one, it backtracks until the first unexplored
path, and takes that path until it finds the exit or another cul-de-sac. To be able to do this, the
program needs to mark the path already visited and also mark the junctions encountered.
Firstly, the program needs to be able to mark the path followed by the robot. To be
able to do that, it will need some kind of map that represents the maze. That map will need to
remember where the robot has been, the intersections it’s crossed, how many path of those
intersections have already been visited and the cells it hasn’t visited yet.
Secondly, to allow the algorithm to work properly, a position control system is needed.
Indeed, to be able mark the map correctly, the robot needs to know its relative position. A
simple 4 way direction system will be used, like the four winds. It doesn’t have to really
represent the 4 winds, just give us the 4 different directions the robot can go.
The program will also need a way to surely identify the presence of walls around the
robot.
Figure 4 Tremaux algorithm flowchart
11
Jean-Luc Pasquier: 10077923
F.
Oxford Brookes University
U08096
Robot movement specification
The robot needs to be able to move. To do that, it has 2 servo motors mounted in
parallel and can rotate clockwise and counter-clockwise. To move forward in a straight line,
both motors should rotate at the same time and at the same speed. When it comes to turn the
robot, the motors need to rotate in different directions. To rotate right, the left motor needs to
turn forward and the right motor needs to rotate backward and vice versa to turn left. Another
important part of this project is the wall finder. The ultrasonic sensor will give us the distance
between the robot and the nearest object, in this case, the nearest wall. The exit detector will
be a simple touch sensor that when pressed, the exit has been found. That means that if it
finds an obstacle in the maze, the robot will simply stop because the algorithm will think it
found the exit, so it’s a good fail-safe system.
12
Jean-Luc Pasquier: 10077923
V.
Oxford Brookes University
U08096
Design and Implementation
A.
Design
1.
Robot design
The building of the robot was not simple. The mounting of the 2 servo motors used to
steer the robot is not complicated, but to integrate the sensors is more complicated. The first
try with the ultrasonic sensor was to mount a third servo motor to rotate the sensor. The idea
was to use some gears to transform the vertical circular movement to a horizontal circular
movement. The problem with that solution was that the gears and the Lego bricks in my
disposal don’t allow me to create a strong enough system to be able to precisely rotate the
sensor 90 degrees to the sides and the back. The main problem is the cable connecting the
sensor to the brick. The cable, a RJ 11, is quite tick and doesn’t allow the sensor to rotate
correctly. The only solution left is to mount the ultrasonic sensor in the front of the robot and
facing the front. That way, the robot needs to rotate to the right and left to be able to identify
the walls around the robot. That solution makes the robot a lot simpler and allows the other
sensor, the touch sensor, to be mounted in the same support as the ultrasonic sensor and be
facing down. Also, that way, the NXT brick can be easily mounted on top of the servo motors
and doesn’t bother the other sensors and makes the robot more compact. The main problem
with that solution is that the robot will have to rotate much more.
2.
Maze solving algorithm
As said earlier the Tremaux algorithm is the algorithm that will be used to control the
robot. The algorithm will use a right-hand priority. That means that the algorithm will always
try to go right, if it cannot go right it will try to go forward and finally if that’s not possible it
will try to take the left route. In order to follow the good route and not to hit the walls of the
maze it will need to know what is around the maze. That means that the robot will have to
give him information about the space around it and be able to store it.
To make the coding simpler and easier to understand we will use 2 different maps.
One that represents the map and that will be filled with the information of the maze, where the
walls are and the value of each cell. That map will tell us the path followed to reach the exit
and it will be needed to know the unexplored paths.
To be able to backtrack, the robot will have to have in memory the path followed until
that cell. That way, it just has to follow its own path and checking each cell for an open
intersection. Using this algorithm will even allow us to go back to the entrance if there is no
exit to the maze for example. But the first map is not suited for that part of the algorithm. To
tackle that problem the program will need another map, but this time, it will track the actual
path followed by the robot. It features a simple 3-way direction system. For each cell it visits
a value will be attributed to the cell, it may be right, left or forward. That way, when it comes
to get back to an unexplored path, it just needs to read the map of the path followed by the
robot used to get there in the first place.
13
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
3.
Robot movement and navigation design
In order to let the robot move around the maze the program a few functions will be
needed. It needs to be able to go forward, turn right, turn left and be able to turn 180 degrees.
The problem about the movements of the robot is that the movements might not be extremely
precise. That makes the coding of a special function that will keep the robot on track and
parallel to the walls. It should take the information available and be able to make the right
corrections so that the robot keeps on track.
4.
Wall finder
The ultrasonic sensor is capable to detect the distance between the robot and the first
object it encounters. Using that information and according to the size of the maze a value will
be fixed to decide whether there is a wall in front of the robot or not. It will be a fixed value
and the function should return a Boolean value.
5.
Exit
To find the exit, the robot will simply look each time the robot advances if it is at the
exit of the maze or not. A simple obstacle should tell the robot when it reached the exit of the
maze.
B.
Implementation
1.
Robot
Having decided which sensors to mount and how to mount them, the final result was
quite satisfactory. The base is composed by the 2 servo motors mounted parallel to each other.
A stable 2 wheel robot is quite hard to make, so the robot features a small wheel at the back so
the robot is more stable and the weight is distributed more evenly. Once the base is done, the
sensors could be mounted in the positions required and on top of the robot is the NXT brick.
As the final result we obtain a stable and compact robot.
Figure 5 The final robot
14
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
2.
Movement and Navigation
The Lejos API has some very useful features when it comes to movement of the robot.
The class TachoPilot lets us use a 2 motor system to make the robot move. It needs to know
which motors are used and some information about the size of the robot. Java being an objectoriented programming language we need to instantiate an object which will be the TachoPilot.
Its constructor needs 4 arguments. The diameter of the wheels, the wheelbase between the two
motors and the sensor ports for the 2 motors. A precise value is needed for the parameters
because a slight variation and the robot will not move precisely. The values can be of any type
(centimetres, inches, etc…) but they have to be the same type for the 2 arguments. In this
case, centimetres are used.
“traveler.robot = new TachoPilot(5.6f,14.8f,Motor.A,Motor.C);”
Once the object is instantiated, we are able to make a few useful functions that will
help us move the robot around. The robot needs to be able to move around, to turn and to able
to turn 180 degrees. So, 4 simple functions are made called: forward(), rotateRight(),
rotateLeft() and backward(). The final robot is around 18 centimetres long so when we move
forward to another cell, the distance travelled should be aroung 18 centimetres, so the forward
function will make the robot travel 18 cm:
“ public void forward() { //The Robot advances one cell
robot.travel(18);
}”
The rotateRight and rotateLeft are simple rotations of the robot 90 degrees to the left
or the right:
“ public void rotateRight(){ //The Robot rotates to the right
robot.rotate(90);
pause(500);
}”
Finally, the backward() function makes the robot travel backward of 1 cell.
“public void backward() { //The Robot travels 18 cm back.
robot.travel(-18);
}”
The big problem with this kind of robot is the lack of precision. Each robot is different
and to be able to create an API that takes in count every single type of robot is not simple. To
try and resolve the possible problem of not going in a straight line or not being parallel to the
walls another functions is needed.
To use the ultrasonic sensor, we need to instantiate the ultrasonic sensor class, it only
needs one argument, the number of the sensor port:
“UltrasonicSensor sensor = new UltrasonicSensor (SensorPort.S1);”
Knowing that each cell has fixed values and that the precision of the rotations is good
we can assume that when the robot is at the entrance it will be put at an X distance from the
right wall and a Y distance from the left. The program will store those distances to compare
them with the new distances each time it advances one cell. If the distance is lower than the
15
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
reference distance, then the robot is not parallel to the walls and a simple rotation to counter
that effect should be enough. That way, even if the robot may not go in a straight line,
hopefully it won’t collide with the walls and be able to find the exit. The rotation needs to be
small to avoid adding more precision errors when turning. This function needs to be called
only if we know there is wall present to the right or to the left. So we obtain two different
functions, one for the right wall and another for the left wall.
“public void testParallelRight(int refdistance){
int Distance;
Distance= sensor.getDistance();
pause(30);
Distance = sensor.getDistance();
if( refdistance > Distance) {
robot.rotate(-5);
}
}”
The code is the same for the left side, only the negative rotation becomes a positive
one. There is a bug in the Lejos system concerning the Ultrasonic sensor which seems to be
affecting only in a few users. If our code contains only one “Distance = sensor.getDistance();”
it won’t return a true result, it will only return 255, which is the distance of not finding
anything. The solution is to leave a pause in the thread, in this case 30 ms, and then call the
method again so we get a satisfactory result.
3.
Tremaux algorithm in Java
The Tremaux algorithm needs a few components to work properly. Before starting the
actual algorithm we’ll look at all the components needed for the algorithm to work.
First of all, we need the maps. The first map is simply to map each cell visited by the
robot. In Java, we will use a simple 2 dimension array. For this method, we suppose that the
maze is no bigger than a 20*20 cell maze. We don’t want to go outside this map so we will
put the outer boundaries to the value of an already visited cell. So, to be able to obtain a
20*20 maze we will need to create a 22*22 array. Once the array is created we need to define
the values to be put in the maze. For this project we will use the following values:
- 0 = not visited
- 1 = all possibilities visited
- 2 = there’s still a path left unvisited
- 3 = there’s still two paths left unvisited
These units will be used to let the robot know which path has been visited and where
to go next after backtracking from a cul-de-sac.
The other map is the one actually recording the path followed by the robot and has 3
possible values:
- 1 = left
- 2 = forward
- 3 = right
The size of that map must be the exact quantity of the possible cells, so that means
20*20. After creating the maps, we obtain:
“int maze_X = 22; int maze_Y = 22;
int maze[][] = new int[maze_X][maze_Y];
int myPath[] = new int[maze_X*maze_Y];”
16
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
Once we have the maps, we will need some methods to know what cells are around us.
For that matter we will create a method that gives the maze coordinates of the cells around the
robot. To simplify the code, only two methods for the one in front and the one behind will be
implemented. To get the ones on the side, we will implement a directions system. The relative
position of the robot will always be saved so we know where the left and the right of the robot
are. We will call those directions North, East, South and West. These directions don’t
correspond to the actual cardinal points and we will suppose that at the entrance of the maze
the robot is facing North. There is a problem however, having decided to show the finished
map once the robot has found the exit we will not follow the orthonormal basis because the
LCD screen in the NXT uses one as follows :
Figure 6 The program basis
With that decision, we can now code the methods to get the cells. The first one is the
cell in front. To simplify even more the coding, we will code 2 different methods to get the
coordinates of the cell. We then get InFrontX() and InFrontY():
“public int InFrontX(int Direction) {
int result;
if (Direction == 0){
result = 1;
}
else if (Direction == 2) {
result = -1;
}
else result = 0;
return result;
}”
The same type of method is coded for the other coordinates methods. The result gives
us the relative coordinates of the cell needed but there is a little problem. We need the left and
right directions of the actual direction of the robot to be able to get those coordinates. That’s
why we need a couple of methods that return the directions of the left and right of the
direction passed as argument. We then get:
“public int RightOf(int Direction){
int newDir;
if (Direction != 3){
newDir = Direction +1;
}
else {
newDir = 0;
}
return newDir;
}”
17
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
Another very important function is the function that allows us to know if there is a
wall near the robot. The wallFinder() will use the ultrasonic sensor to find the distance
between the robot and the nearest wall. Once we’ve got the distance, we need to compare it to
what we consider is the distance between the robot and a wall in that cell. That value will
depend of what on the size of a maze cell and the size of the robot. The value doesn’t have to
be very precise, because of the distance between a wall in the cell next to the robot and the
one 2 cells away is quite big. As a result we obtain:
“public int wallFinder(){
int Distance;
int wallPresence;
sensor.getDistance();
pause(30);
Distance= sensor.getDistance();
LCD.drawInt(Distance, 5, 5);
if (Distance > 12)
{
wallPresence = 0;
}
else
{
wallPresence = 1;
}
return wallPresence;
}”
The last method needed before starting the Tremaux algorithm is the
DecreasedVisited(). That method will decrease the value of the cell. If the value of the cell is
already at 1 it will return the same value. We obtain :
“public int DecreaseVisited(int maze) {
maze = maze -1;
if (maze < 1) {
return 1;
}
else {
return maze;
}
}”
We know have all the methods needed to begin the coding of the main method itself.
This algorithm works in two stages, when it can advance and when it can’t.
The first part is pretty straight-forward. We robot will move through the maze using a
right-hand-priority. Each time the robot moves, the values of the cells will be updated. But
first, we need to know if we are at the exit, because if we are, then there is no need to
continue. Once we are sure that we are not at the exit, we can check if there is any wall in the
right, the left and in front of the robot. Using the methods created earlier we can easily rotate
the robot into position and see if there is a wall near.
while (traveler.exit() == false) { //loop while we haven't found the exit
traveler.rotateLeft();
18
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
pause(500);
walls[0] = traveler.wallFinder();
traveler.rotateRight();
pause(500);
walls[1] = traveler.wallFinder();
traveler.rotateRight();
pause(500);
walls[2] = traveler.wallFinder();
traveler.rotateLeft();”
Once we have the actual information we can put the value in the current cell:
“maze[cellX][cellY] = 3 - walls[2] - walls[1] - walls[0];
if (maze[cellX][cellY] == 0) {
maze[cellX][cellY] = 1;
}”
Now we need to decide which route to take. For that matter we will assume that the
next move is to backtrack. If we change that value, it means that we can move either forward,
left or right. To be able to give the priority to the right, we will first try to go left if we can,
then in front, and finally to the right. That way, if the right path is open the robot will go that
way because it is the last modification done to the cells and the nextMove variable. If we
don’t then it will be the front path and lastly the left path. To be able to go anywhere not only
there must be no wall but the value of the case in front of that path must be 0. That way, when
we come back in an intersection we have already been, in other words backtracking from a
cul-de-sac, it won’t go a path that has already been visited. If there is no wall in front of the
robot and the cell in front of us not available then we need to decrease the value of the current
cell and the cell in front of the robot to keep the map coherent. This is the result for the turn
left, the same principle applies for the right and front moves:
“ if (walls[0] == 0){
RelX = traveler.InFrontX(traveler.LeftOf(Direction));
RelY = traveler.InFrontY(traveler.LeftOf(Direction));
if (maze[cellX+RelX][cellY+RelY] == 0) {
nextMove = 1;
}
else {
maze[cellX][cellY] = traveler.DecreaseVisited(maze[cellX][cellY]);
maze[cellX+RelX][cellY+RelY] =
traveler.DecreaseVisited(maze[cellX+RelX][cellY+RelY]);
}
}”
At this point, we have all the information to actually do the next move, so the robot
will turn if it needs to, update the direction and then move forward. Once the robot has
moved, the actual cell value will be updated and we obtain the following code:
“switch(nextMove) {
case 1:
traveler.rotateLeft();
Direction = traveler.LeftOf(Direction);
break;
case 3:
traveler.rotateRight();
19
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
Direction = traveler.RightOf(Direction);
break;
}
if(nextMove != -1) {
traveler.forward();
myPath[moves++] = nextMove;
cellX = cellX + traveler.InFrontX(Direction);
cellY = cellY + traveler.InFrontY(Direction);
continue;
}”
If the nextMove value hasn’t changed, that means that the robot is surrounded by walls
and we’ve reached the famous cul-de-sac. In that case we need to use the path followed by the
robot to get here so that the robot return to an intersection with an unexplored path. The robot
will go backward until it either reaches a cell with a value superior to 1 or the entrance of the
maze, which means there is no exit. Also, each time it backtracks we need to check if it needs
to turn left or right but, since it is backtracking, when myPath[moves] says it needs to go right
or left, it actually needs to turn the other way. Finally, once the robot has moved, the actual
cell of the robot needs to be updated. The final piece of code looks like this:
“while ((maze[cellX][cellY] < 2) && (moves-- > 0)){
if (myPath[moves] == 1) {
traveler.rotateLeft();
Direction = traveler.RightOf(Direction);
}
else if (myPath[moves] == 3) {
traveler.rotateRight();
Direction = traveler.LeftOf(Direction);
}
traveler.backward();
cellX =cellX + traveler.BehindX(Direction);
cellY =cellY + traveler.BehindY(Direction);
}
}”
Once we put all the pieces together we obtain a fully functional program that achieves
all the objectives set by this project. Next, we will actually test this program to look at its
performance and efficiency.
VI. Testing
A.
Testing protocol
To test this program two different tests are made. First, we will test the algorithm and
the map created by the path followed by the robot. This test is to test only the algorithm so the
robot it’s actually not needed. Another map will be created to represent a maze. Using a value
that is not used to represent the values of the visited cells is necessary to represent the walls.
Once the maze is created we change the wallFinder method to search the maze and not use the
ultrasonic sensor. We will also modify the main program to show the current cell and the path
followed as we go through the maze. The second test is to test the movement and navigation
of the robot. In this case it needs to have a maze and go through it without hitting the walls
and test all the methods used to control the robot.
20
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
B.
Algorithm test
This test is simple, it requires a maze that represents all or almost all possibilities that
the algorithm could face in the real world. In this test, the algorithm should find the exit and
put the good values in each cell. For this test, we will use the following maze, which includes
turns, intersections, cul-de-sac, etc…
5
Entrance
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
exit
5
5
5
5
5
Figure 7 The algorithm test
C.
Navigational test
This test is to test the navigational ability of the robot; it is not about a complicated maze, is
about being able to turn, move forward and being able to stay parallel to the walls. The maze
is made out of cardboard with 18*25 cm cells. The walls are 15 cm high. We will use this
maze for this test:
Figure 8 Navigational test
21
5
5
5
5
5
5
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
VII. Results
A.
Algorithm test
For this test, the results were perfect. It simply went to the exit and accurately mapped
the maze. The depth-first search is an efficient mathematical algorithm to search through a
tree, that’s why it works very well with a maze. It doesn’t try to search the shortest path to the
exit, but it will always find the exit in this kind of maze.
5
Entrance
5
5
5
5
5
5
5
1
1
1
1
1
1
5
5
5
5
5
5
5
1
5
5
5
5
1
1
1
1
1
1
5
1
5
5
5
5
5
5
5
1
5
5
5
5
5
5
1
5
5
5
1
5
5
5
1
1
1
1
1
1
1
2
1
1
1
3
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
5
exit
5
5
5
5
5
Figure 9 The algorithm test partial result
B.
Navigational test
The second test wasn’t completely satisfactory. It worked well but there is a big
problem, the hardware itself. In this case, with only an ultrasonic sensor and a touch sensor, it
is not possible to have a very precise system and it’s even harder to have an absolute
positioning system. In this project, the robot keeps in track but requires a very fine tweaking
of the variables in the TachoPilot, the rotations and the parallel tests. The resulting robot is a
robot that can go straight at first but it will eventually start going from wall to wall, which is
the whole purpose of the parallel test methods.
In this case, the error was to make a small maze, and because the ultrasonic sensor has
a precision of around 3 centimetres it means that sometimes the parallel test doesn’t work
properly. One possible solution is to make everything bigger because when things get bigger,
the precision needed for that system to work gets smaller.
Another solution, which I believe is the best solution for a navigational system, is an
internal compass. An internal compass would be perfect to get an absolute positioning system
and be able to get through a maze without following a line on the floor or touching a wall.
22
5
5
5
5
5
5
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
VIII. Conclusion
For this project, the objective was to create a functional robot that is able to exit a
maze and retain the path followed. I do believe that both of the objectives were met. The
resulting robot is able to enter the maze and find the exit while retaining the path it used. I
also believe that there is room for improvement but it would need the use of more hardware
and finer tuning of the robot.
I’ve definitely learned a lot during this project. First of all, I’ve been introduced to the
world of robotics, which is an expanding domain of the computer science, along with
electronics and mechanics. Robotics is a very interesting domain and there’s still a lot of room
for innovation.
With this project, I’ve also learned a new programming language, Java, which is one
of the most widely used programming language in the world. It’s a very flexible and powerful
language. I would not use it in every situation but it is a very good replacement for languages
like COBOL or FORTRAN.
Finally, doing this project has let me improve my way of managing my time to work
on this project. One big problem with big project is time management. When you are a
student and don’t have any classes for this project it becomes really important to manage your
time working on the project very well. It is quite easy to forget that there is a project in which
you have to spend some time every single week and that can become very stressful. Having
been through that experience a couple of time, I put a strict schedule to work in the project
and I can definitely say that it pays off.
IX. References
1. http://mindstorms.lego.com/en-us/whatisnxt/default.aspx
2. http://en.wikipedia.org/wiki/Lego_Mindstorms_NXT
3. http://www.astrolog.org/labyrnth/algrithm.htm
23
Jean-Luc Pasquier: 10077923
X.
Oxford Brookes University
U08096
Appendices
A.
Program Code
import java.lang.Math.*;
import lejos.nxt.*;
import lejos.robotics.navigation.*;
/*Lego Robot that exits mazes using the Tremaux Algorithm*/
public class TravelTest {
//All the variables and instanciations of the motors, sensors, etc...
TachoPilot robot; // The Class that will deal with the movement of the robot
UltrasonicSensor sensor = new UltrasonicSensor (SensorPort.S1);// The sensor
TouchSensor exitdetector = new TouchSensor(SensorPort.S4);
// *END*
// Extra functions needed
public static void pause(int time) {
try{ Thread.sleep(time);
}
catch(InterruptedException e){}
}
public int getDistance(){
int Distance= sensor.getDistance();
pause(30);
return sensor.getDistance();
}
void fill_map(int maze[][], int maze_X, int maze_Y) {//fill the outer boundaries of the maze
to 1
int a=0;
int b=0;
for ( a=0; a<maze_X; a++) {
for ( b=0; b<maze_Y;b++) {
maze[a][b] = 1;
}
}
for (int c=1; c<maze_X-1; c++) {
for (int d=1; d<maze_Y-1;d++) {
maze[c][d] = 0;
}
}
}
void show_map(int maze[][], int maze_X, int maze_Y) {//show the result
for (int a=1; a<maze_X; a++) {
for (int b=1; b<maze_Y;b++) {
LCD.drawInt(maze[a][b], a, b);
}
24
Jean-Luc Pasquier: 10077923
Oxford Brookes University
}
}
public boolean exit() {
if ( exitdetector.isPressed()== true) {
return true;
}
return false;
}
// *END*
//All the functions needed to get informations and get around the maze
public int RightOf(int Direction){ //Gives the Right direction of the actual direction
int newDir;
if (Direction != 3){
newDir = Direction +1;
}
else {
newDir = 0;
}
return newDir;
}
public int LeftOf(int Direction){ //Gives the Left direction of the actual direction
int newDir;
if (Direction != 0){
newDir = Direction -1;
}
else {
newDir = 3;
}
return newDir;
}
public int BehindX(int Direction) {
int result;
if(Direction == 0){
result = -1;
}
else if (Direction == 2){
result = 1;
}
else result = 0;
return result;
}
public int BehindY(int Direction) {
int result;
if(Direction == 3){
result = 1;
}
else if (Direction == 1){
result = -1;
}
25
U08096
Jean-Luc Pasquier: 10077923
Oxford Brookes University
else result = 0;
return result;
}
public int InFrontX(int Direction) {//the cell in front of the robot
int result;
if (Direction == 0){
result = 1;
}
else if (Direction == 2) {
result = -1;
}
else result = 0;
return result;
}
public int InFrontY(int Direction) {
int result;
if (Direction == 3){
result = -1;
}
else if (Direction == 1) {
result = 1;
}
else result = 0;
return result;
}
public int DecreaseVisited(int maze) {
maze = maze -1;
if (maze < 1) {
return 1;
}
else {
return maze;
}
}
public int wallFinder(){ //the ultrasonic sensors gets the distance to the wall.
int Distance;
int wallPresence;
sensor.getDistance();
pause(30);
Distance= sensor.getDistance();
LCD.drawInt(Distance, 5, 5);
if (Distance > 12)
{
wallPresence = 0;
}
else
{
wallPresence = 1;
}
return wallPresence;
26
U08096
Jean-Luc Pasquier: 10077923
Oxford Brookes University
}
// *END*
//Functions to move the robot
public void forward() { //The Robot advances one cell
robot.travel(19);
}
public void backward() { //The Robot advances one cell
robot.travel(-19);
}
public void backturn() { //The Robot makes a 180 degrees turn
robot.rotate(-180);
}
public void rotateRight(){ //The Robot rotates to the right
robot.rotate(90);
pause(500);
}
public void rotateLeft(){
robot.rotate(-90);
pause(500);
}
public void testParallelRight1(int refdistance){
int Distance;
Distance= sensor.getDistance();
pause(30);
Distance = sensor.getDistance();
if( refdistance > Distance) {
robot.rotate(-5);
}
}
public void testParallelLeft1(int refdistance){
int Distance;
Distance= sensor.getDistance();
pause(30);
Distance = sensor.getDistance();
if( refdistance > Distance) {
robot.rotate(5);
}
}
// *END*
public static void main(String[] args ){
TravelTest traveler = new TravelTest();
traveler.robot = new TachoPilot(5.6f,13.7f,Motor.A,Motor.C);
int walls[] = new int[3];
int cellX;//The current cell we are on.
int cellY;//The current cell we are on.
27
U08096
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
int maze_X = 22;// We can solve a 20 by 20 cell maze
int maze_Y = 22;// So that the robots doesn't want to go outside the maze, we set the outer
cells to 1
int maze[][] = new int[maze_X][maze_Y]; //Identifies each cell of the maze. We can solve
a 20 by 20 cell maze
int myPath[] = new int[maze_X*maze_Y];
int Direction = 0;//we suppose that the robot faces north when it gets in;
int nextMove = 0;
int moves = 0;
int exit = 0;
int RelX;
int RelY;
int oldDistanceL = 0;
int oldDistanceR = 0;
// North = 0; West =1; South = 2; East =3;
//notvisited = 0; we haven't visited that map position.
//visited = 1; we have already visited all the posibles paths of that cell
//onepathleft = 2; we still have one path to go
//twopathsleft = 3;we still have 2 two paths to go
traveler.fill_map(maze,maze_X,maze_Y);
cellX = 4; //we suppose the robots starts at cell 4 2;
cellY = 2;
while (traveler.exit() == false) { //loop while we haven't found the exit
traveler.rotateLeft();
pause(500);
walls[0] = traveler.wallFinder();
if (moves == 0) {
oldDistanceL = traveler.getDistance();
}
else if (walls[0] == 1){
pause(100);
traveler.testParallelLeft1(oldDistanceL);
pause(200);
}
traveler.rotateRight();
pause(500);
walls[1] = traveler.wallFinder();
traveler.rotateRight();
pause(500);
walls[2] = traveler.wallFinder();
if (moves == 0) {
oldDistanceR = traveler.getDistance();
}
else if( walls[2]== 1){
28
Jean-Luc Pasquier: 10077923
Oxford Brookes University
pause(100);
traveler.testParallelRight1(oldDistanceR);
pause(200);
}
traveler.rotateLeft();
//we assume all the paths unvisited
maze[cellX][cellY] = 3 - walls[2] - walls[1] - walls[0];
if (maze[cellX][cellY] == 0) {
maze[cellX][cellY] = 1;
}
nextMove=-1;
if (walls[0] == 0){
RelX = traveler.InFrontX(traveler.LeftOf(Direction));
RelY = traveler.InFrontY(traveler.LeftOf(Direction));
if (maze[cellX+RelX][cellY+RelY] == 0) {
nextMove = 1;
}
else {
maze[cellX][cellY] = traveler.DecreaseVisited(maze[cellX][cellY]);
maze[cellX+RelX][cellY+RelY] =
traveler.DecreaseVisited(maze[cellX+RelX][cellY+RelY]);
}
}
if (walls[1] == 0){
RelX = traveler.InFrontX(Direction);
RelY = traveler.InFrontY(Direction);
if (maze[cellX+RelX][cellY+RelY] == 0) {
nextMove = 2;
}
else {
maze[cellX][cellY] = traveler.DecreaseVisited(maze[cellX][cellY]);
maze[cellX+RelX][cellY+RelY] =
traveler.DecreaseVisited(maze[cellX+RelX][cellY+RelY]);
}
}
if (walls[2] == 0){
RelX = traveler.InFrontX(traveler.RightOf(Direction));
RelY = traveler.InFrontY(traveler.RightOf(Direction));
if (maze[cellX+RelX][cellY+RelY] == 0) {
nextMove = 3;
}
else {
maze[cellX][cellY] = traveler.DecreaseVisited(maze[cellX][cellY]);
maze[cellX+RelX][cellY+RelY] =
traveler.DecreaseVisited(maze[cellX+RelX][cellY+RelY]);
}
}
switch(nextMove) {
29
U08096
Jean-Luc Pasquier: 10077923
Oxford Brookes University
case 1:
traveler.rotateLeft();
Direction = traveler.LeftOf(Direction);
break;
case 3:
traveler.rotateRight();
Direction = traveler.RightOf(Direction);
break;
}
if(nextMove != -1) {
traveler.forward();
myPath[moves++] = nextMove;
cellX = cellX + traveler.InFrontX(Direction);
cellY = cellY + traveler.InFrontY(Direction);
continue;
}
//cul-de-sac
while ((maze[cellX][cellY] < 2) && (moves-- > 0)){
if (myPath[moves] == 1) {
traveler.rotateLeft();
Direction = traveler.RightOf(Direction);
}
else if (myPath[moves] == 3) {
traveler.rotateRight();
Direction = traveler.LeftOf(Direction);
}
traveler.backward();
cellX =cellX + traveler.BehindX(Direction);
cellY =cellY + traveler.BehindY(Direction);
}
}
traveler.show_map(maze, maze_X, maze_Y);
Button.waitForPress();
}
}
B.
Algorithm test Code
import lejos.nxt.*;
/**
*
* @author WandereR
*/
public class Test {
// Extra functions needed
public static void pause(int time) {
try{ Thread.sleep(time);
}
30
U08096
Jean-Luc Pasquier: 10077923
Oxford Brookes University
catch(InterruptedException e){}
}
void fill_map(int maze[][], int maze_X, int maze_Y) {
int a=0;
int b=0;
for ( a=0; a<maze_X; a++) {
for ( b=0; b<maze_Y;b++) {
maze[a][b] = 1;
}
}
for (int c=1; c<maze_X-1; c++) {
for (int d=1; d<maze_Y-1;d++) {
maze[c][d] = 0;
}
}
}
void fill_maze(int maze[][], int maze_X, int maze_Y) {
int a=0;
int b=0;
for ( a=0; a<maze_X; a++) {
for ( b=0; b<maze_Y;b++) {
maze[a][b] = 1;
}
}
for (int c=1; c<maze_X-1; c++) {
for (int d=1; d<maze_Y-1;d++) {
maze[c][d] = 0;
}
}
maze[1][1] = 5;
maze[2][1] = 5;
maze[3][1] = 5;
maze[4][1] = 5;
maze[5][1] = 5;
maze[3][2] = 5;
maze[3][3] = 5;
maze[5][3] = 5;
maze[5][4] = 5;
maze[5][5] = 5;
maze[4][5] = 5;
maze[3][5] = 5;
maze[2][5] = 5;
maze[1][5] = 5;
maze[1][3] = 5;
maze[1][4] = 5;
maze[6][1] = 5;
maze[7][1] = 5;
maze[7][2] = 5;
maze[7][3] = 5;
maze[7][4] = 5;
31
U08096
Jean-Luc Pasquier: 10077923
Oxford Brookes University
maze[7][5] = 5;
maze[4][6] = 5;
maze[4][7] = 5;
maze[5][7] = 5;
maze[6][7] = 5;
maze[7][7] = 5;
maze[8][7] = 5;
maze[9][7] = 5;
maze[9][6] = 5;
maze[9][5] = 5;
maze[8][5] = 5;
}
void show_map(int maze[][], int maze_X, int maze_Y) {
for (int a=1; a<maze_X; a++) {
for (int b=1; b<maze_Y;b++) {
LCD.drawInt(maze[a][b], a, b);
}
}
}
public int decreaseVisited(int maze){
if(maze == 1){
return maze;
}
else {
maze = maze -1;
return maze;
}
}
// *END*
//All the functions needed to get informations and get around the maze
public int RightOf(int Direction){ //Gives the Right direction of the actual direction
int newDir;
if (Direction != 3){
newDir = Direction +1;
}
else {
newDir = 0;
}
return newDir;
}
public int LeftOf(int Direction){ //Gives the Left direction of the actual direction
int newDir;
if (Direction != 0){
newDir = Direction -1;
}
else {
newDir = 3;
32
U08096
Jean-Luc Pasquier: 10077923
Oxford Brookes University
}
return newDir;
}
public int RelPosX(int Direction) {
int result;
if (Direction == 0){
result = 1;
}
else if (Direction == 2) {
result = -1;
}
else result = 0;
return result;
}
public int RelPosY(int Direction) {
int result;
if (Direction == 3){
result = -1;
}
else if (Direction == 1) {
result = 1;
}
else result = 0;
return result;
}
// *END*
//Functions to move the robot
public int wallFinder(int filledMaze[][],int direction,int cellX,int cellY){
if (direction == 3) {
if (filledMaze[cellX][cellY-1] == 5){
return 1;
}
}
else if (direction == 1) {
if (filledMaze[cellX][cellY+1] == 5) {
return 1;
}
}
else if (direction == 0) {
if(filledMaze[cellX+1][cellY] == 5) {
return 1;
}
}
else if (direction == 2) {
if(filledMaze[cellX-1][cellY] == 5) {
return 1;
}
}
33
U08096
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
return 0;
}
// *END*
public static void main(String[] args ){
TravelTest traveler = new TravelTest();
int walls[] = new int[3];
int cellX;//The current cell we are on.
int cellY;//The current cell we are on.
int maze_X = 22;// We can solve a 20 by 20 cell maze
int maze_Y = 22;// So that the robots doesn't want to go outside the maze, we set the outer
cells to 1
int filledMaze[][] = new int[maze_X][maze_Y];
int maze[][] = new int[maze_X][maze_Y]; //Identifies each cell of the maze. We can solve
a 20 by 20 cell maze
int myPath[] = new int[maze_X*maze_Y];
int Direction = 0;//we suppose that the robot faces north when it gets in;
int nextMove;
int moves = 0;
int exit = 0;
int RelX;
int RelY;
// North = 0; West =1; South = 2; East =3;
//notvisited = 0; we haven't visited that map position.
//visited = 1; we have already visited all the posibles paths of that cell
//onepathleft = 2; we still have one path to go
//twopathsleft = 3;we still have 2 two paths to go
traveler.fill_maze(filledMaze, maze_X, maze_Y);
traveler.show_map(filledMaze, maze_X, maze_Y);
Button.waitForPress();
traveler.fill_map(maze,maze_X,maze_Y);
cellX = 1; //we suppose the robots starts at cell 1 2;
cellY = 2;
LCD.drawInt(1, cellX, cellY);
while (exit != 1) { //loop while we haven't found the exit
Button.waitForPress();
LCD.drawInt(Direction, 15, 1);
if((cellX == 7)&&(cellY==6) ){
exit = 1;
}
Direction = traveler.LeftOf(Direction);
walls[0] = traveler.wallFinder(filledMaze,Direction,cellX,cellY);
Direction = traveler.RightOf(Direction);
walls[1] = traveler.wallFinder(filledMaze,Direction,cellX,cellY);
34
Jean-Luc Pasquier: 10077923
Oxford Brookes University
Direction = traveler.RightOf(Direction);
walls[2] = traveler.wallFinder(filledMaze,Direction,cellX,cellY);
Direction = traveler.LeftOf(Direction);
//we assume all the paths unvisited
maze[cellX][cellY] = 3 - walls[2] - walls[1] - walls[0];
if (maze[cellX][cellY] == 0){
maze[cellX][cellY] = 1;
}
nextMove=-1;
//left
if (walls[0] != 1){
RelX = traveler.RelPosX(traveler.LeftOf(Direction));
RelY = traveler.RelPosY(traveler.LeftOf(Direction));
if (maze[cellX+RelX][cellY+RelY] == 0) {
nextMove = 1;
}
else {
maze[cellX][cellY] = traveler.decreaseVisited(maze[cellX][cellY]);
maze[cellX+RelX][cellY+RelY] =
traveler.decreaseVisited(maze[cellX+RelX][cellY+RelY]);
}
}
if (walls[1] != 1){
RelX = traveler.RelPosX(Direction);
RelY = traveler.RelPosY(Direction);
if (maze[cellX+RelX][cellY+RelY] == 0) {
nextMove = 2;
}
else {
maze[cellX][cellY] = traveler.decreaseVisited(maze[cellX][cellY]);
maze[cellX+RelX][cellY+RelY] =
traveler.decreaseVisited(maze[cellX+RelX][cellY+RelY]);
}
}
if (walls[2] != 1){
RelX = traveler.RelPosX(traveler.RightOf(Direction));
RelY = traveler.RelPosY(traveler.RightOf(Direction));
if (maze[cellX+RelX][cellY+RelY] == 0) {
nextMove = 3;
}
else {
maze[cellX][cellY] = traveler.decreaseVisited(maze[cellX][cellY]);
maze[cellX+RelX][cellY+RelY] =
traveler.decreaseVisited(maze[cellX+RelX][cellY+RelY]);
}
}
switch(nextMove) {
case 1:
35
U08096
Jean-Luc Pasquier: 10077923
Oxford Brookes University
Direction = traveler.LeftOf(Direction);
break;
case 3:
Direction = traveler.RightOf(Direction);
break;
}
if(nextMove != -1) {
myPath[moves] = nextMove;
moves++;
LCD.drawInt(maze[cellX][cellY], cellX, cellY);
cellX =cellX + traveler.RelPosX(Direction);
cellY =cellY + traveler.RelPosY(Direction);
LCD.drawInt(cellX, 12, 5);
LCD.drawInt(cellY, 12, 6);
continue;
}
LCD.drawInt(4, cellX, cellY);
LCD.drawInt(7, 15, 6);
Direction = traveler.RightOf(traveler.RightOf(Direction));
LCD.drawInt(Direction, 15, 5);
Button.waitForPress();
while ((maze[cellX][cellY] < 2) && (moves > 0)){
if (myPath[moves] == 1) {
Direction = traveler.RightOf(Direction);
}
else if (myPath[moves] == 3) {
Direction = traveler.LeftOf(Direction);
}
cellX = cellX + traveler.RelPosX(Direction);
cellY = cellY + traveler.RelPosY(Direction);
moves--;
LCD.drawInt(Direction, 15, 5);
LCD.drawInt(cellX, 12, 5);
LCD.drawInt(cellY, 12, 6);
Button.waitForPress();
}
}
LCD.drawString("YOU WON", 10, 10);
Button.waitForPress();
}
}
36
U08096
Jean-Luc Pasquier: 10077923
C.
Oxford Brookes University
U08096
Interim Report
Jean-Luc Pasquier
29/11/10
Interim report
Module: 8096 BSc Computing Project
School of Technology
Design, implementation and testing of a maze
solving LEGO robot
Student: Jean-Luc Pasquier 10077923
Supervisor: Mr. Ken Brownsey
37
Jean-Luc Pasquier: 10077923
I)
Oxford Brookes University
U08096
Introduction
While I was doing the proposal I came across various decisions
to be made that because I didn’t fully understand the environment of
the project and the major challenges it raises. That’s why in this
interim report I tried to explain those challenges, the advantages and
disadvantages of each decisions and my general progress to date.
Firstly, I will describe the different maze solving algorithms,
their advantages and disadvantages followed by my final decision.
After that, I will talk about my choice more in detail. I will explain the
keys issues on applying that algorithm to the constraints of the robot
and the implementation of the code. Finally, I will explain how the
robot will deal with the real world problems such as following a line
or getting back on track after a dead end.
38
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
I) Maze solving Algorithms
I’ve made quite a long research on how robots solve mazes. And
there are quite a few techniques out there. I’ve selected the most
interesting ones that can be coded and used by the robot with the
hardware at my disposal.
1) Wall follower (right-hand or left-hand)
a. Principle
i. The Wall follower method solves a big number
off mazes, but not all of them. It consists in a
simple rule to apply. It states that, when
possible, if you follow the left (or right) and
turn left (or right) whenever possible, you will
find the exit.
b. Advantages
i. You are very likely to find the exit
ii. It’s really simple to apply, only one rule
iii. There is no need to know about position and
orientation
iv. It only needs to follow a line, or a wall and
recognize the exit
c. Disadvantages
i. It doesn’t work for every maze, especially those
containing loops and when the exit or the
entrance aren’t in the external walls
d. Implementation
39
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
i. It is really simple to implement because there
is no need to know any position or track where
you’ve been. It’s simply follow a line or a wall
and being able to recognize the exit.
2) Tremaux Algorithm
a. Principle
i. The Tremaux algorithm consists on storing the
path we follow in the maze. In our case, we can
consider dividing the maze in cells. Each square
is unvisited, marked once or marked twice.
Every time we advance in one direction we
mark it. On arriving at a junction that has not
been visited before, we pick a random
direction and mark the path. When arriving at a
marked junction and if our current path is
marked only once we then turn around and
walk back and mark the path again. If this is
not the case, pick the direction with the fewest
marks. When you finally reach the solution,
paths marked exactly once will indicate a direct
way back to the start.
b. Advantages
i. We will always find the exit
ii. We have stored a direct path to the exit
c. Disadvantages
i. It’s more complicated to implement
d. Implementation
i. The robot will need to know it’s relative
position
40
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
ii. The robot will need “memory” to store the
maze
iii. May be limited by the quantity of ram available
on big mazes
3) Recursive backtracker
a. Principle
i. The Recursive backtracker is essentially a
depth-first search. It tries to move in the four
directions. When we hit a wall, we return false,
else if, we are at the exit, we return true. We
mark the spot when we try a new position and
erase when we return false. That way, we will
get a single solution when we get to the exit.
b. Advantages
i. It is a fairly simply code, because it calls himself
ii. We will always find the exit
c. Disadvantages
i. We don’t get necessarily the shortest solution
ii. It’s more complicated to implement
d. Implementation
i. The implementation is more complicated since
we have a 3D maze, not 2D. We are more likely
to encounter a lot of collision problems.
ii. We need to divide the maze into squares to be
able to well define the path.
41
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
After carefully examination of the 3 different maze solving
algorithms I think that the best suited algorithm to implement is the
Tremaux algorithm.
I cannot use the wall follower because it cannot solve all the
mazes. As for the Recursive backtracker, I find the representation of
the “hitting a wall” not very suited for my project. Finally, the
Tremaux algorithm is designed to be able to be used by a human in a
maze. I find it very interesting because it represents our process to
find the exit with a maze. Even if we cannot give the details of how
we resolve a maze to someone, this algorithms actually does what
our brain does. It goes through the maze, and when we hit a deadend, we go back and go where we haven’t been already.
II) The Tremaux Algorithm
1) Algorithm Implementation
To implement this algorithm in the robot, there are a few things
that are needed. For instance, we need to divide the maze in cells.
We need to have directions. We also need to have memory to
memorize each cell’s marking.
a) Directions
We will follow the usual convention, using north, west, south
and east; these coincide with the north of the maze. For each
42
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
direction we need to assign a number to define it. For example:
North=0, West=1, South=2, East=3.
b) Cell marking
Each cell will have a value that represents the state of that
cell. We need to know if we haven’t been to that cell, if we
passed once, twice or three times. We need them for the “T”
shaped intersections and the four-way intersections. An
example of the cell markings are:
0: It means we haven’t been in that cell
1: This corresponds to a position which we’ve passed
already
2: This corresponds to a cell that we have already visited,
but that we still have one path that we haven’t visited. It usually
to a “T” shaped intersection or when we have visited 2 paths of
a four-way intersection.
3: This represents a path in which two routes have been
left unexplored. It’s is mainly used in four-way intersections.
Apart from the state of each cell, we need to find the
possible paths for each cell, so we need to store which path we
can take.
2) Technical constraints
The NXT brick comes with a limited amount of memory, but in
our case, the maze will not be big enough to use the full amount of
memory, but the problem may arise in very big mazes.
43
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
Having only one ultrasonic sensor, so that means that the robot
will have to do 90 degrees in both directions to get the information
for each cell.
The maze will have to be formatted to correspond to the cells;
otherwise the algorithm won’t work, given that it goes x centimetres
and then it makes its measurements.
III) General Progress
For the most part, I’ve been doing research on the subject and
experimenting with the hardware. I made a prototype of the robot
with all 3 sensors, which I think I won’t use 2 of them. It has 2 servo
motors that work in harmony to go straight and in reverse between
each other to be able to turn, like a tank.
I’ve been experimenting with the ultrasonic sensor, which will
be used to detect the walls and the lack of wall (the path). It works
pretty well with hard white paper. That’s why I think I’ll build a maze
out of white hard paper because it is pretty cheap and easy to get.
The maze will have only straight angle turns. A coloured square will
be drawn in the ground to detect the exit of the maze.
After doing some research, I found that the motors have a
Synchronized Motors feature, so the robot can go in a straight line
without too much problem because earlier versions of the LEJOS NXT
API didn’t have that feature, which made synchronising the two
motors much harder, so the robots actually went zigzagging instead
of straight.
44
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
The maze will have to be quite large so that the robot can turn
properly, but I’ll make the robot as little as possible to avoid those
kinds of problems.
With my current prototype I have a problem with turning
because I use 2 wheels at the back; I take those out and put a free
spinning wheel so that it can turn properly.
IV) References
-
Building with LEGO Mindstorms NXT by Dave Astolfo
http://www.astrolog.org/labyrnth/algrithm.htm
http://en.wikipedia.org/wiki/Maze_solving_algorithm
http://www.enseignement.polytechnique.fr/profs/informatiqu
e/Jean-Jacques.Levy/poly/main5/node9.html
45
Jean-Luc Pasquier: 10077923
Oxford Brookes University
U08096
A maze-solving robot using Lego Mindstorms
This project involved the specification, design and implementation of a maze-solving
robot, coded in Java and using the Lejos API. The program is used to control a robot capable
of finding the exit of a maze using only its sensors and trying to keep the robot on track. The
text first introduces the Lego Mindstorms set, followed by the coding platforms that can be
used with the NXT brick and the maze-solving algorithm. After setting the objectives of the
project, this document sets the specifications and requirements for the robot and the maze.
Having defined the specifications of the robot, the design analyses all the functions
needed to allow the program to work. Once the design is done, the document moves into the
implementation of the movement and navigational systems for the robot and the coding of the
Tremaux algorithm in a java and robotic environment.
With design and implementation done, the testing and results are discussed with
emphasis of the problems encountered and the possible solutions to those problems.
46