I have tried the maze solver demo program and have tried playing around with it to achieve different results. I was wondering what modifications will i have to make to modify the maze solver program so that the 3pi returns automatically using the shortest path.
This would be a pretty straightforward modification. Notice that in the maze solver program, there is an array of characters called ‘path’; this stores the turns that are necessary at each intersection to guide the 3pi from the start to the finish.
Returning to the start would be no more complex than iterating through the stored path instructions in reverse, and applying a mirror (transforming left to right, and right to left) operation to each instruction before executing it, in the same manner that is used to move the 3pi from the start to the finish.
Geoff
I am still having problem getting it to return back through the shortest path!
I’m afraid you’ll need to be more specific; could you describe the actual behaviour? Also, posting the offending code would be very helpful.
Geoff
#include <pololu/3pi.h>
#include "follow-segment.h"
#include "turn.h"
// The path variable will store the path that the robot has taken. It
// is stored as an array of characters, each of which represents the
// turn that should be made at one intersection in the sequence:
// 'L' for left
// 'R' for right
// 'S' for straight (going straight through an intersection)
// 'B' for back (U-turn)
//
// Whenever the robot makes a U-turn, the path can be simplified by
// removing the dead end. The follow_next_turn() function checks for
// this case every time it makes a turn, and it simplifies the path
// appropriately.
char path[100] = "";
unsigned char path_length = 0; // the length of the path
// Displays the current path on the LCD, using two rows if necessary.
void display_path()
{
// Set the last character of the path to a 0 so that the print()
// function can find the end of the string. This is how strings
// are normally terminated in C.
path[path_length] = 0;
clear();
print(path);
if(path_length > 8)
{
lcd_goto_xy(0,1);
print(path+8);
}
}
// This function decides which way to turn during the learning phase of
// maze solving. It uses the variables found_left, found_straight, and
// found_right, which indicate whether there is an exit in each of the
// three directions, applying the "left hand on the wall" strategy.
char select_turn(unsigned char found_left, unsigned char found_straight, unsigned char found_right)
{
// Make a decision about how to turn. The following code
// implements a left-hand-on-the-wall strategy, where we always
// turn as far to the left as possible.
if(found_left)
return 'L';
else if(found_straight)
return 'S';
else if(found_right)
return 'R';
else
return 'B';
}
// Path simplification. The strategy is that whenever we encounter a
// sequence xBx, we can simplify it by cutting out the dead end. For
// example, LBL -> S, because a single S bypasses the dead end
// represented by LBL.
void simplify_path()
{
// only simplify the path if the second-to-last turn was a 'B'
if(path_length < 3 || path[path_length-2] != 'B')
return;
int total_angle = 0;
int i;
for(i=1;i<=3;i++)
{
switch(path[path_length-i])
{
case 'R':
total_angle += 90;
break;
case 'L':
total_angle += 270;
break;
case 'B':
total_angle += 180;
break;
}
}
// Get the angle as a number between 0 and 360 degrees.
total_angle = total_angle % 360;
// Replace all of those turns with a single one.
switch(total_angle)
{
case 0:
path[path_length - 3] = 'S';
break;
case 90:
path[path_length - 3] = 'R';
break;
case 180:
path[path_length - 3] = 'B';
break;
case 270:
path[path_length - 3] = 'L';
break;
}
// The path is now two steps shorter.
path_length -= 2;
}
// This function is called once, from main.c.
void maze_solve()
{
// Loop until we have solved the maze.
while(1)
{
// FIRST MAIN LOOP BODY
follow_segment();
// Drive straight a bit. This helps us in case we entered the
// intersection at an angle.
// Note that we are slowing down - this prevents the robot
// from tipping forward too much.
set_motors(50,50);
delay_ms(100);
// These variables record whether the robot has seen a line to the
// left, straight ahead, and right, whil examining the current
// intersection.
unsigned char found_left=0;
unsigned char found_straight=0;
unsigned char found_right=0;
// Now read the sensors and check the intersection type.
unsigned int sensors[5];
read_line(sensors,IR_EMITTERS_ON);
// Check for left and right exits.
if(sensors[0] > 400)
found_left = 1;
if(sensors[4] > 400)
found_right = 1;
// Drive straight a bit more - this is enough to line up our
// wheels with the intersection.
set_motors(40,40);
delay_ms(200);
// Check for a straight exit.
read_line(sensors,IR_EMITTERS_ON);
if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200)
found_straight = 1;
// Check for the ending spot.
// If all three middle sensors are on dark black, we have
// solved the maze.
if(sensors[1] > 600 && sensors[2] > 600 && sensors[3] > 600)
break;
// Intersection identification is complete.
// If the maze has been solved, we can follow the existing
// path. Otherwise, we need to learn the solution.
unsigned char dir = select_turn(found_left, found_straight, found_right);
// Make the turn indicated by the path.
turn(dir);
// Store the intersection in the path variable.
path[path_length] = dir;
path_length ++;
// You should check to make sure that the path_length does not
// exceed the bounds of the array. We'll ignore that in this
// example.
// Simplify the learned path.
simplify_path();
// Display the path on the LCD.
display_path();
}
// Solved the maze!
// Now enter an infinite loop - we can re-run the maze as many
// times as we want to.
while(1)
{
// Beep to show that we finished the maze.
set_motors(0,0);
play(">>a32");
// Wait for the user to press a button, while displaying
// the solution.
while(!button_is_pressed(BUTTON_B))
{
if(get_ms() % 2000 < 1000)
{
clear();
print("Solved!");
lcd_goto_xy(0,1);
print("Press B");
}
else
display_path();
delay_ms(30);
}
while(button_is_pressed(BUTTON_B));
delay_ms(1000);
// Re-run the maze. It's not necessary to identify the
// intersections, so this loop is really simple.
int i;
for(i=path_length-1;i>=0;i--)
{
// SECOND MAIN LOOP BODY
follow_segment();
// Drive straight while slowing down, as before.
set_motors(50,50);
delay_ms(100);
set_motors(40,40);
delay_ms(200);
// Make a turn according to the instruction stored in
// path[i].
if(path[i] = 'L')
path[i] = 'R';
if(path[i] = 'R')
path[i] = 'L';
}
// Follow the last segment up to the finish.
follow_segment();
// Now we should be at the finish! Restart the loop.
}
}Recall that the original code used the function
turn(path[i]);
to make the appropriate turn at index i. You forgot to actually make the turn after mirroring the direction. 
Also, your turn mirroring is not quite correct; the bot will always transform a turn into a left turn, the way you have coded it. In your code, you transform a left into a right, and then afterwards change the (possibly new) right into a left. You should change your second ‘if’ statement into an ‘else if’ statement (or simply an ‘else’) to avoid this.
It does turn but is not following the path!
/*
* This file contains the maze-solving strategy.
*/
#include <pololu/3pi.h>
#include "follow-segment.h"
#include "turn.h"
// The path variable will store the path that the robot has taken. It
// is stored as an array of characters, each of which represents the
// turn that should be made at one intersection in the sequence:
// 'L' for left
// 'R' for right
// 'S' for straight (going straight through an intersection)
// 'B' for back (U-turn)
//
// Whenever the robot makes a U-turn, the path can be simplified by
// removing the dead end. The follow_next_turn() function checks for
// this case every time it makes a turn, and it simplifies the path
// appropriately.
char path[100] = "";
unsigned char path_length = 0; // the length of the path
// Displays the current path on the LCD, using two rows if necessary.
void display_path()
{
// Set the last character of the path to a 0 so that the print()
// function can find the end of the string. This is how strings
// are normally terminated in C.
path[path_length] = 0;
clear();
print(path);
if(path_length > 8)
{
lcd_goto_xy(0,1);
print(path+8);
}
}
// This function decides which way to turn during the learning phase of
// maze solving. It uses the variables found_left, found_straight, and
// found_right, which indicate whether there is an exit in each of the
// three directions, applying the "left hand on the wall" strategy.
char select_turn(unsigned char found_left, unsigned char found_straight, unsigned char found_right)
{
// Make a decision about how to turn. The following code
// implements a left-hand-on-the-wall strategy, where we always
// turn as far to the left as possible.
if(found_left)
return 'L';
else if(found_straight)
return 'S';
else if(found_right)
return 'R';
else
return 'B';
}
// Path simplification. The strategy is that whenever we encounter a
// sequence xBx, we can simplify it by cutting out the dead end. For
// example, LBL -> S, because a single S bypasses the dead end
// represented by LBL.
void simplify_path()
{
// only simplify the path if the second-to-last turn was a 'B'
if(path_length < 3 || path[path_length-2] != 'B')
return;
int total_angle = 0;
int i;
for(i=1;i<=3;i++)
{
switch(path[path_length-i])
{
case 'R':
total_angle += 90;
break;
case 'L':
total_angle += 270;
break;
case 'B':
total_angle += 180;
break;
}
}
// Get the angle as a number between 0 and 360 degrees.
total_angle = total_angle % 360;
// Replace all of those turns with a single one.
switch(total_angle)
{
case 0:
path[path_length - 3] = 'S';
break;
case 90:
path[path_length - 3] = 'R';
break;
case 180:
path[path_length - 3] = 'B';
break;
case 270:
path[path_length - 3] = 'L';
break;
}
// The path is now two steps shorter.
path_length -= 2;
}
// This function is called once, from main.c.
void maze_solve()
{
// Loop until we have solved the maze.
while(1)
{
// FIRST MAIN LOOP BODY
follow_segment();
// Drive straight a bit. This helps us in case we entered the
// intersection at an angle.
// Note that we are slowing down - this prevents the robot
// from tipping forward too much.
set_motors(50,50);
delay_ms(50);
// These variables record whether the robot has seen a line to the
// left, straight ahead, and right, whil examining the current
// intersection.
unsigned char found_left=0;
unsigned char found_straight=0;
unsigned char found_right=0;
// Now read the sensors and check the intersection type.
unsigned int sensors[5];
read_line(sensors,IR_EMITTERS_ON);
// Check for left and right exits.
if(sensors[0] > 600)
found_left = 1;
if(sensors[4] > 600)
found_right = 1;
// Drive straight a bit more - this is enough to line up our
// wheels with the intersection.
set_motors(40,40);
delay_ms(200);
// Check for a straight exit.
read_line(sensors,IR_EMITTERS_ON);
if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200)
found_straight = 1;
// Check for the ending spot.
// If all three middle sensors are on dark black, we have
// solved the maze.
if(sensors[1] > 600 && sensors[2] > 600 && sensors[3] > 600)
break;
// Intersection identification is complete.
// If the maze has been solved, we can follow the existing
// path. Otherwise, we need to learn the solution.
unsigned char dir = select_turn(found_left, found_straight, found_right);
// Make the turn indicated by the path.
turn(dir);
// Store the intersection in the path variable.
path[path_length] = dir;
path_length ++;
// You should check to make sure that the path_length does not
// exceed the bounds of the array. We'll ignore that in this
// example.
// Simplify the learned path.
simplify_path();
// Display the path on the LCD.
display_path();
}
// Solved the maze!
// Now enter an infinite loop - we can re-run the maze as many
// times as we want to.
while(1)
{
// Beep to show that we finished the maze.
set_motors(0,0);
play(">>a32");
delay_ms(1000);
set_motors(80,-80);
delay_ms(400);
set_motors(0,0);
delay_ms(100);
// Re-run the maze. It's not necessary to identify the
// intersections, so this loop is really simple.
int i;
for(i=path_length-1;i>=0;i--)
{
// SECOND MAIN LOOP BODY
follow_segment();
set_motors(50,50);
delay_ms(50);
set_motors(40,40);
delay_ms(200);
// Make a turn according to the instruction stored in
// path[i].
if(path[i] = 'L')
path[i] = 'R';
else if(path[i] = 'R')
path[i] = 'L';
}
turn(path[i]);
// Follow the last segment up to the finish.
follow_segment();
// Now we should be at the finish! Restart the loop.
}
}
// Local Variables: **
// mode: C **
// c-basic-offset: 4 **
// tab-width: 4 **
// indent-tabs-mode: t **
// end: **
Recall that the original code to follow the shortest path looks like this:
...
// Re-run the maze. It's not necessary to identify the
// intersections, so this loop is really simple.
int i;
for(i=0;i<path_length;i++)
{
// SECOND MAIN LOOP BODY
follow_segment();
// Drive straight while slowing down, as before.
set_motors(50,50);
delay_ms(50);
set_motors(40,40);
delay_ms(200);
// Make a turn according to the instruction stored in
// path[i].
turn(path[i]);
}
...Your function call,
turn(path[i]);must be contained inside the for-loop, since at each element path[i], you might be required to change direction.
Additionally, you need to use == rather than = when checking equality. A single ‘=’ is only used for assignment.
Hope this helps.
Thanks a lot for your help! I finally managed to make it run using the following code:
/*
* Tyler Dodd
* Sulav Bhattarai
* ELEG 3133
* Homework 8
* Maze Solver
*/
#include <pololu/3pi.h>
#include "follow-segment.h"
#include "turn.h"
// The path variable will store the path that the robot has taken. It
// is stored as an array of characters, each of which represents the
// turn that should be made at one intersection in the sequence:
// 'L' for left
// 'R' for right
// 'S' for straight (going straight through an intersection)
// 'B' for back (U-turn)
//
// Whenever the robot makes a U-turn, the path can be simplified by
// removing the dead end. The follow_next_turn() function checks for
// this case every time it makes a turn, and it simplifies the path
// appropriately.
char path[100] = "";
unsigned char path_length = 0; // the length of the path
// Displays the current path on the LCD, using two rows if necessary.
void display_path()
{
// Set the last character of the path to a 0 so that the print()
// function can find the end of the string. This is how strings
// are normally terminated in C.
path[path_length] = 0;
clear();
print(path);
if(path_length > 8)
{
lcd_goto_xy(0,1);
print(path+8);
}
}
// This function decides which way to turn during the learning phase of
// maze solving. It uses the variables found_left, found_straight, and
// found_right, which indicate whether there is an exit in each of the
// three directions, applying the "left hand on the wall" strategy.
char select_turn(unsigned char found_left, unsigned char found_straight, unsigned char found_right)
{
// Make a decision about how to turn. The following code
// implements a left-hand-on-the-wall strategy, where we always
// turn as far to the left as possible.
if(found_left)
return 'L';
else if(found_straight)
return 'S';
else if(found_right)
return 'R';
else
return 'B';
}
// Path simplification. The strategy is that whenever we encounter a
// sequence xBx, we can simplify it by cutting out the dead end. For
// example, LBL -> S, because a single S bypasses the dead end
// represented by LBL.
void simplify_path()
{
// only simplify the path if the second-to-last turn was a 'B'
if(path_length < 3 || path[path_length-2] != 'B')
return;
int total_angle = 0;
int i;
for(i=1;i<=3;i++)
{
switch(path[path_length-i])
{
case 'R':
total_angle += 90;
break;
case 'L':
total_angle += 270;
break;
case 'B':
total_angle += 180;
break;
}
}
// Get the angle as a number between 0 and 360 degrees.
total_angle = total_angle % 360;
// Replace all of those turns with a single one.
switch(total_angle)
{
case 0:
path[path_length - 3] = 'S';
break;
case 90:
path[path_length - 3] = 'R';
break;
case 180:
path[path_length - 3] = 'B';
break;
case 270:
path[path_length - 3] = 'L';
break;
}
// The path is now two steps shorter.
path_length -= 2;
}
// This function is called once, from main.c.
void maze_solve()
{
// Loop until we have solved the maze.
while(1)
{
// FIRST MAIN LOOP BODY
follow_segment();
// Drive straight a bit. This helps us in case we entered the
// intersection at an angle.
// Note that we are slowing down - this prevents the robot
// from tipping forward too much.
set_motors(70,70);
delay_ms(30);
// These variables record whether the robot has seen a line to the
// left, straight ahead, and right, whil examining the current
// intersection.
unsigned char found_left=0;
unsigned char found_straight=0;
unsigned char found_right=0;
// Now read the sensors and check the intersection type.
unsigned int sensors[5];
read_line(sensors,IR_EMITTERS_ON);
// Check for left and right exits.
if(sensors[0] > 600)
found_left = 1;
if(sensors[4] > 600)
found_right = 1;
// Drive straight a bit more - this is enough to line up our
// wheels with the intersection.
set_motors(40,40);
delay_ms(200);
// Check for a straight exit.
read_line(sensors,IR_EMITTERS_ON);
if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200)
found_straight = 1;
// Check for the ending spot.
// If all three middle sensors are on dark black, we have
// solved the maze.
if(sensors[1] > 600 && sensors[2] > 600 && sensors[3] > 600)
break;
// Intersection identification is complete.
// If the maze has been solved, we can follow the existing
// path. Otherwise, we need to learn the solution.
unsigned char dir = select_turn(found_left, found_straight, found_right);
// Make the turn indicated by the path.
turn(dir);
// Store the intersection in the path variable.
path[path_length] = dir;
path_length ++;
// You should check to make sure that the path_length does not
// exceed the bounds of the array. We'll ignore that in this
// example.
// Simplify the learned path.
simplify_path();
// Display the path on the LCD.
display_path();
}
// Solved the maze!
// Now enter an infinite loop - we can re-run the maze as many
// times as we want to.
// Beep to show that we finished the maze.
set_motors(0,0);
play(">>a32");
delay_ms(1000);
// Re-run the maze. It's not necessary to identify the
// intersections, so this loop is really simple.
turn('B');
int i;
for(i=path_length-1;i>=0;i--)
{
// SECOND MAIN LOOP BODY
follow_segment();
// Drive straight while slowing down, as before.
set_motors(70,70);
delay_ms(30);
set_motors(40,40);
delay_ms(200);
switch(path[i])
{
case'L':
path[i] = 'R';
break;
case'R':
path[i] = 'L';
break;
}
display_path();
// Make a turn according to the instruction stored in
// path[i].
turn(path[i]);
}
// Follow the last segment up to the finish.
follow_segment();
set_motors(0,0);
delay_ms(1000);
// Now we should be at the finish! Restart the loop.
}
// Local Variables: **
// mode: C **
// c-basic-offset: 4 **
// tab-width: 4 **
// indent-tabs-mode: t **
// end: **