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