Etienne Stransky
/
programme_bateau_autre
ok
main.cpp
- Committer:
- stersky
- Date:
- 2019-02-12
- Revision:
- 0:47d13d3aec63
File content as of revision 0:47d13d3aec63:
#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; }