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@29:6bce50d6530c, 2019-06-07 (annotated)

Committer:
Wael_H
Date:
Fri Jun 07 09:30:46 2019 +0000
Revision:
29:6bce50d6530c
Parent:
28:82571fd665bf
Child:
31:85d9fb71f921
version propre pour tout le monde avec 3 balles

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
Wael_H 29:6bce50d6530c 6 bool automate_aveugle(Robot&, bool&);
Wael_H 29:6bce50d6530c 7 void automate_ejecte(Robot&, bool&);
Wael_H 27:e103da412e2b 8 void automate_testLasers(Robot&);
Wael_H 29:6bce50d6530c 9 void automate_testABalle(Robot&, bool&);
Wael_H 27:e103da412e2b 10 void automate_deplacement(Robot&);
Wael_H 27:e103da412e2b 11 void automate_vitesse(Robot&);
Wael_H 29:6bce50d6530c 12 void automate_fin_de_partie(Robot&);
Wael_H 29:6bce50d6530c 13 bool automate_arretUrgence(Robot&);
Wael_H 27:e103da412e2b 14
Wael_H 27:e103da412e2b 15 typedef enum{TURN_FAST, CORRECT_GAUCHE, CORRECT_DROITE, GO_STRAIGHT} type_etat_deplacement;
Wael_H 27:e103da412e2b 16 type_etat_deplacement etat_deplacement = TURN_FAST;
Wael_H 27:e103da412e2b 17
Wael_H 8:94ecfe411d02 18
Wael_H 2:8971078b1ccf 19 int main(void)
Wael_H 2:8971078b1ccf 20 {
Wael_H 10:efa507ba2b35 21 Robot robot;
theolp 17:aae5361ddddf 22
Wael_H 29:6bce50d6530c 23 bool ejecte = false;
theolp 20:a1b5d032b422 24
Wael_H 15:3d4543a6c100 25 while(1)
Wael_H 25:033263cf832c 26 if(!Robot::Jack)
theolp 17:aae5361ddddf 27 {
Wael_H 29:6bce50d6530c 28 // Trois balles en début de partie à l'aveugle
Wael_H 29:6bce50d6530c 29 while( !automate_aveugle(robot, ejecte) );
Wael_H 29:6bce50d6530c 30
Wael_H 29:6bce50d6530c 31 automate_ejecte(robot, ejecte);
Wael_H 29:6bce50d6530c 32
Wael_H 29:6bce50d6530c 33 automate_fin_de_partie(robot);
Wael_H 28:82571fd665bf 34
Wael_H 29:6bce50d6530c 35 // Gestion du lancer de balle
Wael_H 29:6bce50d6530c 36 /*automate_testABalle(robot, ejecte);
Wael_H 29:6bce50d6530c 37 automate_ejecte(robot, ejecte);
Wael_H 27:e103da412e2b 38
Wael_H 29:6bce50d6530c 39 // Recherche de balles
Wael_H 27:e103da412e2b 40 automate_testLasers(robot);
Wael_H 27:e103da412e2b 41 if(!robot.aBalle())
Wael_H 27:e103da412e2b 42 automate_deplacement(robot);
Wael_H 29:6bce50d6530c 43
Wael_H 29:6bce50d6530c 44 // Gestion de la vitesse en fonction de l'état actuel
Wael_H 29:6bce50d6530c 45 automate_vitesse(robot);*/
theolp 17:aae5361ddddf 46 }
Wael_H 15:3d4543a6c100 47 }
Wael_H 15:3d4543a6c100 48
Wael_H 29:6bce50d6530c 49 void automate_fin_de_partie(Robot& robot)
Wael_H 28:82571fd665bf 50 {
Wael_H 29:6bce50d6530c 51 typedef enum{AVANCE, EXPLOSE_BALLON, FIN_PARTIE} type_etat;
Wael_H 29:6bce50d6530c 52 static type_etat etat = AVANCE;
Wael_H 28:82571fd665bf 53
Wael_H 29:6bce50d6530c 54 static Timer T_fin;
Wael_H 28:82571fd665bf 55
Wael_H 28:82571fd665bf 56 switch(etat)
Wael_H 28:82571fd665bf 57 {
Wael_H 29:6bce50d6530c 58 case AVANCE:
Wael_H 28:82571fd665bf 59 robot.avance();
Wael_H 29:6bce50d6530c 60 if(automate_arretUrgence(robot))
Wael_H 29:6bce50d6530c 61 etat = EXPLOSE_BALLON;
Wael_H 29:6bce50d6530c 62 break;
Wael_H 29:6bce50d6530c 63
Wael_H 29:6bce50d6530c 64 case EXPLOSE_BALLON:
Wael_H 29:6bce50d6530c 65 robot.stop();
Wael_H 29:6bce50d6530c 66 if(robot.leveBras())
Wael_H 28:82571fd665bf 67 {
Wael_H 29:6bce50d6530c 68 robot.exploseBallon();
Wael_H 29:6bce50d6530c 69 T_fin.start();
Wael_H 29:6bce50d6530c 70 etat = FIN_PARTIE;
Wael_H 28:82571fd665bf 71 }
Wael_H 28:82571fd665bf 72 break;
Wael_H 28:82571fd665bf 73
Wael_H 29:6bce50d6530c 74 case FIN_PARTIE:
Wael_H 29:6bce50d6530c 75 if(T_fin.read() >= 2.0f)
Wael_H 28:82571fd665bf 76 {
Wael_H 29:6bce50d6530c 77 robot.baisseBras();
Wael_H 29:6bce50d6530c 78 robot.arreteDisquette();
Wael_H 28:82571fd665bf 79 }
Wael_H 29:6bce50d6530c 80 break;
Wael_H 29:6bce50d6530c 81 }
Wael_H 29:6bce50d6530c 82 }
Wael_H 29:6bce50d6530c 83
Wael_H 29:6bce50d6530c 84 bool automate_arretUrgence(Robot& robot)
Wael_H 29:6bce50d6530c 85 {
Wael_H 29:6bce50d6530c 86 typedef enum{RAS, PERCEPTION, ARRET_URGENCE, ATTENTE_REPLACEMENT} type_etat;
Wael_H 29:6bce50d6530c 87 static type_etat etat = RAS;
Wael_H 29:6bce50d6530c 88
Wael_H 29:6bce50d6530c 89 // Timer pour la durée sur la ligne blanche
Wael_H 29:6bce50d6530c 90 static Timer timerCNY;
Wael_H 29:6bce50d6530c 91
Wael_H 29:6bce50d6530c 92 switch(etat)
Wael_H 29:6bce50d6530c 93 {
Wael_H 29:6bce50d6530c 94 case RAS :
Wael_H 29:6bce50d6530c 95 if( robot.surBlanc( Robot::CNY_GAUCHE ) && robot.surBlanc( Robot::CNY_DROIT )){
Wael_H 29:6bce50d6530c 96 etat = PERCEPTION;
Wael_H 29:6bce50d6530c 97 timerCNY.start();
Wael_H 28:82571fd665bf 98 }
Wael_H 28:82571fd665bf 99 break;
Wael_H 28:82571fd665bf 100
Wael_H 29:6bce50d6530c 101 case PERCEPTION :
Wael_H 29:6bce50d6530c 102 if( robot.surBlanc( Robot::CNY_GAUCHE ) && robot.surBlanc( Robot::CNY_DROIT ) && timerCNY.read() >= 0.15f )
Wael_H 29:6bce50d6530c 103 etat = ARRET_URGENCE;
Wael_H 29:6bce50d6530c 104 else if( timerCNY.read() >= 0.15f ){
Wael_H 29:6bce50d6530c 105 etat = RAS;
Wael_H 29:6bce50d6530c 106 timerCNY.stop();
Wael_H 29:6bce50d6530c 107 timerCNY.reset();
Wael_H 29:6bce50d6530c 108 }
Wael_H 29:6bce50d6530c 109 break;
Wael_H 29:6bce50d6530c 110
Wael_H 29:6bce50d6530c 111 case ARRET_URGENCE :
Wael_H 29:6bce50d6530c 112 timerCNY.stop();
Wael_H 29:6bce50d6530c 113 timerCNY.reset();
Wael_H 29:6bce50d6530c 114
Wael_H 29:6bce50d6530c 115 etat = ATTENTE_REPLACEMENT;
Wael_H 29:6bce50d6530c 116
Wael_H 29:6bce50d6530c 117 return true;
Wael_H 29:6bce50d6530c 118
Wael_H 29:6bce50d6530c 119 case ATTENTE_REPLACEMENT :
Wael_H 29:6bce50d6530c 120 if( !robot.surBlanc( Robot:: CNY_GAUCHE ) && !robot.surBlanc( Robot::CNY_DROIT ))
Wael_H 29:6bce50d6530c 121 etat = RAS;
Wael_H 28:82571fd665bf 122 break;
Wael_H 28:82571fd665bf 123 }
Wael_H 28:82571fd665bf 124
Wael_H 28:82571fd665bf 125 return false;
Wael_H 28:82571fd665bf 126 }
Wael_H 28:82571fd665bf 127
Wael_H 29:6bce50d6530c 128 bool automate_aveugle(Robot& robot, bool& ejecte)
Wael_H 29:6bce50d6530c 129 {
Wael_H 29:6bce50d6530c 130 typedef enum{STRAIGHT1, PREMIER_TOURNE_D, TOURNE_D, STRAIGHT2, TOURNE_G} type_etat;
Wael_H 29:6bce50d6530c 131 static type_etat etat = STRAIGHT1;
Wael_H 29:6bce50d6530c 132
Wael_H 29:6bce50d6530c 133 static Timer T_ejecte;
Wael_H 29:6bce50d6530c 134 static int cptBalles = 0;
Wael_H 29:6bce50d6530c 135
Wael_H 29:6bce50d6530c 136 if(cptBalles == 2)
Wael_H 29:6bce50d6530c 137 return true;
Wael_H 29:6bce50d6530c 138
Wael_H 29:6bce50d6530c 139 switch(etat)
Wael_H 29:6bce50d6530c 140 {
Wael_H 29:6bce50d6530c 141 case STRAIGHT1:
Wael_H 29:6bce50d6530c 142 if(robot.avance(1850 - DIST_CENTRE))
Wael_H 29:6bce50d6530c 143 {
Wael_H 29:6bce50d6530c 144 ejecte = true;
Wael_H 29:6bce50d6530c 145 etat = PREMIER_TOURNE_D;
Wael_H 29:6bce50d6530c 146 }
Wael_H 29:6bce50d6530c 147 break;
Wael_H 29:6bce50d6530c 148
Wael_H 29:6bce50d6530c 149 case PREMIER_TOURNE_D:
Wael_H 29:6bce50d6530c 150 if(robot.tourne(ANGLE_DROIT))
Wael_H 29:6bce50d6530c 151 etat = STRAIGHT2;
Wael_H 29:6bce50d6530c 152 break;
Wael_H 29:6bce50d6530c 153
Wael_H 29:6bce50d6530c 154 case TOURNE_D:
Wael_H 29:6bce50d6530c 155 if(robot.tourne(ANGLE_DROIT))
Wael_H 29:6bce50d6530c 156 etat = STRAIGHT2;
Wael_H 29:6bce50d6530c 157 break;
Wael_H 29:6bce50d6530c 158
Wael_H 29:6bce50d6530c 159 case STRAIGHT2:
Wael_H 29:6bce50d6530c 160 if(robot.avance(1500))
Wael_H 29:6bce50d6530c 161 etat = TOURNE_G;
Wael_H 29:6bce50d6530c 162 break;
Wael_H 29:6bce50d6530c 163
Wael_H 29:6bce50d6530c 164 case TOURNE_G:
Wael_H 29:6bce50d6530c 165 if(robot.tourne(-ANGLE_DROIT))
Wael_H 29:6bce50d6530c 166 {
Wael_H 29:6bce50d6530c 167 ejecte = true;
Wael_H 29:6bce50d6530c 168 cptBalles++;
Wael_H 29:6bce50d6530c 169 etat = TOURNE_D;
Wael_H 29:6bce50d6530c 170 }
Wael_H 29:6bce50d6530c 171 break;
Wael_H 29:6bce50d6530c 172 }
Wael_H 29:6bce50d6530c 173
Wael_H 29:6bce50d6530c 174 return false;
Wael_H 29:6bce50d6530c 175 }
Wael_H 29:6bce50d6530c 176
Wael_H 29:6bce50d6530c 177 void automate_ejecte(Robot& robot, bool& ejecte)
Wael_H 29:6bce50d6530c 178 {
Wael_H 29:6bce50d6530c 179 typedef enum{GOBE, EJECTE, ATTENTE} type_etat;
Wael_H 29:6bce50d6530c 180 static type_etat etat = GOBE;
Wael_H 29:6bce50d6530c 181
Wael_H 29:6bce50d6530c 182 static Timer timer_ejecte;
Wael_H 29:6bce50d6530c 183
Wael_H 29:6bce50d6530c 184 switch(etat)
Wael_H 29:6bce50d6530c 185 {
Wael_H 29:6bce50d6530c 186 case GOBE:
Wael_H 29:6bce50d6530c 187 robot.gobe();
Wael_H 29:6bce50d6530c 188 if(ejecte)
Wael_H 29:6bce50d6530c 189 etat = EJECTE;
Wael_H 29:6bce50d6530c 190 break;
Wael_H 29:6bce50d6530c 191
Wael_H 29:6bce50d6530c 192 case EJECTE:
Wael_H 29:6bce50d6530c 193 robot.ejecte();
Wael_H 29:6bce50d6530c 194 timer_ejecte.start();
Wael_H 29:6bce50d6530c 195 etat = ATTENTE;
Wael_H 29:6bce50d6530c 196 break;
Wael_H 29:6bce50d6530c 197
Wael_H 29:6bce50d6530c 198 case ATTENTE:
Wael_H 29:6bce50d6530c 199 if( timer_ejecte >= 1.0f ){
Wael_H 29:6bce50d6530c 200 timer_ejecte.stop();
Wael_H 29:6bce50d6530c 201 timer_ejecte.reset();
Wael_H 29:6bce50d6530c 202 ejecte = false;
Wael_H 29:6bce50d6530c 203 etat = GOBE;
Wael_H 29:6bce50d6530c 204 }
Wael_H 29:6bce50d6530c 205 break;
Wael_H 29:6bce50d6530c 206 }
Wael_H 29:6bce50d6530c 207 }
Wael_H 27:e103da412e2b 208
Wael_H 27:e103da412e2b 209 void automate_testLasers(Robot& robot)
Wael_H 27:e103da412e2b 210 {
Wael_H 27:e103da412e2b 211 static const int diffMurBalle = 20;
Wael_H 27:e103da412e2b 212 static float past_gauche, past_droit, actual_gauche, actual_droit;
Wael_H 27:e103da412e2b 213 static float distBalle;
Wael_H 27:e103da412e2b 214
Wael_H 27:e103da412e2b 215 actual_gauche = robot.getDist(Laser::Gauche);
Wael_H 27:e103da412e2b 216 actual_droit = robot.getDist(Laser::Droit);
Wael_H 27:e103da412e2b 217
Wael_H 27:e103da412e2b 218 switch(etat_deplacement)
Wael_H 27:e103da412e2b 219 {
Wael_H 27:e103da412e2b 220 case TURN_FAST:
Wael_H 27:e103da412e2b 221 if(actual_droit < past_droit - diffMurBalle)
Wael_H 27:e103da412e2b 222 {
Wael_H 27:e103da412e2b 223 distBalle = actual_droit;
Wael_H 27:e103da412e2b 224 robot.stop();
Wael_H 27:e103da412e2b 225 etat_deplacement = CORRECT_GAUCHE;
Wael_H 27:e103da412e2b 226 }
Wael_H 27:e103da412e2b 227 break;
Wael_H 27:e103da412e2b 228
Wael_H 27:e103da412e2b 229 case CORRECT_GAUCHE:
Wael_H 27:e103da412e2b 230
Wael_H 27:e103da412e2b 231 break;
Wael_H 27:e103da412e2b 232
Wael_H 27:e103da412e2b 233 case CORRECT_DROITE:
Wael_H 27:e103da412e2b 234 break;
Wael_H 27:e103da412e2b 235
Wael_H 27:e103da412e2b 236 case GO_STRAIGHT:
Wael_H 27:e103da412e2b 237 break;
Wael_H 27:e103da412e2b 238 }
Wael_H 27:e103da412e2b 239
Wael_H 27:e103da412e2b 240
Wael_H 27:e103da412e2b 241 past_gauche = actual_gauche;
Wael_H 27:e103da412e2b 242 past_droit = actual_droit;
Wael_H 27:e103da412e2b 243 }
Wael_H 27:e103da412e2b 244
Wael_H 29:6bce50d6530c 245 void automate_testABalle(Robot& robot, bool& ejecte)
Wael_H 24:314b1f6607c5 246 {
Wael_H 27:e103da412e2b 247 typedef enum {ATTENTE,ABALLE} type_etat;
Wael_H 27:e103da412e2b 248 static type_etat etat = ATTENTE;
Wael_H 23:bb1535360a98 249
Wael_H 7:753e901d441b 250 switch(etat)
Wael_H 9:2113adf37c66 251 {
Wael_H 27:e103da412e2b 252 case ATTENTE:
Wael_H 27:e103da412e2b 253 if(robot.aBalle())
Wael_H 27:e103da412e2b 254 {
Wael_H 27:e103da412e2b 255 robot.stop();
Wael_H 27:e103da412e2b 256 etat = ABALLE;
Wael_H 27:e103da412e2b 257 robot.setSpeed(80,500); //vitesse de rotation vers le terrain ennemi
Wael_H 27:e103da412e2b 258 }
Wael_H 13:9c62e263f245 259 break;
Wael_H 13:9c62e263f245 260
Wael_H 27:e103da412e2b 261 case ABALLE:
Wael_H 29:6bce50d6530c 262 if( robot.tourne( -robot.pos(Robot::THETA) ) )
Wael_H 27:e103da412e2b 263 {
Wael_H 29:6bce50d6530c 264 ejecte = true;
Wael_H 27:e103da412e2b 265 etat = ATTENTE;
Wael_H 27:e103da412e2b 266 etat_deplacement = TURN_FAST;
Wael_H 29:6bce50d6530c 267 }
Wael_H 13:9c62e263f245 268 break;
Wael_H 13:9c62e263f245 269 }
Wael_H 13:9c62e263f245 270 }
Wael_H 23:bb1535360a98 271
Wael_H 27:e103da412e2b 272 void automate_deplacement(Robot& robot)
Wael_H 27:e103da412e2b 273 {
Wael_H 27:e103da412e2b 274 switch(etat_deplacement)
Wael_H 23:bb1535360a98 275 {
Wael_H 27:e103da412e2b 276 case TURN_FAST:
Wael_H 27:e103da412e2b 277 robot.tourne();
Wael_H 27:e103da412e2b 278 break;
Wael_H 27:e103da412e2b 279
Wael_H 27:e103da412e2b 280 case CORRECT_GAUCHE:
Wael_H 27:e103da412e2b 281 robot.tourne(-450);
Wael_H 23:bb1535360a98 282 break;
Wael_H 23:bb1535360a98 283
Wael_H 27:e103da412e2b 284 case CORRECT_DROITE:
Wael_H 27:e103da412e2b 285 robot.tourne(450);
Wael_H 27:e103da412e2b 286
Wael_H 27:e103da412e2b 287 case GO_STRAIGHT:
Wael_H 27:e103da412e2b 288 robot.avance();
Wael_H 27:e103da412e2b 289 break;
Wael_H 27:e103da412e2b 290 }
Wael_H 27:e103da412e2b 291 }
Wael_H 27:e103da412e2b 292
Wael_H 27:e103da412e2b 293 void automate_vitesse(Robot& robot)
Wael_H 27:e103da412e2b 294 {
Wael_H 27:e103da412e2b 295 switch(etat_deplacement)
Wael_H 27:e103da412e2b 296 {
Wael_H 27:e103da412e2b 297 case TURN_FAST:
Wael_H 27:e103da412e2b 298 robot.setSpeed(80,450);
Wael_H 23:bb1535360a98 299 break;
Wael_H 23:bb1535360a98 300
Wael_H 27:e103da412e2b 301 case CORRECT_GAUCHE:
Wael_H 27:e103da412e2b 302 robot.setSpeed(50,300);
Wael_H 27:e103da412e2b 303 break;
Wael_H 23:bb1535360a98 304
Wael_H 27:e103da412e2b 305 case CORRECT_DROITE:
Wael_H 27:e103da412e2b 306 robot.setSpeed(50,300);
Wael_H 27:e103da412e2b 307 break;
Wael_H 24:314b1f6607c5 308
Wael_H 27:e103da412e2b 309 case GO_STRAIGHT:
Wael_H 27:e103da412e2b 310 robot.setSpeed(80,400);
Wael_H 23:bb1535360a98 311 break;
Wael_H 23:bb1535360a98 312 }
Wael_H 27:e103da412e2b 313 }