Hello,
My name is Brian and I’m working with a team of students to create a walled maze solver robot using the 3pi. So far, our robot is able to traverse a maze using a left-hand on the wall strategy. This code is based on the wall follower sample posted on the site. Our robot has one analog distance sensor on the left for scaling the wall (AD7). It also has 3 digital distance sensors on the front, left, and right (PC5, PD0, PD1). Our goal is to integrate the wall follower code with the line maze solver code. In other words, we want to solve a non-looped walled maze by learning on the first run and cutting out deadends on the second run.
Right now, we’re having two distinct problems. The robot is able to stop at a finish line. However, it will stop at any deadend because it stops when all digital sensors are tripped. We aren’t quite sure how to determine the finish line. Secondly, the robot will turn to the right if it ever detects something directly in front, due to our follow segment PID not being able to correct it’s position fast enough. We have found it hard to record turns based on that fact. Here is what our code looks like so far after modifying the sample code. I apologize for it being all in one source code file, and some lines of code that we tested are commented out. If anyone has any suggestions for determining a deadend and/or turn memorization, it would be much appreciated!
#include <pololu/orangutan.h>
// The 3pi include file must be at the beginning of any program that
// uses the Pololu AVR library and 3pi.
#include <pololu/3pi.h>
// This include file allows data to be stored in program space. The
// ATmegaxx8 has 16x more flash than RAM, so large
// pieces of static data should be stored in program space.
#include <avr/pgmspace.h>
// Introductory messages. The "PROGMEM" identifier causes the data to
// go into program space.
const char welcome_line1[] PROGMEM = " Pololu";
const char welcome_line2[] PROGMEM = "3\xf7 Robot";
const char name_line1[] PROGMEM = "Wall";
const char name_line2[] PROGMEM = "Follower";
// A couple of simple tunes, stored in program space.
const char welcome[] PROGMEM = ">g32>>c32";
const char go[] PROGMEM = "L16 cdegreg4";
// Refresh the LCD display every tenth of a second.
const int display_interval_ms = 100;
#define MS_ELAPSED_IS(n) (get_ms() % n == 0)
#define TIME_TO_DISPLAY (MS_ELAPSED_IS(display_interval_ms))
char path[100] = " ";
unsigned char path_length = 0;
void initialize()
{
// Set PC5 as an input with internal pull-up enabled
set_digital_input(IO_C5, PULL_UP_ENABLED);
set_digital_input(IO_D0, PULL_UP_ENABLED);
set_digital_input(IO_D1, PULL_UP_ENABLED);
// Play welcome music and display a message
print_from_program_space(welcome_line1);
lcd_goto_xy(0,1);
print_from_program_space(welcome_line2);
play_from_program_space(welcome);
delay_ms(1000);
clear();
print_from_program_space(name_line1);
lcd_goto_xy(0,1);
print_from_program_space(name_line2);
delay_ms(1000);
// Display battery voltage and wait for button press
while(!button_is_pressed(BUTTON_B))
{
clear();
print_long(read_battery_millivolts());
print("mV");
lcd_goto_xy(0,1);
print("Press B");
delay_ms(100);
}
// Always wait for the button to be released so that 3pi doesn't
// start moving until your hand is away from it.
wait_for_button_release(BUTTON_B);
clear();
print("Go!");
// Play music and wait for it to finish before we start driving.
play_from_program_space(go);
while(is_playing());
}
void turn_in_place() {
/*if (TIME_TO_DISPLAY) {
clear();
lcd_goto_xy(0,0);
print("Front");
lcd_goto_xy(0,1);
print("Obstacle");
}*/
// Turn to the right in place
set_motors(80, -80); //(left clockwise, right counterclockwise)
delay_ms(100);
}
void turn_around() {
/*if (TIME_TO_DISPLAY) {
clear();
lcd_goto_xy(0,0);
print("Dead");
lcd_goto_xy(0,1);
print("End");
}*/
// Turn 180 degrees around
set_motors(80, -80);
delay_ms(200);
}
// Turns according to the parameter dir, which should be 'L', 'R', 'S'
// (straight), or 'B' (back).
void turn(char dir)
{
switch(dir)
{
case 'L':
// Turn left.
set_motors(-80,80);
delay_ms(200);
break;
case 'R':
// Turn right.
set_motors(80,-80);
delay_ms(200);
break;
case 'B':
// Turn around.
set_motors(80,-80);
delay_ms(400);
break;
case 'S':
// Don't do anything!
break;
}
}
// 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;
}
void follow_segment() {
int last_proximity = 0;
const int base_speed = 50;
const int set_point = 300;
long integral = 0;
// This is the "main loop" - it will run forever.
while(1)
{
// If something is directly in front turn to the right in place
int front = is_digital_input_high(IO_C5);
int right = is_digital_input_high(IO_D1);
int left = is_digital_input_high(IO_D0);
int left_proximity = analog_read(7); // 0 (far away) - 650 (close)
//unsigned int sensors[5];
//read_line(sensors,IR_EMITTERS_OFF);
int proportional = left_proximity - set_point;
int derivative = left_proximity - last_proximity;
integral += derivative;
// Proportional-Derivative Control Signal
int pd = proportional / 6 + integral / 200 + derivative * 2/5;
int left_set = base_speed + pd;
int right_set = base_speed - pd;
last_proximity = left_proximity; // remember last proximity for derivative
if (left_set < (base_speed - 100)){
set_motors(base_speed + 10, base_speed - 10);
delay_ms(80);
}
set_motors(left_set, right_set);
//DEADEND
if (front == 0 && right == 0 && left == 0) {
//turn_around();
return;
}
else if (front == 0){
turn_in_place();
continue;
}
}
}
void maze_solve() {
while(1)
{
follow_segment();
// 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;
int front = is_digital_input_high(IO_C5);
int right = is_digital_input_high(IO_D1);
int left = is_digital_input_high(IO_D0);
//unsigned int sensors[5];
//read_line(sensors,IR_EMITTERS_ON);
//unsigned int position = read_line(sensors, IR_EMITTERS_ON);
//if (front == 0)
//turn_in_place();
if (front == 0 && right == 0 && left == 0)
//turn_around();
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();
}
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=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]);
}
// Follow the last segment up to the finish.
follow_segment();
// Now we should be at the finish! Restart the loop.
}
}
int main()
{
// set up the 3pi
initialize();
maze_solve();
// This part of the code is never reached. A robot should
// never reach the end of its program, or unpredictable behavior
// will result as random code starts getting executed. If you
// really want to stop all actions at some point, set your motors
// to 0,0 and run the following command to loop forever:
//
while(1);
}