Maze Following Robot Logic


I am building an automatic maze-solving robot for my Mechatronics class. I have attached pictures of the wiring and everything I used to hook-up the robot. It has 3 HC-SR04 ultrasonic sensors attached on the front, left, and right of the robot. It also has 2 servo motors attached to the Arduomotor shield that drive the wheels.

So, the basic premise of the robot is that it drives forward down the center of the maze until it comes within 4 centimeters of a wall and stops. Once it hits that point it needs to determine whether or not it needs to turn to the right or the left. To do this we have the program compare the distance on the left and the right side of the robot using the ultrasonic sensors.

The functions for the motor and the functions for the ultrasonic sensors work correctly by themselves. The problem I am having is how to write the logic so that the robot can correctly navigate the maze. I will call the functions to drive the motor forward and turn it left or right depending on the readings from the ultrasonic sensors, but the logic needs to correctly determine the readings from the sensors to call the correct functions.

If you could provide me with any sort of help or direction, without seeing the robot in person, from the pictures and the code I have included, that would be awesome! Thanks in advance.


HC-SR04 Ping distance sensor:
 VCC to arduino 5v 
 GND to arduino GND
 Echo to Arduino pin 7 
 Trig to Arduino pin 8
//=======================================================MOTOR DEFINITIONS=====================================================================

#define CW  0 // Clockwise and counter-clockwise definitions.
#define CCW 1 // Depending on how you wired your motors, you may need to swap.
#define MOTOR_A 0 // Motor definitions to make life easier:
#define MOTOR_B 1

//=======================================================SENSOR DEFINITIONS=====================================================================

#define sens_1_echoPin 7 // Echo Pin
#define sens_1_trigPin 8 // Trigger Pin
#define sens_2_echoPin 5 // Echo Pin
#define sens_2_trigPin 6 // Trigger Pin
#define sens_3_echoPin 4 // Echo Pin
#define sens_3_trigPin 2 // Trigger Pin

const byte PWMA = 3;  // PWM control (speed) for motor A
const byte PWMB = 11; // PWM control (speed) for motor B
const byte DIRA = 12; // Direction control for motor A
const byte DIRB = 13; // Direction control for motor B

int maximumRange = 200; // Maximum range needed
int minimumRange = 0; // Minimum range needed
long duration_1;
long duration_2;
long duration_3;
long distance_1;
long distance_2;
long distance_3;

long center;

void setup() {
 Serial.begin (9600);
 setupArdumoto(); // Set all pins as outputs 
 pinMode(sens_1_trigPin, OUTPUT);
 pinMode(sens_1_echoPin, INPUT);
 pinMode(sens_2_trigPin, OUTPUT);
 pinMode(sens_2_echoPin, INPUT);
 pinMode(sens_3_trigPin, OUTPUT);
 pinMode(sens_3_echoPin, INPUT);

void loop() {
center = (distance_2 + distance_3)/2; //calculates center of path
sensor_1(maximumRange, minimumRange);
sensor_2(maximumRange, minimumRange);
sensor_3(maximumRange, minimumRange);

Serial.print("sensor_1        sensor_2           sensor_3");

Serial.print("             ");
Serial.print("                    ");

//============================================================= MAZE SOLVING ALGORITHMN ===============================================================

    while ( (distance_1 >=4) && (2 == center) )
    driveArdumoto(MOTOR_A, CW, 255);  // Motor A at max speed.
    driveArdumoto(MOTOR_B, CW, 255);  // Motor B at max speed.
    if ( (distance_2 && distance_3) != center)
      if( distance_2 > distance_3)
        driveArdumoto(MOTOR_A,CW, 175);
        driveArdumoto(MOTOR_B,CW, 200);
      if( distance_3 > distance_2)
    driveArdumoto(MOTOR_A, CW, 200);  // left turn at half speed
    driveArdumoto(MOTOR_B, CCW, 200);  
    while ( (distance_2 > distance_3) && (distance_1 == 4) );
    driveArdumoto(MOTOR_A, CCW, 200);  // right turn at half speed
    driveArdumoto(MOTOR_B, CW, 200);  
    while ( (distance_3 > distance_2) && (distance_1 == 4) );

}// end of main loop

//=======================================================SENSOR FUNCTIONS=====================================================================

long sensor_1(int maximumRange, int minimumRange){
 digitalWrite(sens_1_trigPin, LOW); 

 digitalWrite(sens_1_trigPin, HIGH);
 digitalWrite(sens_1_trigPin, LOW);
 duration_1 = pulseIn(sens_1_echoPin, HIGH);
 distance_1 = duration_1/58.2; //Calculate the distance (in cm) based on the speed of sound.
 if (distance_1 >= maximumRange || distance_1 <= minimumRange){
 Serial.print("out of range_1"); 
 else {

 return distance_1;
 delay(50);  //Delay 50ms before next reading.   

long sensor_2(int maximumRange, int minimumRange){
 digitalWrite(sens_2_trigPin, LOW); 

 digitalWrite(sens_2_trigPin, HIGH);
 digitalWrite(sens_2_trigPin, LOW);
 duration_2 = pulseIn(sens_2_echoPin, HIGH);
 distance_2 = duration_2/58.2; //Calculate the distance (in cm) based on the speed of sound.
 if (distance_2 >= maximumRange || distance_2 <= minimumRange){
 Serial.print("out of range_2"); 
 else {

 return distance_2;
 delay(50);  //Delay 50ms before next reading.  

long sensor_3(int maximumRange, int minimumRange){
 digitalWrite(sens_3_trigPin, LOW); 

 digitalWrite(sens_3_trigPin, HIGH);
 digitalWrite(sens_3_trigPin, LOW);
 duration_3 = pulseIn(sens_3_echoPin, HIGH);
 distance_3 = duration_3/58.2; //Calculate the distance (in cm) based on the speed of sound.
 if (distance_3 >= maximumRange || distance_3 <= minimumRange){
Serial.print("out of range_3"); 
 else {

 return distance_3;
 delay(50); //Delay 50ms before next reading.  

//=======================================================MOTOR FUNCTIONS=====================================================================

// driveArdumoto drives 'motor' in 'dir' direction at 'spd' speed
void driveArdumoto(byte motor, byte dir, byte spd)
  if (motor == MOTOR_A)
    digitalWrite(DIRA, dir);
    analogWrite(PWMA, spd);
  else if (motor == MOTOR_B)
    digitalWrite(DIRB, dir);
    analogWrite(PWMB, spd);

// stopArdumoto makes a motor stop
void stopArdumoto(byte motor)
  driveArdumoto(motor, 0, 0);

// setupArdumoto initialize all pins
void setupArdumoto()
  // All pins should be setup as outputs:
  pinMode(PWMA, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(DIRA, OUTPUT);
  pinMode(DIRB, OUTPUT);

  // Initialize all pins as low:
  digitalWrite(PWMA, LOW);
  digitalWrite(PWMB, LOW);
  digitalWrite(DIRA, LOW);
  digitalWrite(DIRB, LOW);

I didn’t get far into reading your program before I became confused as to which distance related to which direction, left, right or front so I am afraid that I stopped trying to understand the details at that point.

Can I suggest that you rename your distance variables to reflect the direction that they relate to in order to make the logic clearer. It would also help to have the motors named as left and right rather than A and B and for their directions to be named forward and reverse rather than CW and CCW.

Also, whilst it is nothing to do with the logic of the program, I noticed that you have 3 separate functions to read distances when it could be done with a single function to which you passed a parameter indicating the sensor to be read. You are also passing max and min ranges to the function when there is no need as they are global variables.

...determine whether or not it needs to turn to the right or the left.
...write the logic so that the robot can correctly navigate the maze.

It can't know whether it 'needs' to turn left or right, because it doesn't know which -way- is right.

It's not 'navigating' the maze as much as -solving- the maze.

I don't see anything in your description or code the names how the maze is solved?

Well, maze at least implies that the route is unknown. If this assignment is with a -known- path that you need to measure and navigate through, then the next paragraph is still pretty applicable.

Regardless.... Thinking as a robot, I'd make a mental map, an x/y array, that describes my travel options at each point I traverse and stop and look around. The maze probably has a fixed set of dimensions, the width of halls/doorways/rooms. Heck, maybe the dimensions aren't even know exactly, maybe they even vary - those are more cans of stinky worms to deal with.

From the starting point, measure what you see in each direction, and record it. Traverse a distance. Record again. From any given point you will see walls on one, two, or three sides. Three sides are a dead end. Open sides give the option for more mapping, and blocked sides tell you where you can't traverse.

Mostly I'm trying to give you an idea of how to 'traverse an unknown space' in software. There's probably a whole arcane math science of maze theory that's way over my head. Me, I'd ignore the theory and just hit it with the brute force of faster motors and sensors. LOL!! If you're -expected- to be dealing with this towards an 'optimal solution style' end, then that's why you were given the assignment and probably no one here will simply 'give you the answer.' :wink: Sensors, motors, arrays - we got those. :slight_smile:


The simplest maze solution algorithm is in fact the recursive solution but it does require you to do some research into recursive techniques, its a lot simpler than brute force since you do bot need to store mapping and then use vast conditional analysis to determine where you have been .

If you look at the solution based on orthogonal movements only and use a senario similiar to..

  1. you are in a room (A), it has doors beyond any door could be the exit.

  2. Set up rules such as
    Left door first

  3. Go through Left door .. you are in a room(B) it has doors any door could be the exit

4 Set up rules such as
Left door first

So at 3. above you are recursively calling 1. - its not a jump to 1 its a recursive call, otherwise you have lost the info that you went from ( room A) originally

Keep recursively calling after using Left door until you exit or get to a room that has no left door, if there is no left door then this route will not work, so return through one room and apply your next rule in door selection .. then recursively continue till no doors

You will eventually find the exit door .. This is not the most elegant recursive solution but it works

you will find a more detailed academic discussion written by a good friend of mine..

hope it helps

So, the basic premise of the robot is that it drives forward down the center of the maze until it comes within 4 centimeters of a wall and stops. Once it hits that point it needs to determine whether or not it needs to turn to the right or the left. To do this we have the program compare the distance on the left and the right side of the robot using the ultrasonic sensors.

What does the maze look like? Where is the target destination? Will you take note of side corridors as you pass them?

Way back when, my colleague & I wrote a solver for a robot designed to solve the micro mouse maze - sadly the robot mechanics lagged way behind the solver code. That maze is of known dimensions and the start and target locations are known.

The idealized robot had forward, left and right distance sensors. It explored the maze, preferring straight ahead over turns, but took note of any side passages it encountered, gradually building up a picture of where the walls were. Once it found the destination (the centre), it stopped - in micro mouse, having learned the maze you then get to try for speed. I think the map was just a 2D array of bytes with each byte representing a cell in the maze. Each cell had four bits representing the walls.

Running the maze once a route was found was just a matter of flood filling the number of steps each cell is away from the centre. Then the robot simply navigates by choosing a cell to move to that needs fewer steps than the one it currently occupies.

For an optimal solution in micro mouse at least, the robot should explore the entire maze because there is usually more than one route to the goal.