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@32:84bcb8f2667a, 2019-06-07 (annotated)

Committer:
AlexisCollin
Date:
Fri Jun 07 11:18:42 2019 +0000
Revision:
32:84bcb8f2667a
Parent:
31:85d9fb71f921
Child:
33:c70ded6dd380
perfect alexis v1

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