Etienne Stransky / Mbed 2 deprecated programme_bateau

ok

Dependencies:   mbed SRF05

main.cpp@0:5ab762b6468f, 2019-02-12 (annotated)

Committer:
stersky
Date:
Tue Feb 12 14:47:42 2019 +0000
Revision:
0:5ab762b6468f
ok

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stersky 0:5ab762b6468f 1 #include "mbed.h"
stersky 0:5ab762b6468f 2 #include "SRF05.h"
stersky 0:5ab762b6468f 3
stersky 0:5ab762b6468f 4 Timer chrono1;
stersky 0:5ab762b6468f 5 Timer chrono2;
stersky 0:5ab762b6468f 6 Timer chrono3;
stersky 0:5ab762b6468f 7 Timer chrono4;
stersky 0:5ab762b6468f 8 Timer chrono5;
stersky 0:5ab762b6468f 9 Timer chrono6;
stersky 0:5ab762b6468f 10 Timer chrono7;
stersky 0:5ab762b6468f 11 Timer chrono8;
stersky 0:5ab762b6468f 12
stersky 0:5ab762b6468f 13 InterruptIn ch1(p23);
stersky 0:5ab762b6468f 14 InterruptIn ch2(p24);
stersky 0:5ab762b6468f 15 InterruptIn ch3(p25);
stersky 0:5ab762b6468f 16 InterruptIn ch4(p26);
stersky 0:5ab762b6468f 17 InterruptIn ch5(p29);
stersky 0:5ab762b6468f 18 InterruptIn ch6(p30);
stersky 0:5ab762b6468f 19 InterruptIn Ultra1(p5);
stersky 0:5ab762b6468f 20 InterruptIn Ultra2(p7);
stersky 0:5ab762b6468f 21
stersky 0:5ab762b6468f 22 //SRF05 srf1(p6, p5);//p6:trigger, p5:echo
stersky 0:5ab762b6468f 23 //SRF05 srf2(p8, p7);//p8:trigger, p7:echo
stersky 0:5ab762b6468f 24
stersky 0:5ab762b6468f 25 Serial PC(USBTX, USBRX);
stersky 0:5ab762b6468f 26 Serial moteurs(p9, p10);
stersky 0:5ab762b6468f 27
stersky 0:5ab762b6468f 28 DigitalOut del(LED1);
stersky 0:5ab762b6468f 29 DigitalOut puls1(p6);
stersky 0:5ab762b6468f 30 DigitalOut puls2(p8);
stersky 0:5ab762b6468f 31
stersky 0:5ab762b6468f 32 char etat_US=0;
stersky 0:5ab762b6468f 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:5ab762b6468f 34
stersky 0:5ab762b6468f 35 void initial_PWMIn(void);
stersky 0:5ab762b6468f 36 void initial_Ultrasons(void);
stersky 0:5ab762b6468f 37 int calcul_vitesse(void);
stersky 0:5ab762b6468f 38 int calcul_direction(void);
stersky 0:5ab762b6468f 39 void ecriture_moteurs(int vitesse, int gouvernail);
stersky 0:5ab762b6468f 40 void impulsion1(void);
stersky 0:5ab762b6468f 41 void impulsion2(void);
stersky 0:5ab762b6468f 42
stersky 0:5ab762b6468f 43 void ch1_start() {
stersky 0:5ab762b6468f 44 chrono1.start();
stersky 0:5ab762b6468f 45 }
stersky 0:5ab762b6468f 46
stersky 0:5ab762b6468f 47 void ch1_stop()
stersky 0:5ab762b6468f 48 {
stersky 0:5ab762b6468f 49 chrono1.stop();
stersky 0:5ab762b6468f 50 tps_ch1 = chrono1.read_us();
stersky 0:5ab762b6468f 51 chrono1.reset();
stersky 0:5ab762b6468f 52 }
stersky 0:5ab762b6468f 53
stersky 0:5ab762b6468f 54 void ch2_start() {
stersky 0:5ab762b6468f 55 chrono2.start();
stersky 0:5ab762b6468f 56 }
stersky 0:5ab762b6468f 57
stersky 0:5ab762b6468f 58 void ch2_stop()
stersky 0:5ab762b6468f 59 {
stersky 0:5ab762b6468f 60 chrono2.stop();
stersky 0:5ab762b6468f 61 tps_ch2 = chrono2.read_us();
stersky 0:5ab762b6468f 62 chrono2.reset();
stersky 0:5ab762b6468f 63 }
stersky 0:5ab762b6468f 64
stersky 0:5ab762b6468f 65 void ch3_start() {
stersky 0:5ab762b6468f 66 chrono3.start();
stersky 0:5ab762b6468f 67 }
stersky 0:5ab762b6468f 68
stersky 0:5ab762b6468f 69 void ch3_stop()
stersky 0:5ab762b6468f 70 {
stersky 0:5ab762b6468f 71 chrono3.stop();
stersky 0:5ab762b6468f 72 tps_ch3 = chrono3.read_us();
stersky 0:5ab762b6468f 73 chrono3.reset();
stersky 0:5ab762b6468f 74 }
stersky 0:5ab762b6468f 75
stersky 0:5ab762b6468f 76 void ch4_start() {
stersky 0:5ab762b6468f 77 chrono4.start();
stersky 0:5ab762b6468f 78 }
stersky 0:5ab762b6468f 79
stersky 0:5ab762b6468f 80 void ch4_stop()
stersky 0:5ab762b6468f 81 {
stersky 0:5ab762b6468f 82 chrono4.stop();
stersky 0:5ab762b6468f 83 tps_ch4 = chrono4.read_us();
stersky 0:5ab762b6468f 84 chrono4.reset();
stersky 0:5ab762b6468f 85 }
stersky 0:5ab762b6468f 86
stersky 0:5ab762b6468f 87 void ch5_start() {
stersky 0:5ab762b6468f 88 chrono5.start();
stersky 0:5ab762b6468f 89 }
stersky 0:5ab762b6468f 90
stersky 0:5ab762b6468f 91 void ch5_stop()
stersky 0:5ab762b6468f 92 {
stersky 0:5ab762b6468f 93 chrono5.stop();
stersky 0:5ab762b6468f 94 tps_ch5 = chrono5.read_us();
stersky 0:5ab762b6468f 95 chrono5.reset();
stersky 0:5ab762b6468f 96 }
stersky 0:5ab762b6468f 97
stersky 0:5ab762b6468f 98 void ch6_start() {
stersky 0:5ab762b6468f 99 chrono6.start();
stersky 0:5ab762b6468f 100 }
stersky 0:5ab762b6468f 101
stersky 0:5ab762b6468f 102 void ch6_stop()
stersky 0:5ab762b6468f 103 {
stersky 0:5ab762b6468f 104 chrono6.stop();
stersky 0:5ab762b6468f 105 tps_ch6 = chrono6.read_us();
stersky 0:5ab762b6468f 106 chrono6.reset();
stersky 0:5ab762b6468f 107 }
stersky 0:5ab762b6468f 108
stersky 0:5ab762b6468f 109 void Ultra1_start()
stersky 0:5ab762b6468f 110 {
stersky 0:5ab762b6468f 111 if(etat_US==0)
stersky 0:5ab762b6468f 112 {
stersky 0:5ab762b6468f 113 chrono7.reset();
stersky 0:5ab762b6468f 114 chrono7.start();
stersky 0:5ab762b6468f 115 }
stersky 0:5ab762b6468f 116 }
stersky 0:5ab762b6468f 117
stersky 0:5ab762b6468f 118 void Ultra1_stop()//Arrêt du chronomètre du capteur 1 et impulsion capteur 2
stersky 0:5ab762b6468f 119 {
stersky 0:5ab762b6468f 120 if(etat_US==0)
stersky 0:5ab762b6468f 121 {
stersky 0:5ab762b6468f 122 chrono7.stop();
stersky 0:5ab762b6468f 123 dist_US1=chrono7.read_us()/58.0;//donne la distance en cm
stersky 0:5ab762b6468f 124 etat_US = 1;
stersky 0:5ab762b6468f 125 impulsion2();
stersky 0:5ab762b6468f 126 }
stersky 0:5ab762b6468f 127 }
stersky 0:5ab762b6468f 128
stersky 0:5ab762b6468f 129 void Ultra2_start()//Lancement du chronomètre du capteur 2
stersky 0:5ab762b6468f 130 {
stersky 0:5ab762b6468f 131 if(etat_US==1)
stersky 0:5ab762b6468f 132 {
stersky 0:5ab762b6468f 133 chrono8.reset();
stersky 0:5ab762b6468f 134 chrono8.start();
stersky 0:5ab762b6468f 135 }
stersky 0:5ab762b6468f 136 }
stersky 0:5ab762b6468f 137
stersky 0:5ab762b6468f 138 void Ultra2_stop()//Arrêt du chronomètre du capteur 2 et lancement d'une impulsion sur le capteur 1
stersky 0:5ab762b6468f 139 {
stersky 0:5ab762b6468f 140 if(etat_US==1)
stersky 0:5ab762b6468f 141 {
stersky 0:5ab762b6468f 142 chrono8.stop();
stersky 0:5ab762b6468f 143 dist_US2=chrono8.read_us()/58.0;//donne la distance en cm
stersky 0:5ab762b6468f 144 etat_US = 0;
stersky 0:5ab762b6468f 145 impulsion1();
stersky 0:5ab762b6468f 146 }
stersky 0:5ab762b6468f 147 }
stersky 0:5ab762b6468f 148
stersky 0:5ab762b6468f 149
stersky 0:5ab762b6468f 150 int main() {
stersky 0:5ab762b6468f 151
stersky 0:5ab762b6468f 152 PC.baud(460800);
stersky 0:5ab762b6468f 153 initial_PWMIn();
stersky 0:5ab762b6468f 154 initial_Ultrasons();
stersky 0:5ab762b6468f 155 impulsion1();
stersky 0:5ab762b6468f 156
stersky 0:5ab762b6468f 157 int vitesse,direction;
stersky 0:5ab762b6468f 158
stersky 0:5ab762b6468f 159 while(true) {
stersky 0:5ab762b6468f 160
stersky 0:5ab762b6468f 161 if(tps_ch6 > 1200) del = 1; //Allume la led si ch6 en position haute
stersky 0:5ab762b6468f 162 if(tps_ch6 < 1200) del = 0; //Allume la led si ch6 en position basse
stersky 0:5ab762b6468f 163
stersky 0:5ab762b6468f 164 //vitesse = calcul_vitesse();
stersky 0:5ab762b6468f 165 //direction = calcul_direction();
stersky 0:5ab762b6468f 166
stersky 0:5ab762b6468f 167 //ecriture_moteurs(vitesse,direction);
stersky 0:5ab762b6468f 168
stersky 0:5ab762b6468f 169 PC.printf("Tps haut entree 1:%.lfus\n\r",tps_ch1);
stersky 0:5ab762b6468f 170 PC.printf("Tps haut entree 2:%.lfus\n\r",tps_ch2);
stersky 0:5ab762b6468f 171 PC.printf("Tps haut entree 3:%.lfus\n\r",tps_ch3);
stersky 0:5ab762b6468f 172 PC.printf("Tps haut entree 4:%.lfus\n\r",tps_ch4);
stersky 0:5ab762b6468f 173 PC.printf("Tps haut entree 5:%.lfus\n\r",tps_ch5);
stersky 0:5ab762b6468f 174 PC.printf("Tps haut entree 6:%.lfus\n\r",tps_ch6);
stersky 0:5ab762b6468f 175 PC.printf("Distance 1:%.lfcm\n\r",dist_US1);
stersky 0:5ab762b6468f 176 PC.printf("Distance 2:%.lfcm\n\r",dist_US2);
stersky 0:5ab762b6468f 177 //PC.printf("Vitesse :%d\n\r",vitesse);
stersky 0:5ab762b6468f 178 //PC.printf("Direction : %d\n\r",direction);
stersky 0:5ab762b6468f 179 PC.printf("\n\r");
stersky 0:5ab762b6468f 180 wait(0.2);
stersky 0:5ab762b6468f 181 }
stersky 0:5ab762b6468f 182 }
stersky 0:5ab762b6468f 183
stersky 0:5ab762b6468f 184 void initial_PWMIn(void)
stersky 0:5ab762b6468f 185 {
stersky 0:5ab762b6468f 186 ch1.rise(&ch1_start);
stersky 0:5ab762b6468f 187 ch1.fall(&ch1_stop);
stersky 0:5ab762b6468f 188
stersky 0:5ab762b6468f 189 ch2.rise(&ch2_start);
stersky 0:5ab762b6468f 190 ch2.fall(&ch2_stop);
stersky 0:5ab762b6468f 191
stersky 0:5ab762b6468f 192 ch3.rise(&ch3_start);
stersky 0:5ab762b6468f 193 ch3.fall(&ch3_stop);
stersky 0:5ab762b6468f 194
stersky 0:5ab762b6468f 195 ch4.rise(&ch4_start);
stersky 0:5ab762b6468f 196 ch4.fall(&ch4_stop);
stersky 0:5ab762b6468f 197
stersky 0:5ab762b6468f 198 ch5.rise(&ch5_start);
stersky 0:5ab762b6468f 199 ch5.fall(&ch5_stop);
stersky 0:5ab762b6468f 200
stersky 0:5ab762b6468f 201 ch6.rise(&ch6_start);
stersky 0:5ab762b6468f 202 ch6.fall(&ch6_stop);
stersky 0:5ab762b6468f 203
stersky 0:5ab762b6468f 204 chrono1.reset();
stersky 0:5ab762b6468f 205 chrono2.reset();
stersky 0:5ab762b6468f 206 chrono3.reset();
stersky 0:5ab762b6468f 207 chrono4.reset();
stersky 0:5ab762b6468f 208 chrono5.reset();
stersky 0:5ab762b6468f 209 chrono6.reset();
stersky 0:5ab762b6468f 210 }
stersky 0:5ab762b6468f 211
stersky 0:5ab762b6468f 212 void initial_Ultrasons(void)
stersky 0:5ab762b6468f 213 {
stersky 0:5ab762b6468f 214 Ultra1.rise(&Ultra1_start);
stersky 0:5ab762b6468f 215 Ultra1.fall(&Ultra1_stop);
stersky 0:5ab762b6468f 216
stersky 0:5ab762b6468f 217 Ultra2.rise(&Ultra2_start);
stersky 0:5ab762b6468f 218 Ultra2.fall(&Ultra2_stop);
stersky 0:5ab762b6468f 219 }
stersky 0:5ab762b6468f 220
stersky 0:5ab762b6468f 221 void impulsion1(void)
stersky 0:5ab762b6468f 222 {
stersky 0:5ab762b6468f 223 puls1 = 1;
stersky 0:5ab762b6468f 224 wait (0.00002);
stersky 0:5ab762b6468f 225 puls1 = 0;
stersky 0:5ab762b6468f 226 }
stersky 0:5ab762b6468f 227
stersky 0:5ab762b6468f 228 void impulsion2(void)
stersky 0:5ab762b6468f 229 {
stersky 0:5ab762b6468f 230 puls2 = 1;
stersky 0:5ab762b6468f 231 wait (0.00002);
stersky 0:5ab762b6468f 232 puls2 = 0;
stersky 0:5ab762b6468f 233 }
stersky 0:5ab762b6468f 234
stersky 0:5ab762b6468f 235 int calcul_vitesse(void)
stersky 0:5ab762b6468f 236 {
stersky 0:5ab762b6468f 237 int val_ch2,val_ch3,vitesse;
stersky 0:5ab762b6468f 238
stersky 0:5ab762b6468f 239 val_ch2 = (tps_ch2-1500)/5; //calcul d'une valeur entre -100 et 100 selon la position du joystick
stersky 0:5ab762b6468f 240 val_ch3 = (tps_ch3-1500)/5;
stersky 0:5ab762b6468f 241
stersky 0:5ab762b6468f 242 //On compare les valeurs absolues des valeurs:c'est le joystick le plus éloigné
stersky 0:5ab762b6468f 243 //de la position centrale qui domine
stersky 0:5ab762b6468f 244 if(abs(val_ch2)>abs(val_ch3)) vitesse = val_ch2;
stersky 0:5ab762b6468f 245 else vitesse = val_ch3;
stersky 0:5ab762b6468f 246
stersky 0:5ab762b6468f 247 //vitesse nulle si les infos ne sont pas cohérentes
stersky 0:5ab762b6468f 248 if((tps_ch2<950)||(tps_ch2>2050)||(tps_ch3<950)||(tps_ch3>2050)) vitesse = 0;
stersky 0:5ab762b6468f 249
stersky 0:5ab762b6468f 250 if(abs(vitesse)<5) vitesse = 0;//Moteur immobile si jostick pas actionné
stersky 0:5ab762b6468f 251 if(vitesse>100) vitesse = 100; //saturation
stersky 0:5ab762b6468f 252 if(vitesse<-100) vitesse = -100; //saturation
stersky 0:5ab762b6468f 253
stersky 0:5ab762b6468f 254 return vitesse;
stersky 0:5ab762b6468f 255 }
stersky 0:5ab762b6468f 256
stersky 0:5ab762b6468f 257 int calcul_direction(void)
stersky 0:5ab762b6468f 258 {
stersky 0:5ab762b6468f 259 int val_ch4,direction;
stersky 0:5ab762b6468f 260
stersky 0:5ab762b6468f 261 //calcul d'une valeur entre -100 et 100 selon la position du joustick
stersky 0:5ab762b6468f 262 val_ch4 = (tps_ch4-1500)/5;
stersky 0:5ab762b6468f 263
stersky 0:5ab762b6468f 264 //On compare les valeurs absolues des valeurs:c'est le joystick le plus éloigné
stersky 0:5ab762b6468f 265 //de la position centrale qui domine
stersky 0:5ab762b6468f 266 direction = val_ch4;
stersky 0:5ab762b6468f 267
stersky 0:5ab762b6468f 268 //vitesse nulle si les infos ne sont pas cohérentes
stersky 0:5ab762b6468f 269 if((tps_ch4<950)||(tps_ch4>2050)) direction = 0;
stersky 0:5ab762b6468f 270
stersky 0:5ab762b6468f 271 if(abs(direction)<5) direction = 0;//Moteur immobile si jostick pas actionné
stersky 0:5ab762b6468f 272 if(direction>100) direction = 100; //saturation
stersky 0:5ab762b6468f 273 if(direction<-100) direction = -100; //saturation
stersky 0:5ab762b6468f 274
stersky 0:5ab762b6468f 275 return direction;
stersky 0:5ab762b6468f 276 }
stersky 0:5ab762b6468f 277
stersky 0:5ab762b6468f 278 void ecriture_moteurs(int vitesse, int gouvernail)
stersky 0:5ab762b6468f 279 {
stersky 0:5ab762b6468f 280 moteurs.printf("$%d|%d*",vitesse,gouvernail);
stersky 0:5ab762b6468f 281 }