Etienne Stransky / Mbed 2 deprecated programme_bateau_copy

ok

Dependencies:   mbed SRF05

main.cpp@0:329768ee4088, 2019-02-12 (annotated)

Committer:
stersky
Date:
Tue Feb 12 14:48:35 2019 +0000
Revision:
0:329768ee4088
ok

Who changed what in which revision?

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