dalal dehmani
Projet véhicules autonomes
Dependencies: FileSystem_POPS TextLCD m3pi mbed
Fork of
m3PI_TP_POPS_II2015v0
by 
Samir Bouaziz
Embed:
(wiki syntax)
Show/hide line numbers
main3.cpp
00001 //Alaa et Dalal 00002 #include "mbed.h" 00003 #include "m3pi.h" 00004 #include "MSCFileSystem.h" 00005 #include "TextLCD.h" 00006 00007 00008 m3pi m3pi; // Initialise the m3pi 00009 TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD20x4); // rs, e, d4-d7 00010 Serial xbee(p28,p27); 00011 DigitalOut resetxbee(p26); 00012 Serial pc(USBTX, USBRX); // For debugging and pc 00013 Timer t; 00014 Ticker tick1; 00015 int count=0; 00016 unsigned char vecteur; 00017 bool cmd; 00018 int valeur; 00019 int k=1; 00020 float co; 00021 00022 void test(); 00023 BusOut myleds(LED1, LED2, LED3, LED4); 00024 00025 #define D_TERM 0.0 00026 #define I_TERM 0.1 00027 #define I_TERMO 0.1 00028 #define P_TERM 0.9 00029 #define MAX 0.3 00030 #define MIN -0.2 00031 00032 #define seuil(x) (x>600 ? 1 : 0) 00033 00034 float current_pos_of_line, 00035 previous_pos_of_line, 00036 derivate, 00037 proportional, 00038 power, 00039 integral, 00040 left, 00041 right, 00042 speed=0.2; 00043 char chain[15]; 00044 int v; 00045 char delai600ms; 00046 int etat; 00047 char etat1; 00048 float correction = 0.2; 00049 char aff[10],aff1[10]; 00050 unsigned short tabsensor[5]; 00051 //right= (right>MAX ? MAX :(right<MIN ? MIN : right)); 00052 //left= (left>MAX ? MAX :(left<MIN ? MIN : left)); 00053 unsigned char statecapt; 00054 bool mu; 00055 int countt,countt1; 00056 char save[255]=" ";//"LRBRLRBW"; 00057 char save1[15]="00000000000000"; 00058 int j=0; 00059 00060 00061 00062 //PID 00063 char PIDf(char commande) 00064 { 00065 if(commande==1) 00066 { 00067 if(statecapt==0x1F) 00068 { 00069 m3pi.left_motor(0); 00070 m3pi.right_motor(0); 00071 m3pi.forward(speed); 00072 myleds=0xF; 00073 return 0; 00074 } 00075 else 00076 { 00077 // Get the position of the line 00078 current_pos_of_line = m3pi.line_position(); 00079 proportional = current_pos_of_line; 00080 // Compute the derivate 00081 derivate = current_pos_of_line - previous_pos_of_line; 00082 // Compute the integral 00083 integral = (integral+I_TERMO*proportional)/(1+I_TERMO); 00084 // Remember the last postion 00085 previous_pos_of_line = current_pos_of_line; 00086 // Compute the power 00087 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + 00088 (derivate*(D_TERM)); 00089 // Compute new speeds 00090 right = speed-(power*MAX); 00091 left = speed+(power*MAX); 00092 // Limits checks on motor control 00093 right= (right>MAX ? MAX :(right<MIN ? MIN : right)); 00094 left= (left>MAX ? MAX :(left<MIN ? MIN : left)); 00095 // Send command to motors 00096 m3pi.left_motor(left); 00097 m3pi.right_motor(right); 00098 } 00099 } 00100 return 1; 00101 } 00102 00103 //Fonction superviseur 00104 //////////////////////////////////////*******************************optimisation de chaine***************//////////////////////////////////// 00105 void optim(char save[], int countt) 00106 { 00107 int i; 00108 int k; 00109 for(i=0;i<countt;i++) 00110 { 00111 00112 if ((save[i]=='R')&(save[i+1]=='B')&(save[i+2]=='R')) 00113 { 00114 save[i]='W'; 00115 for (k=1; k<(countt-i);k++) 00116 { 00117 save[i+k]=save[i+k+2]; 00118 } 00119 i++; 00120 } 00121 if ((save[i]=='W')&(save[i+1]=='B')&(save[i+2]=='R')) 00122 { 00123 save[i]='L'; 00124 for (k=1; k<(countt-i);k++) 00125 { 00126 save[i+k]=save[i+k+2]; 00127 } 00128 i++; 00129 } 00130 if ((save[i]=='W')&(save[i+1]=='B')&(save[i+2]=='W')) 00131 { 00132 save[i]='B'; 00133 for (k=1; k<(countt-i);k++) 00134 { 00135 save[i+k]=save[i+k+2]; 00136 } 00137 i++; 00138 } 00139 00140 if ((save[i]=='L')&(save[i+1]=='B')&(save[i+2]=='R')) 00141 { 00142 save[i]='B'; 00143 for (k=1; k<(countt-i);k++) 00144 { 00145 save[i+k]=save[i+k+2]; 00146 } 00147 i++; 00148 } 00149 if ((save[i]=='R')&(save[i+1]=='B')&(save[i+2]=='L')) 00150 { 00151 save[i]='B'; 00152 for (k=1; k<(countt-i);k++) 00153 { 00154 save[i+k]=save[i+k+2]; 00155 } 00156 i++; 00157 } 00158 if ((save[i]=='R')&(save[i+1]=='B')&(save[i+2]=='W')) 00159 { 00160 save[i]='L'; 00161 for (k=1; k<(countt-i);k++) 00162 { 00163 save[i+k]=save[i+k+2]; 00164 } 00165 i++; 00166 } 00167 00168 else 00169 { 00170 for (k=0; k<(countt-1);k++) 00171 { 00172 save[k]=save[k]; 00173 } 00174 } 00175 00176 }} 00177 00178 00179 00180 /////////////////////////////////////*****************************************************optimisation**************************///////////////////////// 00181 void test1 () 00182 { 00183 int i; 00184 m3pi.calibrated_sensors(tabsensor); 00185 for(i=0; i<5; i++) 00186 { 00187 00188 statecapt = (statecapt << 1) | seuil(tabsensor[i]); 00189 00190 } 00191 vecteur = (tabsensor[0]>600)*1+ (tabsensor[1]>600)*2+(tabsensor[2]>600)*4+ 00192 (tabsensor[3]>600)*8+(tabsensor[4]>600)*16; 00193 00194 if(k==1) 00195 { 00196 if ((tabsensor[0]<400)&(tabsensor[4]<400)) 00197 { 00198 if ((tabsensor[1]>400)|(tabsensor[2]>400)|(tabsensor[3]>400)) 00199 { 00200 etat1 ='P'; 00201 } 00202 else if ((tabsensor[1]<100)&(tabsensor[2]<100)&(tabsensor[3]<100)){ etat1=save[countt1];countt1++;} 00203 } 00204 else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]<100)&(tabsensor[4]>600)) ) 00205 { 00206 etat1=save[countt1];countt1++; 00207 } 00208 else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]>600)&(tabsensor[4]<100)) ) 00209 { 00210 etat1=save[countt1];countt1++; 00211 } 00212 else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]>600)&(tabsensor[4]>600)))//intersection + croix 00213 { 00214 etat1=save[countt1] ;countt1++; 00215 } 00216 } 00217 switch (etat1) 00218 { 00219 case 'B': 00220 m3pi.left_motor(speed); 00221 m3pi.right_motor(-speed); 00222 strcpy( aff,"back "); 00223 wait(0.74); 00224 PIDf(1); 00225 wait(0.1); 00226 break; 00227 00228 case 'E': 00229 m3pi.stop(); 00230 m3pi.cls(); 00231 m3pi.locate(0,1); 00232 m3pi.printf(save);; 00233 wait(1000); 00234 break; 00235 00236 case 'R' : 00237 strcpy( aff,"Right "); 00238 m3pi.left_motor(0); 00239 m3pi.right_motor(0); 00240 wait(0.01); 00241 m3pi.left_motor(1.2*(1.5/2)*speed); 00242 m3pi.right_motor(1.2*(1.5/2)*speed); 00243 wait(0.16); 00244 save[countt-1]='R'; 00245 m3pi.left_motor((1.5/2)*speed); 00246 m3pi.right_motor(-(1.5/2)*speed); 00247 wait(0.5); 00248 m3pi.left_motor((1.5/2)*speed); 00249 m3pi.right_motor((1.5/2)*speed); 00250 wait(0.1); 00251 PIDf(1); 00252 wait(0.001); 00253 m3pi.calibrated_sensors(tabsensor); 00254 break; 00255 00256 case 'L' : 00257 m3pi.left_motor(0); 00258 m3pi.right_motor(0); 00259 wait(0.01); 00260 m3pi.right_motor(1.2*(1.5/2)*speed); 00261 m3pi.left_motor(1.2*(1.5/2)*speed); 00262 wait(0.16); 00263 save[countt-1]='R'; 00264 m3pi.right_motor((1.5/2)*speed); 00265 m3pi.left_motor(-(1.5/2)*speed); 00266 wait(0.42); 00267 m3pi.left_motor((1.5/2)*speed); 00268 m3pi.right_motor((1.5/2)*speed); 00269 wait(0.1); 00270 PIDf(1); 00271 wait(0.001); 00272 m3pi.calibrated_sensors(tabsensor); 00273 break; 00274 00275 case 'W': 00276 m3pi.left_motor(0); 00277 m3pi.right_motor(0); 00278 wait(0.01); 00279 m3pi.left_motor(2*(1.5/2)*0.8*speed); 00280 m3pi.right_motor(2*(1.5/2)*0.8*speed); 00281 wait(0.14); 00282 strcpy( aff,"t_forward "); 00283 PIDf(1); 00284 m3pi.left_motor(0); 00285 m3pi.right_motor(0); 00286 strcpy( aff,"w"); 00287 m3pi.locate(0,1); 00288 m3pi.printf(aff); 00289 m3pi.stop(); 00290 wait(0.01); 00291 PIDf(1); 00292 wait(0.1); 00293 break; 00294 00295 case 'P': 00296 PIDf(1); 00297 sprintf(aff,"%d",countt); 00298 m3pi.locate(0,1); 00299 m3pi.printf(aff); 00300 wait(0.001); 00301 break; 00302 00303 }} 00304 00305 00306 ///////////////////////////////////////////////////////***************Superviseur principal**************///////////////////////////// 00307 void test () 00308 { 00309 int i; 00310 m3pi.calibrated_sensors(tabsensor); 00311 for(i=0; i<5; i++) 00312 { 00313 00314 statecapt = (statecapt << 1) | seuil(tabsensor[i]); 00315 00316 } 00317 vecteur = (tabsensor[0]>600)*1+ (tabsensor[1]>600)*2+(tabsensor[2]>600)*4+ 00318 (tabsensor[3]>600)*8+(tabsensor[4]>600)*16; 00319 00320 if(k==1) 00321 { 00322 if ((tabsensor[0]<400)&(tabsensor[4]<400)) 00323 { 00324 if ((tabsensor[1]>400)|(tabsensor[2]>400)|(tabsensor[3]>400)) 00325 etat =2; 00326 else if ((tabsensor[1]<100)&(tabsensor[2]<100)&(tabsensor[3]<100)){ etat =0;countt++;} 00327 } 00328 else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]<400)&(tabsensor[4]>600)) ) 00329 { 00330 etat=1;countt++; 00331 } 00332 else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]>800)&(tabsensor[4]<100)) ) 00333 { 00334 etat=4;countt++; 00335 } 00336 else if (((tabsensor[1]>600)|(tabsensor[2]>600)|(tabsensor[3]>600))& ((tabsensor[0]>600)&(tabsensor[4]>600)))//intersection + croix 00337 { 00338 etat=3 ;countt++; 00339 } 00340 } 00341 switch (etat) 00342 { 00343 case 0: 00344 m3pi.left_motor(1.2*(1.5/2)*speed); 00345 m3pi.right_motor(1.2*(1.5/2)*speed); 00346 wait(0.1); 00347 m3pi.left_motor(speed); 00348 m3pi.right_motor(-speed); 00349 strcpy( aff,"back "); 00350 wait(0.74); 00351 PIDf(1); 00352 wait(0.1); 00353 save[countt-1]='B'; 00354 break; 00355 case 1 : 00356 strcpy( aff,"Right "); 00357 m3pi.left_motor(0); 00358 m3pi.right_motor(0); 00359 wait(0.01); 00360 m3pi.left_motor(1.2*(1.5/2)*speed); 00361 m3pi.right_motor(1.2*(1.5/2)*speed); 00362 wait(0.16); 00363 save[countt-1]='R'; 00364 m3pi.left_motor((1.5/2)*speed); 00365 m3pi.right_motor(-(1.5/2)*speed); 00366 wait(0.42); 00367 m3pi.left_motor((1.5/2)*speed); 00368 m3pi.right_motor((1.5/2)*speed); 00369 wait(0.1); 00370 PIDf(1); 00371 wait(0.001); 00372 m3pi.calibrated_sensors(tabsensor); 00373 break; 00374 case 2: 00375 PIDf(1); 00376 sprintf(aff,"%d",countt);//strcpy( aff,"bye nombre"); 00377 m3pi.locate(0,1); 00378 m3pi.printf(aff); 00379 m3pi.calibrated_sensors(tabsensor); 00380 break; 00381 case 3: 00382 m3pi.left_motor(0); 00383 m3pi.right_motor(0); 00384 wait(0.01); 00385 m3pi.left_motor(1.2*(1.5/2)*speed); 00386 m3pi.right_motor(1.2*(1.5/2)*speed); 00387 wait(0.16); 00388 strcpy( aff,"t_forward "); 00389 m3pi.calibrated_sensors(tabsensor); 00390 wait(0.02); 00391 if ((tabsensor[0]>600)&(tabsensor[4]>200)&(tabsensor[2]>600)&(tabsensor[3]>600)&(tabsensor[1]>600))//the end 00392 { 00393 save[countt-1]='E'; 00394 m3pi.left_motor(2*0); 00395 m3pi.right_motor(2*0); 00396 cmd=false; 00397 m3pi.leds(255); 00398 wait(2); 00399 strcpy( aff,"Optim "); 00400 m3pi.leds(255); 00401 m3pi.locate(0,1); 00402 m3pi.printf(aff); 00403 wait(0.5); 00404 m3pi.leds(255); 00405 wait(0.5); 00406 m3pi.leds(127); 00407 wait(0.5); 00408 m3pi.leds(63); 00409 wait(0.5); 00410 strcpy( aff,"2 "); 00411 m3pi.locate(0,1); 00412 m3pi.printf(aff); 00413 m3pi.leds(31); 00414 wait(0.5); 00415 m3pi.leds(15); 00416 wait(0.5); 00417 m3pi.leds(7); 00418 strcpy( aff,"1 "); 00419 m3pi.locate(0,1); 00420 m3pi.printf(aff); 00421 m3pi.leds(3); 00422 wait(0.5); 00423 m3pi.leds(1); 00424 wait(0.5); 00425 strcpy( aff,"0 "); 00426 m3pi.locate(0,1); 00427 m3pi.printf(aff); 00428 wait(1); 00429 m3pi.leds(0); 00430 strcpy( aff,"GOOOOOOOOOO"); 00431 m3pi.locate(0,1); 00432 m3pi.printf(aff); 00433 wait(1); 00434 } 00435 else 00436 { 00437 save[countt-1]='R'; 00438 m3pi.left_motor((1.5/2)*speed); 00439 m3pi.right_motor(-(1.5/2)*speed); 00440 wait(0.53); 00441 m3pi.left_motor((1.5/2)*speed); 00442 m3pi.right_motor((1.5/2)*speed); 00443 wait(0.1); 00444 PIDf(1); 00445 wait(0.001); 00446 strcpy( aff,"turned "); 00447 } 00448 00449 break; 00450 case 4: 00451 m3pi.left_motor(0); 00452 m3pi.right_motor(0); 00453 wait(0.01); 00454 m3pi.left_motor(2*(1.5/2)*0.8*speed); 00455 m3pi.right_motor(2*(1.5/2)*0.8*speed); 00456 wait(0.14); 00457 strcpy( aff,"t_forward "); 00458 PIDf(1); 00459 m3pi.left_motor(0); 00460 m3pi.right_motor(0); 00461 m3pi.calibrated_sensors(tabsensor); 00462 save[countt-1]='W'; 00463 if (((tabsensor[0]<400)&(tabsensor[4]<400))&(tabsensor[2]>900))//forward 00464 { 00465 PIDf(1); 00466 wait(0.001); 00467 } 00468 if ((tabsensor[1]<100)&(tabsensor[2]<100)&(tabsensor[3]<100))//turn left 00469 { 00470 save[countt-1]='L'; 00471 m3pi.left_motor(-(1.5/2)*speed); 00472 m3pi.right_motor((1.5/2)*speed); 00473 wait(0.48); 00474 m3pi.left_motor((1.5/2)*speed); 00475 m3pi.right_motor((1.5/2)*speed); 00476 wait(0.21); 00477 } 00478 break; 00479 }} 00480 ////////////////////////*********************************************///////////////////////////////////////////////// 00481 00482 volatile char flag10ms; 00483 00484 void inter1() 00485 { 00486 flag10ms=1; 00487 00488 } 00489 00490 int main() 00491 { 00492 k=1; 00493 countt=0; 00494 countt1=0; 00495 cmd=1; 00496 resetxbee =0; 00497 wait(0.01); 00498 resetxbee =1; 00499 statecapt=0; 00500 m3pi.sensor_auto_calibrate(); 00501 wait(1.); 00502 tick1.attach(&inter1,0.01); 00503 mu=1; 00504 while(1) 00505 { 00506 if (1) 00507 { 00508 if(flag10ms==1) 00509 { 00510 flag10ms=0; 00511 t.reset(); 00512 t.start(); 00513 m3pi.calibrated_sensors(tabsensor); 00514 if(cmd) 00515 { 00516 test(); 00517 } 00518 else 00519 { 00520 optim(save,countt);optim(save,countt);optim(save,countt);optim(save,countt); 00521 test1(); 00522 } 00523 t.stop(); 00524 v=t.read_us(); 00525 delai600ms++; 00526 } 00527 } 00528 } 00529 00530 for (k=0 ;k<countt; k++ ) 00531 { 00532 if(k<8) 00533 { 00534 aff[k]=save[k]; 00535 } 00536 else 00537 { 00538 aff1[k]=save1[k]; 00539 } 00540 00541 } 00542 m3pi.locate(0,1); 00543 m3pi.printf(aff); 00544 00545 } 00546
Generated on Wed Jul 13 2022 03:19:29 by
1.7.2