Jon Marsh
/
m3pi_MazeSolver
This is work in progress - maze solving with m3pi robot. Not complete
main.cpp
- Committer:
- jonmarsh
- Date:
- 2011-03-03
- Revision:
- 1:3ac7462953df
- Parent:
- 0:6e52eae6412a
File content as of revision 1:3ac7462953df:
#include "mbed.h" #include "m3pimaze.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); }