Jon Marsh
/
m3pi_MazeSolver
This is work in progress - maze solving with m3pi robot. Not complete
Diff: main.cpp
- Revision:
- 0:6e52eae6412a
- Child:
- 1:3ac7462953df
diff -r 000000000000 -r 6e52eae6412a main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Mar 03 09:58:58 2011 +0000 @@ -0,0 +1,337 @@ +#include "mbed.h" +#include "m3pijon.h" + +BusOut leds(LED1,LED2,LED3,LED4); +m3pi m3pi(p23,p9,p10); + +#define MAX 0.5 +#define MIN 0 + + +#define P_TERM 1 +#define I_TERM 0 +#define D_TERM 20 + +// Global variables +// The path array stores the route info. Each element shows what we did at an intersection + +// 'L' for left +// 'R' for right +// 'F' for forward +// 'B' for back +// +char path[1000] = ""; +unsigned char path_length = 0; // the length of the path so far + + +void follow_line() +{ + float right; + float left; + 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 + + int sensors[5]; + while (foundjunction==0) { + + // 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 += 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 { + // Next, we are going to use the sensors to look for whether there is still a line ahead + // and try to detect dead ends and possible left or right turns. + m3pi.readsensor(sensors); + + if(sensors[1] < 100 && sensors[2] < 100 && sensors[3] < 100) + { + // There is no line visible ahead, and we didn't see any + // intersection. Must be a dead end. + foundjunction=1; + } + else if(sensors[0] > 200 || sensors[4] > 200) + { + // Found an intersection. + foundjunction=1; + } + } //else countdown + } //while +// straighten up a bit, by steering opposite direction +// not sure if this is needed +// m3pi.left_motor(right); +// m3pi.right_motor(left); +// wait(0.02); + +} + +// 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 turn(unsigned char found_left, unsigned char found_forward, unsigned char found_right) +{ + // The order of the statements in this "if" is sufficient to implement a follow left-hand wall algorithm + if(found_left) + return 'L'; + else if(found_forward) + return 'F'; + else if(found_right) + return 'R'; + else + return 'B'; +} + +void doturn(unsigned char dir) +{ + if (dir=='L') + {m3pi.left(0.25);wait(0.28);} + else if(dir=='R') + {m3pi.right(0.25);wait(0.28);} + else if(dir=='F') + {m3pi.forward(0.3);wait(0.15);} + else if(dir=='B') + {m3pi.right(0.25);wait(0.6);} + + m3pi.forward(0.1);wait(0.1);m3pi.forward(0); + return; +} + +// change LBL to S (etc), to bypass dead ends +void simplify() +{ + // 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] = 'F'; + 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 mazesolve() +{ + // These variables record whether the robot has seen a line to the + // left, straight ahead, and right, while examining the current + // intersection. + unsigned char found_left=0; + unsigned char found_forward=0; + unsigned char found_right=0; + int sensors[5]; + // Loop until we have solved the maze. + while(1) + { + + // Follow the line until an intersection is detected + follow_line(); + + // Inch forward a bit. This helps us in case we entered the + // intersection at an angle. + found_left=0;found_forward=0;found_right=0; + // m3pi.forward(0.1); + // wait(0.05); + + // Now read the sensors and check the intersection type. + + + + m3pi.forward(0.0); + + // Check for a forward exit. + m3pi.readsensor(sensors); + if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200) + found_forward = 1; + + m3pi.readsensor(sensors); + + // Check for left and right exits. + if(sensors[0] > 200) + found_left = 1; + if(sensors[4] > 200) + found_right = 1; + + //debug code + m3pi.cls(); + if (found_left==1) + m3pi.printf("L"); + if (found_right==1) + m3pi.printf("R"); + if (found_forward==1) + m3pi.printf("F"); + wait (3); + // Check for the ending spot. + // If all five sensors are on dark black, we have + // solved the maze. + if(sensors[0]>600 && sensors[1] > 600 && sensors[2] > 600 && sensors[3] > 600 && sensors[4]>600) + break; + + // Drive straight a bit more - this is enough to line up our + // wheels with the intersection. + m3pi.forward(0.15); + wait(0.02); + + unsigned char dir = turn(found_left, found_forward, found_right); + + // Make the turn indicated by the path. + doturn(dir); + + // Store the intersection in the path variable. + path[path_length] = dir; + path_length ++; + + // Need to insert check to make sure that the path_length does not + // exceed the bounds of the array. + + // Simplify the learned path. + simplify(); + + } + + // Solved the maze! + + // Now enter an infinite loop - we can re-run the maze as many + // times as we want to. + while(1) + { + + m3pi.forward(0.0); + m3pi.printf("Finished"); + + // wait 15s to give time to turn off, or put the robot back to the start + wait(15); + // ideally we would use a button press here + // but I don't think it can easily be read + + // 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++) + { + + follow_line(); + + // Drive straight while slowing down + //m3pi.forward(0.5); + //wait(0.05); + m3pi.forward(0.3); + wait(0.1); + + // Make a turn according to the instruction stored in + // path[i]. + doturn(path[i]); + } + + // Follow the last segment up to the finish. + follow_line(); + + // Now we should be at the finish! Restart the loop. + } +} + + void checksensors() +{ +int sensors[5]; +while (1) { + m3pi.readsensor(sensors); + m3pi.cls(); + if (sensors[0]>200) + m3pi.printf("D"); + else m3pi.printf("L"); + if (sensors[1]>200) + m3pi.printf("D"); + else m3pi.printf("L"); + if (sensors[2]>200) + m3pi.printf("D"); + else m3pi.printf("L"); + if (sensors[3]>200) + m3pi.printf("D"); + else m3pi.printf("L"); + if (sensors[4]>200) + m3pi.printf("D"); + else m3pi.printf("L"); + } +} +int main() { + // int sensors[5]; + m3pi.locate(0,1); + m3pi.sensor_auto_calibrate(); + m3pi.printf("MazeSolve"); + + wait(2.0); + + mazesolve(); + +m3pi.forward(0.0); + +}