Etienne Stransky
ok
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 00003 Timer chrono1; 00004 Timer chrono2; 00005 Timer chrono3; 00006 Timer chrono4; 00007 Timer chrono5; 00008 Timer chrono6; 00009 Timer chrono7; 00010 Timer chrono8; 00011 00012 InterruptIn ch1(p23); 00013 InterruptIn ch2(p24); 00014 InterruptIn ch3(p25); 00015 InterruptIn ch4(p26); 00016 InterruptIn ch5(p29); 00017 InterruptIn ch6(p30); 00018 InterruptIn Ultra1(p5); 00019 InterruptIn Ultra2(p7); 00020 00021 //SRF05 srf1(p6, p5);//p6:trigger, p5:echo 00022 //SRF05 srf2(p8, p7);//p8:trigger, p7:echo 00023 00024 Serial PC(USBTX, USBRX); 00025 Serial moteurs(p9, p10); 00026 Serial GPS(p13, p14); 00027 00028 DigitalOut del(LED1); 00029 DigitalOut puls1(p6); 00030 DigitalOut puls2(p8); 00031 00032 char etat_US=0; 00033 double tps_ch1=0,tps_ch2=0,tps_ch3=0,tps_ch4=0,tps_ch5=0,tps_ch6=0,dist_US1=0,dist_US2=0; 00034 00035 00036 void initial_PWMIn(void); 00037 void initial_Ultrasons(void); 00038 int calcul_vitesse(void); 00039 int calcul_direction(void); 00040 void ecriture_moteurs(int vitesse, int gouvernail); 00041 void impulsion1(void); 00042 void impulsion2(void); 00043 int lecture_GPS(double *ptr_heure,double *ptr_lat,double *ptr_longi,double *ptr_vitesse,double *ptr_route); 00044 double transfo_format(double a); 00045 00046 void ch1_start() { 00047 chrono1.start(); 00048 } 00049 00050 void ch1_stop() 00051 { 00052 chrono1.stop(); 00053 tps_ch1 = chrono1.read_us(); 00054 chrono1.reset(); 00055 } 00056 00057 void ch2_start() { 00058 chrono2.start(); 00059 } 00060 00061 void ch2_stop() 00062 { 00063 chrono2.stop(); 00064 tps_ch2 = chrono2.read_us(); 00065 chrono2.reset(); 00066 } 00067 00068 void ch3_start() { 00069 chrono3.start(); 00070 } 00071 00072 void ch3_stop() 00073 { 00074 chrono3.stop(); 00075 tps_ch3 = chrono3.read_us(); 00076 chrono3.reset(); 00077 } 00078 00079 void ch4_start() { 00080 chrono4.start(); 00081 } 00082 00083 void ch4_stop() 00084 { 00085 chrono4.stop(); 00086 tps_ch4 = chrono4.read_us(); 00087 chrono4.reset(); 00088 } 00089 00090 void ch5_start() { 00091 chrono5.start(); 00092 } 00093 00094 void ch5_stop() 00095 { 00096 chrono5.stop(); 00097 tps_ch5 = chrono5.read_us(); 00098 chrono5.reset(); 00099 } 00100 00101 void ch6_start() { 00102 chrono6.start(); 00103 } 00104 00105 void ch6_stop() 00106 { 00107 chrono6.stop(); 00108 tps_ch6 = chrono6.read_us(); 00109 chrono6.reset(); 00110 } 00111 00112 void Ultra1_start() 00113 { 00114 if(etat_US==0) 00115 { 00116 chrono7.reset(); 00117 chrono7.start(); 00118 } 00119 } 00120 00121 void Ultra1_stop()//Arrêt du chronomètre du capteur 1 et impulsion capteur 2 00122 { 00123 if(etat_US==0) 00124 { 00125 chrono7.stop(); 00126 dist_US1=chrono7.read_us()/58.0;//donne la distance en cm 00127 etat_US = 1; 00128 } 00129 } 00130 00131 void Ultra2_start()//Lancement du chronomètre du capteur 2 00132 { 00133 if(etat_US==1) 00134 { 00135 chrono8.reset(); 00136 chrono8.start(); 00137 } 00138 } 00139 00140 void Ultra2_stop()//Arrêt du chronomètre du capteur 2 et lancement d'une impulsion sur le capteur 1 00141 { 00142 if(etat_US==1) 00143 { 00144 chrono8.stop(); 00145 dist_US1=chrono8.read_us()/58.0;//donne la distance en cm 00146 etat_US = 0; 00147 impulsion1(); 00148 } 00149 } 00150 00151 00152 int main() { 00153 00154 double heure,latitude = 0,longitude = 0,vitesse_GPS,route,distance_restante; 00155 double lat_destination = 48.787068,longi_destination = 2.327229; 00156 double *ptr_h=&heure,*ptr_lat=&latitude,*ptr_long=&longitude,*ptr_v=&vitesse_GPS,*ptr_rte=&route; 00157 double route_th,diff_routes=0,gouvernail=0; 00158 int validite,vitesse,direction; 00159 00160 GPS.baud(9600); 00161 PC.baud(460800); 00162 initial_PWMIn(); 00163 initial_Ultrasons(); 00164 impulsion1(); 00165 00166 00167 while(true) { 00168 00169 validite = lecture_GPS(ptr_h,ptr_lat,ptr_long,ptr_v,ptr_rte);//Lecture du GPS. validite permet de savoir si le reception est bonne 00170 00171 if(validite == 1)//La réception est valide 00172 { 00173 latitude = transfo_format(latitude);//On change le format des coordonnées GPS 00174 longitude = transfo_format(longitude); 00175 PC.printf("LAT : %f, LONGI : %f\n\rLAT_DEST : %f LONGI_DEST : %f\r\n",latitude,longitude,lat_destination,longi_destination); 00176 00177 //Calcul de la route a suivre : 00178 route_th = 57.2956455309649*atan2(longi_destination-longitude,lat_destination-latitude); 00179 if(route_th<0) route_th += 360;//On s'assure que la route est comprise entre 0 et 360° 00180 00181 //Calcul de la distance à parcourir en mètres 00182 distance_restante = sqrt(pow(111111.1*(lat_destination-latitude),2)+pow(75000*(longi_destination-longitude),2)); 00183 00184 PC.printf("Route : %lf\n\rRoute theorique : %lf\r\nDistance restance : %lf\r\n",route,route_th,distance_restante); 00185 00186 00187 //Calcul, avec la route à suivre et la route suivie, des indications à donner 00188 //On utilise ici une variable "gouvernail", allant de -100 à 100 00189 //-100 correcpond à "à gauche toute" et 100 à "à droite toute" 00190 00191 if(distance_restante<10.0) 00192 { 00193 PC.printf("VOUS ETES ARRIVES\r\n"); 00194 } 00195 else 00196 { 00197 if((route<=route_th+5.0)&&(route>=route_th-5)) 00198 { 00199 PC.printf("TOUT DROIT\r\n"); 00200 gouvernail = 0; 00201 } 00202 else 00203 { 00204 //On calcule la différence d'angle entre la route actuellement suivie 00205 diff_routes = abs(route_th-route); 00206 if(diff_routes > 180.0) diff_routes = 360.0 - diff_routes; 00207 00208 //On vérifie la direction à suivre, s'il faut aller à droite ou à gauche. 00209 if((route_th>=route)&&(route_th<=(route+180))) 00210 { 00211 PC.printf("Direction : droite\r\n"); 00212 //On calcule la valeur à donner au gouvernail avec un correcteur proportionnel 00213 //si la différence est sup à 45°, on sature à 100, sinon on a une action 00214 //proportionnelle 00215 if(diff_routes>45.0) gouvernail = 100.0; 00216 else gouvernail = 2.22*diff_routes; 00217 PC.printf("Instruction donnee au gouvernail : %lf\r\n",gouvernail); 00218 } 00219 else 00220 { 00221 PC.printf("Direction : gauche\r\n"); 00222 //On fait de même pour tourner à gauche que pour tourner à droite, mais on a des 00223 //coefficients négatifs 00224 if(diff_routes>45.0) gouvernail = -100.0; 00225 else gouvernail = -2.22*diff_routes; 00226 PC.printf("Instruction donnee au gouvernail : %lf\r\n",gouvernail); 00227 } 00228 } 00229 } 00230 } 00231 else//Il y a une erreur de reception 00232 { 00233 PC.printf("erreur de reception"); 00234 } 00235 00236 //Programme mode manuel 00237 if(tps_ch6 > 1200) del = 1; //Allume la led si ch6 en position haute 00238 if(tps_ch6 < 1200) del = 0; //Allume la led si ch6 en position basse 00239 00240 00241 vitesse = calcul_vitesse(); 00242 direction = calcul_direction(); 00243 00244 ecriture_moteurs(vitesse,direction); 00245 00246 /*PC.printf("Tps haut entree 1:%.lfus\n\r",tps_ch1); 00247 PC.printf("Tps haut entree 2:%.lfus\n\r",tps_ch2); 00248 PC.printf("Tps haut entree 3:%.lfus\n\r",tps_ch3); 00249 PC.printf("Tps haut entree 4:%.lfus\n\r",tps_ch4); 00250 PC.printf("Tps haut entree 5:%.lfus\n\r",tps_ch5); 00251 PC.printf("Tps haut entree 6:%.lfus\n\r",tps_ch6);*/ 00252 PC.printf("Distance 1:%.lfcm\n\r",dist_US1); 00253 PC.printf("Distance 2:%.lfcm\n\r",dist_US2); 00254 PC.printf("Vitesse :%d\n\r",vitesse); 00255 PC.printf("Direction : %d\n\r",direction); 00256 PC.printf("\n\r"); 00257 wait(0.2); 00258 } 00259 } 00260 00261 void initial_PWMIn(void) 00262 { 00263 ch1.rise(&ch1_start); 00264 ch1.fall(&ch1_stop); 00265 00266 ch2.rise(&ch2_start); 00267 ch2.fall(&ch2_stop); 00268 00269 ch3.rise(&ch3_start); 00270 ch3.fall(&ch3_stop); 00271 00272 ch4.rise(&ch4_start); 00273 ch4.fall(&ch4_stop); 00274 00275 ch5.rise(&ch5_start); 00276 ch5.fall(&ch5_stop); 00277 00278 ch6.rise(&ch6_start); 00279 ch6.fall(&ch6_stop); 00280 00281 chrono1.reset(); 00282 chrono2.reset(); 00283 chrono3.reset(); 00284 chrono4.reset(); 00285 chrono5.reset(); 00286 chrono6.reset(); 00287 } 00288 00289 void initial_Ultrasons(void) 00290 { 00291 Ultra1.rise(&Ultra1_start); 00292 Ultra1.fall(&Ultra1_stop); 00293 00294 Ultra2.rise(&Ultra2_start); 00295 Ultra2.fall(&Ultra2_stop); 00296 } 00297 00298 void impulsion1(void) 00299 { 00300 puls1 = 1; 00301 wait (0.000002); 00302 puls1 = 0; 00303 } 00304 00305 void impulsion2(void) 00306 { 00307 puls2 = 1; 00308 wait (0.000002); 00309 puls2 = 0; 00310 } 00311 00312 int calcul_vitesse(void) 00313 { 00314 int val_ch2,val_ch3,vitesse; 00315 00316 val_ch2 = (tps_ch2-1500)/5; //calcul d'une valeur entre -100 et 100 selon la position du joustick 00317 val_ch3 = (tps_ch3-1500)/5; 00318 00319 //On compare les valeurs absolues des valeurs:c'est le joystick le plus éloigné 00320 //de la position centrale qui domine 00321 if(abs(val_ch2)>abs(val_ch3)) vitesse = val_ch2; 00322 else vitesse = val_ch3; 00323 00324 //vitesse nulle si les infos ne sont pas cohérentes 00325 if((tps_ch2<950)||(tps_ch2>2050)||(tps_ch3<950)||(tps_ch3>2050)) vitesse = 0; 00326 00327 if(abs(vitesse)<5) vitesse = 0;//Moteur immobile si jostick pas actionné 00328 if(vitesse>100) vitesse = 100; //saturation 00329 if(vitesse<-100) vitesse = -100; //saturation 00330 00331 return vitesse; 00332 } 00333 00334 int calcul_direction(void) 00335 { 00336 int val_ch4,direction; 00337 00338 //calcul d'une valeur entre -100 et 100 selon la position du joustick 00339 val_ch4 = (tps_ch4-1500)/5; 00340 00341 //On compare les valeurs absolues des valeurs:c'est le joystick le plus éloigné 00342 //de la position centrale qui domine 00343 direction = val_ch4; 00344 00345 //vitesse nulle si les infos ne sont pas cohérentes 00346 if((tps_ch4<950)||(tps_ch4>2050)) direction = 0; 00347 00348 if(abs(direction)<5) direction = 0;//Moteur immobile si jostick pas actionné 00349 if(direction>100) direction = 100; //saturation 00350 if(direction<-100) direction = -100; //saturation 00351 00352 return direction; 00353 } 00354 00355 void ecriture_moteurs(int vitesse, int gouvernail) 00356 { 00357 moteurs.printf("$%d|%d*",vitesse,gouvernail); 00358 } 00359 00360 //transforme les coordonnees au format dd.mmssss au lieu de ddmm.mmmm 00361 double transfo_format(double a) 00362 { 00363 double e; 00364 int c,b,d; 00365 d = a/100; 00366 c = a*10000; 00367 b = c%1000000; 00368 e = d+b/(60.0*10000.0); 00369 return e; 00370 } 00371 00372 //Fonction permettant de récupérer les données envoyées par la liaison série 00373 int lecture_GPS(double *ptr_heure,double *ptr_lat,double *ptr_longi,double *ptr_vitesse,double *ptr_route) 00374 { 00375 char i=2; 00376 char buffer[120]; 00377 int test_reception = 0; 00378 char a, etat=0; 00379 double h,lat,lng,v,dir; 00380 00381 while(test_reception == 0) 00382 { 00383 if (GPS.readable()) { // attention PC.readable reste à 1 tant qu'il n'y a pas eu de getc qui vide le buffer 00384 a=GPS.getc(); 00385 00386 switch(etat) 00387 { 00388 case 0 : 00389 if(a == '$') etat = 1; //On détecte le début de la trame 00390 break; 00391 case 1 : 00392 if(a != '$') //On s'assure que l'on reçoit bien un autre carcatère que celui de début de trame 00393 { 00394 etat = 2; 00395 buffer[0] = '$';//On stocke le caractère de début de trame dans la chaine (mais c'est juste pour faire joli en fait) 00396 buffer[1] = a;//On stocke le premier caractère utile de la trame 00397 } 00398 break; 00399 00400 case 2 : 00401 buffer[i] = a;//On stocke l'octet reçu dans la trame 00402 i++; 00403 if(a == 'C')//Si un caractère entré est un 'C', alors la trame nous intéresse : on poursuit 00404 { 00405 etat = 3; 00406 } 00407 else if(i>=7)//S'il n'y a pas de 'C' au 7ème caractère utile, la trame ne nous intéresse pas : on arrête l'acquisition 00408 { 00409 etat = 5; 00410 } 00411 break; 00412 00413 case 3 : 00414 00415 if(a=='$')//Si on revoit le caractère de début de trame, on s'arrête 00416 { 00417 etat = 4; 00418 } 00419 else 00420 { 00421 buffer[i]=a;//Sinon on stocke le caractère entré dans la chaîne de caractères 00422 i++; 00423 } 00424 break; 00425 00426 case 4 : 00427 00428 if((buffer[17]=='A')||(buffer[18]=='A')) 00429 { 00430 sscanf(buffer,"$GPRMC,%lf,A,%lf,N,%lf,E,%lf,%lf",&h,&lat,&lng,&v,&dir); 00431 *ptr_heure = h; 00432 *ptr_lat = lat; 00433 *ptr_longi = lng; 00434 *ptr_vitesse = v; 00435 *ptr_route = dir; 00436 test_reception = 1; 00437 } 00438 else 00439 { 00440 test_reception = 2; 00441 } 00442 PC.printf("%s",buffer);//On affiche la chaîne 00443 etat = 5; 00444 break; 00445 00446 case 5 : 00447 etat = 0;//On réinitialise l'acquisition 00448 for(i=0;i<120;i++)//On réinitialise la chaîne de caractères 00449 { 00450 buffer[i] = 0; 00451 } 00452 i=2; 00453 break; 00454 00455 default : 00456 etat = 0; 00457 break; 00458 } 00459 } 00460 00461 } 00462 return test_reception; 00463 } 00464
Generated on Fri Aug 5 2022 09:49:35 by
1.7.2