Etienne Stransky
ok
Diff: main.cpp
- Revision:
- 0:47d13d3aec63
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Feb 12 14:48:10 2019 +0000
@@ -0,0 +1,464 @@
+ #include "mbed.h"
+
+Timer chrono1;
+Timer chrono2;
+Timer chrono3;
+Timer chrono4;
+Timer chrono5;
+Timer chrono6;
+Timer chrono7;
+Timer chrono8;
+
+InterruptIn ch1(p23);
+InterruptIn ch2(p24);
+InterruptIn ch3(p25);
+InterruptIn ch4(p26);
+InterruptIn ch5(p29);
+InterruptIn ch6(p30);
+InterruptIn Ultra1(p5);
+InterruptIn Ultra2(p7);
+
+//SRF05 srf1(p6, p5);//p6:trigger, p5:echo
+//SRF05 srf2(p8, p7);//p8:trigger, p7:echo
+
+Serial PC(USBTX, USBRX);
+Serial moteurs(p9, p10);
+Serial GPS(p13, p14);
+
+DigitalOut del(LED1);
+DigitalOut puls1(p6);
+DigitalOut puls2(p8);
+
+char etat_US=0;
+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;
+
+
+void initial_PWMIn(void);
+void initial_Ultrasons(void);
+int calcul_vitesse(void);
+int calcul_direction(void);
+void ecriture_moteurs(int vitesse, int gouvernail);
+void impulsion1(void);
+void impulsion2(void);
+int lecture_GPS(double *ptr_heure,double *ptr_lat,double *ptr_longi,double *ptr_vitesse,double *ptr_route);
+double transfo_format(double a);
+
+void ch1_start() {
+ chrono1.start();
+}
+
+void ch1_stop()
+{
+ chrono1.stop();
+ tps_ch1 = chrono1.read_us();
+ chrono1.reset();
+}
+
+void ch2_start() {
+ chrono2.start();
+}
+
+void ch2_stop()
+{
+ chrono2.stop();
+ tps_ch2 = chrono2.read_us();
+ chrono2.reset();
+}
+
+void ch3_start() {
+ chrono3.start();
+}
+
+void ch3_stop()
+{
+ chrono3.stop();
+ tps_ch3 = chrono3.read_us();
+ chrono3.reset();
+}
+
+void ch4_start() {
+ chrono4.start();
+}
+
+void ch4_stop()
+{
+ chrono4.stop();
+ tps_ch4 = chrono4.read_us();
+ chrono4.reset();
+}
+
+void ch5_start() {
+ chrono5.start();
+}
+
+void ch5_stop()
+{
+ chrono5.stop();
+ tps_ch5 = chrono5.read_us();
+ chrono5.reset();
+}
+
+void ch6_start() {
+ chrono6.start();
+}
+
+void ch6_stop()
+{
+ chrono6.stop();
+ tps_ch6 = chrono6.read_us();
+ chrono6.reset();
+}
+
+void Ultra1_start()
+{
+ if(etat_US==0)
+ {
+ chrono7.reset();
+ chrono7.start();
+ }
+}
+
+void Ultra1_stop()//Arrêt du chronomètre du capteur 1 et impulsion capteur 2
+{
+ if(etat_US==0)
+ {
+ chrono7.stop();
+ dist_US1=chrono7.read_us()/58.0;//donne la distance en cm
+ etat_US = 1;
+ }
+}
+
+void Ultra2_start()//Lancement du chronomètre du capteur 2
+{
+ if(etat_US==1)
+ {
+ chrono8.reset();
+ chrono8.start();
+ }
+}
+
+void Ultra2_stop()//Arrêt du chronomètre du capteur 2 et lancement d'une impulsion sur le capteur 1
+{
+ if(etat_US==1)
+ {
+ chrono8.stop();
+ dist_US1=chrono8.read_us()/58.0;//donne la distance en cm
+ etat_US = 0;
+ impulsion1();
+ }
+}
+
+
+int main() {
+
+ double heure,latitude = 0,longitude = 0,vitesse_GPS,route,distance_restante;
+ double lat_destination = 48.787068,longi_destination = 2.327229;
+ double *ptr_h=&heure,*ptr_lat=&latitude,*ptr_long=&longitude,*ptr_v=&vitesse_GPS,*ptr_rte=&route;
+ double route_th,diff_routes=0,gouvernail=0;
+ int validite,vitesse,direction;
+
+ GPS.baud(9600);
+ PC.baud(460800);
+ initial_PWMIn();
+ initial_Ultrasons();
+ impulsion1();
+
+
+ while(true) {
+
+ 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
+
+ if(validite == 1)//La réception est valide
+ {
+ latitude = transfo_format(latitude);//On change le format des coordonnées GPS
+ 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 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\nDistance restance : %lf\r\n",route,route_th,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);
+ }
+ }
+ }
+ }
+ else//Il y a une erreur de reception
+ {
+ PC.printf("erreur de reception");
+ }
+
+ //Programme mode manuel
+ if(tps_ch6 > 1200) del = 1; //Allume la led si ch6 en position haute
+ if(tps_ch6 < 1200) del = 0; //Allume la led si ch6 en position basse
+
+
+ vitesse = calcul_vitesse();
+ direction = calcul_direction();
+
+ ecriture_moteurs(vitesse,direction);
+
+ /*PC.printf("Tps haut entree 1:%.lfus\n\r",tps_ch1);
+ PC.printf("Tps haut entree 2:%.lfus\n\r",tps_ch2);
+ PC.printf("Tps haut entree 3:%.lfus\n\r",tps_ch3);
+ PC.printf("Tps haut entree 4:%.lfus\n\r",tps_ch4);
+ PC.printf("Tps haut entree 5:%.lfus\n\r",tps_ch5);
+ PC.printf("Tps haut entree 6:%.lfus\n\r",tps_ch6);*/
+ PC.printf("Distance 1:%.lfcm\n\r",dist_US1);
+ PC.printf("Distance 2:%.lfcm\n\r",dist_US2);
+ PC.printf("Vitesse :%d\n\r",vitesse);
+ PC.printf("Direction : %d\n\r",direction);
+ PC.printf("\n\r");
+ wait(0.2);
+ }
+}
+
+void initial_PWMIn(void)
+{
+ ch1.rise(&ch1_start);
+ ch1.fall(&ch1_stop);
+
+ ch2.rise(&ch2_start);
+ ch2.fall(&ch2_stop);
+
+ ch3.rise(&ch3_start);
+ ch3.fall(&ch3_stop);
+
+ ch4.rise(&ch4_start);
+ ch4.fall(&ch4_stop);
+
+ ch5.rise(&ch5_start);
+ ch5.fall(&ch5_stop);
+
+ ch6.rise(&ch6_start);
+ ch6.fall(&ch6_stop);
+
+ chrono1.reset();
+ chrono2.reset();
+ chrono3.reset();
+ chrono4.reset();
+ chrono5.reset();
+ chrono6.reset();
+}
+
+void initial_Ultrasons(void)
+{
+ Ultra1.rise(&Ultra1_start);
+ Ultra1.fall(&Ultra1_stop);
+
+ Ultra2.rise(&Ultra2_start);
+ Ultra2.fall(&Ultra2_stop);
+}
+
+void impulsion1(void)
+{
+ puls1 = 1;
+ wait (0.000002);
+ puls1 = 0;
+}
+
+void impulsion2(void)
+{
+ puls2 = 1;
+ wait (0.000002);
+ puls2 = 0;
+}
+
+int calcul_vitesse(void)
+{
+ int val_ch2,val_ch3,vitesse;
+
+ val_ch2 = (tps_ch2-1500)/5; //calcul d'une valeur entre -100 et 100 selon la position du joustick
+ val_ch3 = (tps_ch3-1500)/5;
+
+ //On compare les valeurs absolues des valeurs:c'est le joystick le plus éloigné
+ //de la position centrale qui domine
+ if(abs(val_ch2)>abs(val_ch3)) vitesse = val_ch2;
+ else vitesse = val_ch3;
+
+ //vitesse nulle si les infos ne sont pas cohérentes
+ if((tps_ch2<950)||(tps_ch2>2050)||(tps_ch3<950)||(tps_ch3>2050)) vitesse = 0;
+
+ if(abs(vitesse)<5) vitesse = 0;//Moteur immobile si jostick pas actionné
+ if(vitesse>100) vitesse = 100; //saturation
+ if(vitesse<-100) vitesse = -100; //saturation
+
+ return vitesse;
+}
+
+int calcul_direction(void)
+{
+ int val_ch4,direction;
+
+ //calcul d'une valeur entre -100 et 100 selon la position du joustick
+ val_ch4 = (tps_ch4-1500)/5;
+
+ //On compare les valeurs absolues des valeurs:c'est le joystick le plus éloigné
+ //de la position centrale qui domine
+ direction = val_ch4;
+
+ //vitesse nulle si les infos ne sont pas cohérentes
+ if((tps_ch4<950)||(tps_ch4>2050)) direction = 0;
+
+ if(abs(direction)<5) direction = 0;//Moteur immobile si jostick pas actionné
+ if(direction>100) direction = 100; //saturation
+ if(direction<-100) direction = -100; //saturation
+
+ return direction;
+}
+
+void ecriture_moteurs(int vitesse, int gouvernail)
+{
+ moteurs.printf("$%d|%d*",vitesse,gouvernail);
+}
+
+//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;
+}
+
+//Fonction permettant de récupérer les données envoyées par la liaison série
+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;
+}
+