belkaid sylia
MaszeSolver
Dependencies: mbed FileSystem_POPS m3pi
Embed:
(wiki syntax)
Show/hide line numbers
main3.cpp
00001 #include "mbed.h" 00002 #include "m3pi.h" 00003 #include "MSCFileSystem.h" 00004 00005 m3pi m3pi; // Initialise the m3pi 00006 char *replaceWord(const char *s, const char *oldW, const char *newW); 00007 Serial xbee(p28,p27); 00008 DigitalOut resetxbee(p26); 00009 Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging 00010 00011 Timer t; 00012 Ticker tick1; 00013 char affichage[3]={0}; 00014 char maze[100]={0}; 00015 char *optm; 00016 BusOut myleds(LED1, LED2, LED3, LED4); 00017 00018 #define D_TERM 0.0 00019 #define I_TERM 0.1 00020 #define I_TERMO 0.1 00021 #define P_TERM 0.9 00022 #define MAX 0.3 00023 #define MIN -0.2 00024 00025 #define seuil(x) (x>300 ? 1 : 0) 00026 00027 float current_pos_of_line, 00028 previous_pos_of_line, 00029 derivate, 00030 proportional, 00031 power, 00032 integral, 00033 left, 00034 right, 00035 speed=0.3; 00036 00037 char chain[10]; 00038 00039 int j = 0; 00040 00041 00042 unsigned short tabsensor[5]; 00043 volatile unsigned char sensors; 00044 00045 volatile char flag10ms; 00046 00047 char command=1; 00048 00049 void inter1() { 00050 flag10ms=1; 00051 } 00052 00053 void current_state() { 00054 unsigned char i; 00055 sensors=0; 00056 m3pi.calibrated_sensors(tabsensor); 00057 for(i=0; i<5; i++) { 00058 sensors = (sensors << 1) | seuil(tabsensor[i]); 00059 } 00060 } 00061 00062 void step() { 00063 m3pi.forward(0.12); 00064 wait(0.22); 00065 } 00066 00067 void step2() { 00068 m3pi.forward(0.12); 00069 wait(0.06); 00070 } 00071 00072 /* 00073 * Results 00074 * 1 -> PID 00075 * 2 -> Turn back 00076 * 3 -> Turn left 00077 * 4 -> Turn right 00078 */ 00079 char PIDf(char commande) { 00080 if(commande==1) { 00081 char result; 00082 current_state(); 00083 switch(sensors) { 00084 case 0x00: 00085 // Deadend 00086 // Back 00087 m3pi.cls(); 00088 strcat(maze,"B"); 00089 m3pi.locate(0,1); 00090 m3pi.printf(maze); 00091 m3pi.stop(); 00092 00093 result = 2; 00094 break; 00095 case 0x1C: case 0x18: case 0x10: 00096 // Forward/Left or Left Only 00097 step(); 00098 current_state(); 00099 if ((sensors == 0x00) || (sensors == 0x10)) { 00100 // Turn Left 00101 m3pi.cls(); 00102 strcat(maze,"L"); 00103 m3pi.locate(0,1); 00104 m3pi.printf(maze); 00105 00106 result = 3; 00107 } else { 00108 // Forward 00109 m3pi.cls(); 00110 strcat(maze,"F"); 00111 m3pi.locate(0,1); 00112 m3pi.printf(maze); 00113 step(); 00114 00115 result = 1; 00116 } 00117 break; 00118 case 0x07: case 0x03: case 0x01: 00119 // Forward/Right or Right Only 00120 m3pi.cls(); 00121 strcat(maze,"R"); 00122 m3pi.locate(0,1); 00123 m3pi.printf(maze); 00124 00125 step(); 00126 00127 result = 4; 00128 break; 00129 case 0x1F: 00130 // 'T' or Intersection or End 00131 step(); 00132 current_state(); 00133 if (sensors == 0x1F) { 00134 // End 00135 m3pi.cls(); 00136 strcat(maze,"E"); 00137 m3pi.locate(0,1); 00138 m3pi.printf(maze); 00139 00140 m3pi.stop(); 00141 00142 result = 5; 00143 } else { 00144 // 'T' -> Turn Right 00145 m3pi.cls(); 00146 strcat(maze, "R"); 00147 m3pi.locate(0,1); 00148 m3pi.printf(maze); 00149 00150 result = 4; 00151 } 00152 break; 00153 case 0x0F: case 0x1E: 00154 // 'T' or Intersection or End -> LR or RFL 00155 step2(); 00156 current_state(); 00157 if (sensors == 0x1F) { 00158 // End 00159 m3pi.cls(); 00160 strcat(maze,"E"); 00161 m3pi.locate(0,1); 00162 m3pi.printf(maze); 00163 00164 m3pi.stop(); 00165 00166 result = 5; 00167 } else if ((sensors == 0x10) || (sensors == 0x18)) { 00168 // Turn Left 00169 m3pi.cls(); 00170 strcat(maze,"L"); 00171 m3pi.locate(0,1); 00172 m3pi.printf(maze); 00173 00174 step2(); 00175 step2(); 00176 00177 result = 3; 00178 } else if ((sensors == 0x14) || (sensors == 0x16) || (sensors == 0x06) || (sensors == 0x1C)) { 00179 // Forward 00180 m3pi.cls(); 00181 strcat(maze,"F"); 00182 m3pi.locate(0,1); 00183 m3pi.printf(maze); 00184 00185 step2(); 00186 step2(); 00187 00188 result = 1; 00189 } else { 00190 // Turn Right 00191 m3pi.cls(); 00192 strcat(maze, "R"); 00193 m3pi.locate(0,1); 00194 m3pi.printf(maze); 00195 00196 step2(); 00197 step2(); 00198 00199 result = 4; 00200 } 00201 break; 00202 case 0x04: case 0x0C: case 0x06: case 0x0E: case 0x02: case 0x08: 00203 //PID 00204 // Get the position of the line 00205 current_pos_of_line = m3pi.line_position(); 00206 proportional = current_pos_of_line; 00207 // Compute the derivate 00208 derivate = current_pos_of_line - previous_pos_of_line; 00209 // Compute the integral 00210 integral = (integral+I_TERMO*proportional)/(1+I_TERMO); 00211 // Remember the last postion 00212 previous_pos_of_line = current_pos_of_line; 00213 // Compute the power 00214 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivate*(D_TERM)); 00215 // Compute new speeds 00216 right = speed-(power*MAX); 00217 left = speed+(power*MAX); 00218 // Limits checks on motor control 00219 right = (right>MAX ? MAX :(right<MIN ? MIN : right)); 00220 left = (left>MAX ? MAX :(left<MIN ? MIN : left)); 00221 // Send command to motors 00222 m3pi.left_motor(left); 00223 m3pi.right_motor(right); 00224 00225 result = 1; 00226 break; 00227 default: 00228 // Faire rien 00229 m3pi.stop(); 00230 00231 result = 0; 00232 break; 00233 } 00234 return result; 00235 } 00236 } 00237 00238 00239 /* 00240 * Results 00241 * 1 -> PID 00242 * 2 -> Turn back 00243 * 3 -> Turn left 00244 * 4 -> Turn right 00245 */ 00246 char turn(char command) { 00247 if(command > 1 && command < 5) { 00248 char result; 00249 current_state(); 00250 switch(command) { 00251 case 2: 00252 // Turn Back 00253 if(sensors != 0x01) { 00254 m3pi.right(speed); 00255 00256 result = 2; 00257 } else { 00258 do{ 00259 current_state(); 00260 m3pi.right(0.4*speed); 00261 }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F); 00262 m3pi.stop(); 00263 00264 result = 1; 00265 } 00266 break; 00267 case 3: 00268 // Turn Left 00269 if(sensors != 0x10) { 00270 m3pi.left(speed); 00271 00272 result = 3; 00273 } else { 00274 do{ 00275 current_state(); 00276 m3pi.left(0.4*speed); 00277 }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F); 00278 m3pi.stop(); 00279 00280 result = 1; 00281 } 00282 break; 00283 case 4: 00284 // Turn Right 00285 if(sensors != 0x01) { 00286 m3pi.right(speed); 00287 00288 result = 4; 00289 } else { 00290 do{ 00291 current_state(); 00292 m3pi.right(0.4*speed); 00293 }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F); 00294 m3pi.stop(); 00295 00296 result = 1; 00297 } 00298 break; 00299 } 00300 return result; 00301 } 00302 } 00303 00304 char find(char str[], char c) { 00305 unsigned char len = strlen(str); 00306 int i; 00307 00308 for(i = 0; i < len; i++) { 00309 if(str[i] == c) 00310 return 1; 00311 } 00312 00313 return 0; 00314 } 00315 00316 00317 char *replaceWord(const char *s, const char *oldW, const char *newW) { 00318 char *result; 00319 int i, cnt = 0; 00320 int newWlen = strlen(newW); 00321 int oldWlen = strlen(oldW); 00322 00323 // Counting the number of times old word 00324 // occur in the string 00325 for (i = 0; s[i] != '\0'; i++) { 00326 if (strstr(&s[i], oldW) == &s[i]) { 00327 cnt++; 00328 00329 // Jumping to index after the old word. 00330 i += oldWlen - 1; 00331 } 00332 } 00333 00334 // Making new string of enough length 00335 result = (char *)malloc(i + cnt * (newWlen - oldWlen) + 1); 00336 00337 i = 0; 00338 while (*s) { 00339 // compare the substring with the result 00340 if (strstr(s, oldW) == s) { 00341 strcpy(&result[i], newW); 00342 i += newWlen; 00343 s += oldWlen; 00344 } else 00345 result[i++] = *s++; 00346 } 00347 00348 result[i] = '\0'; 00349 return result; 00350 } 00351 00352 char *optimiser(char *str) { 00353 char *buf1 = str; 00354 char *buf2; 00355 00356 while(find(buf1,'B')) { 00357 buf2 = replaceWord(buf1, "RBR", "F"); 00358 buf1 = replaceWord(buf2, "RBL", "B"); 00359 buf2 = replaceWord(buf1, "RBF", "L"); 00360 00361 buf1 = replaceWord(buf2, "LBR", "B"); 00362 buf2 = replaceWord(buf1, "LBL", "F"); 00363 buf1 = replaceWord(buf2, "LBF", "R"); 00364 00365 buf2 = replaceWord(buf1, "FBR", "L"); 00366 buf1 = replaceWord(buf2, "FBL", "R"); 00367 buf2 = replaceWord(buf1, "FBF", "B"); 00368 00369 buf1 = buf2; 00370 } 00371 00372 return buf1; 00373 } 00374 00375 /* 1 -> PID 00376 * 2 -> Turn back 00377 * 3 -> Turn left 00378 * 4 -> Turn right 00379 */ 00380 char labyrinth(char commande) { 00381 if(commande==6) { 00382 int result = 6; 00383 00384 m3pi.cls(); 00385 m3pi.locate(0, 1); 00386 m3pi.printf(optm); 00387 current_state(); 00388 switch(sensors) { 00389 case 0x00: case 0x1C: case 0x18: case 0x10: case 0x07: case 0x03: case 0x1F: case 0x0F: case 0x1E: 00390 switch (optm[j]) { 00391 case 'B': 00392 m3pi.cls(); 00393 m3pi.locate(0, 1); 00394 m3pi.printf("B"); 00395 step(); 00396 do{ 00397 m3pi.right(speed); 00398 current_state(); 00399 }while(sensors!=0x01); 00400 do{ 00401 m3pi.right(0.4*speed); 00402 current_state(); 00403 }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F); 00404 m3pi.stop(); 00405 00406 break; 00407 case 'R': 00408 m3pi.cls(); 00409 m3pi.locate(0, 1); 00410 m3pi.printf("R"); 00411 step(); 00412 do{ 00413 m3pi.right(speed); 00414 current_state(); 00415 }while(sensors!=0x01); 00416 do{ 00417 m3pi.right(0.4*speed); 00418 current_state(); 00419 }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F); 00420 //wait(0.1); 00421 m3pi.stop(); 00422 00423 break; 00424 case 'L': 00425 m3pi.cls(); 00426 m3pi.locate(0, 1); 00427 m3pi.printf("L"); 00428 step(); 00429 do{ 00430 m3pi.left(speed); 00431 current_state(); 00432 }while(sensors!=0x10); 00433 do{ 00434 m3pi.left(0.4*speed); 00435 current_state(); 00436 }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F); 00437 //wait(0.1); 00438 m3pi.stop(); 00439 00440 break; 00441 case 'F': 00442 m3pi.cls(); 00443 m3pi.locate(0, 1); 00444 m3pi.printf("F"); 00445 step(); 00446 break; 00447 default: 00448 result = 0; 00449 break; 00450 } 00451 j++; 00452 break; 00453 case 0x04: case 0x0C: case 0x06: case 0x0E: case 0x02: case 0x08: 00454 //PID 00455 // Get the position of the line 00456 current_pos_of_line = m3pi.line_position(); 00457 proportional = current_pos_of_line; 00458 // Compute the derivate 00459 derivate = current_pos_of_line - previous_pos_of_line; 00460 // Compute the integral 00461 integral = (integral+I_TERMO*proportional)/(1+I_TERMO); 00462 // Remember the last postion 00463 previous_pos_of_line = current_pos_of_line; 00464 // Compute the power 00465 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivate*(D_TERM)); 00466 // Compute new speeds 00467 right = speed-(power*MAX); 00468 left = speed+(power*MAX); 00469 // Limits checks on motor control 00470 right = (right>MAX ? MAX :(right<MIN ? MIN : right)); 00471 left = (left>MAX ? MAX :(left<MIN ? MIN : left)); 00472 // Send command to motors 00473 m3pi.left_motor(left); 00474 m3pi.right_motor(right); 00475 00476 break; 00477 } 00478 return result; 00479 } 00480 } 00481 00482 00483 00484 int main() { 00485 resetxbee=0; 00486 wait(0.01); 00487 resetxbee =1; 00488 00489 FILE *p= fopen("/fs/tt.txt","a+"); 00490 m3pi.sensor_auto_calibrate(); 00491 wait(1.); 00492 tick1.attach(&inter1,0.01); 00493 00494 // fprintf(p,"ecrire dans la cle USB\r\n"); 00495 // fclose(p); 00496 00497 while(1) { 00498 if(flag10ms==1) { 00499 switch(command) { 00500 case 0: 00501 // Faire Rien 00502 m3pi.stop(); 00503 break; 00504 case 1: 00505 // PID 00506 command = PIDf(command); 00507 break; 00508 case 2: case 3: case 4: 00509 // 2 -> Back 00510 // 3 -> Left 00511 // 4 -> Right 00512 command = turn(command); 00513 break; 00514 case 5: 00515 // Optimisation 00516 optm = optimiser(maze); 00517 wait(5.0); 00518 m3pi.cls(); 00519 m3pi.locate(0, 1); 00520 m3pi.printf(optm); 00521 command = 6; 00522 break; 00523 case 6: 00524 // Labyrinth 00525 command = labyrinth(command); 00526 break; 00527 00528 } 00529 } 00530 } 00531 }
Generated on Fri Jul 15 2022 01:49:26 by
1.7.2