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@40:c89872c80e75, 2019-06-08 (annotated)

Committer:
AlexisCollin
Date:
Sat Jun 08 09:11:13 2019 +0000
Revision:
40:c89872c80e75
Parent:
39:82c1de84bf40
Child:
41:ee856eef5f6d
code detecteur de balles parfait v2; tester et garantis sans erreur

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