James Allen
/
MyMazeSolver
working modified m3pimazesolver
main.cpp
- Committer:
- jalle1714
- Date:
- 2013-04-06
- Revision:
- 2:73f7cc4c30c7
- Parent:
- 1:f4db9b0bb5e2
File content as of revision 2:73f7cc4c30c7:
#include "mbed.h" #include "m3pimaze.h" BusOut leds(LED1,LED2,LED3,LED4); m3pi m3pi(p23,p9,p10); char path[1000] = ""; unsigned char path_length = 0; // the length of the path so far int sensors[5]; DigitalIn pb(p21); // Minimum and maximum motor speeds #define MAX 0.25 #define MIN 0 // PID terms #define P_TERM 1 #define I_TERM 0 #define D_TERM 20 /*========================================================================================================================== FOLLOW LINE ============================================================================================================================*/ void follow_line() { m3pi.locate(0,1); m3pi.cls(); m3pi.printf("GO!!"); //wait(2.0); //m3pi.sensor_auto_calibrate();//don't want this here. float right; float left; float current_pos_of_line = 0.0; float previous_pos_of_line = 0.0; float derivative,proportional,integral = 0; float power; float speed = MAX; int foundjunction=0; int countdown=100; //make sure we don't stop for a junction too soon after starting while (foundjunction==0) { // Get the position of the line. current_pos_of_line = m3pi.line_position(); proportional = current_pos_of_line; // Compute the derivative derivative = current_pos_of_line - previous_pos_of_line; // Compute the integral // integral += proportional; // Remember the last position. previous_pos_of_line = current_pos_of_line; // Compute the power 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] < 400) && (sensors[2] < 400) && (sensors[3] < 400)) { // There is no line visible ahead, and we didn't see any // intersection. Must be a dead end. foundjunction=1; return; } else if((sensors[0] > 700) || (sensors[4] > 700)) { // Found an intersection. foundjunction=1; return; } else foundjunction=0; } } } /*========================================================================================================================= TURN ===========================================================================================================================*/ // 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'; } /*========================================================================================================================== DO TURN ============================================================================================================================*/ 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; } /*========================================================================================================================== SIMPLIFY ============================================================================================================================*/ // 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; } /*========================================================================================================================== PRESSED ============================================================================================================================*/ void pressed() { // wait 1s to give time to GET OUT OF WAY wait(1); // // 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.2); wait(0.2); // Make a turn according to the instruction stored in // path[i]. doturn(path[i]); } // Follow the last segment up to the finish. follow_line(); m3pi.forward(0.2); wait(0.6); //celebrate with music char dixie[]={'V','1','5','O','5','G','1','6','E','1','6','C','8','R','2','4','C','8','R','2','4','C','1','6','D','1','6','E','1','6','F','1','6','G','8','R','2','4','G','8','R','2','4','G','8','E','1','6'}; int numb=49; m3pi.playtune(dixie,numb); return; // Now we should be at the finish! Restart the loop. } /*========================================================================================================================= MAZESOLVE ===========================================================================================================================*/ // 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; unsigned char found_back=0; int sensors[5]; // Loop until we have solved the maze. while(1) { // Follow the line until an intersection is detected follow_line(); // Bump forward a bit in case sensor was triggered at an angle m3pi.forward(0.1); wait(0.1); found_left=0; found_forward=0; found_right=0; m3pi.readsensor(sensors);/*sensors are turned off outside the follow line function so we must call sensors eachtime they are needed*/ // Check for left and right exits. if((sensors[1]<700) && (sensors [2]<700) && (sensors[3]<700)) { found_back=1; } if(sensors[0] > 700) { // Drive straight a bit more - this is enough to line up our // wheels with the intersection. m3pi.forward(0.2); wait(0.2); m3pi.readsensor(sensors);//what's beyond the intersection // Check for the ending spot. // If all five sensors are on dark black, we have // solved the maze. if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900)) { //move upon the home pad to show off m3pi.forward(0.2); wait(0.4); m3pi.printf("Press P21 Restart"); break; } else found_left = 1; } else if(sensors[4] > 700 ) { //move wheels to intersection m3pi.forward(0.2); wait(0.2); //what is past the intersection m3pi.readsensor(sensors); // Check for the ending spot. // If all five sensors are on dark black, we have // solved the maze. if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900)) { m3pi.forward(0.2); wait(0.4); m3pi.printf("Press P21 Restart"); break; } //can we go forward else if((sensors[1] > 700 )|| (sensors[2] > 700) || (sensors[3] > 700)) { found_forward = 1; } //then go right else 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"); if (found_back==1) m3pi.printf("B"); //wait (3); unsigned char dir = turn(found_left, found_forward, found_right); // Make the turn indicated by the path. //doturn(dir); 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); pb.mode(PullUp); wait(1); if(pb) { do { } while(pb); pressed(); } } } /*========================================================================================================================= MAIN ===========================================================================================================================*/ int main() { // int sensors[5]; m3pi.locate(0,1); m3pi.sensor_auto_calibrate(); m3pi.printf("MazeSolve"); wait(2.0); mazesolve(); m3pi.forward(0.0); }