Etienne Stransky / Mbed 2 deprecated programme_bateau_autre

ok

Dependencies:   mbed SRF05

main.cpp@0:47d13d3aec63, 2019-02-12 (annotated)

Committer:
stersky
Date:
Tue Feb 12 14:48:10 2019 +0000
Revision:
0:47d13d3aec63
ok

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stersky 0:47d13d3aec63 1 #include "mbed.h"
stersky 0:47d13d3aec63 2
stersky 0:47d13d3aec63 3 Timer chrono1;
stersky 0:47d13d3aec63 4 Timer chrono2;
stersky 0:47d13d3aec63 5 Timer chrono3;
stersky 0:47d13d3aec63 6 Timer chrono4;
stersky 0:47d13d3aec63 7 Timer chrono5;
stersky 0:47d13d3aec63 8 Timer chrono6;
stersky 0:47d13d3aec63 9 Timer chrono7;
stersky 0:47d13d3aec63 10 Timer chrono8;
stersky 0:47d13d3aec63 11
stersky 0:47d13d3aec63 12 InterruptIn ch1(p23);
stersky 0:47d13d3aec63 13 InterruptIn ch2(p24);
stersky 0:47d13d3aec63 14 InterruptIn ch3(p25);
stersky 0:47d13d3aec63 15 InterruptIn ch4(p26);
stersky 0:47d13d3aec63 16 InterruptIn ch5(p29);
stersky 0:47d13d3aec63 17 InterruptIn ch6(p30);
stersky 0:47d13d3aec63 18 InterruptIn Ultra1(p5);
stersky 0:47d13d3aec63 19 InterruptIn Ultra2(p7);
stersky 0:47d13d3aec63 20
stersky 0:47d13d3aec63 21 //SRF05 srf1(p6, p5);//p6:trigger, p5:echo
stersky 0:47d13d3aec63 22 //SRF05 srf2(p8, p7);//p8:trigger, p7:echo
stersky 0:47d13d3aec63 23
stersky 0:47d13d3aec63 24 Serial PC(USBTX, USBRX);
stersky 0:47d13d3aec63 25 Serial moteurs(p9, p10);
stersky 0:47d13d3aec63 26 Serial GPS(p13, p14);
stersky 0:47d13d3aec63 27
stersky 0:47d13d3aec63 28 DigitalOut del(LED1);
stersky 0:47d13d3aec63 29 DigitalOut puls1(p6);
stersky 0:47d13d3aec63 30 DigitalOut puls2(p8);
stersky 0:47d13d3aec63 31
stersky 0:47d13d3aec63 32 char etat_US=0;
stersky 0:47d13d3aec63 33 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;
stersky 0:47d13d3aec63 34
stersky 0:47d13d3aec63 35
stersky 0:47d13d3aec63 36 void initial_PWMIn(void);
stersky 0:47d13d3aec63 37 void initial_Ultrasons(void);
stersky 0:47d13d3aec63 38 int calcul_vitesse(void);
stersky 0:47d13d3aec63 39 int calcul_direction(void);
stersky 0:47d13d3aec63 40 void ecriture_moteurs(int vitesse, int gouvernail);
stersky 0:47d13d3aec63 41 void impulsion1(void);
stersky 0:47d13d3aec63 42 void impulsion2(void);
stersky 0:47d13d3aec63 43 int lecture_GPS(double *ptr_heure,double *ptr_lat,double *ptr_longi,double *ptr_vitesse,double *ptr_route);
stersky 0:47d13d3aec63 44 double transfo_format(double a);
stersky 0:47d13d3aec63 45
stersky 0:47d13d3aec63 46 void ch1_start() {
stersky 0:47d13d3aec63 47 chrono1.start();
stersky 0:47d13d3aec63 48 }
stersky 0:47d13d3aec63 49
stersky 0:47d13d3aec63 50 void ch1_stop()
stersky 0:47d13d3aec63 51 {
stersky 0:47d13d3aec63 52 chrono1.stop();
stersky 0:47d13d3aec63 53 tps_ch1 = chrono1.read_us();
stersky 0:47d13d3aec63 54 chrono1.reset();
stersky 0:47d13d3aec63 55 }
stersky 0:47d13d3aec63 56
stersky 0:47d13d3aec63 57 void ch2_start() {
stersky 0:47d13d3aec63 58 chrono2.start();
stersky 0:47d13d3aec63 59 }
stersky 0:47d13d3aec63 60
stersky 0:47d13d3aec63 61 void ch2_stop()
stersky 0:47d13d3aec63 62 {
stersky 0:47d13d3aec63 63 chrono2.stop();
stersky 0:47d13d3aec63 64 tps_ch2 = chrono2.read_us();
stersky 0:47d13d3aec63 65 chrono2.reset();
stersky 0:47d13d3aec63 66 }
stersky 0:47d13d3aec63 67
stersky 0:47d13d3aec63 68 void ch3_start() {
stersky 0:47d13d3aec63 69 chrono3.start();
stersky 0:47d13d3aec63 70 }
stersky 0:47d13d3aec63 71
stersky 0:47d13d3aec63 72 void ch3_stop()
stersky 0:47d13d3aec63 73 {
stersky 0:47d13d3aec63 74 chrono3.stop();
stersky 0:47d13d3aec63 75 tps_ch3 = chrono3.read_us();
stersky 0:47d13d3aec63 76 chrono3.reset();
stersky 0:47d13d3aec63 77 }
stersky 0:47d13d3aec63 78
stersky 0:47d13d3aec63 79 void ch4_start() {
stersky 0:47d13d3aec63 80 chrono4.start();
stersky 0:47d13d3aec63 81 }
stersky 0:47d13d3aec63 82
stersky 0:47d13d3aec63 83 void ch4_stop()
stersky 0:47d13d3aec63 84 {
stersky 0:47d13d3aec63 85 chrono4.stop();
stersky 0:47d13d3aec63 86 tps_ch4 = chrono4.read_us();
stersky 0:47d13d3aec63 87 chrono4.reset();
stersky 0:47d13d3aec63 88 }
stersky 0:47d13d3aec63 89
stersky 0:47d13d3aec63 90 void ch5_start() {
stersky 0:47d13d3aec63 91 chrono5.start();
stersky 0:47d13d3aec63 92 }
stersky 0:47d13d3aec63 93
stersky 0:47d13d3aec63 94 void ch5_stop()
stersky 0:47d13d3aec63 95 {
stersky 0:47d13d3aec63 96 chrono5.stop();
stersky 0:47d13d3aec63 97 tps_ch5 = chrono5.read_us();
stersky 0:47d13d3aec63 98 chrono5.reset();
stersky 0:47d13d3aec63 99 }
stersky 0:47d13d3aec63 100
stersky 0:47d13d3aec63 101 void ch6_start() {
stersky 0:47d13d3aec63 102 chrono6.start();
stersky 0:47d13d3aec63 103 }
stersky 0:47d13d3aec63 104
stersky 0:47d13d3aec63 105 void ch6_stop()
stersky 0:47d13d3aec63 106 {
stersky 0:47d13d3aec63 107 chrono6.stop();
stersky 0:47d13d3aec63 108 tps_ch6 = chrono6.read_us();
stersky 0:47d13d3aec63 109 chrono6.reset();
stersky 0:47d13d3aec63 110 }
stersky 0:47d13d3aec63 111
stersky 0:47d13d3aec63 112 void Ultra1_start()
stersky 0:47d13d3aec63 113 {
stersky 0:47d13d3aec63 114 if(etat_US==0)
stersky 0:47d13d3aec63 115 {
stersky 0:47d13d3aec63 116 chrono7.reset();
stersky 0:47d13d3aec63 117 chrono7.start();
stersky 0:47d13d3aec63 118 }
stersky 0:47d13d3aec63 119 }
stersky 0:47d13d3aec63 120
stersky 0:47d13d3aec63 121 void Ultra1_stop()//Arrêt du chronomètre du capteur 1 et impulsion capteur 2
stersky 0:47d13d3aec63 122 {
stersky 0:47d13d3aec63 123 if(etat_US==0)
stersky 0:47d13d3aec63 124 {
stersky 0:47d13d3aec63 125 chrono7.stop();
stersky 0:47d13d3aec63 126 dist_US1=chrono7.read_us()/58.0;//donne la distance en cm
stersky 0:47d13d3aec63 127 etat_US = 1;
stersky 0:47d13d3aec63 128 }
stersky 0:47d13d3aec63 129 }
stersky 0:47d13d3aec63 130
stersky 0:47d13d3aec63 131 void Ultra2_start()//Lancement du chronomètre du capteur 2
stersky 0:47d13d3aec63 132 {
stersky 0:47d13d3aec63 133 if(etat_US==1)
stersky 0:47d13d3aec63 134 {
stersky 0:47d13d3aec63 135 chrono8.reset();
stersky 0:47d13d3aec63 136 chrono8.start();
stersky 0:47d13d3aec63 137 }
stersky 0:47d13d3aec63 138 }
stersky 0:47d13d3aec63 139
stersky 0:47d13d3aec63 140 void Ultra2_stop()//Arrêt du chronomètre du capteur 2 et lancement d'une impulsion sur le capteur 1
stersky 0:47d13d3aec63 141 {
stersky 0:47d13d3aec63 142 if(etat_US==1)
stersky 0:47d13d3aec63 143 {
stersky 0:47d13d3aec63 144 chrono8.stop();
stersky 0:47d13d3aec63 145 dist_US1=chrono8.read_us()/58.0;//donne la distance en cm
stersky 0:47d13d3aec63 146 etat_US = 0;
stersky 0:47d13d3aec63 147 impulsion1();
stersky 0:47d13d3aec63 148 }
stersky 0:47d13d3aec63 149 }
stersky 0:47d13d3aec63 150
stersky 0:47d13d3aec63 151
stersky 0:47d13d3aec63 152 int main() {
stersky 0:47d13d3aec63 153
stersky 0:47d13d3aec63 154 double heure,latitude = 0,longitude = 0,vitesse_GPS,route,distance_restante;
stersky 0:47d13d3aec63 155 double lat_destination = 48.787068,longi_destination = 2.327229;
stersky 0:47d13d3aec63 156 double *ptr_h=&heure,*ptr_lat=&latitude,*ptr_long=&longitude,*ptr_v=&vitesse_GPS,*ptr_rte=&route;
stersky 0:47d13d3aec63 157 double route_th,diff_routes=0,gouvernail=0;
stersky 0:47d13d3aec63 158 int validite,vitesse,direction;
stersky 0:47d13d3aec63 159
stersky 0:47d13d3aec63 160 GPS.baud(9600);
stersky 0:47d13d3aec63 161 PC.baud(460800);
stersky 0:47d13d3aec63 162 initial_PWMIn();
stersky 0:47d13d3aec63 163 initial_Ultrasons();
stersky 0:47d13d3aec63 164 impulsion1();
stersky 0:47d13d3aec63 165
stersky 0:47d13d3aec63 166
stersky 0:47d13d3aec63 167 while(true) {
stersky 0:47d13d3aec63 168
stersky 0:47d13d3aec63 169 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
stersky 0:47d13d3aec63 170
stersky 0:47d13d3aec63 171 if(validite == 1)//La réception est valide
stersky 0:47d13d3aec63 172 {
stersky 0:47d13d3aec63 173 latitude = transfo_format(latitude);//On change le format des coordonnées GPS
stersky 0:47d13d3aec63 174 longitude = transfo_format(longitude);
stersky 0:47d13d3aec63 175 PC.printf("LAT : %f, LONGI : %f\n\rLAT_DEST : %f LONGI_DEST : %f\r\n",latitude,longitude,lat_destination,longi_destination);
stersky 0:47d13d3aec63 176
stersky 0:47d13d3aec63 177 //Calcul de la route a suivre :
stersky 0:47d13d3aec63 178 route_th = 57.2956455309649*atan2(longi_destination-longitude,lat_destination-latitude);
stersky 0:47d13d3aec63 179 if(route_th<0) route_th += 360;//On s'assure que la route est comprise entre 0 et 360°
stersky 0:47d13d3aec63 180
stersky 0:47d13d3aec63 181 //Calcul de la distance à parcourir en mètres
stersky 0:47d13d3aec63 182 distance_restante = sqrt(pow(111111.1*(lat_destination-latitude),2)+pow(75000*(longi_destination-longitude),2));
stersky 0:47d13d3aec63 183
stersky 0:47d13d3aec63 184 PC.printf("Route : %lf\n\rRoute theorique : %lf\r\nDistance restance : %lf\r\n",route,route_th,distance_restante);
stersky 0:47d13d3aec63 185
stersky 0:47d13d3aec63 186
stersky 0:47d13d3aec63 187 //Calcul, avec la route à suivre et la route suivie, des indications à donner
stersky 0:47d13d3aec63 188 //On utilise ici une variable "gouvernail", allant de -100 à 100
stersky 0:47d13d3aec63 189 //-100 correcpond à "à gauche toute" et 100 à "à droite toute"
stersky 0:47d13d3aec63 190
stersky 0:47d13d3aec63 191 if(distance_restante<10.0)
stersky 0:47d13d3aec63 192 {
stersky 0:47d13d3aec63 193 PC.printf("VOUS ETES ARRIVES\r\n");
stersky 0:47d13d3aec63 194 }
stersky 0:47d13d3aec63 195 else
stersky 0:47d13d3aec63 196 {
stersky 0:47d13d3aec63 197 if((route<=route_th+5.0)&&(route>=route_th-5))
stersky 0:47d13d3aec63 198 {
stersky 0:47d13d3aec63 199 PC.printf("TOUT DROIT\r\n");
stersky 0:47d13d3aec63 200 gouvernail = 0;
stersky 0:47d13d3aec63 201 }
stersky 0:47d13d3aec63 202 else
stersky 0:47d13d3aec63 203 {
stersky 0:47d13d3aec63 204 //On calcule la différence d'angle entre la route actuellement suivie
stersky 0:47d13d3aec63 205 diff_routes = abs(route_th-route);
stersky 0:47d13d3aec63 206 if(diff_routes > 180.0) diff_routes = 360.0 - diff_routes;
stersky 0:47d13d3aec63 207
stersky 0:47d13d3aec63 208 //On vérifie la direction à suivre, s'il faut aller à droite ou à gauche.
stersky 0:47d13d3aec63 209 if((route_th>=route)&&(route_th<=(route+180)))
stersky 0:47d13d3aec63 210 {
stersky 0:47d13d3aec63 211 PC.printf("Direction : droite\r\n");
stersky 0:47d13d3aec63 212 //On calcule la valeur à donner au gouvernail avec un correcteur proportionnel
stersky 0:47d13d3aec63 213 //si la différence est sup à 45°, on sature à 100, sinon on a une action
stersky 0:47d13d3aec63 214 //proportionnelle
stersky 0:47d13d3aec63 215 if(diff_routes>45.0) gouvernail = 100.0;
stersky 0:47d13d3aec63 216 else gouvernail = 2.22*diff_routes;
stersky 0:47d13d3aec63 217 PC.printf("Instruction donnee au gouvernail : %lf\r\n",gouvernail);
stersky 0:47d13d3aec63 218 }
stersky 0:47d13d3aec63 219 else
stersky 0:47d13d3aec63 220 {
stersky 0:47d13d3aec63 221 PC.printf("Direction : gauche\r\n");
stersky 0:47d13d3aec63 222 //On fait de même pour tourner à gauche que pour tourner à droite, mais on a des
stersky 0:47d13d3aec63 223 //coefficients négatifs
stersky 0:47d13d3aec63 224 if(diff_routes>45.0) gouvernail = -100.0;
stersky 0:47d13d3aec63 225 else gouvernail = -2.22*diff_routes;
stersky 0:47d13d3aec63 226 PC.printf("Instruction donnee au gouvernail : %lf\r\n",gouvernail);
stersky 0:47d13d3aec63 227 }
stersky 0:47d13d3aec63 228 }
stersky 0:47d13d3aec63 229 }
stersky 0:47d13d3aec63 230 }
stersky 0:47d13d3aec63 231 else//Il y a une erreur de reception
stersky 0:47d13d3aec63 232 {
stersky 0:47d13d3aec63 233 PC.printf("erreur de reception");
stersky 0:47d13d3aec63 234 }
stersky 0:47d13d3aec63 235
stersky 0:47d13d3aec63 236 //Programme mode manuel
stersky 0:47d13d3aec63 237 if(tps_ch6 > 1200) del = 1; //Allume la led si ch6 en position haute
stersky 0:47d13d3aec63 238 if(tps_ch6 < 1200) del = 0; //Allume la led si ch6 en position basse
stersky 0:47d13d3aec63 239
stersky 0:47d13d3aec63 240
stersky 0:47d13d3aec63 241 vitesse = calcul_vitesse();
stersky 0:47d13d3aec63 242 direction = calcul_direction();
stersky 0:47d13d3aec63 243
stersky 0:47d13d3aec63 244 ecriture_moteurs(vitesse,direction);
stersky 0:47d13d3aec63 245
stersky 0:47d13d3aec63 246 /*PC.printf("Tps haut entree 1:%.lfus\n\r",tps_ch1);
stersky 0:47d13d3aec63 247 PC.printf("Tps haut entree 2:%.lfus\n\r",tps_ch2);
stersky 0:47d13d3aec63 248 PC.printf("Tps haut entree 3:%.lfus\n\r",tps_ch3);
stersky 0:47d13d3aec63 249 PC.printf("Tps haut entree 4:%.lfus\n\r",tps_ch4);
stersky 0:47d13d3aec63 250 PC.printf("Tps haut entree 5:%.lfus\n\r",tps_ch5);
stersky 0:47d13d3aec63 251 PC.printf("Tps haut entree 6:%.lfus\n\r",tps_ch6);*/
stersky 0:47d13d3aec63 252 PC.printf("Distance 1:%.lfcm\n\r",dist_US1);
stersky 0:47d13d3aec63 253 PC.printf("Distance 2:%.lfcm\n\r",dist_US2);
stersky 0:47d13d3aec63 254 PC.printf("Vitesse :%d\n\r",vitesse);
stersky 0:47d13d3aec63 255 PC.printf("Direction : %d\n\r",direction);
stersky 0:47d13d3aec63 256 PC.printf("\n\r");
stersky 0:47d13d3aec63 257 wait(0.2);
stersky 0:47d13d3aec63 258 }
stersky 0:47d13d3aec63 259 }
stersky 0:47d13d3aec63 260
stersky 0:47d13d3aec63 261 void initial_PWMIn(void)
stersky 0:47d13d3aec63 262 {
stersky 0:47d13d3aec63 263 ch1.rise(&ch1_start);
stersky 0:47d13d3aec63 264 ch1.fall(&ch1_stop);
stersky 0:47d13d3aec63 265
stersky 0:47d13d3aec63 266 ch2.rise(&ch2_start);
stersky 0:47d13d3aec63 267 ch2.fall(&ch2_stop);
stersky 0:47d13d3aec63 268
stersky 0:47d13d3aec63 269 ch3.rise(&ch3_start);
stersky 0:47d13d3aec63 270 ch3.fall(&ch3_stop);
stersky 0:47d13d3aec63 271
stersky 0:47d13d3aec63 272 ch4.rise(&ch4_start);
stersky 0:47d13d3aec63 273 ch4.fall(&ch4_stop);
stersky 0:47d13d3aec63 274
stersky 0:47d13d3aec63 275 ch5.rise(&ch5_start);
stersky 0:47d13d3aec63 276 ch5.fall(&ch5_stop);
stersky 0:47d13d3aec63 277
stersky 0:47d13d3aec63 278 ch6.rise(&ch6_start);
stersky 0:47d13d3aec63 279 ch6.fall(&ch6_stop);
stersky 0:47d13d3aec63 280
stersky 0:47d13d3aec63 281 chrono1.reset();
stersky 0:47d13d3aec63 282 chrono2.reset();
stersky 0:47d13d3aec63 283 chrono3.reset();
stersky 0:47d13d3aec63 284 chrono4.reset();
stersky 0:47d13d3aec63 285 chrono5.reset();
stersky 0:47d13d3aec63 286 chrono6.reset();
stersky 0:47d13d3aec63 287 }
stersky 0:47d13d3aec63 288
stersky 0:47d13d3aec63 289 void initial_Ultrasons(void)
stersky 0:47d13d3aec63 290 {
stersky 0:47d13d3aec63 291 Ultra1.rise(&Ultra1_start);
stersky 0:47d13d3aec63 292 Ultra1.fall(&Ultra1_stop);
stersky 0:47d13d3aec63 293
stersky 0:47d13d3aec63 294 Ultra2.rise(&Ultra2_start);
stersky 0:47d13d3aec63 295 Ultra2.fall(&Ultra2_stop);
stersky 0:47d13d3aec63 296 }
stersky 0:47d13d3aec63 297
stersky 0:47d13d3aec63 298 void impulsion1(void)
stersky 0:47d13d3aec63 299 {
stersky 0:47d13d3aec63 300 puls1 = 1;
stersky 0:47d13d3aec63 301 wait (0.000002);
stersky 0:47d13d3aec63 302 puls1 = 0;
stersky 0:47d13d3aec63 303 }
stersky 0:47d13d3aec63 304
stersky 0:47d13d3aec63 305 void impulsion2(void)
stersky 0:47d13d3aec63 306 {
stersky 0:47d13d3aec63 307 puls2 = 1;
stersky 0:47d13d3aec63 308 wait (0.000002);
stersky 0:47d13d3aec63 309 puls2 = 0;
stersky 0:47d13d3aec63 310 }
stersky 0:47d13d3aec63 311
stersky 0:47d13d3aec63 312 int calcul_vitesse(void)
stersky 0:47d13d3aec63 313 {
stersky 0:47d13d3aec63 314 int val_ch2,val_ch3,vitesse;
stersky 0:47d13d3aec63 315
stersky 0:47d13d3aec63 316 val_ch2 = (tps_ch2-1500)/5; //calcul d'une valeur entre -100 et 100 selon la position du joustick
stersky 0:47d13d3aec63 317 val_ch3 = (tps_ch3-1500)/5;
stersky 0:47d13d3aec63 318
stersky 0:47d13d3aec63 319 //On compare les valeurs absolues des valeurs:c'est le joystick le plus éloigné
stersky 0:47d13d3aec63 320 //de la position centrale qui domine
stersky 0:47d13d3aec63 321 if(abs(val_ch2)>abs(val_ch3)) vitesse = val_ch2;
stersky 0:47d13d3aec63 322 else vitesse = val_ch3;
stersky 0:47d13d3aec63 323
stersky 0:47d13d3aec63 324 //vitesse nulle si les infos ne sont pas cohérentes
stersky 0:47d13d3aec63 325 if((tps_ch2<950)||(tps_ch2>2050)||(tps_ch3<950)||(tps_ch3>2050)) vitesse = 0;
stersky 0:47d13d3aec63 326
stersky 0:47d13d3aec63 327 if(abs(vitesse)<5) vitesse = 0;//Moteur immobile si jostick pas actionné
stersky 0:47d13d3aec63 328 if(vitesse>100) vitesse = 100; //saturation
stersky 0:47d13d3aec63 329 if(vitesse<-100) vitesse = -100; //saturation
stersky 0:47d13d3aec63 330
stersky 0:47d13d3aec63 331 return vitesse;
stersky 0:47d13d3aec63 332 }
stersky 0:47d13d3aec63 333
stersky 0:47d13d3aec63 334 int calcul_direction(void)
stersky 0:47d13d3aec63 335 {
stersky 0:47d13d3aec63 336 int val_ch4,direction;
stersky 0:47d13d3aec63 337
stersky 0:47d13d3aec63 338 //calcul d'une valeur entre -100 et 100 selon la position du joustick
stersky 0:47d13d3aec63 339 val_ch4 = (tps_ch4-1500)/5;
stersky 0:47d13d3aec63 340
stersky 0:47d13d3aec63 341 //On compare les valeurs absolues des valeurs:c'est le joystick le plus éloigné
stersky 0:47d13d3aec63 342 //de la position centrale qui domine
stersky 0:47d13d3aec63 343 direction = val_ch4;
stersky 0:47d13d3aec63 344
stersky 0:47d13d3aec63 345 //vitesse nulle si les infos ne sont pas cohérentes
stersky 0:47d13d3aec63 346 if((tps_ch4<950)||(tps_ch4>2050)) direction = 0;
stersky 0:47d13d3aec63 347
stersky 0:47d13d3aec63 348 if(abs(direction)<5) direction = 0;//Moteur immobile si jostick pas actionné
stersky 0:47d13d3aec63 349 if(direction>100) direction = 100; //saturation
stersky 0:47d13d3aec63 350 if(direction<-100) direction = -100; //saturation
stersky 0:47d13d3aec63 351
stersky 0:47d13d3aec63 352 return direction;
stersky 0:47d13d3aec63 353 }
stersky 0:47d13d3aec63 354
stersky 0:47d13d3aec63 355 void ecriture_moteurs(int vitesse, int gouvernail)
stersky 0:47d13d3aec63 356 {
stersky 0:47d13d3aec63 357 moteurs.printf("$%d|%d*",vitesse,gouvernail);
stersky 0:47d13d3aec63 358 }
stersky 0:47d13d3aec63 359
stersky 0:47d13d3aec63 360 //transforme les coordonnees au format dd.mmssss au lieu de ddmm.mmmm
stersky 0:47d13d3aec63 361 double transfo_format(double a)
stersky 0:47d13d3aec63 362 {
stersky 0:47d13d3aec63 363 double e;
stersky 0:47d13d3aec63 364 int c,b,d;
stersky 0:47d13d3aec63 365 d = a/100;
stersky 0:47d13d3aec63 366 c = a*10000;
stersky 0:47d13d3aec63 367 b = c%1000000;
stersky 0:47d13d3aec63 368 e = d+b/(60.0*10000.0);
stersky 0:47d13d3aec63 369 return e;
stersky 0:47d13d3aec63 370 }
stersky 0:47d13d3aec63 371
stersky 0:47d13d3aec63 372 //Fonction permettant de récupérer les données envoyées par la liaison série
stersky 0:47d13d3aec63 373 int lecture_GPS(double *ptr_heure,double *ptr_lat,double *ptr_longi,double *ptr_vitesse,double *ptr_route)
stersky 0:47d13d3aec63 374 {
stersky 0:47d13d3aec63 375 char i=2;
stersky 0:47d13d3aec63 376 char buffer[120];
stersky 0:47d13d3aec63 377 int test_reception = 0;
stersky 0:47d13d3aec63 378 char a, etat=0;
stersky 0:47d13d3aec63 379 double h,lat,lng,v,dir;
stersky 0:47d13d3aec63 380
stersky 0:47d13d3aec63 381 while(test_reception == 0)
stersky 0:47d13d3aec63 382 {
stersky 0:47d13d3aec63 383 if (GPS.readable()) { // attention PC.readable reste à 1 tant qu'il n'y a pas eu de getc qui vide le buffer
stersky 0:47d13d3aec63 384 a=GPS.getc();
stersky 0:47d13d3aec63 385
stersky 0:47d13d3aec63 386 switch(etat)
stersky 0:47d13d3aec63 387 {
stersky 0:47d13d3aec63 388 case 0 :
stersky 0:47d13d3aec63 389 if(a == '$') etat = 1; //On détecte le début de la trame
stersky 0:47d13d3aec63 390 break;
stersky 0:47d13d3aec63 391 case 1 :
stersky 0:47d13d3aec63 392 if(a != '$') //On s'assure que l'on reçoit bien un autre carcatère que celui de début de trame
stersky 0:47d13d3aec63 393 {
stersky 0:47d13d3aec63 394 etat = 2;
stersky 0:47d13d3aec63 395 buffer[0] = '$';//On stocke le caractère de début de trame dans la chaine (mais c'est juste pour faire joli en fait)
stersky 0:47d13d3aec63 396 buffer[1] = a;//On stocke le premier caractère utile de la trame
stersky 0:47d13d3aec63 397 }
stersky 0:47d13d3aec63 398 break;
stersky 0:47d13d3aec63 399
stersky 0:47d13d3aec63 400 case 2 :
stersky 0:47d13d3aec63 401 buffer[i] = a;//On stocke l'octet reçu dans la trame
stersky 0:47d13d3aec63 402 i++;
stersky 0:47d13d3aec63 403 if(a == 'C')//Si un caractère entré est un 'C', alors la trame nous intéresse : on poursuit
stersky 0:47d13d3aec63 404 {
stersky 0:47d13d3aec63 405 etat = 3;
stersky 0:47d13d3aec63 406 }
stersky 0:47d13d3aec63 407 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
stersky 0:47d13d3aec63 408 {
stersky 0:47d13d3aec63 409 etat = 5;
stersky 0:47d13d3aec63 410 }
stersky 0:47d13d3aec63 411 break;
stersky 0:47d13d3aec63 412
stersky 0:47d13d3aec63 413 case 3 :
stersky 0:47d13d3aec63 414
stersky 0:47d13d3aec63 415 if(a=='$')//Si on revoit le caractère de début de trame, on s'arrête
stersky 0:47d13d3aec63 416 {
stersky 0:47d13d3aec63 417 etat = 4;
stersky 0:47d13d3aec63 418 }
stersky 0:47d13d3aec63 419 else
stersky 0:47d13d3aec63 420 {
stersky 0:47d13d3aec63 421 buffer[i]=a;//Sinon on stocke le caractère entré dans la chaîne de caractères
stersky 0:47d13d3aec63 422 i++;
stersky 0:47d13d3aec63 423 }
stersky 0:47d13d3aec63 424 break;
stersky 0:47d13d3aec63 425
stersky 0:47d13d3aec63 426 case 4 :
stersky 0:47d13d3aec63 427
stersky 0:47d13d3aec63 428 if((buffer[17]=='A')||(buffer[18]=='A'))
stersky 0:47d13d3aec63 429 {
stersky 0:47d13d3aec63 430 sscanf(buffer,"$GPRMC,%lf,A,%lf,N,%lf,E,%lf,%lf",&h,&lat,&lng,&v,&dir);
stersky 0:47d13d3aec63 431 *ptr_heure = h;
stersky 0:47d13d3aec63 432 *ptr_lat = lat;
stersky 0:47d13d3aec63 433 *ptr_longi = lng;
stersky 0:47d13d3aec63 434 *ptr_vitesse = v;
stersky 0:47d13d3aec63 435 *ptr_route = dir;
stersky 0:47d13d3aec63 436 test_reception = 1;
stersky 0:47d13d3aec63 437 }
stersky 0:47d13d3aec63 438 else
stersky 0:47d13d3aec63 439 {
stersky 0:47d13d3aec63 440 test_reception = 2;
stersky 0:47d13d3aec63 441 }
stersky 0:47d13d3aec63 442 PC.printf("%s",buffer);//On affiche la chaîne
stersky 0:47d13d3aec63 443 etat = 5;
stersky 0:47d13d3aec63 444 break;
stersky 0:47d13d3aec63 445
stersky 0:47d13d3aec63 446 case 5 :
stersky 0:47d13d3aec63 447 etat = 0;//On réinitialise l'acquisition
stersky 0:47d13d3aec63 448 for(i=0;i<120;i++)//On réinitialise la chaîne de caractères
stersky 0:47d13d3aec63 449 {
stersky 0:47d13d3aec63 450 buffer[i] = 0;
stersky 0:47d13d3aec63 451 }
stersky 0:47d13d3aec63 452 i=2;
stersky 0:47d13d3aec63 453 break;
stersky 0:47d13d3aec63 454
stersky 0:47d13d3aec63 455 default :
stersky 0:47d13d3aec63 456 etat = 0;
stersky 0:47d13d3aec63 457 break;
stersky 0:47d13d3aec63 458 }
stersky 0:47d13d3aec63 459 }
stersky 0:47d13d3aec63 460
stersky 0:47d13d3aec63 461 }
stersky 0:47d13d3aec63 462 return test_reception;
stersky 0:47d13d3aec63 463 }
stersky 0:47d13d3aec63 464