Benjamin Brou / Mbed 2 deprecated Version_Final_RobotSDL

dhfdf

Dependencies:   mbed tsi_sensor TextLCD

main.cpp@0:80e5a644fec8, 2020-01-05 (annotated)

Committer:
benbrou06
Date:
Sun Jan 05 16:49:08 2020 +0000
Revision:
0:80e5a644fec8
rtsjtr

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benbrou06 0:80e5a644fec8 1 ///////////////////////////////////////////////////////////////////
benbrou06 0:80e5a644fec8 2 ///////////////////////////////////////////////////////////////////
benbrou06 0:80e5a644fec8 3 // Programme Robot Sans_Titre.jpg : //
benbrou06 0:80e5a644fec8 4 ///////////////////////////////////////////////////////////////////
benbrou06 0:80e5a644fec8 5 ///////////////////////////////////////////////////////////////////
benbrou06 0:80e5a644fec8 6
benbrou06 0:80e5a644fec8 7
benbrou06 0:80e5a644fec8 8 #include <stdio.h>
benbrou06 0:80e5a644fec8 9 #include "mbed.h"
benbrou06 0:80e5a644fec8 10 #include <stdlib.h>
benbrou06 0:80e5a644fec8 11 #include "TextLCD.h"
benbrou06 0:80e5a644fec8 12
benbrou06 0:80e5a644fec8 13 TextLCD lcd(D15, D14, D13, D12, D11, D10); // rs, e, d4-d7
benbrou06 0:80e5a644fec8 14
benbrou06 0:80e5a644fec8 15 //Décla globale des entrées et sorties
benbrou06 0:80e5a644fec8 16 DigitalIn Jack(D3); //C'est le jack qu'il faudra retirer pour lancer le robot
benbrou06 0:80e5a644fec8 17
benbrou06 0:80e5a644fec8 18 PwmOut Motd(PTC8); //Moteur 1 = Moteur droit
benbrou06 0:80e5a644fec8 19 PwmOut Motg(PTA13); //Moteur 2 = Moteur gauche
benbrou06 0:80e5a644fec8 20
benbrou06 0:80e5a644fec8 21 //Entrées analogiques des capteurs
benbrou06 0:80e5a644fec8 22 AnalogIn Captd1(PTB1); //capteur droite 1
benbrou06 0:80e5a644fec8 23 AnalogIn Captd2(PTB2); //capteur droite 2
benbrou06 0:80e5a644fec8 24 AnalogIn Captg1(PTC2); //capteur gauche 1
benbrou06 0:80e5a644fec8 25 AnalogIn Captg2(PTB3); //capteur gauche 2
benbrou06 0:80e5a644fec8 26
benbrou06 0:80e5a644fec8 27 AnalogIn CaptDist(PTE30); //capteur de distance
benbrou06 0:80e5a644fec8 28
benbrou06 0:80e5a644fec8 29 DigitalOut In1(D7); //Sens rotation moteur droit
benbrou06 0:80e5a644fec8 30 DigitalOut In2(D9); //Sens rotation moteur gauche
benbrou06 0:80e5a644fec8 31
benbrou06 0:80e5a644fec8 32 DigitalIn FDC(D2); //Contact Fin de course (ou PTD4)
benbrou06 0:80e5a644fec8 33
benbrou06 0:80e5a644fec8 34 DigitalIn bp1(D5); //Boutons du IHM
benbrou06 0:80e5a644fec8 35 DigitalIn bp2(D4);
benbrou06 0:80e5a644fec8 36
benbrou06 0:80e5a644fec8 37 DigitalOut ledR(LED_RED); //Déclaration Pwm des 3 leds de couleur Rouge, Verte et Bleu (dans l'ordre)
benbrou06 0:80e5a644fec8 38 DigitalOut ledV(LED_GREEN);
benbrou06 0:80e5a644fec8 39 DigitalOut ledB(LED_BLUE);
benbrou06 0:80e5a644fec8 40
benbrou06 0:80e5a644fec8 41
benbrou06 0:80e5a644fec8 42 int main()
benbrou06 0:80e5a644fec8 43 {
benbrou06 0:80e5a644fec8 44 int b1,b2;
benbrou06 0:80e5a644fec8 45 int state=0;
benbrou06 0:80e5a644fec8 46 int mode=0;
benbrou06 0:80e5a644fec8 47 float tcarre=0.6;
benbrou06 0:80e5a644fec8 48 int Depart=0,etat=0;
benbrou06 0:80e5a644fec8 49 float Vmg=0,Vmd=0; //Les 2 moteurs
benbrou06 0:80e5a644fec8 50 float FinDeCourse=0;
benbrou06 0:80e5a644fec8 51 int confettis;
benbrou06 0:80e5a644fec8 52 float d1,d2,d3; //Delta entre les capteurs
benbrou06 0:80e5a644fec8 53 float CG1,CG2,CD1,CD2,Cdist;
benbrou06 0:80e5a644fec8 54 Motg.period_us(100);
benbrou06 0:80e5a644fec8 55 Motd.period_us(100); //Decla des variables du main
benbrou06 0:80e5a644fec8 56 Motg.pulsewidth_us(Vmg); //Vitesses des 2 moteurs
benbrou06 0:80e5a644fec8 57 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 58
benbrou06 0:80e5a644fec8 59
benbrou06 0:80e5a644fec8 60
benbrou06 0:80e5a644fec8 61 printf("debut pgrm !\n\r"); //Test teraterm pour le début du prgm
benbrou06 0:80e5a644fec8 62
benbrou06 0:80e5a644fec8 63
benbrou06 0:80e5a644fec8 64 /////////////////////////////////////////////////////////////////
benbrou06 0:80e5a644fec8 65 /////Demarage du robot /////
benbrou06 0:80e5a644fec8 66 /////Debut suivi de ligne /////
benbrou06 0:80e5a644fec8 67 /////////////////////////////////////////////////////////////////
benbrou06 0:80e5a644fec8 68
benbrou06 0:80e5a644fec8 69 while (1) {
benbrou06 0:80e5a644fec8 70 b1=bp1;
benbrou06 0:80e5a644fec8 71 b2=bp2;
benbrou06 0:80e5a644fec8 72
benbrou06 0:80e5a644fec8 73 switch (state) { // gestion des états
benbrou06 0:80e5a644fec8 74 case 0 :
benbrou06 0:80e5a644fec8 75 if (b1==0) {
benbrou06 0:80e5a644fec8 76 lcd.cls();
benbrou06 0:80e5a644fec8 77 state=100;
benbrou06 0:80e5a644fec8 78 }
benbrou06 0:80e5a644fec8 79 break;
benbrou06 0:80e5a644fec8 80
benbrou06 0:80e5a644fec8 81 case 100 : //etat de transition de E0 vers E1
benbrou06 0:80e5a644fec8 82 if (b1==1) {
benbrou06 0:80e5a644fec8 83 state=1;
benbrou06 0:80e5a644fec8 84 }
benbrou06 0:80e5a644fec8 85 break;
benbrou06 0:80e5a644fec8 86
benbrou06 0:80e5a644fec8 87 case 1 : // choix 1 : bp1= suivant, bp2= confirmation
benbrou06 0:80e5a644fec8 88 if (b1==0) {
benbrou06 0:80e5a644fec8 89 state=12;
benbrou06 0:80e5a644fec8 90 } else if (b2==0) {
benbrou06 0:80e5a644fec8 91 state=13;
benbrou06 0:80e5a644fec8 92 }
benbrou06 0:80e5a644fec8 93 break;
benbrou06 0:80e5a644fec8 94
benbrou06 0:80e5a644fec8 95 case 11 :
benbrou06 0:80e5a644fec8 96 if (b2==0 && b1==0) {
benbrou06 0:80e5a644fec8 97 state=1000;
benbrou06 0:80e5a644fec8 98 }
benbrou06 0:80e5a644fec8 99 break;
benbrou06 0:80e5a644fec8 100
benbrou06 0:80e5a644fec8 101 case 12 :
benbrou06 0:80e5a644fec8 102 if (b1==1) {
benbrou06 0:80e5a644fec8 103 state=2;
benbrou06 0:80e5a644fec8 104 }
benbrou06 0:80e5a644fec8 105 break;
benbrou06 0:80e5a644fec8 106
benbrou06 0:80e5a644fec8 107 case 13 :
benbrou06 0:80e5a644fec8 108 if (b2==1) {
benbrou06 0:80e5a644fec8 109 state=11;
benbrou06 0:80e5a644fec8 110 }
benbrou06 0:80e5a644fec8 111 break;
benbrou06 0:80e5a644fec8 112
benbrou06 0:80e5a644fec8 113 case 2 : // choix 2 : bp1= suivant, bp2= confirmation
benbrou06 0:80e5a644fec8 114 if (b1==0) {
benbrou06 0:80e5a644fec8 115 state=22;
benbrou06 0:80e5a644fec8 116 } else if (b2==0) {
benbrou06 0:80e5a644fec8 117 state=23;
benbrou06 0:80e5a644fec8 118 }
benbrou06 0:80e5a644fec8 119 break;
benbrou06 0:80e5a644fec8 120
benbrou06 0:80e5a644fec8 121 case 21 :
benbrou06 0:80e5a644fec8 122 if (b2==0 && b1==0) {
benbrou06 0:80e5a644fec8 123 state=1000;
benbrou06 0:80e5a644fec8 124 }
benbrou06 0:80e5a644fec8 125 break;
benbrou06 0:80e5a644fec8 126
benbrou06 0:80e5a644fec8 127 case 22 :
benbrou06 0:80e5a644fec8 128 if (b1==1) {
benbrou06 0:80e5a644fec8 129 state=3;
benbrou06 0:80e5a644fec8 130 }
benbrou06 0:80e5a644fec8 131 break;
benbrou06 0:80e5a644fec8 132
benbrou06 0:80e5a644fec8 133 case 23 :
benbrou06 0:80e5a644fec8 134 if (b2==1) {
benbrou06 0:80e5a644fec8 135 state=21;
benbrou06 0:80e5a644fec8 136 }
benbrou06 0:80e5a644fec8 137 break;
benbrou06 0:80e5a644fec8 138
benbrou06 0:80e5a644fec8 139 case 3 : // choix 3 : bp1= suivant, bp2= reglage puis confirmation
benbrou06 0:80e5a644fec8 140 if (b1==0) {
benbrou06 0:80e5a644fec8 141 state=321;
benbrou06 0:80e5a644fec8 142 } else if (b2==0) {
benbrou06 0:80e5a644fec8 143 state=33;
benbrou06 0:80e5a644fec8 144 }
benbrou06 0:80e5a644fec8 145 break;
benbrou06 0:80e5a644fec8 146
benbrou06 0:80e5a644fec8 147
benbrou06 0:80e5a644fec8 148 case 31 : //reglage taille carre et stockage dans la variable tcarre (float) | bp1= augmente taille carré, bp2= confirmation
benbrou06 0:80e5a644fec8 149 if (b1==0) {
benbrou06 0:80e5a644fec8 150 state=311;
benbrou06 0:80e5a644fec8 151 } else if (b2==0) {
benbrou06 0:80e5a644fec8 152 state=322;
benbrou06 0:80e5a644fec8 153 } else if (tcarre==2.1) {
benbrou06 0:80e5a644fec8 154 tcarre=0;
benbrou06 0:80e5a644fec8 155 }
benbrou06 0:80e5a644fec8 156 break;
benbrou06 0:80e5a644fec8 157
benbrou06 0:80e5a644fec8 158
benbrou06 0:80e5a644fec8 159 case 311 :
benbrou06 0:80e5a644fec8 160 if (b1==1) {
benbrou06 0:80e5a644fec8 161 state=31;
benbrou06 0:80e5a644fec8 162 tcarre+=0.1;
benbrou06 0:80e5a644fec8 163 }
benbrou06 0:80e5a644fec8 164 break;
benbrou06 0:80e5a644fec8 165
benbrou06 0:80e5a644fec8 166
benbrou06 0:80e5a644fec8 167 case 32 :
benbrou06 0:80e5a644fec8 168 if (b2==0 && b1==0) {
benbrou06 0:80e5a644fec8 169 state=1000;
benbrou06 0:80e5a644fec8 170 }
benbrou06 0:80e5a644fec8 171 break;
benbrou06 0:80e5a644fec8 172
benbrou06 0:80e5a644fec8 173 case 321 :
benbrou06 0:80e5a644fec8 174 if (b1==1) {
benbrou06 0:80e5a644fec8 175 state=1;
benbrou06 0:80e5a644fec8 176 }
benbrou06 0:80e5a644fec8 177 break;
benbrou06 0:80e5a644fec8 178
benbrou06 0:80e5a644fec8 179 case 322 :
benbrou06 0:80e5a644fec8 180 if (b2==1) {
benbrou06 0:80e5a644fec8 181 state=32;
benbrou06 0:80e5a644fec8 182 }
benbrou06 0:80e5a644fec8 183 break;
benbrou06 0:80e5a644fec8 184
benbrou06 0:80e5a644fec8 185 case 33 :
benbrou06 0:80e5a644fec8 186 if (b2==1) {
benbrou06 0:80e5a644fec8 187 state=31;
benbrou06 0:80e5a644fec8 188 }
benbrou06 0:80e5a644fec8 189 break;
benbrou06 0:80e5a644fec8 190
benbrou06 0:80e5a644fec8 191 case 1000 : // etat de remise à 0
benbrou06 0:80e5a644fec8 192 if (b2==1 && b1==1) {
benbrou06 0:80e5a644fec8 193 state=0;
benbrou06 0:80e5a644fec8 194 }
benbrou06 0:80e5a644fec8 195
benbrou06 0:80e5a644fec8 196 }
benbrou06 0:80e5a644fec8 197
benbrou06 0:80e5a644fec8 198 switch (state) { // gestion des sorties
benbrou06 0:80e5a644fec8 199 case 0 : // case début choix
benbrou06 0:80e5a644fec8 200 lcd.cls();
benbrou06 0:80e5a644fec8 201 lcd.locate(0,0);
benbrou06 0:80e5a644fec8 202 lcd.printf("Choisir");
benbrou06 0:80e5a644fec8 203 lcd.locate(0,1);
benbrou06 0:80e5a644fec8 204 lcd.printf(" mode");
benbrou06 0:80e5a644fec8 205 ledR.write(1);
benbrou06 0:80e5a644fec8 206 ledV.write(1); //Eteint!
benbrou06 0:80e5a644fec8 207 ledB.write(1);
benbrou06 0:80e5a644fec8 208 break;
benbrou06 0:80e5a644fec8 209
benbrou06 0:80e5a644fec8 210 case 1 :
benbrou06 0:80e5a644fec8 211 lcd.cls();
benbrou06 0:80e5a644fec8 212 lcd.locate(0,0);
benbrou06 0:80e5a644fec8 213 lcd.printf("Suivi ?");
benbrou06 0:80e5a644fec8 214 ledR.write(1);
benbrou06 0:80e5a644fec8 215 ledV.write(1); //Eteint!
benbrou06 0:80e5a644fec8 216 ledB.write(1);
benbrou06 0:80e5a644fec8 217 break;
benbrou06 0:80e5a644fec8 218
benbrou06 0:80e5a644fec8 219 case 2 :
benbrou06 0:80e5a644fec8 220 lcd.cls();
benbrou06 0:80e5a644fec8 221 lcd.locate(0,0);
benbrou06 0:80e5a644fec8 222 lcd.printf("Confetti");
benbrou06 0:80e5a644fec8 223 lcd.locate(0,1);
benbrou06 0:80e5a644fec8 224 lcd.printf(" ?");
benbrou06 0:80e5a644fec8 225 break;
benbrou06 0:80e5a644fec8 226
benbrou06 0:80e5a644fec8 227 case 3 :
benbrou06 0:80e5a644fec8 228 lcd.cls();
benbrou06 0:80e5a644fec8 229 lcd.locate(0,0);
benbrou06 0:80e5a644fec8 230 lcd.printf("Carre ?");
benbrou06 0:80e5a644fec8 231 break;
benbrou06 0:80e5a644fec8 232
benbrou06 0:80e5a644fec8 233 case 11 : //suivi de ligne en cours
benbrou06 0:80e5a644fec8 234 ledR.write(0);
benbrou06 0:80e5a644fec8 235 ledV.write(1); //Rouge!
benbrou06 0:80e5a644fec8 236 ledB.write(1);
benbrou06 0:80e5a644fec8 237 lcd.cls();
benbrou06 0:80e5a644fec8 238 lcd.locate(0,0);
benbrou06 0:80e5a644fec8 239 lcd.printf("SDL en ");
benbrou06 0:80e5a644fec8 240 lcd.locate(0,1);
benbrou06 0:80e5a644fec8 241 lcd.printf("cours");
benbrou06 0:80e5a644fec8 242 mode=1;
benbrou06 0:80e5a644fec8 243 etat=0;
benbrou06 0:80e5a644fec8 244 break;
benbrou06 0:80e5a644fec8 245
benbrou06 0:80e5a644fec8 246 case 21 : // mode confetti en cours
benbrou06 0:80e5a644fec8 247 ledR.write(0);
benbrou06 0:80e5a644fec8 248 ledV.write(0); //Blanc!
benbrou06 0:80e5a644fec8 249 ledB.write(0);
benbrou06 0:80e5a644fec8 250 lcd.cls();
benbrou06 0:80e5a644fec8 251 lcd.locate(0,0);
benbrou06 0:80e5a644fec8 252 lcd.printf("CFT en ");
benbrou06 0:80e5a644fec8 253 lcd.locate(0,1);
benbrou06 0:80e5a644fec8 254 lcd.printf("cours");
benbrou06 0:80e5a644fec8 255 mode=2;
benbrou06 0:80e5a644fec8 256 etat=0;
benbrou06 0:80e5a644fec8 257 break;
benbrou06 0:80e5a644fec8 258
benbrou06 0:80e5a644fec8 259 case 31 : //reglage taille carre et stockage dans la variable tcarre (float)
benbrou06 0:80e5a644fec8 260 lcd.cls();
benbrou06 0:80e5a644fec8 261 lcd.locate(0,0);
benbrou06 0:80e5a644fec8 262 lcd.printf("Taille : ");
benbrou06 0:80e5a644fec8 263 lcd.locate(0,1);
benbrou06 0:80e5a644fec8 264 lcd.printf("%gm",tcarre);
benbrou06 0:80e5a644fec8 265 break;
benbrou06 0:80e5a644fec8 266
benbrou06 0:80e5a644fec8 267 case 32 : //mode carre en cours
benbrou06 0:80e5a644fec8 268 ledR.write(1);
benbrou06 0:80e5a644fec8 269 ledV.write(1); //Bleu!
benbrou06 0:80e5a644fec8 270 ledB.write(0);
benbrou06 0:80e5a644fec8 271 lcd.cls();
benbrou06 0:80e5a644fec8 272 lcd.locate(0,0);
benbrou06 0:80e5a644fec8 273 lcd.printf("CRE en ");
benbrou06 0:80e5a644fec8 274 lcd.locate(0,1);
benbrou06 0:80e5a644fec8 275 lcd.printf("cours");
benbrou06 0:80e5a644fec8 276 mode=3;
benbrou06 0:80e5a644fec8 277 etat=0;
benbrou06 0:80e5a644fec8 278 break;
benbrou06 0:80e5a644fec8 279
benbrou06 0:80e5a644fec8 280 case 1000 : // etat de remise à 0
benbrou06 0:80e5a644fec8 281 lcd.cls();
benbrou06 0:80e5a644fec8 282 tcarre=0.6;
benbrou06 0:80e5a644fec8 283 etat=0;
benbrou06 0:80e5a644fec8 284 break;
benbrou06 0:80e5a644fec8 285 }
benbrou06 0:80e5a644fec8 286
benbrou06 0:80e5a644fec8 287
benbrou06 0:80e5a644fec8 288
benbrou06 0:80e5a644fec8 289 if (mode==1) { //code du SDL
benbrou06 0:80e5a644fec8 290
benbrou06 0:80e5a644fec8 291 Depart = Jack.read(); //Si le Jack est enlevé, alors Depart = 1, donc le robot démarre (Etat = 1). Sinon il reste à l'arrët
benbrou06 0:80e5a644fec8 292 FinDeCourse = FDC.read(); //Si FDC activé, alors FinDeCourse = 1, donc le robot s'arrete (Etat = 100-->0)
benbrou06 0:80e5a644fec8 293
benbrou06 0:80e5a644fec8 294 CG1 = Captg1;
benbrou06 0:80e5a644fec8 295 CG2 = Captg2;
benbrou06 0:80e5a644fec8 296 CD1 = Captd1;
benbrou06 0:80e5a644fec8 297 CD2 = Captd2;
benbrou06 0:80e5a644fec8 298
benbrou06 0:80e5a644fec8 299 Cdist = CaptDist;
benbrou06 0:80e5a644fec8 300
benbrou06 0:80e5a644fec8 301 d1 = CG2-CG1;
benbrou06 0:80e5a644fec8 302 d2 = CG1-CD1;
benbrou06 0:80e5a644fec8 303 d3 = CD1-CD2;
benbrou06 0:80e5a644fec8 304
benbrou06 0:80e5a644fec8 305
benbrou06 0:80e5a644fec8 306 switch (etat) { // gestion des états
benbrou06 0:80e5a644fec8 307 case 0 :
benbrou06 0:80e5a644fec8 308 if (Depart==1) {
benbrou06 0:80e5a644fec8 309 //printf("Depart \n\r");
benbrou06 0:80e5a644fec8 310 etat=1;
benbrou06 0:80e5a644fec8 311 }
benbrou06 0:80e5a644fec8 312 break ;
benbrou06 0:80e5a644fec8 313
benbrou06 0:80e5a644fec8 314 case 1 : //roule tout droit
benbrou06 0:80e5a644fec8 315 if (Depart ==0) {
benbrou06 0:80e5a644fec8 316 etat=101;
benbrou06 0:80e5a644fec8 317 } else if (FinDeCourse==1) {
benbrou06 0:80e5a644fec8 318 etat=100;
benbrou06 0:80e5a644fec8 319 } else if (CaptDist == 4) { //capteur Distance actif
benbrou06 0:80e5a644fec8 320 etat=6;
benbrou06 0:80e5a644fec8 321 } else if ((Captg1 < 0.1)&&(Captg2 < 0.1)) { //capteur CG1 et CG2 sont sur du blanc
benbrou06 0:80e5a644fec8 322 etat=17;
benbrou06 0:80e5a644fec8 323 } else if ((Captg1 < 0.1)&&(Captg2 < 0.1)&&(Captd1 < 0.1)&&(Captd2 < 0.1)) { //capteur CG1, CG2, CD1, CD2 sont sur du blanc
benbrou06 0:80e5a644fec8 324 etat=9;
benbrou06 0:80e5a644fec8 325 } else if (d2<-0.4) { //capteur CG1 sur du blanc
benbrou06 0:80e5a644fec8 326 etat=2;
benbrou06 0:80e5a644fec8 327 } else if (d2>0.4) { //capteur CD1 sur du blanc
benbrou06 0:80e5a644fec8 328 etat=3;
benbrou06 0:80e5a644fec8 329 }
benbrou06 0:80e5a644fec8 330 break ;
benbrou06 0:80e5a644fec8 331
benbrou06 0:80e5a644fec8 332
benbrou06 0:80e5a644fec8 333 case 17 : //état de transition (vers E7)
benbrou06 0:80e5a644fec8 334 if (Depart ==0) {
benbrou06 0:80e5a644fec8 335 etat=101;
benbrou06 0:80e5a644fec8 336 } else if (FinDeCourse==1) {
benbrou06 0:80e5a644fec8 337 etat=100;
benbrou06 0:80e5a644fec8 338 } else if ((Captg1 > 0.1)&&(Captg2 > 0.1)) { //capteur CG1 et CG2 ne sont plus sur du blanc
benbrou06 0:80e5a644fec8 339 etat=171;
benbrou06 0:80e5a644fec8 340 }
benbrou06 0:80e5a644fec8 341 break ;
benbrou06 0:80e5a644fec8 342
benbrou06 0:80e5a644fec8 343 case 171: //virage scotch coté gauche
benbrou06 0:80e5a644fec8 344 if (Depart ==0) {
benbrou06 0:80e5a644fec8 345 etat=101;
benbrou06 0:80e5a644fec8 346 } else if (FinDeCourse==1) {
benbrou06 0:80e5a644fec8 347 etat=100;
benbrou06 0:80e5a644fec8 348 } else if ((Captg1 < 0.1)&&(Captg2 < 0.1)) {
benbrou06 0:80e5a644fec8 349 etat=7;
benbrou06 0:80e5a644fec8 350 }
benbrou06 0:80e5a644fec8 351 break;
benbrou06 0:80e5a644fec8 352
benbrou06 0:80e5a644fec8 353 case 2 : //virage gauche
benbrou06 0:80e5a644fec8 354 if (Depart == 0) {
benbrou06 0:80e5a644fec8 355 etat=101;
benbrou06 0:80e5a644fec8 356 } else if (FinDeCourse==1) {
benbrou06 0:80e5a644fec8 357 etat=100;
benbrou06 0:80e5a644fec8 358 } else if (d1<-0.4) {
benbrou06 0:80e5a644fec8 359 etat=4;
benbrou06 0:80e5a644fec8 360 } else if ((Captg1>0.1)&&(Captd1>0.1)) {
benbrou06 0:80e5a644fec8 361 etat=1;
benbrou06 0:80e5a644fec8 362 }
benbrou06 0:80e5a644fec8 363 break ;
benbrou06 0:80e5a644fec8 364
benbrou06 0:80e5a644fec8 365 case 3 : //virage droit
benbrou06 0:80e5a644fec8 366 if (Depart ==0) {
benbrou06 0:80e5a644fec8 367 etat=101;
benbrou06 0:80e5a644fec8 368 } else if (FinDeCourse==1) {
benbrou06 0:80e5a644fec8 369 etat=100;
benbrou06 0:80e5a644fec8 370 } else if (d3>0.4) {
benbrou06 0:80e5a644fec8 371 etat=5;
benbrou06 0:80e5a644fec8 372 } else if ((Captg1>0.1)&&(Captd1>0.1)) {
benbrou06 0:80e5a644fec8 373 etat=1;
benbrou06 0:80e5a644fec8 374 }
benbrou06 0:80e5a644fec8 375 break ;
benbrou06 0:80e5a644fec8 376
benbrou06 0:80e5a644fec8 377 case 4: //virage ++ gauche
benbrou06 0:80e5a644fec8 378 if (Depart ==0) {
benbrou06 0:80e5a644fec8 379 etat=101;
benbrou06 0:80e5a644fec8 380 } else if (FinDeCourse==1) {
benbrou06 0:80e5a644fec8 381 etat=100;
benbrou06 0:80e5a644fec8 382 } else if ((Captg1>0.1)&&(Captd1>0.1)&&(Captg2>0.1)&&(Captd2>0.1)) {
benbrou06 0:80e5a644fec8 383 etat=1;
benbrou06 0:80e5a644fec8 384 }
benbrou06 0:80e5a644fec8 385 break;
benbrou06 0:80e5a644fec8 386
benbrou06 0:80e5a644fec8 387 case 5: //virage ++ droite
benbrou06 0:80e5a644fec8 388 if (Depart ==0) {
benbrou06 0:80e5a644fec8 389 etat=101;
benbrou06 0:80e5a644fec8 390 } else if (FinDeCourse==1) {
benbrou06 0:80e5a644fec8 391 etat=100;
benbrou06 0:80e5a644fec8 392 } else if ((Captg1>0.1)&&(Captd1>0.1)&&(Captg2>0.1)&&(Captd2>0.1)) {
benbrou06 0:80e5a644fec8 393 etat=1;
benbrou06 0:80e5a644fec8 394 }
benbrou06 0:80e5a644fec8 395 break;
benbrou06 0:80e5a644fec8 396
benbrou06 0:80e5a644fec8 397 case 6: //Capteur distance actif
benbrou06 0:80e5a644fec8 398 if (Depart ==0) {
benbrou06 0:80e5a644fec8 399 etat=101;
benbrou06 0:80e5a644fec8 400 } else if (FinDeCourse==1) {
benbrou06 0:80e5a644fec8 401 etat=100;
benbrou06 0:80e5a644fec8 402 } else if (CaptDist != 4 ) {/*valeur à tester*/
benbrou06 0:80e5a644fec8 403 etat=1;
benbrou06 0:80e5a644fec8 404 }
benbrou06 0:80e5a644fec8 405 break;
benbrou06 0:80e5a644fec8 406
benbrou06 0:80e5a644fec8 407 case 7: //Virage stotch
benbrou06 0:80e5a644fec8 408 if (Depart ==0) {
benbrou06 0:80e5a644fec8 409 etat=101;
benbrou06 0:80e5a644fec8 410 } else if (FinDeCourse==1) {
benbrou06 0:80e5a644fec8 411 etat=100;
benbrou06 0:80e5a644fec8 412 } else if ((Captg1>0.1)&&(Captg2>0.1)) {
benbrou06 0:80e5a644fec8 413 etat=1;
benbrou06 0:80e5a644fec8 414 }
benbrou06 0:80e5a644fec8 415 break ;
benbrou06 0:80e5a644fec8 416
benbrou06 0:80e5a644fec8 417 case 9: //plot
benbrou06 0:80e5a644fec8 418 if ((Captg1 < 0.1)&&(Captg2 < 0.1)&&(Captd1 < 0.1)&&(Captd2 < 0.1)) {
benbrou06 0:80e5a644fec8 419 etat=1;
benbrou06 0:80e5a644fec8 420 }
benbrou06 0:80e5a644fec8 421 break ;
benbrou06 0:80e5a644fec8 422
benbrou06 0:80e5a644fec8 423 case 100 :
benbrou06 0:80e5a644fec8 424 if (Depart==0) {
benbrou06 0:80e5a644fec8 425 etat=0;
benbrou06 0:80e5a644fec8 426 }
benbrou06 0:80e5a644fec8 427 break ;
benbrou06 0:80e5a644fec8 428
benbrou06 0:80e5a644fec8 429 case 101 :
benbrou06 0:80e5a644fec8 430 etat=0;
benbrou06 0:80e5a644fec8 431 break ;
benbrou06 0:80e5a644fec8 432
benbrou06 0:80e5a644fec8 433 }
benbrou06 0:80e5a644fec8 434
benbrou06 0:80e5a644fec8 435 switch (etat) { // gestion des sorties
benbrou06 0:80e5a644fec8 436 case 0 :
benbrou06 0:80e5a644fec8 437 Vmg=0;
benbrou06 0:80e5a644fec8 438 Vmd=0;
benbrou06 0:80e5a644fec8 439 Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (arreté)
benbrou06 0:80e5a644fec8 440 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 441 break ;
benbrou06 0:80e5a644fec8 442
benbrou06 0:80e5a644fec8 443 case 1 :
benbrou06 0:80e5a644fec8 444 In1=0;
benbrou06 0:80e5a644fec8 445 In2=1; //avance
benbrou06 0:80e5a644fec8 446 Vmg=40;
benbrou06 0:80e5a644fec8 447 Vmd=40;
benbrou06 0:80e5a644fec8 448 Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (roule à 40%)
benbrou06 0:80e5a644fec8 449 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 450 break ;
benbrou06 0:80e5a644fec8 451
benbrou06 0:80e5a644fec8 452 case 17 :
benbrou06 0:80e5a644fec8 453 Vmg=30;
benbrou06 0:80e5a644fec8 454 Vmd=30;
benbrou06 0:80e5a644fec8 455 Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (roule à 30%)
benbrou06 0:80e5a644fec8 456 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 457 break ;
benbrou06 0:80e5a644fec8 458
benbrou06 0:80e5a644fec8 459 case 2 :
benbrou06 0:80e5a644fec8 460 Vmd=100*abs(d2)- 40; //Exemple si d2 = -0.8 (donc CG1 = 0.1 et CD1 = 0.9), alors la vitesse de la roue droite est 100*0.8-40 = 40
benbrou06 0:80e5a644fec8 461 Vmg=Vmd-10;
benbrou06 0:80e5a644fec8 462 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 463 Motg.pulsewidth_us(Vmg);
benbrou06 0:80e5a644fec8 464 break ;
benbrou06 0:80e5a644fec8 465
benbrou06 0:80e5a644fec8 466 case 3 :
benbrou06 0:80e5a644fec8 467 Vmg=100*abs(d2)- 40; //Exemple si d2 = 0.8 (donc CG1 = 0.9 et CD1 = 0.1), alors la vitesse de la roue gauche est 100*0.8-40 = 40
benbrou06 0:80e5a644fec8 468 Vmd=Vmg-10;
benbrou06 0:80e5a644fec8 469 Motg.pulsewidth_us(Vmg);
benbrou06 0:80e5a644fec8 470 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 471 break ;
benbrou06 0:80e5a644fec8 472
benbrou06 0:80e5a644fec8 473 case 4 :
benbrou06 0:80e5a644fec8 474 Vmd=100*abs(d1)- 50; //Exemple si d1 = -0.8 (donc CG2 = 0.1 et CG1 = 0.9), alors la vitesse de la roue droite est 100*0.8-50 = 30
benbrou06 0:80e5a644fec8 475 Vmg=Vmd-10;
benbrou06 0:80e5a644fec8 476 Motg.pulsewidth_us(Vmg);
benbrou06 0:80e5a644fec8 477 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 478 break;
benbrou06 0:80e5a644fec8 479
benbrou06 0:80e5a644fec8 480 case 5 :
benbrou06 0:80e5a644fec8 481 Vmg=100*abs(d3)- 50; //Exemple si d3 = 0.8 (donc CD1 = 0.9 et CD2 = 0.1), alors la vitesse de la roue gauche est 100*0.8-50 = 30
benbrou06 0:80e5a644fec8 482 Vmd=Vmg-10;
benbrou06 0:80e5a644fec8 483 Motg.pulsewidth_us(Vmg);
benbrou06 0:80e5a644fec8 484 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 485 break;
benbrou06 0:80e5a644fec8 486
benbrou06 0:80e5a644fec8 487 case 6: //Capteur distance actif
benbrou06 0:80e5a644fec8 488 Vmg=0;
benbrou06 0:80e5a644fec8 489 Vmd=0;
benbrou06 0:80e5a644fec8 490 Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (roule à 0%)
benbrou06 0:80e5a644fec8 491 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 492 break;
benbrou06 0:80e5a644fec8 493
benbrou06 0:80e5a644fec8 494 case 7: //virage scotch coté gauche
benbrou06 0:80e5a644fec8 495 Vmg=0;
benbrou06 0:80e5a644fec8 496 Vmd=0;
benbrou06 0:80e5a644fec8 497 wait(0.5);
benbrou06 0:80e5a644fec8 498 Vmg=0;
benbrou06 0:80e5a644fec8 499 Vmd=20;
benbrou06 0:80e5a644fec8 500 Motg.pulsewidth_us(Vmg);
benbrou06 0:80e5a644fec8 501 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 502 break;
benbrou06 0:80e5a644fec8 503
benbrou06 0:80e5a644fec8 504
benbrou06 0:80e5a644fec8 505 case 100 :
benbrou06 0:80e5a644fec8 506 Vmg=0;
benbrou06 0:80e5a644fec8 507 Vmd=0;
benbrou06 0:80e5a644fec8 508 Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (arreté)
benbrou06 0:80e5a644fec8 509 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 510 break ;
benbrou06 0:80e5a644fec8 511 }
benbrou06 0:80e5a644fec8 512 } //fin SDL
benbrou06 0:80e5a644fec8 513
benbrou06 0:80e5a644fec8 514
benbrou06 0:80e5a644fec8 515
benbrou06 0:80e5a644fec8 516
benbrou06 0:80e5a644fec8 517 if (mode==2) { //code du mode confettis
benbrou06 0:80e5a644fec8 518
benbrou06 0:80e5a644fec8 519 Depart = Jack.read(); //Si le Jack est enlevé, alors Depart = 1, donc le robot démarre (Etat = 1). Sinon il reste à l'arrët
benbrou06 0:80e5a644fec8 520 FinDeCourse = FDC.read(); //Si FDC activé, alors FinDeCourse = 1, donc le robot s'arrete (Etat = 100-->0)
benbrou06 0:80e5a644fec8 521
benbrou06 0:80e5a644fec8 522
benbrou06 0:80e5a644fec8 523 switch (etat) { // gestion des états
benbrou06 0:80e5a644fec8 524 case 0 :
benbrou06 0:80e5a644fec8 525 if (Depart==1) {
benbrou06 0:80e5a644fec8 526 etat=1;
benbrou06 0:80e5a644fec8 527 }
benbrou06 0:80e5a644fec8 528 break ;
benbrou06 0:80e5a644fec8 529
benbrou06 0:80e5a644fec8 530 case 1 :
benbrou06 0:80e5a644fec8 531 if (Depart ==0) {
benbrou06 0:80e5a644fec8 532 etat=101;
benbrou06 0:80e5a644fec8 533 } else if (FinDeCourse==1) {
benbrou06 0:80e5a644fec8 534 etat=100;
benbrou06 0:80e5a644fec8 535 }
benbrou06 0:80e5a644fec8 536
benbrou06 0:80e5a644fec8 537 if ((Captg1<0.3)&&(Captd1<0.3)&&(Captg2<0.3)&&(Captd2<0.3)) {
benbrou06 0:80e5a644fec8 538 confettis++;
benbrou06 0:80e5a644fec8 539 wait(1);
benbrou06 0:80e5a644fec8 540 }
benbrou06 0:80e5a644fec8 541
benbrou06 0:80e5a644fec8 542 if (confettis ==3) {
benbrou06 0:80e5a644fec8 543 etat=0;
benbrou06 0:80e5a644fec8 544 } else {
benbrou06 0:80e5a644fec8 545 confettis=0;
benbrou06 0:80e5a644fec8 546 }
benbrou06 0:80e5a644fec8 547 break ;
benbrou06 0:80e5a644fec8 548
benbrou06 0:80e5a644fec8 549 case 100 :
benbrou06 0:80e5a644fec8 550 if (Depart==0) {
benbrou06 0:80e5a644fec8 551 etat=0;
benbrou06 0:80e5a644fec8 552 }
benbrou06 0:80e5a644fec8 553
benbrou06 0:80e5a644fec8 554 break ;
benbrou06 0:80e5a644fec8 555
benbrou06 0:80e5a644fec8 556 case 101 :
benbrou06 0:80e5a644fec8 557 etat=0;
benbrou06 0:80e5a644fec8 558 break ;
benbrou06 0:80e5a644fec8 559 }
benbrou06 0:80e5a644fec8 560
benbrou06 0:80e5a644fec8 561
benbrou06 0:80e5a644fec8 562
benbrou06 0:80e5a644fec8 563 switch (etat) { // gestion des sorties
benbrou06 0:80e5a644fec8 564 case 0 :
benbrou06 0:80e5a644fec8 565 Vmg=0;
benbrou06 0:80e5a644fec8 566 Vmd=0;
benbrou06 0:80e5a644fec8 567 Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (arreté)
benbrou06 0:80e5a644fec8 568 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 569 ledR.write(0);
benbrou06 0:80e5a644fec8 570 ledV.write(1); //Led = Rouge!
benbrou06 0:80e5a644fec8 571 ledB.write(1);
benbrou06 0:80e5a644fec8 572 break ;
benbrou06 0:80e5a644fec8 573
benbrou06 0:80e5a644fec8 574
benbrou06 0:80e5a644fec8 575 case 1 :
benbrou06 0:80e5a644fec8 576 Vmg=20;
benbrou06 0:80e5a644fec8 577 Vmd=20;
benbrou06 0:80e5a644fec8 578 Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (roule à 20%)
benbrou06 0:80e5a644fec8 579 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 580 ledR.write(1);
benbrou06 0:80e5a644fec8 581 ledV.write(0); //Led = Verte!
benbrou06 0:80e5a644fec8 582 ledB.write(1);
benbrou06 0:80e5a644fec8 583 break ;
benbrou06 0:80e5a644fec8 584
benbrou06 0:80e5a644fec8 585 case 100 :
benbrou06 0:80e5a644fec8 586 Vmg=0;
benbrou06 0:80e5a644fec8 587 Vmd=0;
benbrou06 0:80e5a644fec8 588 Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (arreté)
benbrou06 0:80e5a644fec8 589 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 590 ledR.write(0);
benbrou06 0:80e5a644fec8 591 ledV.write(1); //Led = Rouge!
benbrou06 0:80e5a644fec8 592 ledB.write(1);
benbrou06 0:80e5a644fec8 593 break ;
benbrou06 0:80e5a644fec8 594
benbrou06 0:80e5a644fec8 595 }
benbrou06 0:80e5a644fec8 596 }//fin confettis
benbrou06 0:80e5a644fec8 597
benbrou06 0:80e5a644fec8 598 if (mode==3) { //code du mode carré
benbrou06 0:80e5a644fec8 599 Depart = Jack.read(); //Si le Jack est enlevé, alors Depart = 1, donc le robot démarre (Etat = 1). Sinon il reste à l'arrët
benbrou06 0:80e5a644fec8 600 FinDeCourse = FDC.read(); //Si FDC activé, alors FinDeCourse = 1, donc le robot s'arrete (Etat = 100-->0)
benbrou06 0:80e5a644fec8 601
benbrou06 0:80e5a644fec8 602
benbrou06 0:80e5a644fec8 603 switch (etat) { // gestion des états
benbrou06 0:80e5a644fec8 604 case 0 :
benbrou06 0:80e5a644fec8 605 if (Depart==1) {
benbrou06 0:80e5a644fec8 606 etat=1;
benbrou06 0:80e5a644fec8 607 }
benbrou06 0:80e5a644fec8 608 break ;
benbrou06 0:80e5a644fec8 609
benbrou06 0:80e5a644fec8 610 case 1 :
benbrou06 0:80e5a644fec8 611 if (Depart ==0) {
benbrou06 0:80e5a644fec8 612 etat=101;
benbrou06 0:80e5a644fec8 613 } else if (FinDeCourse==1) {
benbrou06 0:80e5a644fec8 614 etat=100;
benbrou06 0:80e5a644fec8 615 }
benbrou06 0:80e5a644fec8 616 break ;
benbrou06 0:80e5a644fec8 617
benbrou06 0:80e5a644fec8 618 case 100 :
benbrou06 0:80e5a644fec8 619 if (Depart==0) {
benbrou06 0:80e5a644fec8 620 etat=0;
benbrou06 0:80e5a644fec8 621 }
benbrou06 0:80e5a644fec8 622
benbrou06 0:80e5a644fec8 623 break ;
benbrou06 0:80e5a644fec8 624
benbrou06 0:80e5a644fec8 625 case 101 :
benbrou06 0:80e5a644fec8 626 etat=0;
benbrou06 0:80e5a644fec8 627 break ;
benbrou06 0:80e5a644fec8 628 }
benbrou06 0:80e5a644fec8 629
benbrou06 0:80e5a644fec8 630
benbrou06 0:80e5a644fec8 631
benbrou06 0:80e5a644fec8 632 switch (etat) { // gestion des sorties
benbrou06 0:80e5a644fec8 633 case 0 :
benbrou06 0:80e5a644fec8 634 Vmg=0;
benbrou06 0:80e5a644fec8 635 Vmd=0;
benbrou06 0:80e5a644fec8 636 Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (arreté)
benbrou06 0:80e5a644fec8 637 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 638 break ;
benbrou06 0:80e5a644fec8 639
benbrou06 0:80e5a644fec8 640
benbrou06 0:80e5a644fec8 641 case 1 :
benbrou06 0:80e5a644fec8 642 Vmg=20;
benbrou06 0:80e5a644fec8 643 Vmd=20;
benbrou06 0:80e5a644fec8 644 Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (roule à 20%)
benbrou06 0:80e5a644fec8 645 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 646 break ;
benbrou06 0:80e5a644fec8 647
benbrou06 0:80e5a644fec8 648 case 100 :
benbrou06 0:80e5a644fec8 649 Vmg=0;
benbrou06 0:80e5a644fec8 650 Vmd=0;
benbrou06 0:80e5a644fec8 651 Motg.pulsewidth_us(Vmg);//Vitesses des 2 moteurs (arreté)
benbrou06 0:80e5a644fec8 652 Motd.pulsewidth_us(Vmd);
benbrou06 0:80e5a644fec8 653 ledR.write(0);
benbrou06 0:80e5a644fec8 654 ledV.write(1); //Led = Rouge!
benbrou06 0:80e5a644fec8 655 ledB.write(1);
benbrou06 0:80e5a644fec8 656 break ;
benbrou06 0:80e5a644fec8 657
benbrou06 0:80e5a644fec8 658 }
benbrou06 0:80e5a644fec8 659 }//fin carré
benbrou06 0:80e5a644fec8 660
benbrou06 0:80e5a644fec8 661 }//fin de la boucle
benbrou06 0:80e5a644fec8 662 }