Revision 0:a3beb1f37be6, committed 2019-02-12

Comitter:

stersky

Date:

Tue Feb 12 14:46:28 2019 +0000

Commit message:

ok

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Feb 12 14:46:28 2019 +0000
@@ -0,0 +1,214 @@
+#include "mbed.h"
+
+
+int lecture_GPS(double *ptr_heure,double *ptr_lat,double *ptr_longi,double *ptr_vitesse,double *ptr_route);
+double transfo_format(double a);
+
+Serial PC(USBTX, USBRX);
+Serial GPS(p9,p10);
+
+
+
+int main()
+{
+    double heure,latitude = 0,longitude = 0,vitesse,route,distance_restante,lat_t_m_1 = 0,long_t_m_1 = 0;
+    double lat_destination = 48.787068,longi_destination = 2.327229;
+    double *ptr_h=&heure,*ptr_lat=&latitude,*ptr_long=&longitude,*ptr_v=&vitesse,*ptr_rte=&route;
+    double route_th,route_exp=0,route_exp_t_m_1=0,diff_routes=0,gouvernail=0;
+    int a; 
+    
+    GPS.baud(9600);
+    PC.baud(460800);
+
+    while(1) {
+        
+        //Mise en mémoire des anciennes coordonnées gps
+        lat_t_m_1 = latitude;
+        long_t_m_1 = longitude;
+        
+        //Lecture des données du GPS
+        do
+        {
+        a = lecture_GPS(ptr_h,ptr_lat,ptr_long,ptr_v,ptr_rte);
+        if (a!=1) {PC.printf("Erreur de reception\n\r\n\r");}
+        }
+        while(a==2);
+        
+        latitude = transfo_format(latitude);
+        longitude = transfo_format(longitude);
+        
+        PC.printf("LAT : %f, LONGI : %f\n\rLAT_DEST : %f LONGI_DEST : %f\r\n",latitude,longitude,lat_destination,longi_destination);
+        
+        //Calcul de la route a suivre :
+        route_th = 57.2956455309649*atan2(longi_destination-longitude,lat_destination-latitude);
+        if(route_th<0) route_th += 360;//On s'assure que la route est comprise entre 0 et 360°
+        
+        //Calcul de la route suivie ssi la position a changé
+        if((lat_t_m_1 > latitude+0.00001)||(lat_t_m_1 < latitude-0.00001)||(long_t_m_1 > longitude+0.00001)||(long_t_m_1 < latitude-0.00001))
+        {
+            route_exp = 0.5*(route_exp_t_m_1+57.2956455309649*atan2(longitude-long_t_m_1,latitude-lat_t_m_1));
+            if(route_exp<0) route_exp += 360;//On s'assure que la route est comprise entre 0 et 360°
+        }   
+        
+        //Calcul de la distance à parcourir en mètres
+        distance_restante = sqrt(pow(111111.1*(lat_destination-latitude),2)+pow(75000*(longi_destination-longitude),2));
+        
+        PC.printf("Route : %lf\n\rRoute theorique : %lf\r\nRoute experimentale : %lf\r\nDistance restance : %lf\r\n",route,route_th,route_exp,distance_restante);
+        
+        
+        //Calcul, avec la route à suivre et la route suivie, des indications à donner
+        //On utilise ici une variable "gouvernail", allant de -100 à 100
+        //-100 correcpond à "à gauche toute" et 100 à "à droite toute"
+        
+        if(distance_restante<10.0)
+        {
+            PC.printf("VOUS ETES ARRIVES\r\n");
+        }
+        else
+        {
+        if((route<=route_th+5.0)&&(route>=route_th-5))
+        {
+            PC.printf("TOUT DROIT\r\n");
+            gouvernail = 0;
+        }
+        else
+        {
+            //On calcule la différence d'angle entre la route actuellement suivie
+            diff_routes = abs(route_th-route);
+            if(diff_routes > 180.0) diff_routes = 360.0 - diff_routes;
+        
+        //On vérifie la direction à suivre, s'il faut aller à droite ou à gauche.    
+        if((route_th>=route)&&(route_th<=(route+180)))
+        {
+            PC.printf("Direction : droite\r\n");
+            //On calcule la valeur à donner au gouvernail avec un correcteur proportionnel
+            //si la différence est sup à 45°, on sature à 100, sinon on a une action
+            //proportionnelle
+            if(diff_routes>45.0) gouvernail = 100.0;
+            else gouvernail = 2.22*diff_routes;
+            PC.printf("Instruction donnee au gouvernail : %lf\r\n",gouvernail);
+        }
+        else
+        {
+            PC.printf("Direction : gauche\r\n");
+            //On fait de même pour tourner à gauche que pour tourner à droite, mais on a des
+            //coefficients négatifs
+            if(diff_routes>45.0) gouvernail = -100.0;
+            else gouvernail = -2.22*diff_routes;
+            PC.printf("Instruction donnee au gouvernail : %lf\r\n",gouvernail);
+        }
+        }
+        }
+        
+        PC.printf("\n\r");
+        route_exp_t_m_1 = route_exp;
+    }
+}
+
+//
+
+//transforme les coordonnees au format dd.mmssss au lieu de ddmm.mmmm
+double transfo_format(double a)
+{
+    double e;
+    int c,b,d;
+    d = a/100;
+    c = a*10000;
+    b = c%1000000;
+    e = d+b/(60.0*10000.0);
+    return e;
+}
+
+
+
+int lecture_GPS(double *ptr_heure,double *ptr_lat,double *ptr_longi,double *ptr_vitesse,double *ptr_route)
+{
+    char i=2;
+    char buffer[120];
+    int test_reception = 0;
+    char a, etat=0;
+    double h,lat,lng,v,dir;
+    
+    while(test_reception == 0)
+    {
+    if (GPS.readable()) {  // attention PC.readable reste à 1 tant qu'il n'y a pas eu de getc qui vide le buffer
+            a=GPS.getc();
+        
+        switch(etat)
+        {
+            case 0 :
+                if(a == '$') etat = 1; //On détecte le début de la trame
+                break;
+            case 1 :
+                if(a != '$') //On s'assure que l'on reçoit bien un autre carcatère que celui de début de trame
+                {
+                    etat = 2;
+                    buffer[0] = '$';//On stocke le caractère de début de trame dans la chaine (mais c'est juste pour faire joli en fait)
+                    buffer[1] = a;//On stocke le premier caractère utile de la trame
+                } 
+                break;
+                
+            case 2 :
+                buffer[i] = a;//On stocke l'octet reçu dans la trame
+                i++;
+                if(a == 'C')//Si un caractère entré est un 'C', alors la trame nous intéresse : on poursuit
+                {
+                    etat = 3;
+                }
+                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
+                {
+                    etat = 5;
+                }
+                break;
+                
+            case 3 :
+            
+                if(a=='$')//Si on revoit le caractère de début de trame, on s'arrête
+                {
+                    etat = 4;
+                }
+                else 
+                {
+                    buffer[i]=a;//Sinon on stocke le caractère entré dans la chaîne de caractères
+                    i++;
+                }
+                break;
+            
+            case 4 :
+            
+                if((buffer[17]=='A')||(buffer[18]=='A'))
+                {
+                        sscanf(buffer,"$GPRMC,%lf,A,%lf,N,%lf,E,%lf,%lf",&h,&lat,&lng,&v,&dir);
+                        *ptr_heure = h;
+                        *ptr_lat = lat;
+                        *ptr_longi = lng;
+                        *ptr_vitesse = v;
+                        *ptr_route = dir;
+                        test_reception = 1;
+                }
+                else
+                {
+                        test_reception = 2;
+                }
+                PC.printf("%s",buffer);//On affiche la chaîne
+                etat = 5;
+                break;
+                
+            case 5 :
+                etat = 0;//On réinitialise l'acquisition
+                for(i=0;i<120;i++)//On réinitialise la chaîne de caractères
+                {
+                    buffer[i] = 0;
+                }
+                i=2;
+                break;
+                
+            default :
+                etat = 0;
+                break;
+        }
+        }
+        
+        }
+        return test_reception;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Feb 12 14:46:28 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187
\ No newline at end of file