This is work in progress - maze solving with m3pi robot. Not complete

Dependencies:   mbed m3pimaze

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "m3pimaze.h"
00003 
00004 BusOut leds(LED1,LED2,LED3,LED4);
00005 m3pi m3pi(p23,p9,p10);
00006 
00007 #define MAX 0.5
00008 #define MIN 0
00009 
00010 
00011 #define P_TERM 1
00012 #define I_TERM 0
00013 #define D_TERM 20
00014 
00015 // Global variables
00016 // The path array stores the route info. Each element shows what we did at an intersection
00017 
00018 //  'L' for left
00019 //  'R' for right
00020 //  'F' for forward
00021 //  'B' for back 
00022 //
00023 char path[1000] = "";
00024 unsigned char path_length = 0; // the length of the path so far
00025  
00026 
00027 void follow_line()
00028 {
00029     float right;
00030     float left;
00031     float position_of_line = 0.0;
00032     float prev_pos_of_line = 0.0;
00033     float derivative,proportional;
00034     float integral = 0;
00035     float power;
00036     float speed = MAX;
00037     int foundjunction=0;
00038     int countdown=150; //make sure we don't stop for a junction too soon after starting
00039 
00040     int sensors[5];    
00041     while (foundjunction==0) {
00042 
00043         // Get the position of the line.
00044         position_of_line = m3pi.line_position();
00045         proportional = position_of_line;
00046         // Compute the derivative
00047         derivative = position_of_line - prev_pos_of_line;
00048         // Compute the integral
00049         integral += proportional;
00050          // Remember the last position.
00051         prev_pos_of_line = position_of_line;
00052         // Compute
00053         power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
00054         
00055         //    Compute new speeds   
00056         right = speed+power;
00057         left  = speed-power;
00058         // limit checks
00059         if (right < MIN)
00060             right = MIN;
00061         else if (right > MAX)
00062             right = MAX;
00063             
00064         if (left < MIN)
00065             left = MIN;
00066         else if (left > MAX)
00067             left = MAX;
00068             
00069        // set speed 
00070         m3pi.left_motor(left);
00071         m3pi.right_motor(right);
00072 
00073     if (countdown>0) countdown--; else {
00074        // Next, we are going to use the sensors to look for whether there is still a line ahead
00075        // and try to detect dead ends and possible left or right turns.
00076        m3pi.readsensor(sensors);
00077       
00078         if(sensors[1] < 100 && sensors[2] < 100 && sensors[3] < 100)
00079         {
00080             // There is no line visible ahead, and we didn't see any
00081             // intersection.  Must be a dead end.
00082             foundjunction=1;
00083         }
00084         else if(sensors[0] > 200 || sensors[4] > 200)
00085         {
00086             // Found an intersection.
00087             foundjunction=1;
00088         }
00089        } //else countdown
00090      } //while
00091 // straighten up a bit, by steering opposite direction
00092 // not sure if this is needed
00093 //        m3pi.left_motor(right);
00094 //        m3pi.right_motor(left);
00095 //       wait(0.02);
00096 
00097 }
00098 
00099 // This function decides which way to turn during the learning phase of
00100 // maze solving.  It uses the variables found_left, found_straight, and
00101 // found_right, which indicate whether there is an exit in each of the
00102 // three directions, applying the "left hand on the wall" strategy.
00103 
00104 char turn(unsigned char found_left, unsigned char found_forward, unsigned char found_right)
00105 {
00106     // The order of the statements in this "if" is sufficient to implement a follow left-hand wall algorithm
00107     if(found_left)
00108         return 'L';
00109     else if(found_forward)
00110         return 'F';
00111     else if(found_right)
00112         return 'R';
00113     else
00114         return 'B';
00115 }
00116 
00117 void doturn(unsigned char dir)
00118 {
00119     if (dir=='L')
00120         {m3pi.left(0.25);wait(0.28);}   
00121      else if(dir=='R')
00122       {m3pi.right(0.25);wait(0.28);}  
00123      else if(dir=='F')
00124       {m3pi.forward(0.3);wait(0.15);}
00125      else if(dir=='B')
00126       {m3pi.right(0.25);wait(0.6);}
00127        
00128        m3pi.forward(0.1);wait(0.1);m3pi.forward(0);
00129         return;
00130 }
00131 
00132 // change LBL to S (etc), to bypass dead ends
00133 void simplify()
00134 {
00135     // only simplify the path if the second-to-last turn was a 'B'
00136     if(path_length < 3 || path[path_length-2] != 'B')
00137         return;
00138 
00139 
00140     int total_angle = 0;
00141     int i;
00142     for(i=1;i<=3;i++)
00143     {
00144         switch(path[path_length-i])
00145         {
00146         case 'R':
00147             total_angle += 90;
00148             break;
00149         case 'L':
00150             total_angle += 270;
00151             break;
00152         case 'B':
00153             total_angle += 180;
00154             break;
00155         }
00156     }
00157 
00158     // Get the angle as a number between 0 and 360 degrees.
00159     total_angle = total_angle % 360;
00160 
00161     // Replace all of those turns with a single one.
00162     switch(total_angle)
00163     {
00164     case 0:
00165         path[path_length - 3] = 'F';
00166         break;
00167     case 90:
00168         path[path_length - 3] = 'R';
00169         break;
00170     case 180:
00171         path[path_length - 3] = 'B';
00172         break;
00173     case 270:
00174         path[path_length - 3] = 'L';
00175         break;
00176     }
00177 
00178     // The path is now two steps shorter.
00179     path_length -= 2;
00180 }
00181 
00182 // This function is called once, from main.c.
00183 void mazesolve()
00184 {
00185     // These variables record whether the robot has seen a line to the    
00186     // left, straight ahead, and right, while examining the current
00187     // intersection.
00188         unsigned char found_left=0;
00189         unsigned char found_forward=0;
00190         unsigned char found_right=0;
00191     int sensors[5];
00192     // Loop until we have solved the maze.
00193     while(1)
00194     {
00195     
00196         // Follow the line until an intersection is detected
00197         follow_line();
00198 
00199         // Inch forward a bit.  This helps us in case we entered the
00200         // intersection at an angle.
00201         found_left=0;found_forward=0;found_right=0;
00202      //   m3pi.forward(0.1);
00203        // wait(0.05);
00204         
00205         // Now read the sensors and check the intersection type.
00206 
00207  
00208       
00209         m3pi.forward(0.0);    
00210       
00211         // Check for a forward exit.
00212         m3pi.readsensor(sensors);
00213         if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200)
00214             found_forward = 1;
00215         
00216                m3pi.readsensor(sensors);
00217 
00218         // Check for left and right exits.
00219         if(sensors[0] > 200)
00220             found_left = 1;
00221         if(sensors[4] > 200)
00222             found_right = 1;
00223         
00224         //debug code
00225         m3pi.cls();
00226         if (found_left==1)
00227            m3pi.printf("L");
00228         if (found_right==1)
00229            m3pi.printf("R");
00230         if (found_forward==1)
00231            m3pi.printf("F");
00232         wait (3);
00233         // Check for the ending spot.
00234         // If all five sensors are on dark black, we have
00235         // solved the maze.
00236         if(sensors[0]>600 && sensors[1] > 600 && sensors[2] > 600 && sensors[3] > 600 && sensors[4]>600)
00237             break;
00238 
00239         // Drive straight a bit more - this is enough to line up our
00240         // wheels with the intersection.
00241         m3pi.forward(0.15);
00242         wait(0.02);  
00243 
00244         unsigned char dir = turn(found_left, found_forward, found_right);
00245 
00246         // Make the turn indicated by the path.
00247         doturn(dir);
00248 
00249         // Store the intersection in the path variable.
00250         path[path_length] = dir;
00251         path_length ++;
00252 
00253         // Need to insert check to make sure that the path_length does not
00254         // exceed the bounds of the array.  
00255 
00256         // Simplify the learned path.
00257         simplify();
00258 
00259     }
00260 
00261     // Solved the maze!
00262 
00263     // Now enter an infinite loop - we can re-run the maze as many
00264     // times as we want to.
00265     while(1)
00266     {
00267 
00268         m3pi.forward(0.0);
00269         m3pi.printf("Finished");
00270 
00271     // wait 15s to give time to turn off, or put the robot back to the start
00272     wait(15);
00273     // ideally we would use a button press here       
00274     // but I don't think it can easily be read
00275 
00276         // Re-run the maze.  It's not necessary to identify the
00277         // intersections, so this loop is really simple.
00278         int i;
00279         for(i=0;i<path_length;i++)
00280         {
00281              
00282             follow_line();
00283 
00284             // Drive straight while slowing down
00285             //m3pi.forward(0.5);
00286             //wait(0.05);
00287             m3pi.forward(0.3);
00288             wait(0.1);
00289 
00290             // Make a turn according to the instruction stored in
00291             // path[i].
00292             doturn(path[i]);
00293         }
00294         
00295         // Follow the last segment up to the finish.
00296         follow_line();
00297 
00298         // Now we should be at the finish!  Restart the loop.
00299     }
00300 }
00301 
00302  void checksensors()
00303 {
00304 int sensors[5];
00305 while (1) {
00306       m3pi.readsensor(sensors);
00307         m3pi.cls();
00308         if (sensors[0]>200)
00309            m3pi.printf("D");
00310            else  m3pi.printf("L");
00311         if (sensors[1]>200)
00312            m3pi.printf("D");
00313            else  m3pi.printf("L");     
00314              if (sensors[2]>200)
00315            m3pi.printf("D");
00316            else  m3pi.printf("L");
00317              if (sensors[3]>200)
00318            m3pi.printf("D");
00319            else  m3pi.printf("L");
00320              if (sensors[4]>200)
00321            m3pi.printf("D");
00322            else  m3pi.printf("L");
00323     }
00324 }
00325 int main() {
00326  //  int sensors[5]; 
00327     m3pi.locate(0,1);
00328     m3pi.sensor_auto_calibrate();
00329     m3pi.printf("MazeSolve");
00330 
00331     wait(2.0);
00332 
00333   mazesolve();
00334 
00335 m3pi.forward(0.0);
00336 
00337 }