Jon Marsh
/
m3pi_MazeSolver
This is work in progress - maze solving with m3pi robot. Not complete
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sat Jul 16 2022 00:01:34 by 1.7.2