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

Committer:
Wael_H
Date:
Fri Jun 07 15:46:27 2019 +0000
Revision:
35:6c6321b97ae6
Parent:
34:cea05fa02f37
Child:
36:0a6cb92024c7
3 balles + cherche balles version a ameliorer

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&); //
Wael_H 35:6c6321b97ae6 8 void 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 35:6c6321b97ae6 16 typedef enum{TURN_FAST,RETROUVE,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 35:6c6321b97ae6 24 dbug.printf("%d\n\r",etat_deplacement);
Wael_H 35:6c6321b97ae6 25
Wael_H 15:3d4543a6c100 26 while(1)
Wael_H 34:cea05fa02f37 27 automate_run(robot);
AlexisCollin 32:84bcb8f2667a 28 }
AlexisCollin 32:84bcb8f2667a 29
AlexisCollin 32:84bcb8f2667a 30 void automate_run(Robot& robot)
AlexisCollin 32:84bcb8f2667a 31 {
AlexisCollin 32:84bcb8f2667a 32 bool ejecte = false;
Wael_H 34:cea05fa02f37 33 typedef enum{ATTENTE, RUN_AVEUGLE, RECHERCHE_BALLES, FIN_PARTIE} type_etat;
AlexisCollin 32:84bcb8f2667a 34 static type_etat etat = ATTENTE;
AlexisCollin 32:84bcb8f2667a 35 static Timer T_partie;
Wael_H 31:85d9fb71f921 36
Wael_H 34:cea05fa02f37 37 if(T_partie.read() > 80.0f)
Wael_H 34:cea05fa02f37 38 etat = FIN_PARTIE;
Wael_H 34:cea05fa02f37 39
AlexisCollin 32:84bcb8f2667a 40 switch(etat)
AlexisCollin 32:84bcb8f2667a 41 {
AlexisCollin 32:84bcb8f2667a 42 case ATTENTE:
AlexisCollin 32:84bcb8f2667a 43 if(!Robot::Jack)
AlexisCollin 32:84bcb8f2667a 44 {
AlexisCollin 32:84bcb8f2667a 45 T_partie.start();
Wael_H 34:cea05fa02f37 46 etat = RUN_AVEUGLE;
AlexisCollin 32:84bcb8f2667a 47 }
AlexisCollin 32:84bcb8f2667a 48 break;
Wael_H 34:cea05fa02f37 49
Wael_H 34:cea05fa02f37 50 case RUN_AVEUGLE:
Wael_H 35:6c6321b97ae6 51 if(automate_aveugle(robot, ejecte) && robot.GoToXYT(2000,2500,0))
Wael_H 35:6c6321b97ae6 52 etat = RECHERCHE_BALLES;
Wael_H 34:cea05fa02f37 53 automate_ejecte(robot, ejecte);
AlexisCollin 32:84bcb8f2667a 54 break;
Wael_H 34:cea05fa02f37 55
Wael_H 34:cea05fa02f37 56 case RECHERCHE_BALLES:
Wael_H 34:cea05fa02f37 57 // Gestion du lancer de balle
Wael_H 34:cea05fa02f37 58 automate_testABalle(robot, ejecte);
Wael_H 34:cea05fa02f37 59 automate_ejecte(robot, ejecte);
Wael_H 34:cea05fa02f37 60
Wael_H 34:cea05fa02f37 61 // Recherche de balles
Wael_H 35:6c6321b97ae6 62 testLasers(robot);
Wael_H 34:cea05fa02f37 63 if(!robot.aBalle())
Wael_H 34:cea05fa02f37 64 automate_deplacement(robot);
Wael_H 34:cea05fa02f37 65
Wael_H 34:cea05fa02f37 66 // Gestion de la vitesse en fonction de l'état actuel
Wael_H 34:cea05fa02f37 67 automate_vitesse(robot);
Wael_H 34:cea05fa02f37 68 break;
Wael_H 34:cea05fa02f37 69
Wael_H 34:cea05fa02f37 70 case FIN_PARTIE:
Wael_H 35:6c6321b97ae6 71 automate_fin_de_partie(robot);
Wael_H 34:cea05fa02f37 72 break;
AlexisCollin 32:84bcb8f2667a 73 }
Wael_H 15:3d4543a6c100 74 }
Wael_H 15:3d4543a6c100 75
Wael_H 35:6c6321b97ae6 76 void testLasers(Robot& robot)
Wael_H 35:6c6321b97ae6 77 {
Wael_H 35:6c6321b97ae6 78 static float past_droit, actual_droit;
Wael_H 29:6bce50d6530c 79
Wael_H 27:e103da412e2b 80 actual_droit = robot.getDist(Laser::Droit);
Wael_H 27:e103da412e2b 81
Wael_H 35:6c6321b97ae6 82 dbug.printf("%f\n\r",actual_droit);
Wael_H 35:6c6321b97ae6 83 if(actual_droit < past_droit-30 && actual_droit <= 130 && etat_deplacement == TURN_FAST)
Wael_H 27:e103da412e2b 84 {
Wael_H 35:6c6321b97ae6 85 robot.stop();
Wael_H 35:6c6321b97ae6 86 etat_deplacement = RETROUVE;
Wael_H 35:6c6321b97ae6 87 }
Wael_H 35:6c6321b97ae6 88 else if(actual_droit < past_droit-30 && etat_deplacement == RETROUVE)
Wael_H 35:6c6321b97ae6 89 {
Wael_H 35:6c6321b97ae6 90 robot.stop();
Wael_H 35:6c6321b97ae6 91 etat_deplacement = GO_STRAIGHT;
Wael_H 27:e103da412e2b 92 }
Wael_H 27:e103da412e2b 93
Wael_H 27:e103da412e2b 94
Wael_H 27:e103da412e2b 95 past_droit = actual_droit;
Wael_H 27:e103da412e2b 96 }
Wael_H 27:e103da412e2b 97
Wael_H 29:6bce50d6530c 98 void automate_testABalle(Robot& robot, bool& ejecte)
Wael_H 24:314b1f6607c5 99 {
Wael_H 27:e103da412e2b 100 typedef enum {ATTENTE,ABALLE} type_etat;
Wael_H 27:e103da412e2b 101 static type_etat etat = ATTENTE;
Wael_H 23:bb1535360a98 102
Wael_H 7:753e901d441b 103 switch(etat)
Wael_H 9:2113adf37c66 104 {
Wael_H 27:e103da412e2b 105 case ATTENTE:
Wael_H 27:e103da412e2b 106 if(robot.aBalle())
Wael_H 27:e103da412e2b 107 {
Wael_H 27:e103da412e2b 108 robot.stop();
Wael_H 27:e103da412e2b 109 etat = ABALLE;
Wael_H 27:e103da412e2b 110 robot.setSpeed(80,500); //vitesse de rotation vers le terrain ennemi
Wael_H 27:e103da412e2b 111 }
Wael_H 13:9c62e263f245 112 break;
Wael_H 13:9c62e263f245 113
Wael_H 27:e103da412e2b 114 case ABALLE:
Wael_H 29:6bce50d6530c 115 if( robot.tourne( -robot.pos(Robot::THETA) ) )
Wael_H 27:e103da412e2b 116 {
Wael_H 29:6bce50d6530c 117 ejecte = true;
Wael_H 27:e103da412e2b 118 etat = ATTENTE;
Wael_H 27:e103da412e2b 119 etat_deplacement = TURN_FAST;
Wael_H 29:6bce50d6530c 120 }
Wael_H 13:9c62e263f245 121 break;
Wael_H 13:9c62e263f245 122 }
Wael_H 13:9c62e263f245 123 }
Wael_H 23:bb1535360a98 124
Wael_H 27:e103da412e2b 125 void automate_deplacement(Robot& robot)
Wael_H 27:e103da412e2b 126 {
Wael_H 27:e103da412e2b 127 switch(etat_deplacement)
Wael_H 23:bb1535360a98 128 {
Wael_H 27:e103da412e2b 129 case TURN_FAST:
Wael_H 27:e103da412e2b 130 robot.tourne();
Wael_H 27:e103da412e2b 131 break;
Wael_H 27:e103da412e2b 132
Wael_H 35:6c6321b97ae6 133 case RETROUVE:
Wael_H 35:6c6321b97ae6 134 robot.tourne(-30);
Wael_H 34:cea05fa02f37 135 break;
Wael_H 27:e103da412e2b 136
Wael_H 27:e103da412e2b 137 case GO_STRAIGHT:
Wael_H 35:6c6321b97ae6 138 robot.avance(robot.getDist(Laser::Droit,"mm"));
Wael_H 27:e103da412e2b 139 break;
Wael_H 27:e103da412e2b 140 }
Wael_H 27:e103da412e2b 141 }
Wael_H 27:e103da412e2b 142
Wael_H 27:e103da412e2b 143 void automate_vitesse(Robot& robot)
Wael_H 27:e103da412e2b 144 {
Wael_H 27:e103da412e2b 145 switch(etat_deplacement)
Wael_H 27:e103da412e2b 146 {
Wael_H 27:e103da412e2b 147 case TURN_FAST:
Wael_H 35:6c6321b97ae6 148 robot.setSpeed(150,200);
Wael_H 23:bb1535360a98 149 break;
Wael_H 23:bb1535360a98 150
Wael_H 35:6c6321b97ae6 151 case RETROUVE:
Wael_H 35:6c6321b97ae6 152 robot.setSpeed(100,300);
Wael_H 27:e103da412e2b 153 break;
Wael_H 24:314b1f6607c5 154
Wael_H 27:e103da412e2b 155 case GO_STRAIGHT:
Wael_H 35:6c6321b97ae6 156 robot.setSpeed(320,1600);
Wael_H 35:6c6321b97ae6 157 break;
Wael_H 35:6c6321b97ae6 158 }
Wael_H 35:6c6321b97ae6 159 }
Wael_H 35:6c6321b97ae6 160
Wael_H 35:6c6321b97ae6 161 bool automate_fin_de_partie(Robot& robot)
Wael_H 35:6c6321b97ae6 162 {
Wael_H 35:6c6321b97ae6 163 typedef enum{AVANCE, EXPLOSE_BALLON, FIN_PARTIE} type_etat;
Wael_H 35:6c6321b97ae6 164 static type_etat etat = AVANCE;
Wael_H 35:6c6321b97ae6 165
Wael_H 35:6c6321b97ae6 166 static Timer T_fin;
Wael_H 35:6c6321b97ae6 167
Wael_H 35:6c6321b97ae6 168 switch(etat)
Wael_H 35:6c6321b97ae6 169 {
Wael_H 35:6c6321b97ae6 170 case AVANCE:
Wael_H 35:6c6321b97ae6 171 robot.stopRouleau();
Wael_H 35:6c6321b97ae6 172 if(robot.GoToXYT(2000,3800,0))
Wael_H 35:6c6321b97ae6 173 etat = EXPLOSE_BALLON;
Wael_H 35:6c6321b97ae6 174 break;
Wael_H 35:6c6321b97ae6 175
Wael_H 35:6c6321b97ae6 176 case EXPLOSE_BALLON:
Wael_H 35:6c6321b97ae6 177 if(robot.leveBras())
Wael_H 35:6c6321b97ae6 178 {
Wael_H 35:6c6321b97ae6 179 robot.exploseBallon();
Wael_H 35:6c6321b97ae6 180 T_fin.start();
Wael_H 35:6c6321b97ae6 181 etat = FIN_PARTIE;
Wael_H 35:6c6321b97ae6 182 }
Wael_H 35:6c6321b97ae6 183 break;
Wael_H 35:6c6321b97ae6 184
Wael_H 35:6c6321b97ae6 185 case FIN_PARTIE:
Wael_H 35:6c6321b97ae6 186 if(T_fin.read() >= 2.0f && robot.baisseBras())
Wael_H 35:6c6321b97ae6 187 {
Wael_H 35:6c6321b97ae6 188 robot.arreteDisquette();
Wael_H 35:6c6321b97ae6 189 return true;
Wael_H 35:6c6321b97ae6 190 }
Wael_H 35:6c6321b97ae6 191 }
Wael_H 35:6c6321b97ae6 192
Wael_H 35:6c6321b97ae6 193 return false;
Wael_H 35:6c6321b97ae6 194 }
Wael_H 35:6c6321b97ae6 195
Wael_H 35:6c6321b97ae6 196 bool automate_aveugle(Robot& robot, bool& ejecte)
Wael_H 35:6c6321b97ae6 197 {
Wael_H 35:6c6321b97ae6 198 typedef enum{STRAIGHT1, PREMIER_TOURNE_D, TOURNE_D, STRAIGHT2, TOURNE_G} type_etat;
Wael_H 35:6c6321b97ae6 199 static type_etat etat = STRAIGHT1;
Wael_H 35:6c6321b97ae6 200
Wael_H 35:6c6321b97ae6 201 static Timer T_ejecte;
Wael_H 35:6c6321b97ae6 202 static int cptBalles = 0;
Wael_H 35:6c6321b97ae6 203
Wael_H 35:6c6321b97ae6 204 if(cptBalles == 2)
Wael_H 35:6c6321b97ae6 205 return true;
Wael_H 35:6c6321b97ae6 206
Wael_H 35:6c6321b97ae6 207 switch(etat)
Wael_H 35:6c6321b97ae6 208 {
Wael_H 35:6c6321b97ae6 209 case STRAIGHT1:
Wael_H 35:6c6321b97ae6 210 if(robot.avance(1850 - DIST_CENTRE))
Wael_H 35:6c6321b97ae6 211 {
Wael_H 35:6c6321b97ae6 212 ejecte = true;
Wael_H 35:6c6321b97ae6 213 etat = PREMIER_TOURNE_D;
Wael_H 35:6c6321b97ae6 214 }
Wael_H 35:6c6321b97ae6 215 break;
Wael_H 35:6c6321b97ae6 216
Wael_H 35:6c6321b97ae6 217 case PREMIER_TOURNE_D:
Wael_H 35:6c6321b97ae6 218 if(robot.tourne(ANGLE_DROIT))
Wael_H 35:6c6321b97ae6 219 etat = STRAIGHT2;
Wael_H 35:6c6321b97ae6 220 break;
Wael_H 35:6c6321b97ae6 221
Wael_H 35:6c6321b97ae6 222 case TOURNE_D:
Wael_H 35:6c6321b97ae6 223 if(robot.tourne(ANGLE_DROIT))
Wael_H 35:6c6321b97ae6 224 etat = STRAIGHT2;
Wael_H 35:6c6321b97ae6 225 break;
Wael_H 35:6c6321b97ae6 226
Wael_H 35:6c6321b97ae6 227 case STRAIGHT2:
Wael_H 35:6c6321b97ae6 228 if(robot.avance(1500))
Wael_H 35:6c6321b97ae6 229 etat = TOURNE_G;
Wael_H 35:6c6321b97ae6 230 break;
Wael_H 35:6c6321b97ae6 231
Wael_H 35:6c6321b97ae6 232 case TOURNE_G:
Wael_H 35:6c6321b97ae6 233 if(robot.tourne(-ANGLE_DROIT))
Wael_H 35:6c6321b97ae6 234 {
Wael_H 35:6c6321b97ae6 235 ejecte = true;
Wael_H 35:6c6321b97ae6 236 cptBalles++;
Wael_H 35:6c6321b97ae6 237 etat = TOURNE_D;
Wael_H 35:6c6321b97ae6 238 }
Wael_H 35:6c6321b97ae6 239 break;
Wael_H 35:6c6321b97ae6 240 }
Wael_H 35:6c6321b97ae6 241
Wael_H 35:6c6321b97ae6 242 return false;
Wael_H 35:6c6321b97ae6 243 }
Wael_H 35:6c6321b97ae6 244
Wael_H 35:6c6321b97ae6 245 void automate_ejecte(Robot& robot, bool& ejecte)
Wael_H 35:6c6321b97ae6 246 {
Wael_H 35:6c6321b97ae6 247 typedef enum{GOBE, EJECTE, ATTENTE} type_etat;
Wael_H 35:6c6321b97ae6 248 static type_etat etat = GOBE;
Wael_H 35:6c6321b97ae6 249
Wael_H 35:6c6321b97ae6 250 static Timer timer_ejecte;
Wael_H 35:6c6321b97ae6 251
Wael_H 35:6c6321b97ae6 252 switch(etat)
Wael_H 35:6c6321b97ae6 253 {
Wael_H 35:6c6321b97ae6 254 case GOBE:
Wael_H 35:6c6321b97ae6 255 robot.gobe();
Wael_H 35:6c6321b97ae6 256 if(ejecte)
Wael_H 35:6c6321b97ae6 257 etat = EJECTE;
Wael_H 35:6c6321b97ae6 258 break;
Wael_H 35:6c6321b97ae6 259
Wael_H 35:6c6321b97ae6 260 case EJECTE:
Wael_H 35:6c6321b97ae6 261 robot.ejecte();
Wael_H 35:6c6321b97ae6 262 timer_ejecte.start();
Wael_H 35:6c6321b97ae6 263 etat = ATTENTE;
Wael_H 35:6c6321b97ae6 264 break;
Wael_H 35:6c6321b97ae6 265
Wael_H 35:6c6321b97ae6 266 case ATTENTE:
Wael_H 35:6c6321b97ae6 267 if( timer_ejecte >= 1.0f ){
Wael_H 35:6c6321b97ae6 268 timer_ejecte.stop();
Wael_H 35:6c6321b97ae6 269 timer_ejecte.reset();
Wael_H 35:6c6321b97ae6 270 ejecte = false;
Wael_H 35:6c6321b97ae6 271 etat = GOBE;
Wael_H 35:6c6321b97ae6 272 }
Wael_H 23:bb1535360a98 273 break;
Wael_H 23:bb1535360a98 274 }
Wael_H 27:e103da412e2b 275 }