# M3PI - Maze (not necessarily right angles) solver

Hi there,
I was working on a way to solve mazes that have non right angles. And I finally write a code that works pretty well. The only thing it miss is a way to simplify the path but I have some ideas.
The main idea is to follow a line until the robot sees something on its left. In order to do this I rewrite a part of the get line position function to not care about the right most sensor. When the robot sees something on its left, it turns on this direction until it change the following path.
Here is the code:

``````#include "mbed.h"
#include "m3pimaze.h"

#define MAX 0.5 //speed
#define MIN 0
#define P_TERM 1
#define I_TERM 0
#define D_TERM 20
#define SEN 200

m3pi m3pi(p23,p9,p10);

int sensors; // to store the sensors values
int debug=0;    // to store how many times we try to turn left when all sensors are full black
int vect;

// The line_position function has been change by this function which give the line position without taking care of the right sensor.
// In order to use this function the robot must spin to the right when arrives to a dead end because right turn will be see as dead end.
float get_left_position()
{
vect = -2500;
vect = -1000;
vect = 0;
vect = 1000;
vect = 0;   // in the real line_position function the value of vect is 2500
float avg;
float sum;
avg = sensors*vect + sensors*vect + sensors*vect + sensors*vect + sensors*vect;
sum = sensors + sensors + sensors + sensors + sensors;
float position = -2;
if (sum>0)
{
position = (avg/(sum*2500));
}
return position;
}

// line_following_pid function is a function who adjust the speed of each wheel in order to have the black line under the robot.
// The black line position is detected through the five front IR sensors. It also uses a PID to correct the direction.
bool line_following_pid()
{

float position_of_line = 0.0;
float prev_pos_of_line = 0.0;
float derivative,proportional;
float integral = 0;
float power;
float right;
float left;
float speed = MAX;
int foundjunction=0;

while (foundjunction==0) // This function ends when the robot find an other path.
{
// If all sensors are on full black, that means that there is a T or X junction or the robot is on the end.
if(sensors>SEN && sensors>SEN && sensors>SEN && sensors>SEN && sensors>SEN)
{
// end of maze
foundjunction=1;
return true;
}
// If all sensors are on full white, that means that there is no visible line ahead so the path the robot was following is a dead end.
else if(sensors<SEN && sensors<SEN && sensors<SEN && sensors<SEN && sensors<SEN)
{
foundjunction=1;
return true;
}
// If the most left sensor is on full black, that meands that there is a more left path the robot has to take.
else if(sensors>SEN)
{
// Found an intersection.
foundjunction=1;
return true;
}
// Get the position of the line.
position_of_line = get_left_position();
if (position_of_line==-2 || debug>0)    // this "if" is uses when the robot got his fives sensors full black and we want to be sure that it takes the left most exit if the junction is a T or a X junction and if the sensors stay full black so it the end and not a junction.
{
position_of_line = -0.999; // Here we say to the robot that the line is on its left
}
proportional = position_of_line;
// Compute the derivative
derivative = position_of_line - prev_pos_of_line;
// Compute the integral
integral = integral + proportional;
// Remember the last position.
prev_pos_of_line = position_of_line;
// Compute
power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;

// Compute new speeds
right = speed+(power*MAX);
left  = speed-(power*MAX);
// limit checks
if (right < MIN)
{right = MIN;}
else if (right > MAX)
{right = MAX;}
if (left < MIN)
{left = MIN;}
else if (left > MAX)
{left = MAX;}

// set speed
m3pi.left_motor(left);
m3pi.right_motor(right);
}
// Here the robot had find a junction so it stops and return to the main function.
m3pi.forward(0);
return true;
}

int main()
{
bool end=false;
bool line=true;

// In order to use the IR sensors, they must be calibrated.
m3pi.sensor_auto_calibrate();
wait(1);
// The robot will now follow the most left path untill the end is detected.
while (end!=true)
{
line = line_following_pid();
// Due to the inertia the robot must read again its sensors once stopped.
// All sensors are on full black so the robot must see if the junction is the end or a T or X junction
if (sensors>SEN && sensors>SEN && sensors>SEN && sensors>SEN && sensors>SEN)
{
debug++; // The debug variable is incremented until an other kind of junction or the end is detected.
if (debug>20)   // this "if" is uses when the robot got his fives sensors full black and we want to be sure that it takes the left most exit if the junction is a T or a X junction and if the sensors stay full black so it the end and not a junction.
{
end=true;
}
}
// All the sensors are on full white so the robot is on a dead end. The robot will then spin to its right to be sure not to miss any path on its right.
else if (sensors<SEN && sensors<SEN && sensors<SEN && sensors<SEN && sensors<SEN)
{
debug=0; // The debug variable is put to 0 according to its use.
while (sensors<SEN) // The robot spins until it is on a path.
{
m3pi.right(MAX);
}
}
// The most left sensor is on full black so the robot must follow a new path. So the robot spin to the new path until it reachs it.
else if (sensors>SEN)
{
debug=0;
while (sensors>SEN)
{
m3pi.left(MAX);
}
}
} // Here the robot is on the end of the maze so it now must stop.
m3pi.cls();
m3pi.locate(0,0);
m3pi.printf("End");
m3pi.stop();
}``````

The idea I had to simplify the path is to use some wheel encoders to map the maze or at least to know on which junction we are and then record every left turn and spin-back. For example if the maze is like this: The robot will do :
Start, L, B, L, B, L, L, B, L, B, L, End.
And it can be simplify by :
Start, not this exit, not this exit, this one, not this exit, not this exit, this one, End.
or
Start, 3rd exit, 3rd exit, End.
So every “L, B” sequence will be translate as “don’t take this exit” and when two L follow each other then we know that the robot is on an other junction.
Here is a video of the last test I made with the solver without simplification of the path :
https://www.dropbox.com/s/o0qo5nz5xf0dx9r/unperpendicular-maze%20solver%20vf%20-%2050.MP4

I apologize if I made some mistakes in English (I am French) and I hope it will help some of you.

[i]Here is my first idea that I stopped due to too much problem due to the robot precision and the robot itself.
First I try to solve this “unperpendicular maze” with this idea:

follow the line until a junction,
if the junction is not the end of the maze
then move to the middle of this junction (this is the most difficult part) and take the most left exit
else stop

In order to take the most left exit the robot must do half a spin then turn clockwise until it sees an exit which is not the way it comes from. In order to move to the middle of the junction I was using a little equation that measure the time that the most left or most right sensor takes to cross a path which is not the main path that the robot follows. Once you got this time you are able to know the angle made by this new path and the one the robot follows and then know how far you are from the middle of the junction. To use this equation the path must always has the same thickness and also the robot must not turn when it has it most left or most right sensor on black tape. The main problem with this idea was that due to the positions of the sensors (following the curving border of the robot) I can’t made the distinguish some angles like 70° and 110°. For those who want to try to this code and improve it here is the code WARNING this a test code so it is not clean at all and the comments are in french :

``````#define MAX 0.2
#define MIN 0
#define P_TERM 1
#define I_TERM 0
#define D_TERM 20

m3pi m3pi(p23,p9,p10);

PinDetect  pin( p21 );

bool Erreur = false;
bool Chemin = false;
bool Fin = false;
bool intersection;
int leds = 0;
float right;
float left;
int warn=0;
Timer timer;
int cot=0;
int sensors;

//------------------------

bool line_following_pid()
{

float position_of_line = 0.0;
float prev_pos_of_line = 0.0;
float derivative,proportional;
float integral = 0;
float power;
float speed = MAX;
int foundjunction=0;
int countdown=150; //make sure we don't stop for a junction too soon after starting

while (foundjunction==0)
{
if(((sensors > 300)||(sensors > 300)))
{
// Found an intersection.
foundjunction=1;
return true;
}
// Get the position of the line.
position_of_line = m3pi.line_position();
proportional = position_of_line;
// Compute the derivative
derivative = position_of_line - prev_pos_of_line;
// Compute the integral
integral = integral + proportional;
// Remember the last position.
prev_pos_of_line = position_of_line;
// Compute
power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;

// Compute new speeds
right = speed+power;
left  = speed-power;
// limit checks
if (right < MIN)
{right = MIN;}
else if (right > MAX)
{right = MAX;}
if (left < MIN)
{left = MIN;}
else if (left > MAX)
{left = MAX;}

// set speed
m3pi.left_motor(left);
m3pi.right_motor(right);

if (countdown>0)
{countdown--;}
else {
if((sensors < 100) && (sensors < 100) && (sensors < 100) && (sensors < 100) && (sensors < 100))
{
// There is no line visible ahead, and we didn't see any
// intersection.  Must be a dead end.
foundjunction=1;
}
else if(((sensors > 350) && (sensors > 200) && (sensors > 200) && (sensors > 200))  || ((sensors > 200) && (sensors > 200) && (sensors > 200) && (sensors > 350)))
{
// Found an intersection.
foundjunction=1;
}
else if ((sensors > 600) && (sensors > 600) && (sensors > 600))
{
foundjunction=1;
warn=1;
}
} //else countdown
} //while
m3pi.forward(0);
return true;
}

bool follow()
{
bool jonction = false;
while (jonction == false)
{
jonction = line_following_pid() ;
// la fonction line-following-pid devra être modifié de façon à ce qu'elle retourne vrai lorsque
// les capteurs 0,1,2,3 ou 1,2,3,4 sont obstrués. De plus pour éviter de tourner un peu au début de
// la détection d'une intersection, il faudrait suivre la ligne seulement avec les capteurs 1, 2 et 3.
// cf "nvlle fonction line following pid.txt"
}
if (jonction == true)
{
if ((sensors>sensors && sensors<100)||(sensors>sensors && sensors<100))
{
cot = 1;
}
return true;
}
else // cette option ne devrait pas arriver mais elle permet plus de robustesse.
{
return false;
}
}

//------------------------

void gomid(int times){

long double pi = 3.14159265;
long double x = 0.0175;
long double v = 0.145; // not the real speed but works better
long double y = v*times;
long double angle1 = 0;
if (cot==0)
{angle1 = asin(x/y);}
else
{angle1 = 180-asin(x/y);}
long double a = 0.0251;
long double b = 0.035;
long double dist1 = (((b*tan(angle1))+a)/(tan(angle1)))-(y/2);

long double tps1 = dist1/v;

int temps1 = 1*tps1;
m3pi.cls();
m3pi.locate(0,1);
m3pi.printf("%d    ", temps1);
//wait(2);
int op1 = 0;

if (tps1<0)
{
op1=1;
tps1=0-tps1;
}
if (op1!=1)
{
m3pi.forward(0.2);
wait_ms(tps1);
m3pi.forward(0);
}
else
{
m3pi.forward(-0.2);
wait_ms(tps1);
m3pi.forward(0);
}

}

void takeleft()
{
// Le robot est normalement sur une intersection
bool sortie = false;
m3pi.left(0.65);
wait(0.155);
m3pi.forward(0);

int sensors;
while (sensors > 200)
{
m3pi.right(0.1);
}
m3pi.forward(0);
// on démarre maintenant la recherche de sortie
m3pi.right(0.2);
while (sortie == false) // on finiera toujours par trouver une sortie car si il n'y a aucune sortie alors on fera demi-tour car le chemin d'où l'on vient sera considéré comme la sortie la plus à gauche
{
if (((sensors > 200) && (sensors > 200)) || ((sensors > 200) && (sensors > 200)))
{
while((sensors<200) || (sensors<200) || (sensors<200))

sortie = true;
}
}
m3pi.forward(0); // car on est face à la sortie la plus à gauche
while ((sensors > 200) || (sensors > 200))
{
m3pi.forward(0.2);
}
m3pi.forward(0);
// Le robot à maintenant avoir avancé sur la sortie la plus à gauche jusqu'à ce qu'il ne voit plus qu'un seul chemin
}

int main(){
warn=0;
m3pi.sensor_auto_calibrate();
wait(1);
intersection =false;
while(1){
while (intersection!=true)
{
intersection = follow();
}
if (warn==1)
{
if (sensors<300 && sensors<300)
{
while (sensors>600) // to be sure that we aren't on a deadend
{
m3pi.cls();
m3pi.locate(0,0);
m3pi.printf("warn");
m3pi.forward(MAX);
while(sensors<300 && sensors<300)
{
}
}
}
}
timer.reset();
m3pi.forward(MAX);
timer.start();
while (sensors>300 || sensors>300)
timer.stop();

float bat=m3pi.battery();
m3pi.forward(0);
m3pi.cls();
m3pi.locate(0,0);
m3pi.printf("%f       ",bat);
m3pi.locate(0,1);
m3pi.printf("%d       ",time);
//wait(3);
gomid(time);
takeleft();
intersection=false;}
}
``````

[/i]

Once again sorry for my English but I’m not a native English.

Hi, Maxime.

Cool, the robot navigated the maze very well! Thank you for posting your project details and your video.

- Ryan