FRC_equipe1 / Mbed 2 deprecated FRC_2019

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@37:65650aab8387, 2019-06-08 (annotated)

Committer:
theolp
Date:
Sat Jun 08 02:47:31 2019 +0000
Revision:
37:65650aab8387
Parent:
36:0a6cb92024c7
Child:
38:a8b84be7dce5
VERSION A UTILISER POUR LES QUALIFS SAMEDI 8 JUIN

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wael_H 2:8971078b1ccf 1 #include "mbed.h"
Wael_H 5:34048faec367 2 #include "CAN_asser.h"
Wael_H 10:efa507ba2b35 3 #include "Robot.h"
Wael_H 29:6bce50d6530c 4 #include "math.h"
Wael_H 10:efa507ba2b35 5
AlexisCollin 32:84bcb8f2667a 6 bool automate_aveugle(Robot&, bool&); //automate de début de partie
AlexisCollin 32:84bcb8f2667a 7 void automate_ejecte(Robot&, bool&); //
theolp 36:0a6cb92024c7 8 void automate_testLasers(Robot&); //
AlexisCollin 32:84bcb8f2667a 9 void automate_testABalle(Robot&, bool&); //
AlexisCollin 32:84bcb8f2667a 10 void automate_deplacement(Robot&); //
AlexisCollin 32:84bcb8f2667a 11 bool automate_fin_de_partie(Robot&); //automate crève ballon de fin de partie
AlexisCollin 32:84bcb8f2667a 12 bool automate_arretUrgence(Robot&); //automate d'arrêt d'urgence au filet
AlexisCollin 32:84bcb8f2667a 13 void automate_run(Robot&); //automate principale de partie
theolp 37:65650aab8387 14 void securitePos(Robot&);
Wael_H 27:e103da412e2b 15
theolp 36:0a6cb92024c7 16 typedef enum{TURN_FAST, TURN_FAST_GO, STRAIGHT, STRAIGHT_GO, CORRECT_GAUCHE, CORRECT_GAUCHE_GO} type_etat_deplacement;
Wael_H 27:e103da412e2b 17 type_etat_deplacement etat_deplacement = TURN_FAST;
Wael_H 27:e103da412e2b 18
theolp 37:65650aab8387 19 bool forceRAS = false;
Wael_H 8:94ecfe411d02 20
Wael_H 2:8971078b1ccf 21 int main(void)
Wael_H 2:8971078b1ccf 22 {
Wael_H 10:efa507ba2b35 23 Robot robot;
theolp 17:aae5361ddddf 24
Wael_H 15:3d4543a6c100 25 while(1)
theolp 37:65650aab8387 26 {
theolp 37:65650aab8387 27 automate_run(robot);
theolp 37:65650aab8387 28 }
AlexisCollin 32:84bcb8f2667a 29 }
AlexisCollin 32:84bcb8f2667a 30
AlexisCollin 32:84bcb8f2667a 31 void automate_run(Robot& robot)
AlexisCollin 32:84bcb8f2667a 32 {
theolp 36:0a6cb92024c7 33 static bool ejecte = false;
Wael_H 34:cea05fa02f37 34 typedef enum{ATTENTE, RUN_AVEUGLE, RECHERCHE_BALLES, FIN_PARTIE} type_etat;
AlexisCollin 32:84bcb8f2667a 35 static type_etat etat = ATTENTE;
AlexisCollin 32:84bcb8f2667a 36 static Timer T_partie;
Wael_H 31:85d9fb71f921 37
Wael_H 34:cea05fa02f37 38 if(T_partie.read() > 80.0f)
theolp 37:65650aab8387 39 {
Wael_H 34:cea05fa02f37 40 etat = FIN_PARTIE;
theolp 37:65650aab8387 41 robot.stop();
theolp 37:65650aab8387 42 robot.ejecte();
theolp 37:65650aab8387 43 T_partie.stop();
theolp 37:65650aab8387 44 T_partie.reset();
theolp 37:65650aab8387 45 }
Wael_H 34:cea05fa02f37 46
AlexisCollin 32:84bcb8f2667a 47 switch(etat)
AlexisCollin 32:84bcb8f2667a 48 {
AlexisCollin 32:84bcb8f2667a 49 case ATTENTE:
AlexisCollin 32:84bcb8f2667a 50 if(!Robot::Jack)
AlexisCollin 32:84bcb8f2667a 51 {
AlexisCollin 32:84bcb8f2667a 52 T_partie.start();
theolp 37:65650aab8387 53 etat = RUN_AVEUGLE;
AlexisCollin 32:84bcb8f2667a 54 }
AlexisCollin 32:84bcb8f2667a 55 break;
Wael_H 34:cea05fa02f37 56
Wael_H 34:cea05fa02f37 57 case RUN_AVEUGLE:
Wael_H 35:6c6321b97ae6 58 if(automate_aveugle(robot, ejecte) && robot.GoToXYT(2000,2500,0))
Wael_H 35:6c6321b97ae6 59 etat = RECHERCHE_BALLES;
Wael_H 34:cea05fa02f37 60 automate_ejecte(robot, ejecte);
AlexisCollin 32:84bcb8f2667a 61 break;
Wael_H 34:cea05fa02f37 62
Wael_H 34:cea05fa02f37 63 case RECHERCHE_BALLES:
Wael_H 34:cea05fa02f37 64
theolp 37:65650aab8387 65 //le robot ne s'approche pas des murs
theolp 37:65650aab8387 66 securitePos(robot);
theolp 37:65650aab8387 67
theolp 37:65650aab8387 68 if(!robot.aBalle() && !forceRAS)
Wael_H 34:cea05fa02f37 69 automate_deplacement(robot);
Wael_H 34:cea05fa02f37 70
theolp 36:0a6cb92024c7 71 // Recherche de balles
theolp 36:0a6cb92024c7 72 automate_testLasers(robot);
theolp 36:0a6cb92024c7 73
theolp 36:0a6cb92024c7 74 // Gestion du lancer de balle
theolp 36:0a6cb92024c7 75 automate_testABalle(robot, ejecte);
theolp 36:0a6cb92024c7 76
theolp 36:0a6cb92024c7 77 //Ejecte ou non en fonction des commandes des autres automates
theolp 36:0a6cb92024c7 78 automate_ejecte(robot, ejecte);
Wael_H 34:cea05fa02f37 79 break;
Wael_H 34:cea05fa02f37 80
Wael_H 34:cea05fa02f37 81 case FIN_PARTIE:
theolp 37:65650aab8387 82 automate_fin_de_partie(robot);
Wael_H 34:cea05fa02f37 83 break;
AlexisCollin 32:84bcb8f2667a 84 }
Wael_H 15:3d4543a6c100 85 }
Wael_H 15:3d4543a6c100 86
theolp 36:0a6cb92024c7 87 void automate_testLasers(Robot& robot)
theolp 36:0a6cb92024c7 88 {
theolp 36:0a6cb92024c7 89 typedef enum{RAS, SUSPICION, CONFIRMATION, VERIFICATION, CORRIGE, ABALLE, ATTENTE_VISION} type_etat;
theolp 36:0a6cb92024c7 90 static type_etat etat = RAS;
Wael_H 29:6bce50d6530c 91
theolp 36:0a6cb92024c7 92 static const int distMurBalle = 20;
theolp 37:65650aab8387 93 static Timer T_suspi, T_arret, T_balle, T_corrige;
theolp 36:0a6cb92024c7 94 static float droit[2] = { robot.getDist(Laser::Droit),robot.getDist(Laser::Droit) },
theolp 36:0a6cb92024c7 95 gauche[2] = { robot.getDist(Laser::Gauche),robot.getDist(Laser::Gauche) },
theolp 36:0a6cb92024c7 96 distBalle;
theolp 36:0a6cb92024c7 97
theolp 36:0a6cb92024c7 98 droit[0] = robot.getDist(Laser::Droit);
theolp 36:0a6cb92024c7 99 gauche[0] = robot.getDist(Laser::Gauche);
theolp 36:0a6cb92024c7 100
theolp 36:0a6cb92024c7 101 if(robot.aBalle())
theolp 36:0a6cb92024c7 102 etat = ABALLE;
theolp 37:65650aab8387 103
theolp 37:65650aab8387 104 if(forceRAS)
theolp 37:65650aab8387 105 etat = RAS;
Wael_H 27:e103da412e2b 106
theolp 36:0a6cb92024c7 107 switch(etat)
Wael_H 27:e103da412e2b 108 {
theolp 36:0a6cb92024c7 109 case RAS:
theolp 36:0a6cb92024c7 110 if(etat_deplacement != TURN_FAST_GO)
theolp 36:0a6cb92024c7 111 etat_deplacement = TURN_FAST;
theolp 37:65650aab8387 112 if( droit[1] > (droit[0] + distMurBalle) && droit[0] <= 150)
theolp 36:0a6cb92024c7 113 {
theolp 36:0a6cb92024c7 114 T_suspi.start();
theolp 36:0a6cb92024c7 115 etat = SUSPICION;
theolp 36:0a6cb92024c7 116 dbug.printf("SUSPICION\n\r");
theolp 36:0a6cb92024c7 117 }
theolp 36:0a6cb92024c7 118 break;
theolp 36:0a6cb92024c7 119
theolp 36:0a6cb92024c7 120 case SUSPICION:
theolp 36:0a6cb92024c7 121 if(T_suspi.read_ms() < 500)
theolp 36:0a6cb92024c7 122 {
theolp 36:0a6cb92024c7 123 if( gauche[1] > (gauche[0] + distMurBalle) )
theolp 36:0a6cb92024c7 124 {
theolp 36:0a6cb92024c7 125 etat = CONFIRMATION;
theolp 36:0a6cb92024c7 126 dbug.printf("CONFIRMATION\n\r");
theolp 36:0a6cb92024c7 127 T_suspi.stop();
theolp 36:0a6cb92024c7 128 T_suspi.reset();
theolp 36:0a6cb92024c7 129 }
theolp 36:0a6cb92024c7 130 }
theolp 36:0a6cb92024c7 131 else
theolp 36:0a6cb92024c7 132 {
theolp 36:0a6cb92024c7 133 T_suspi.stop();
theolp 36:0a6cb92024c7 134 T_suspi.reset();
theolp 36:0a6cb92024c7 135 etat = RAS;
theolp 36:0a6cb92024c7 136 dbug.printf("RAS\n\r");
theolp 36:0a6cb92024c7 137 }
theolp 36:0a6cb92024c7 138 break;
theolp 36:0a6cb92024c7 139
theolp 36:0a6cb92024c7 140 case CONFIRMATION:
theolp 36:0a6cb92024c7 141 distBalle = gauche[0];
theolp 36:0a6cb92024c7 142 robot.stop();
theolp 36:0a6cb92024c7 143 T_arret.start();
theolp 36:0a6cb92024c7 144 etat = VERIFICATION;
theolp 36:0a6cb92024c7 145 dbug.printf("VERIFICATION\n\r");
theolp 36:0a6cb92024c7 146 break;
theolp 36:0a6cb92024c7 147
theolp 36:0a6cb92024c7 148 case VERIFICATION:
theolp 36:0a6cb92024c7 149 if(T_arret.read_ms() > 300)
theolp 36:0a6cb92024c7 150 {
theolp 36:0a6cb92024c7 151 T_arret.stop();
theolp 36:0a6cb92024c7 152 T_arret.reset();
theolp 36:0a6cb92024c7 153
theolp 36:0a6cb92024c7 154 if( fabs(gauche[0] - droit[0]) <= 5.5f && fabs(gauche[0]-distBalle) <= 6.0f)
theolp 36:0a6cb92024c7 155 {
theolp 36:0a6cb92024c7 156 if(etat_deplacement != STRAIGHT_GO)
theolp 36:0a6cb92024c7 157 {
theolp 36:0a6cb92024c7 158 etat_deplacement = STRAIGHT;
theolp 36:0a6cb92024c7 159 dbug.printf("STRAIGHT\n\r");
theolp 36:0a6cb92024c7 160 }
theolp 36:0a6cb92024c7 161 }
theolp 36:0a6cb92024c7 162 else
theolp 36:0a6cb92024c7 163 {
theolp 36:0a6cb92024c7 164 etat_deplacement = CORRECT_GAUCHE;
theolp 37:65650aab8387 165 T_corrige.start(); //Timer pour abandonner en cas de non retrouvage de balle
theolp 36:0a6cb92024c7 166 etat = CORRIGE;
theolp 36:0a6cb92024c7 167 dbug.printf("CORRIGE\n\r");
theolp 36:0a6cb92024c7 168 }
theolp 36:0a6cb92024c7 169 }
theolp 36:0a6cb92024c7 170 break;
theolp 36:0a6cb92024c7 171
theolp 36:0a6cb92024c7 172 case CORRIGE:
theolp 36:0a6cb92024c7 173 if( droit[1] > (droit[0] + distMurBalle) )
theolp 36:0a6cb92024c7 174 {
theolp 36:0a6cb92024c7 175 if(etat_deplacement != STRAIGHT_GO)
theolp 36:0a6cb92024c7 176 {
theolp 36:0a6cb92024c7 177 etat_deplacement = STRAIGHT;
theolp 36:0a6cb92024c7 178 dbug.printf("STRAIGHT\n\r");
theolp 37:65650aab8387 179 T_corrige.stop();
theolp 37:65650aab8387 180 T_corrige.reset();
theolp 36:0a6cb92024c7 181 }
theolp 36:0a6cb92024c7 182 }
theolp 37:65650aab8387 183 else if( T_corrige.read() > 1.5f )
theolp 37:65650aab8387 184 {
theolp 37:65650aab8387 185 etat = RAS;
theolp 37:65650aab8387 186 T_corrige.stop();
theolp 37:65650aab8387 187 T_corrige.reset();
theolp 37:65650aab8387 188 }
theolp 36:0a6cb92024c7 189 break;
theolp 36:0a6cb92024c7 190
theolp 36:0a6cb92024c7 191 case ABALLE:
theolp 36:0a6cb92024c7 192 if(!robot.aBalle())
theolp 36:0a6cb92024c7 193 {
theolp 36:0a6cb92024c7 194 T_balle.start();
theolp 36:0a6cb92024c7 195 etat = ATTENTE_VISION;
theolp 36:0a6cb92024c7 196 dbug.printf("ATTENTE_VISION\n\r");
theolp 36:0a6cb92024c7 197 etat_deplacement = TURN_FAST;
theolp 36:0a6cb92024c7 198 }
theolp 36:0a6cb92024c7 199 break;
theolp 36:0a6cb92024c7 200
theolp 36:0a6cb92024c7 201 case ATTENTE_VISION:
theolp 36:0a6cb92024c7 202 if(T_balle.read() > 1.0f)
theolp 36:0a6cb92024c7 203 {
theolp 36:0a6cb92024c7 204 T_balle.stop();
theolp 36:0a6cb92024c7 205 T_balle.reset();
theolp 36:0a6cb92024c7 206 etat = RAS;
theolp 36:0a6cb92024c7 207 dbug.printf("RAS\n\r");
theolp 36:0a6cb92024c7 208 }
theolp 36:0a6cb92024c7 209 break;
Wael_H 27:e103da412e2b 210 }
Wael_H 27:e103da412e2b 211
theolp 36:0a6cb92024c7 212 // Conversions past
theolp 36:0a6cb92024c7 213 droit[1] = droit[0];
theolp 36:0a6cb92024c7 214 gauche[1] = gauche[0];
Wael_H 27:e103da412e2b 215 }
Wael_H 27:e103da412e2b 216
theolp 37:65650aab8387 217 void securitePos(Robot& robot)
theolp 37:65650aab8387 218 {
theolp 37:65650aab8387 219 static int etat;
theolp 37:65650aab8387 220
theolp 37:65650aab8387 221 switch(etat)
theolp 37:65650aab8387 222 {
theolp 37:65650aab8387 223 case 0:
theolp 37:65650aab8387 224 if(robot.pos(Robot::X) < 400 || robot.pos(Robot::X) > 3600 || robot.pos(Robot::Y) < 400 || robot.pos(Robot::Y) > 3500)
theolp 37:65650aab8387 225 {
theolp 37:65650aab8387 226 forceRAS = true;
theolp 37:65650aab8387 227 robot.stop();
theolp 37:65650aab8387 228 etat = 1;
theolp 37:65650aab8387 229 dbug.printf("GoToXYT\n\r");
theolp 37:65650aab8387 230 }
theolp 37:65650aab8387 231 else
theolp 37:65650aab8387 232 forceRAS = false;
theolp 37:65650aab8387 233 break;
theolp 37:65650aab8387 234
theolp 37:65650aab8387 235 case 1:
theolp 37:65650aab8387 236 if(robot.GoToXYT(2000,2000,0))
theolp 37:65650aab8387 237 {
theolp 37:65650aab8387 238 forceRAS = false;
theolp 37:65650aab8387 239 etat = 0;
theolp 37:65650aab8387 240 dbug.printf("Remis au milieu\n\r");
theolp 37:65650aab8387 241 }
theolp 37:65650aab8387 242 break;
theolp 37:65650aab8387 243 }
theolp 37:65650aab8387 244 }
theolp 37:65650aab8387 245
Wael_H 29:6bce50d6530c 246 void automate_testABalle(Robot& robot, bool& ejecte)
Wael_H 24:314b1f6607c5 247 {
theolp 36:0a6cb92024c7 248 typedef enum {ATTENTE, ABALLE} type_etat;
Wael_H 27:e103da412e2b 249 static type_etat etat = ATTENTE;
Wael_H 23:bb1535360a98 250
Wael_H 7:753e901d441b 251 switch(etat)
Wael_H 9:2113adf37c66 252 {
Wael_H 27:e103da412e2b 253 case ATTENTE:
Wael_H 27:e103da412e2b 254 if(robot.aBalle())
Wael_H 27:e103da412e2b 255 {
Wael_H 27:e103da412e2b 256 robot.stop();
Wael_H 27:e103da412e2b 257 etat = ABALLE;
theolp 36:0a6cb92024c7 258 robot.setSpeed(150,800); //vitesse de rotation vers le terrain ennemi
Wael_H 27:e103da412e2b 259 }
Wael_H 13:9c62e263f245 260 break;
Wael_H 13:9c62e263f245 261
Wael_H 27:e103da412e2b 262 case ABALLE:
Wael_H 29:6bce50d6530c 263 if( robot.tourne( -robot.pos(Robot::THETA) ) )
Wael_H 27:e103da412e2b 264 {
Wael_H 29:6bce50d6530c 265 ejecte = true;
Wael_H 27:e103da412e2b 266 etat = ATTENTE;
Wael_H 27:e103da412e2b 267 etat_deplacement = TURN_FAST;
Wael_H 29:6bce50d6530c 268 }
Wael_H 13:9c62e263f245 269 break;
Wael_H 13:9c62e263f245 270 }
Wael_H 13:9c62e263f245 271 }
Wael_H 23:bb1535360a98 272
Wael_H 27:e103da412e2b 273 void automate_deplacement(Robot& robot)
Wael_H 27:e103da412e2b 274 {
Wael_H 27:e103da412e2b 275 switch(etat_deplacement)
theolp 36:0a6cb92024c7 276 {
Wael_H 27:e103da412e2b 277 case TURN_FAST:
theolp 36:0a6cb92024c7 278 robot.stop();
theolp 36:0a6cb92024c7 279 robot.setSpeed(90,450);
theolp 36:0a6cb92024c7 280 etat_deplacement = TURN_FAST_GO;
Wael_H 27:e103da412e2b 281 break;
Wael_H 27:e103da412e2b 282
theolp 36:0a6cb92024c7 283 case TURN_FAST_GO:
theolp 36:0a6cb92024c7 284 if(robot.immobile())
theolp 36:0a6cb92024c7 285 robot.tourne();
Wael_H 34:cea05fa02f37 286 break;
Wael_H 27:e103da412e2b 287
theolp 36:0a6cb92024c7 288 case STRAIGHT:
theolp 36:0a6cb92024c7 289 robot.stop();
theolp 36:0a6cb92024c7 290 robot.setSpeed(150,800);
theolp 36:0a6cb92024c7 291 etat_deplacement = STRAIGHT_GO;
Wael_H 23:bb1535360a98 292 break;
Wael_H 23:bb1535360a98 293
theolp 36:0a6cb92024c7 294 case STRAIGHT_GO:
theolp 36:0a6cb92024c7 295 if(robot.immobile())
theolp 36:0a6cb92024c7 296 robot.avance( robot.getDist(Laser::Gauche,"mm") );
Wael_H 27:e103da412e2b 297 break;
Wael_H 24:314b1f6607c5 298
theolp 36:0a6cb92024c7 299 case CORRECT_GAUCHE:
theolp 36:0a6cb92024c7 300 robot.stop();
theolp 36:0a6cb92024c7 301 robot.setSpeed(35,450);
theolp 36:0a6cb92024c7 302 etat_deplacement = CORRECT_GAUCHE_GO;
theolp 36:0a6cb92024c7 303 break;
theolp 36:0a6cb92024c7 304
theolp 36:0a6cb92024c7 305 case CORRECT_GAUCHE_GO:
theolp 36:0a6cb92024c7 306 if(robot.immobile())
theolp 36:0a6cb92024c7 307 robot.tourne(-100);
Wael_H 35:6c6321b97ae6 308 break;
Wael_H 35:6c6321b97ae6 309 }
Wael_H 35:6c6321b97ae6 310 }
Wael_H 35:6c6321b97ae6 311
Wael_H 35:6c6321b97ae6 312 bool automate_fin_de_partie(Robot& robot)
Wael_H 35:6c6321b97ae6 313 {
Wael_H 35:6c6321b97ae6 314 typedef enum{AVANCE, EXPLOSE_BALLON, FIN_PARTIE} type_etat;
Wael_H 35:6c6321b97ae6 315 static type_etat etat = AVANCE;
Wael_H 35:6c6321b97ae6 316
Wael_H 35:6c6321b97ae6 317 static Timer T_fin;
Wael_H 35:6c6321b97ae6 318
Wael_H 35:6c6321b97ae6 319 switch(etat)
Wael_H 35:6c6321b97ae6 320 {
Wael_H 35:6c6321b97ae6 321 case AVANCE:
theolp 36:0a6cb92024c7 322 if(robot.GoToXYT(robot.pos(Robot::X),3600,0))
Wael_H 35:6c6321b97ae6 323 etat = EXPLOSE_BALLON;
Wael_H 35:6c6321b97ae6 324 break;
Wael_H 35:6c6321b97ae6 325
Wael_H 35:6c6321b97ae6 326 case EXPLOSE_BALLON:
Wael_H 35:6c6321b97ae6 327 if(robot.leveBras())
Wael_H 35:6c6321b97ae6 328 {
Wael_H 35:6c6321b97ae6 329 robot.exploseBallon();
Wael_H 35:6c6321b97ae6 330 T_fin.start();
Wael_H 35:6c6321b97ae6 331 etat = FIN_PARTIE;
Wael_H 35:6c6321b97ae6 332 }
Wael_H 35:6c6321b97ae6 333 break;
Wael_H 35:6c6321b97ae6 334
Wael_H 35:6c6321b97ae6 335 case FIN_PARTIE:
Wael_H 35:6c6321b97ae6 336 if(T_fin.read() >= 2.0f && robot.baisseBras())
Wael_H 35:6c6321b97ae6 337 {
Wael_H 35:6c6321b97ae6 338 robot.arreteDisquette();
theolp 36:0a6cb92024c7 339 robot.stopRouleau();
Wael_H 35:6c6321b97ae6 340 return true;
Wael_H 35:6c6321b97ae6 341 }
Wael_H 35:6c6321b97ae6 342 }
Wael_H 35:6c6321b97ae6 343
Wael_H 35:6c6321b97ae6 344 return false;
Wael_H 35:6c6321b97ae6 345 }
Wael_H 35:6c6321b97ae6 346
Wael_H 35:6c6321b97ae6 347 bool automate_aveugle(Robot& robot, bool& ejecte)
Wael_H 35:6c6321b97ae6 348 {
Wael_H 35:6c6321b97ae6 349 typedef enum{STRAIGHT1, PREMIER_TOURNE_D, TOURNE_D, STRAIGHT2, TOURNE_G} type_etat;
Wael_H 35:6c6321b97ae6 350 static type_etat etat = STRAIGHT1;
Wael_H 35:6c6321b97ae6 351
Wael_H 35:6c6321b97ae6 352 static Timer T_ejecte;
Wael_H 35:6c6321b97ae6 353 static int cptBalles = 0;
Wael_H 35:6c6321b97ae6 354
Wael_H 35:6c6321b97ae6 355 if(cptBalles == 2)
Wael_H 35:6c6321b97ae6 356 return true;
Wael_H 35:6c6321b97ae6 357
Wael_H 35:6c6321b97ae6 358 switch(etat)
Wael_H 35:6c6321b97ae6 359 {
Wael_H 35:6c6321b97ae6 360 case STRAIGHT1:
Wael_H 35:6c6321b97ae6 361 if(robot.avance(1850 - DIST_CENTRE))
Wael_H 35:6c6321b97ae6 362 {
Wael_H 35:6c6321b97ae6 363 ejecte = true;
Wael_H 35:6c6321b97ae6 364 etat = PREMIER_TOURNE_D;
Wael_H 35:6c6321b97ae6 365 }
Wael_H 35:6c6321b97ae6 366 break;
Wael_H 35:6c6321b97ae6 367
Wael_H 35:6c6321b97ae6 368 case PREMIER_TOURNE_D:
Wael_H 35:6c6321b97ae6 369 if(robot.tourne(ANGLE_DROIT))
Wael_H 35:6c6321b97ae6 370 etat = STRAIGHT2;
Wael_H 35:6c6321b97ae6 371 break;
Wael_H 35:6c6321b97ae6 372
Wael_H 35:6c6321b97ae6 373 case TOURNE_D:
Wael_H 35:6c6321b97ae6 374 if(robot.tourne(ANGLE_DROIT))
Wael_H 35:6c6321b97ae6 375 etat = STRAIGHT2;
Wael_H 35:6c6321b97ae6 376 break;
Wael_H 35:6c6321b97ae6 377
Wael_H 35:6c6321b97ae6 378 case STRAIGHT2:
Wael_H 35:6c6321b97ae6 379 if(robot.avance(1500))
Wael_H 35:6c6321b97ae6 380 etat = TOURNE_G;
Wael_H 35:6c6321b97ae6 381 break;
Wael_H 35:6c6321b97ae6 382
Wael_H 35:6c6321b97ae6 383 case TOURNE_G:
Wael_H 35:6c6321b97ae6 384 if(robot.tourne(-ANGLE_DROIT))
Wael_H 35:6c6321b97ae6 385 {
Wael_H 35:6c6321b97ae6 386 ejecte = true;
Wael_H 35:6c6321b97ae6 387 cptBalles++;
Wael_H 35:6c6321b97ae6 388 etat = TOURNE_D;
Wael_H 35:6c6321b97ae6 389 }
Wael_H 35:6c6321b97ae6 390 break;
Wael_H 35:6c6321b97ae6 391 }
Wael_H 35:6c6321b97ae6 392
Wael_H 35:6c6321b97ae6 393 return false;
Wael_H 35:6c6321b97ae6 394 }
Wael_H 35:6c6321b97ae6 395
Wael_H 35:6c6321b97ae6 396 void automate_ejecte(Robot& robot, bool& ejecte)
Wael_H 35:6c6321b97ae6 397 {
Wael_H 35:6c6321b97ae6 398 typedef enum{GOBE, EJECTE, ATTENTE} type_etat;
Wael_H 35:6c6321b97ae6 399 static type_etat etat = GOBE;
Wael_H 35:6c6321b97ae6 400
Wael_H 35:6c6321b97ae6 401 static Timer timer_ejecte;
Wael_H 35:6c6321b97ae6 402
Wael_H 35:6c6321b97ae6 403 switch(etat)
Wael_H 35:6c6321b97ae6 404 {
Wael_H 35:6c6321b97ae6 405 case GOBE:
Wael_H 35:6c6321b97ae6 406 robot.gobe();
Wael_H 35:6c6321b97ae6 407 if(ejecte)
Wael_H 35:6c6321b97ae6 408 etat = EJECTE;
Wael_H 35:6c6321b97ae6 409 break;
Wael_H 35:6c6321b97ae6 410
Wael_H 35:6c6321b97ae6 411 case EJECTE:
Wael_H 35:6c6321b97ae6 412 robot.ejecte();
Wael_H 35:6c6321b97ae6 413 timer_ejecte.start();
Wael_H 35:6c6321b97ae6 414 etat = ATTENTE;
Wael_H 35:6c6321b97ae6 415 break;
Wael_H 35:6c6321b97ae6 416
Wael_H 35:6c6321b97ae6 417 case ATTENTE:
Wael_H 35:6c6321b97ae6 418 if( timer_ejecte >= 1.0f ){
Wael_H 35:6c6321b97ae6 419 timer_ejecte.stop();
Wael_H 35:6c6321b97ae6 420 timer_ejecte.reset();
Wael_H 35:6c6321b97ae6 421 ejecte = false;
Wael_H 35:6c6321b97ae6 422 etat = GOBE;
Wael_H 35:6c6321b97ae6 423 }
Wael_H 23:bb1535360a98 424 break;
Wael_H 23:bb1535360a98 425 }
Wael_H 27:e103da412e2b 426 }