CRAC Team / Mbed 2 deprecated carte_straegie_2019

Programme carte strategie (disco)

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@27:76ead555a63d, 2017-05-26 (annotated)

Committer:
antbig
Date:
Fri May 26 19:01:56 2017 +0000
Revision:
27:76ead555a63d
Parent:
26:2f4fcc2354f3
Child:
28:acd18776ed2d
Debut du code pour le match 5

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ClementBreteau 14:c8fc06c4887f 1 #include "Strategie.h"
ClementBreteau 26:2f4fcc2354f3 2 #define M_PI 3.14159265358979323846
antbig 0:ad97421fb1fb 3
antbig 5:dcd817534b57 4 E_stratGameEtat gameEtat = ETAT_CHECK_CARTE_SCREEN;
antbig 0:ad97421fb1fb 5 E_stratGameEtat lastEtat = ETAT_CHECK_CARTE_SCREEN;
antbig 0:ad97421fb1fb 6 unsigned char screenChecktry = 0;
antbig 0:ad97421fb1fb 7 Timer cartesCheker;//Le timer pour le timeout de la vérification des cartes
antbig 0:ad97421fb1fb 8 Timer fakeJack;
antbig 0:ad97421fb1fb 9 Timer gameTimer;
antbig 0:ad97421fb1fb 10 Timer debugetatTimer;
antbig 5:dcd817534b57 11 Timer timeoutWarning;
antbig 5:dcd817534b57 12 Timer timeoutWarningWaitEnd;
antbig 0:ad97421fb1fb 13 Timeout chronoEnd;//permet d'envoyer la trame CAN pour la fin
antbig 0:ad97421fb1fb 14
antbig 0:ad97421fb1fb 15 unsigned short waitingAckID = 0;//L'id du ack attendu
antbig 0:ad97421fb1fb 16 unsigned short waitingAckFrom = 0;//La provenance du ack attendu
ClementBreteau 14:c8fc06c4887f 17 char modeTelemetre; // Si à 1, indique que l'on attend une reponse du telemetre
ClementBreteau 21:590cdacb6a35 18 unsigned short telemetreDistance = 0;
antbig 0:ad97421fb1fb 19 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
antbig 0:ad97421fb1fb 20
antbig 0:ad97421fb1fb 21 signed short x_robot,y_robot,theta_robot;//La position du robot
ClementBreteau 25:f140c93a8666 22 signed short target_x_robot, target_y_robot, target_theta_robot;
ClementBreteau 25:f140c93a8666 23 E_InstructionType actionPrecedente;
antbig 12:14729d584500 24 signed short start_move_x,start_move_y,start_move_theta;//La position du robot lors du début d'un mouvement, utilisé pour reprendre le mouvement apres stop balise
antbig 12:14729d584500 25
antbig 5:dcd817534b57 26 #ifdef ROBOT_BIG
ClementBreteau 14:c8fc06c4887f 27 //unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS,CHECK_AX12,CHECK_POMPES};
ClementBreteau 14:c8fc06c4887f 28 //unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS,ALIVE_AX12,ALIVE_POMPES};
ClementBreteau 14:c8fc06c4887f 29
ClementBreteau 23:ab87d308eaf9 30 unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE,CHECK_ACTIONNEURS_AVANT,CHECK_ACTIONNEURS_ARRIERE};
ClementBreteau 23:ab87d308eaf9 31 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE,ALIVE_ACTIONNEURS_AVANT,ALIVE_ACTIONNEURS_ARRIERE};
ClementBreteau 15:c2fc239e85df 32
ClementBreteau 15:c2fc239e85df 33 InterruptIn jack(p25); // entrée analogique en interruption pour le jack
ClementBreteau 15:c2fc239e85df 34 #else
ClementBreteau 15:c2fc239e85df 35 //unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS};
ClementBreteau 15:c2fc239e85df 36 //unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS};
ClementBreteau 15:c2fc239e85df 37
ClementBreteau 14:c8fc06c4887f 38 unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR};
ClementBreteau 14:c8fc06c4887f 39 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR};
ClementBreteau 14:c8fc06c4887f 40
ClementBreteau 18:cc5fec34ed9c 41 InterruptIn jack(p25); // entrée analogique en interruption pour le jack
antbig 5:dcd817534b57 42 #endif
antbig 0:ad97421fb1fb 43 unsigned char checkCurrent = 0;
antbig 0:ad97421fb1fb 44 unsigned char countAliveCard = 0;
antbig 0:ad97421fb1fb 45
antbig 2:8d8e2cf798a3 46 unsigned char InversStrat = 1;//Si à 1, indique que l'on part de l'autre cote de la table(inversion des Y)
antbig 1:116040d14164 47
ClementBreteau 14:c8fc06c4887f 48 unsigned char ModeDemo = 0; // Si à 1, indique que l'on est dans le mode demo
ClementBreteau 14:c8fc06c4887f 49
antbig 5:dcd817534b57 50 unsigned char countRobotNear = 0;//Le nombre de robot à proximité
antbig 5:dcd817534b57 51
antbig 12:14729d584500 52 unsigned char ingnorBaliseOnce = 0;
antbig 12:14729d584500 53
ClementBreteau 14:c8fc06c4887f 54
antbig 4:88431b537477 55 /****************************************************************************************/
antbig 4:88431b537477 56 /* FUNCTION NAME: chronometre_ISR */
antbig 4:88431b537477 57 /* DESCRIPTION : Interruption à la fin des 90s du match */
antbig 4:88431b537477 58 /****************************************************************************************/
antbig 0:ad97421fb1fb 59 void chronometre_ISR (void)
antbig 0:ad97421fb1fb 60 {
antbig 0:ad97421fb1fb 61 SendRawId(ASSERVISSEMENT_STOP);//On stope les moteurs
antbig 0:ad97421fb1fb 62 SendRawId(GLOBAL_GAME_END);//Indication fin de match
antbig 0:ad97421fb1fb 63 gameTimer.stop();//Arret du timer
antbig 0:ad97421fb1fb 64
antbig 1:116040d14164 65 #ifdef ROBOT_BIG
antbig 12:14729d584500 66 wait_ms(2000);
antbig 0:ad97421fb1fb 67 doFunnyAction();
antbig 0:ad97421fb1fb 68 #endif
antbig 0:ad97421fb1fb 69
antbig 1:116040d14164 70 while(1);//On bloque la programme dans l'interruption
antbig 0:ad97421fb1fb 71 }
antbig 0:ad97421fb1fb 72
antbig 4:88431b537477 73 /****************************************************************************************/
antbig 8:0edc7dfb7f7e 74 /* FUNCTION NAME: jack_ISR */
antbig 8:0edc7dfb7f7e 75 /* DESCRIPTION : Interruption en changement d'état sur le Jack */
antbig 8:0edc7dfb7f7e 76 /****************************************************************************************/
antbig 8:0edc7dfb7f7e 77 void jack_ISR (void)
antbig 8:0edc7dfb7f7e 78 {
antbig 8:0edc7dfb7f7e 79 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
antbig 8:0edc7dfb7f7e 80 led4=1;
antbig 8:0edc7dfb7f7e 81 gameEtat = ETAT_GAME_START;//On débute le match
antbig 8:0edc7dfb7f7e 82 }
antbig 8:0edc7dfb7f7e 83 }
antbig 8:0edc7dfb7f7e 84
antbig 8:0edc7dfb7f7e 85 /****************************************************************************************/
antbig 4:88431b537477 86 /* FUNCTION NAME: automate_process */
antbig 4:88431b537477 87 /* DESCRIPTION : Automate de gestion de la stratégie du robot */
antbig 4:88431b537477 88 /****************************************************************************************/
antbig 0:ad97421fb1fb 89 void automate_process(void)
antbig 0:ad97421fb1fb 90 {
antbig 0:ad97421fb1fb 91 static struct S_Instruction instruction;
antbig 1:116040d14164 92 static unsigned char AX12_enchainement = 0;
antbig 1:116040d14164 93 static unsigned char MV_enchainement = 0;
antbig 0:ad97421fb1fb 94 signed char localData1 = 0;
antbig 0:ad97421fb1fb 95 signed short localData2 = 0;
antbig 0:ad97421fb1fb 96 unsigned short localData3 = 0;
ClementBreteau 14:c8fc06c4887f 97 //signed short localData4 = 0;
antbig 1:116040d14164 98 unsigned char localData5 = 0;
antbig 0:ad97421fb1fb 99
antbig 12:14729d584500 100 if(gameTimer.read_ms() >= 89000) {//Fin du match (On autorise 2s pour déposer des éléments
antbig 0:ad97421fb1fb 101 gameTimer.stop();
antbig 0:ad97421fb1fb 102 gameTimer.reset();
antbig 0:ad97421fb1fb 103 gameEtat = ETAT_END;//Fin du temps
antbig 0:ad97421fb1fb 104 }
antbig 0:ad97421fb1fb 105
antbig 0:ad97421fb1fb 106 if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) {
antbig 0:ad97421fb1fb 107 lastEtat = gameEtat;
antbig 0:ad97421fb1fb 108 debugetatTimer.reset();
antbig 11:ed13a480ddca 109 sendStratEtat((unsigned char)gameEtat, (unsigned char)actual_instruction);
antbig 0:ad97421fb1fb 110 }
antbig 0:ad97421fb1fb 111
antbig 0:ad97421fb1fb 112 switch(gameEtat)
antbig 0:ad97421fb1fb 113 {
antbig 0:ad97421fb1fb 114 case ETAT_CHECK_CARTE_SCREEN:
antbig 0:ad97421fb1fb 115 /*
antbig 0:ad97421fb1fb 116 Verification de l'état de la carte ecran
antbig 0:ad97421fb1fb 117 */
antbig 0:ad97421fb1fb 118 waitingAckFrom = ALIVE_IHM;//On indique que l'on attend un ack de la carte IHM
antbig 0:ad97421fb1fb 119 SendRawId(CHECK_IHM);//On demande à la carte IHM d'insiquer ça présence
antbig 0:ad97421fb1fb 120
antbig 0:ad97421fb1fb 121 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 122 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 123 cartesCheker.start();//On lance le timer pour le timeout
antbig 0:ad97421fb1fb 124 gameEtat = ETAT_CHECK_CARTE_SCREEN_WAIT_ACK;
antbig 0:ad97421fb1fb 125
ClementBreteau 14:c8fc06c4887f 126 //gameEtat = ETAT_GAME_START;
ClementBreteau 14:c8fc06c4887f 127
antbig 0:ad97421fb1fb 128 break;
antbig 0:ad97421fb1fb 129 case ETAT_CHECK_CARTE_SCREEN_WAIT_ACK:
antbig 0:ad97421fb1fb 130 /*
antbig 0:ad97421fb1fb 131 Attente du ALIVE de la carte écran.
antbig 0:ad97421fb1fb 132
antbig 0:ad97421fb1fb 133 Si la carte ne répond pas apres 10ms, on retoune dans l'etat ETAT_CHECK_CARTE_SCREEN
antbig 0:ad97421fb1fb 134 maximum 3 tentatives
antbig 0:ad97421fb1fb 135 Si pas de réponse, clignotement de toutes les leds possible
antbig 0:ad97421fb1fb 136 */
antbig 0:ad97421fb1fb 137 if(waitingAckFrom == 0) {//C'est bon la carte est en ligne
antbig 0:ad97421fb1fb 138 cartesCheker.stop();
antbig 0:ad97421fb1fb 139 screenChecktry = 0;
antbig 0:ad97421fb1fb 140 gameEtat = ETAT_CHECK_CARTES;
antbig 0:ad97421fb1fb 141 } else if(cartesCheker.read_ms () > 100) {
antbig 0:ad97421fb1fb 142 cartesCheker.stop();
antbig 0:ad97421fb1fb 143 if(screenChecktry >=3) {
antbig 0:ad97421fb1fb 144 errorLoop();//Erreur La carte IHM n'est pas en ligne
antbig 0:ad97421fb1fb 145 } else {
antbig 0:ad97421fb1fb 146 gameEtat = ETAT_CHECK_CARTE_SCREEN;
antbig 0:ad97421fb1fb 147 }
antbig 0:ad97421fb1fb 148 }
antbig 0:ad97421fb1fb 149 break;
antbig 0:ad97421fb1fb 150 case ETAT_CHECK_CARTES:
antbig 0:ad97421fb1fb 151 /*
antbig 0:ad97421fb1fb 152 Il faut faire une boucle pour verifier toutes les cartes les une apres les autres
antbig 0:ad97421fb1fb 153 */
antbig 0:ad97421fb1fb 154 waitingAckFrom = id_alive[checkCurrent];//On indique que l'on attend un ack de la carte IHM
antbig 11:ed13a480ddca 155 SendRawId(id_check[checkCurrent]);//On demande à la carte d'indiquer ça présence
antbig 0:ad97421fb1fb 156
antbig 0:ad97421fb1fb 157 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 158 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 159 cartesCheker.start();//On lance le timer pour le timeout
antbig 0:ad97421fb1fb 160 gameEtat = ETAT_CHECK_CARTES_WAIT_ACK;
ClementBreteau 14:c8fc06c4887f 161
ClementBreteau 14:c8fc06c4887f 162
ClementBreteau 14:c8fc06c4887f 163
ClementBreteau 14:c8fc06c4887f 164
antbig 0:ad97421fb1fb 165 break;
antbig 0:ad97421fb1fb 166 case ETAT_CHECK_CARTES_WAIT_ACK:
antbig 0:ad97421fb1fb 167 /*
antbig 0:ad97421fb1fb 168 On attend l'ack de la carte en cours de vérification
antbig 0:ad97421fb1fb 169 */
antbig 0:ad97421fb1fb 170 //printf("cartesCheker = %d waitingAckFrom = %d\n",cartesCheker.read_ms(), waitingAckFrom);
antbig 0:ad97421fb1fb 171 if(waitingAckFrom == 0) {//C'est bon la carte est en ligne
antbig 0:ad97421fb1fb 172 cartesCheker.stop();
antbig 0:ad97421fb1fb 173 screenChecktry = 0;
antbig 0:ad97421fb1fb 174 countAliveCard++;
antbig 11:ed13a480ddca 175 checkCurrent++;
antbig 0:ad97421fb1fb 176 if(checkCurrent >= NOMBRE_CARTES) {
antbig 12:14729d584500 177 //printf("all card check, missing %d cards\n",(NOMBRE_CARTES-countAliveCard));
antbig 0:ad97421fb1fb 178 if(countAliveCard >= NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 179 gameEtat = ETAT_CONFIG;
antbig 0:ad97421fb1fb 180 SendRawId(ECRAN_ALL_CHECK);//On dit à l'IHM que toutes les cartes sont en ligne
antbig 5:dcd817534b57 181 tactile_printf("Selection couleur et strategie");
antbig 0:ad97421fb1fb 182 } else {
antbig 0:ad97421fb1fb 183 gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
antbig 0:ad97421fb1fb 184 waitingAckFrom = ECRAN_ALL_CHECK;
antbig 0:ad97421fb1fb 185 }
antbig 0:ad97421fb1fb 186 } else {
antbig 0:ad97421fb1fb 187 gameEtat = ETAT_CHECK_CARTES;
antbig 0:ad97421fb1fb 188 }
antbig 0:ad97421fb1fb 189 } else if(cartesCheker.read_ms () > 100) {
antbig 0:ad97421fb1fb 190 cartesCheker.stop();
antbig 0:ad97421fb1fb 191 if(screenChecktry >=3) {
antbig 12:14729d584500 192 //printf("missing card %d\n",id_check[checkCurrent]);
antbig 0:ad97421fb1fb 193 screenChecktry = 0;
antbig 11:ed13a480ddca 194 checkCurrent++;
antbig 12:14729d584500 195
antbig 0:ad97421fb1fb 196 if(checkCurrent >= NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 197 if(countAliveCard == NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 198 gameEtat = ETAT_CONFIG;
antbig 0:ad97421fb1fb 199 SendRawId(ECRAN_ALL_CHECK);//On dit à l'IHM que toutes les cartes sont en ligne
antbig 0:ad97421fb1fb 200 } else {
antbig 0:ad97421fb1fb 201 gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
antbig 0:ad97421fb1fb 202 waitingAckFrom = ECRAN_ALL_CHECK;
antbig 0:ad97421fb1fb 203 }
antbig 0:ad97421fb1fb 204 } else {
antbig 0:ad97421fb1fb 205 gameEtat = ETAT_CHECK_CARTES;
antbig 0:ad97421fb1fb 206 }
antbig 0:ad97421fb1fb 207 } else {
antbig 0:ad97421fb1fb 208 gameEtat = ETAT_CHECK_CARTES;
antbig 0:ad97421fb1fb 209 }
antbig 0:ad97421fb1fb 210 }
antbig 0:ad97421fb1fb 211 break;
antbig 0:ad97421fb1fb 212 case ETAT_WAIT_FORCE:
antbig 0:ad97421fb1fb 213 /*
antbig 0:ad97421fb1fb 214 Attente du forçage de la part de la carte IHM
antbig 0:ad97421fb1fb 215 */
antbig 0:ad97421fb1fb 216 if(waitingAckFrom == 0) {
antbig 0:ad97421fb1fb 217 gameEtat = ETAT_CONFIG;
antbig 0:ad97421fb1fb 218 }
antbig 0:ad97421fb1fb 219 break;
antbig 0:ad97421fb1fb 220 case ETAT_CONFIG:
antbig 0:ad97421fb1fb 221 /*
antbig 0:ad97421fb1fb 222 Attente de l'odre de choix de mode,
antbig 0:ad97421fb1fb 223 Il est possible de modifier la couleur et l'id de la stratégie
antbig 0:ad97421fb1fb 224 Il est aussi possible d'envoyer les ordres de debug
antbig 0:ad97421fb1fb 225 */
ClementBreteau 14:c8fc06c4887f 226 modeTelemetre = 0;
antbig 0:ad97421fb1fb 227 break;
antbig 1:116040d14164 228 case ETAT_GAME_INIT:
antbig 0:ad97421fb1fb 229 //On charge la liste des instructions
antbig 0:ad97421fb1fb 230 loadAllInstruction();//Mise en cache de toute les instructions
ClementBreteau 14:c8fc06c4887f 231
antbig 0:ad97421fb1fb 232 gameEtat = ETAT_GAME_WAIT_FOR_JACK;
antbig 0:ad97421fb1fb 233 SendRawId(ECRAN_ACK_START_MATCH);
antbig 0:ad97421fb1fb 234 tactile_printf("Attente du JACK.");
antbig 12:14729d584500 235 setAsservissementEtat(1);//On réactive l'asservissement
antbig 12:14729d584500 236 jack.mode(PullDown); // désactivation de la résistance interne du jack
antbig 8:0edc7dfb7f7e 237 jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack
antbig 12:14729d584500 238
antbig 12:14729d584500 239 #ifdef ROBOT_BIG //le gros robot n'a pas de recalage bordure pour ce placer au début, on lui envoit donc ça position
antbig 12:14729d584500 240 localData2 = POSITION_DEBUT_T;
antbig 12:14729d584500 241 localData3 = POSITION_DEBUT_Y;
antbig 12:14729d584500 242 if(InversStrat == 1) {
antbig 27:76ead555a63d 243 localData2 = 1800-localData2;//Inversion theta
antbig 27:76ead555a63d 244 if(localData2 > 1800) localData2 -= 1800;
antbig 27:76ead555a63d 245 else if(localData2 <= -1800) localData2 += 1800;
antbig 12:14729d584500 246 localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
antbig 12:14729d584500 247 }
antbig 12:14729d584500 248 SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2);
ClementBreteau 18:cc5fec34ed9c 249 #endif
ClementBreteau 18:cc5fec34ed9c 250 #ifdef ROBOT_SMALL
clementlignie 22:a466d08ac42b 251 localData2 = POSITION_DEBUT_T;
clementlignie 22:a466d08ac42b 252 localData3 = POSITION_DEBUT_Y;
clementlignie 22:a466d08ac42b 253 if(InversStrat == 1) {
clementlignie 22:a466d08ac42b 254 localData2 = -localData2;//Inversion theta
clementlignie 22:a466d08ac42b 255 localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
clementlignie 22:a466d08ac42b 256 }
clementlignie 22:a466d08ac42b 257 SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,localData3,localData2);
ClementBreteau 18:cc5fec34ed9c 258 #endif
antbig 0:ad97421fb1fb 259 break;
antbig 0:ad97421fb1fb 260 case ETAT_GAME_WAIT_FOR_JACK:
antbig 12:14729d584500 261 //On attend le jack
antbig 1:116040d14164 262 break;
antbig 1:116040d14164 263 case ETAT_GAME_START:
antbig 1:116040d14164 264 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
ClementBreteau 14:c8fc06c4887f 265
ClementBreteau 14:c8fc06c4887f 266 if (ModeDemo == 0){
ClementBreteau 14:c8fc06c4887f 267 chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s
ClementBreteau 14:c8fc06c4887f 268 gameTimer.start();
ClementBreteau 14:c8fc06c4887f 269 }
antbig 1:116040d14164 270 gameTimer.reset();
antbig 12:14729d584500 271 jack.fall(NULL);//On désactive l'interruption du jack
antbig 10:a788d9cf60f2 272 SendRawId(GLOBAL_START);
antbig 12:14729d584500 273 tactile_printf("Start");//Pas vraiment util mais bon
antbig 0:ad97421fb1fb 274 break;
antbig 0:ad97421fb1fb 275 case ETAT_GAME_LOAD_NEXT_INSTRUCTION:
antbig 0:ad97421fb1fb 276 /*
antbig 0:ad97421fb1fb 277 Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction
antbig 0:ad97421fb1fb 278 */
antbig 0:ad97421fb1fb 279 //printf("load next instruction\n");
ClementBreteau 14:c8fc06c4887f 280
antbig 0:ad97421fb1fb 281 if(actual_instruction >= nb_instructions || actual_instruction == 255) {
antbig 0:ad97421fb1fb 282 gameEtat = ETAT_END;
antbig 0:ad97421fb1fb 283 //Il n'y a plus d'instruction, fin du jeu
antbig 0:ad97421fb1fb 284 } else {
antbig 0:ad97421fb1fb 285 instruction = strat_instructions[actual_instruction];
antbig 0:ad97421fb1fb 286 //On effectue le traitement de l'instruction
antbig 0:ad97421fb1fb 287 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
antbig 0:ad97421fb1fb 288 }
antbig 0:ad97421fb1fb 289 screenChecktry = 0;
antbig 0:ad97421fb1fb 290 break;
antbig 0:ad97421fb1fb 291 case ETAT_GAME_PROCESS_INSTRUCTION:
antbig 0:ad97421fb1fb 292 /*
antbig 0:ad97421fb1fb 293 Traitement de l'instruction, envoie de la trame CAN
antbig 0:ad97421fb1fb 294 */
antbig 0:ad97421fb1fb 295 //debug_Instruction(instruction);
ClementBreteau 25:f140c93a8666 296
ClementBreteau 25:f140c93a8666 297 actionPrecedente = instruction.order;
ClementBreteau 25:f140c93a8666 298
antbig 0:ad97421fb1fb 299 switch(instruction.order)
antbig 0:ad97421fb1fb 300 {
antbig 12:14729d584500 301 case MV_COURBURE://C'est un rayon de courbure
ClementBreteau 26:2f4fcc2354f3 302 actionPrecedente = MV_COURBURE;
antbig 6:eddfa414fd11 303 waitingAckID = ASSERVISSEMENT_COURBURE;
antbig 6:eddfa414fd11 304 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 305 if(instruction.nextActionType == ENCHAINEMENT) {
antbig 6:eddfa414fd11 306 MV_enchainement++;
antbig 6:eddfa414fd11 307 localData5 = 1;
antbig 6:eddfa414fd11 308 } else {
antbig 6:eddfa414fd11 309 if(MV_enchainement > 0) {
antbig 6:eddfa414fd11 310 localData5 = 2;
antbig 6:eddfa414fd11 311 MV_enchainement = 0;
antbig 6:eddfa414fd11 312 } else {
antbig 6:eddfa414fd11 313 localData5 = 0;
antbig 6:eddfa414fd11 314 }
antbig 0:ad97421fb1fb 315 }
antbig 11:ed13a480ddca 316 localData1 = ((instruction.direction == LEFT)?1:-1);
ClementBreteau 23:ab87d308eaf9 317 localData2 = instruction.arg3;
antbig 12:14729d584500 318 if(InversStrat == 1)
antbig 12:14729d584500 319 {
antbig 12:14729d584500 320 localData1 = -localData1;//Inversion de la direction
ClementBreteau 23:ab87d308eaf9 321 #ifdef ROBOT_BIG
ClementBreteau 25:f140c93a8666 322 localData2 = -localData2;
ClementBreteau 26:2f4fcc2354f3 323 localData1 = -localData1;//Inversion de la direction
ClementBreteau 23:ab87d308eaf9 324 #endif
antbig 12:14729d584500 325 }
ClementBreteau 25:f140c93a8666 326 BendRadius(instruction.arg1, localData2, localData1, localData5);
ClementBreteau 18:cc5fec34ed9c 327
ClementBreteau 26:2f4fcc2354f3 328 if(instruction.direction == LEFT){
ClementBreteau 26:2f4fcc2354f3 329 target_theta_robot = theta_robot + localData2;
ClementBreteau 26:2f4fcc2354f3 330 }else{
ClementBreteau 26:2f4fcc2354f3 331 target_theta_robot = theta_robot - localData2;
ClementBreteau 26:2f4fcc2354f3 332 }
ClementBreteau 26:2f4fcc2354f3 333
antbig 0:ad97421fb1fb 334 break;
antbig 12:14729d584500 335 case MV_LINE://Ligne droite
antbig 0:ad97421fb1fb 336 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 337 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 338 if(instruction.nextActionType == ENCHAINEMENT) {
antbig 1:116040d14164 339 MV_enchainement++;
antbig 1:116040d14164 340 localData5 = 1;
antbig 1:116040d14164 341 } else {
antbig 12:14729d584500 342 if(MV_enchainement > 0) {//Utilisé en cas d'enchainement,
antbig 1:116040d14164 343 localData5 = 2;
antbig 1:116040d14164 344 MV_enchainement = 0;
antbig 1:116040d14164 345 } else {
antbig 1:116040d14164 346 localData5 = 0;
antbig 1:116040d14164 347 }
antbig 0:ad97421fb1fb 348 }
ClementBreteau 16:7321fb3bb396 349 #ifdef ROBOT_BIG
ClementBreteau 15:c2fc239e85df 350 if(InversStrat == 1) {
ClementBreteau 15:c2fc239e85df 351 /*if (instruction.direction == FORWARD) instruction.direction = BACKWARD;
ClementBreteau 15:c2fc239e85df 352 else instruction.direction = FORWARD;*/
ClementBreteau 15:c2fc239e85df 353 instruction.direction = ((instruction.direction == FORWARD)?BACKWARD:FORWARD);
ClementBreteau 15:c2fc239e85df 354 }
ClementBreteau 16:7321fb3bb396 355 #endif
antbig 1:116040d14164 356 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
antbig 1:116040d14164 357 GoStraight(localData2, 0, 0, localData5);
ClementBreteau 14:c8fc06c4887f 358
ClementBreteau 26:2f4fcc2354f3 359 target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800);
ClementBreteau 26:2f4fcc2354f3 360 target_y_robot = y_robot + localData2*sin((double)theta_robot*M_PI/1800);
ClementBreteau 25:f140c93a8666 361 target_theta_robot = theta_robot;
ClementBreteau 25:f140c93a8666 362
antbig 0:ad97421fb1fb 363 break;
antbig 12:14729d584500 364 case MV_TURN: //Rotation sur place
antbig 0:ad97421fb1fb 365 if(instruction.direction == RELATIVE) {
antbig 0:ad97421fb1fb 366 localData2 = instruction.arg3;
antbig 12:14729d584500 367 } else {//C'est un rotation absolu, il faut la convertir en relative
antbig 12:14729d584500 368 localData2 = instruction.arg3;
antbig 12:14729d584500 369
antbig 12:14729d584500 370 localData2 = (localData2 - theta_robot)%3600;
antbig 12:14729d584500 371 if(localData2 > 1800) {
antbig 12:14729d584500 372 localData2 = localData2-3600;
antbig 12:14729d584500 373 }
antbig 12:14729d584500 374
antbig 0:ad97421fb1fb 375 }
ClementBreteau 23:ab87d308eaf9 376 #ifdef ROBOT_SMALL
clementlignie 22:a466d08ac42b 377 if(InversStrat == 1) {
clementlignie 22:a466d08ac42b 378 localData2 = -localData2;
clementlignie 22:a466d08ac42b 379 }
ClementBreteau 23:ab87d308eaf9 380 #endif
antbig 0:ad97421fb1fb 381 Rotate(localData2);
antbig 0:ad97421fb1fb 382 waitingAckID = ASSERVISSEMENT_ROTATION;
antbig 0:ad97421fb1fb 383 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
ClementBreteau 26:2f4fcc2354f3 384
antbig 0:ad97421fb1fb 385 break;
antbig 0:ad97421fb1fb 386 case MV_XYT:
antbig 0:ad97421fb1fb 387 if(instruction.direction == BACKWARD) {
antbig 0:ad97421fb1fb 388 localData1 = -1;
antbig 0:ad97421fb1fb 389 } else {
antbig 0:ad97421fb1fb 390 localData1 = 1;
antbig 0:ad97421fb1fb 391 }
antbig 2:8d8e2cf798a3 392
antbig 2:8d8e2cf798a3 393 if(InversStrat == 1) {
antbig 12:14729d584500 394 localData2 = -instruction.arg3;
antbig 2:8d8e2cf798a3 395 localData3 = 3000 - instruction.arg2;//Inversion du Y
antbig 2:8d8e2cf798a3 396 } else {
antbig 2:8d8e2cf798a3 397 localData3 = instruction.arg2;
antbig 12:14729d584500 398 localData2 = instruction.arg3;
antbig 2:8d8e2cf798a3 399 }
ClementBreteau 23:ab87d308eaf9 400
ClementBreteau 23:ab87d308eaf9 401 #ifdef ROBOT_BIG
ClementBreteau 23:ab87d308eaf9 402 if(InversStrat == 1) {
ClementBreteau 23:ab87d308eaf9 403 localData1 = -localData1;
antbig 27:76ead555a63d 404 localData2 = 1800-instruction.arg3;//Inversion theta
antbig 27:76ead555a63d 405 if(localData2 > 1800) localData2 -= 1800;
antbig 27:76ead555a63d 406 else if(localData2 <= -1800) localData2 += 1800;
ClementBreteau 23:ab87d308eaf9 407 }
ClementBreteau 23:ab87d308eaf9 408 #endif
ClementBreteau 23:ab87d308eaf9 409
antbig 12:14729d584500 410 GoToPosition(instruction.arg1,localData3,localData2,localData1);
antbig 0:ad97421fb1fb 411 waitingAckID = ASSERVISSEMENT_XYT;
antbig 0:ad97421fb1fb 412 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
ClementBreteau 25:f140c93a8666 413
ClementBreteau 25:f140c93a8666 414 target_x_robot = instruction.arg1;
ClementBreteau 25:f140c93a8666 415 target_y_robot = localData3;
ClementBreteau 25:f140c93a8666 416 target_theta_robot = localData2;
ClementBreteau 25:f140c93a8666 417
antbig 0:ad97421fb1fb 418 break;
antbig 0:ad97421fb1fb 419 case MV_RECALAGE:
antbig 0:ad97421fb1fb 420 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 421 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 11:ed13a480ddca 422 instruction.nextActionType = WAIT;
antbig 12:14729d584500 423 localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler
antbig 11:ed13a480ddca 424
antbig 11:ed13a480ddca 425 if(instruction.precision == RECALAGE_Y) {
antbig 11:ed13a480ddca 426 localData5 = 2;
antbig 11:ed13a480ddca 427 if(InversStrat == 1) {
antbig 11:ed13a480ddca 428 localData3 = 3000 - instruction.arg1;//Inversion du Y
antbig 11:ed13a480ddca 429 } else {
antbig 11:ed13a480ddca 430 localData3 = instruction.arg1;
antbig 11:ed13a480ddca 431 }
antbig 11:ed13a480ddca 432 } else {
antbig 11:ed13a480ddca 433 localData5 = 1;
antbig 11:ed13a480ddca 434 localData3 = instruction.arg1;
antbig 11:ed13a480ddca 435 }
antbig 11:ed13a480ddca 436
antbig 11:ed13a480ddca 437 GoStraight(localData2, localData5, localData3, 0);
antbig 0:ad97421fb1fb 438 break;
antbig 0:ad97421fb1fb 439 case ACTION:
ClementBreteau 18:cc5fec34ed9c 440 int tempo = 0;
ClementBreteau 18:cc5fec34ed9c 441 waitingAckID= SERVO_AX12_ACTION;
ClementBreteau 15:c2fc239e85df 442 waitingAckFrom = ACKNOWLEDGE_AX12;
ClementBreteau 18:cc5fec34ed9c 443 tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
ClementBreteau 18:cc5fec34ed9c 444 if(tempo == 1){
antbig 0:ad97421fb1fb 445 //L'action est spécifique
antbig 11:ed13a480ddca 446 if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) {
ClementBreteau 18:cc5fec34ed9c 447
antbig 11:ed13a480ddca 448 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 11:ed13a480ddca 449 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 11:ed13a480ddca 450 } else {
ClementBreteau 15:c2fc239e85df 451 gameEtat = ETAT_GAME_WAIT_ACK;
antbig 11:ed13a480ddca 452 }
ClementBreteau 18:cc5fec34ed9c 453 #ifdef ROBOT_SMALL
ClementBreteau 18:cc5fec34ed9c 454 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
ClementBreteau 18:cc5fec34ed9c 455 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
ClementBreteau 18:cc5fec34ed9c 456 #endif
antbig 11:ed13a480ddca 457 return;
ClementBreteau 18:cc5fec34ed9c 458 #ifdef ROBOT_SMALL
ClementBreteau 18:cc5fec34ed9c 459 } else if (tempo == 2) {
ClementBreteau 18:cc5fec34ed9c 460 // on est dans le cas de l'avance selon le telemetre
ClementBreteau 18:cc5fec34ed9c 461 waitingAckID = ASSERVISSEMENT_RECALAGE;
ClementBreteau 18:cc5fec34ed9c 462 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
ClementBreteau 18:cc5fec34ed9c 463
ClementBreteau 18:cc5fec34ed9c 464 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
ClementBreteau 18:cc5fec34ed9c 465 GoStraight(telemetreDistance, 0, 0, 0);
ClementBreteau 18:cc5fec34ed9c 466 // on reset la distance du telemetre à 0
ClementBreteau 18:cc5fec34ed9c 467 telemetreDistance = 5000;
ClementBreteau 18:cc5fec34ed9c 468 #endif
ClementBreteau 18:cc5fec34ed9c 469 }else{
antbig 0:ad97421fb1fb 470 //C'est un AX12 qu'il faut bouger
ClementBreteau 15:c2fc239e85df 471 //AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
ClementBreteau 15:c2fc239e85df 472 //AX12_enchainement++;
ClementBreteau 14:c8fc06c4887f 473
antbig 0:ad97421fb1fb 474 }
antbig 0:ad97421fb1fb 475 break;
antbig 0:ad97421fb1fb 476 default:
antbig 0:ad97421fb1fb 477 //Instruction inconnue, on l'ignore
antbig 0:ad97421fb1fb 478 break;
antbig 0:ad97421fb1fb 479 }
antbig 0:ad97421fb1fb 480
antbig 0:ad97421fb1fb 481
antbig 0:ad97421fb1fb 482
antbig 0:ad97421fb1fb 483 if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) {
antbig 0:ad97421fb1fb 484 gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge
antbig 0:ad97421fb1fb 485 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 486 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 487 cartesCheker.start();
antbig 1:116040d14164 488 if(AX12_enchainement > 0) {
ClementBreteau 15:c2fc239e85df 489 //AX12_processChange();//Il faut lancer le déplacement des AX12
ClementBreteau 15:c2fc239e85df 490 //AX12_enchainement = 0;
antbig 1:116040d14164 491 }
antbig 0:ad97421fb1fb 492 } else {//C'est un enchainement
ClementBreteau 15:c2fc239e85df 493 if(instruction.order == MV_LINE){
ClementBreteau 15:c2fc239e85df 494 gameEtat = ETAT_GAME_WAIT_ACK;
ClementBreteau 15:c2fc239e85df 495
ClementBreteau 15:c2fc239e85df 496 }else{
ClementBreteau 15:c2fc239e85df 497 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
ClementBreteau 15:c2fc239e85df 498 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
ClementBreteau 15:c2fc239e85df 499 }
antbig 0:ad97421fb1fb 500 }
antbig 0:ad97421fb1fb 501
antbig 0:ad97421fb1fb 502 break;
antbig 0:ad97421fb1fb 503 case ETAT_GAME_WAIT_ACK:
antbig 0:ad97421fb1fb 504 /*
antbig 0:ad97421fb1fb 505 Attente de l'ack de l'instruction
antbig 0:ad97421fb1fb 506 */
ClementBreteau 14:c8fc06c4887f 507 if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continu
antbig 0:ad97421fb1fb 508 //if(true) {
antbig 0:ad97421fb1fb 509 cartesCheker.stop();
antbig 0:ad97421fb1fb 510 if(instruction.nextActionType == JUMP) {
antbig 11:ed13a480ddca 511 if(instruction.jumpAction == JUMP_POSITION) {
antbig 11:ed13a480ddca 512 gameEtat = ETAT_GAME_JUMP_POSITION;
antbig 11:ed13a480ddca 513 } else {//Pour eviter les erreurs, on dit que c'est par défaut un jump time
antbig 0:ad97421fb1fb 514 gameEtat = ETAT_GAME_JUMP_TIME;
antbig 0:ad97421fb1fb 515 cartesCheker.reset();//On reset le timeOut
antbig 11:ed13a480ddca 516 cartesCheker.start();
antbig 0:ad97421fb1fb 517 }
antbig 0:ad97421fb1fb 518 } else if(instruction.nextActionType == WAIT) {
antbig 0:ad97421fb1fb 519 gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
antbig 0:ad97421fb1fb 520 switch(instruction.order)
antbig 0:ad97421fb1fb 521 {
antbig 0:ad97421fb1fb 522 case MV_COURBURE:
antbig 0:ad97421fb1fb 523 waitingAckID = ASSERVISSEMENT_COURBURE;
antbig 0:ad97421fb1fb 524 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 525 break;
antbig 0:ad97421fb1fb 526 case MV_LINE:
antbig 0:ad97421fb1fb 527 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 528 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 529 break;
antbig 0:ad97421fb1fb 530 case MV_TURN:
antbig 0:ad97421fb1fb 531 waitingAckID = ASSERVISSEMENT_ROTATION;
antbig 0:ad97421fb1fb 532 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 533 break;
antbig 0:ad97421fb1fb 534 case MV_XYT:
antbig 0:ad97421fb1fb 535 waitingAckID = ASSERVISSEMENT_XYT;
antbig 0:ad97421fb1fb 536 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 537 break;
antbig 0:ad97421fb1fb 538 case MV_RECALAGE:
antbig 0:ad97421fb1fb 539 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 540 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 541 break;
antbig 0:ad97421fb1fb 542 case ACTION:
ClementBreteau 15:c2fc239e85df 543
ClementBreteau 15:c2fc239e85df 544 if (modeTelemetre == 0){
ClementBreteau 18:cc5fec34ed9c 545 if (telemetreDistance == 0){
ClementBreteau 18:cc5fec34ed9c 546 waitingAckID = SERVO_AX12_ACTION;// instruction.arg1;
ClementBreteau 18:cc5fec34ed9c 547 waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE;
ClementBreteau 18:cc5fec34ed9c 548 }else if(telemetreDistance == 5000){
ClementBreteau 18:cc5fec34ed9c 549 // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre
ClementBreteau 18:cc5fec34ed9c 550 waitingAckID = ASSERVISSEMENT_RECALAGE;
ClementBreteau 18:cc5fec34ed9c 551 waitingAckFrom = INSTRUCTION_END_MOTEUR;
ClementBreteau 18:cc5fec34ed9c 552 telemetreDistance = 0;
ClementBreteau 18:cc5fec34ed9c 553 }
ClementBreteau 15:c2fc239e85df 554 }else{ // si on attend la reponse du telemetre
ClementBreteau 15:c2fc239e85df 555 //modeTelemetre = 1;
ClementBreteau 15:c2fc239e85df 556 waitingAckID = OBJET_SUR_TABLE;
ClementBreteau 15:c2fc239e85df 557 waitingAckFrom = 0;
ClementBreteau 15:c2fc239e85df 558 }
antbig 0:ad97421fb1fb 559 break;
antbig 0:ad97421fb1fb 560 default:
antbig 0:ad97421fb1fb 561 break;
antbig 0:ad97421fb1fb 562 }
antbig 0:ad97421fb1fb 563 } else {
antbig 0:ad97421fb1fb 564 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 0:ad97421fb1fb 565 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 566 }
antbig 0:ad97421fb1fb 567 } else if(cartesCheker.read_ms () > 50){
antbig 0:ad97421fb1fb 568 cartesCheker.stop();
antbig 0:ad97421fb1fb 569 if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur
antbig 0:ad97421fb1fb 570 actual_instruction = instruction.nextLineError;
antbig 0:ad97421fb1fb 571 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 0:ad97421fb1fb 572 } else {
antbig 0:ad97421fb1fb 573 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
antbig 0:ad97421fb1fb 574 }
antbig 0:ad97421fb1fb 575 }
antbig 0:ad97421fb1fb 576 break;
antbig 0:ad97421fb1fb 577
antbig 0:ad97421fb1fb 578 case ETAT_GAME_JUMP_TIME:
antbig 0:ad97421fb1fb 579 if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) {
antbig 0:ad97421fb1fb 580 cartesCheker.stop();//On arrete le timer
antbig 0:ad97421fb1fb 581 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 582 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
antbig 0:ad97421fb1fb 583 }
antbig 0:ad97421fb1fb 584 break;
ClementBreteau 15:c2fc239e85df 585
ClementBreteau 15:c2fc239e85df 586 case ETAT_GAME_JUMP_CONFIG:
ClementBreteau 15:c2fc239e85df 587 signed int depasX = 1, depasY = 1; // servent à indiquer le sens de dépassement des coordonnées
ClementBreteau 15:c2fc239e85df 588 // 1 si l'instruction est plus grande que la position du robot
ClementBreteau 15:c2fc239e85df 589 // -1 si l'instruction est plus petite que la position du robot
ClementBreteau 15:c2fc239e85df 590 // 0 si l'instruction et position du robot sont proche de moins de 1cm
ClementBreteau 15:c2fc239e85df 591 if (abs(x_robot-instruction.JumpTimeOrX)<10){
ClementBreteau 15:c2fc239e85df 592 depasX = 0;
ClementBreteau 15:c2fc239e85df 593 }else if(x_robot > instruction.JumpTimeOrX){
ClementBreteau 15:c2fc239e85df 594 depasX = -1;
ClementBreteau 15:c2fc239e85df 595 }
ClementBreteau 15:c2fc239e85df 596
ClementBreteau 15:c2fc239e85df 597 if(abs(y_robot-instruction.JumpY)<10){
ClementBreteau 15:c2fc239e85df 598 depasY = 0;
ClementBreteau 15:c2fc239e85df 599 }else if(y_robot > instruction.JumpY){
ClementBreteau 15:c2fc239e85df 600 depasY = -1;
ClementBreteau 15:c2fc239e85df 601 }
ClementBreteau 15:c2fc239e85df 602
ClementBreteau 15:c2fc239e85df 603 gameEtat = ETAT_GAME_JUMP_POSITION;
ClementBreteau 15:c2fc239e85df 604 break;
antbig 0:ad97421fb1fb 605 case ETAT_GAME_JUMP_POSITION:
ClementBreteau 15:c2fc239e85df 606 bool Xok = false, Yok = false;
ClementBreteau 15:c2fc239e85df 607
ClementBreteau 15:c2fc239e85df 608 if (depasX == 0){
ClementBreteau 15:c2fc239e85df 609 Xok = true;
ClementBreteau 15:c2fc239e85df 610 }else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5){
ClementBreteau 15:c2fc239e85df 611 Xok = true;
ClementBreteau 15:c2fc239e85df 612 }
ClementBreteau 15:c2fc239e85df 613
ClementBreteau 15:c2fc239e85df 614 if (depasY == 0){
ClementBreteau 15:c2fc239e85df 615 Yok = true;
ClementBreteau 15:c2fc239e85df 616 }else if ((instruction.JumpY - y_robot)*depasY < -5){
ClementBreteau 15:c2fc239e85df 617 Yok = true;
ClementBreteau 15:c2fc239e85df 618 }
ClementBreteau 15:c2fc239e85df 619
ClementBreteau 15:c2fc239e85df 620 // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante
ClementBreteau 15:c2fc239e85df 621 if (Xok && Yok){
ClementBreteau 15:c2fc239e85df 622 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
ClementBreteau 15:c2fc239e85df 623 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
ClementBreteau 15:c2fc239e85df 624 }
ClementBreteau 15:c2fc239e85df 625
antbig 0:ad97421fb1fb 626 break;
antbig 0:ad97421fb1fb 627 case ETAT_GAME_WAIT_END_INSTRUCTION:
antbig 0:ad97421fb1fb 628 if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée
antbig 0:ad97421fb1fb 629 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 630 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
antbig 0:ad97421fb1fb 631 }
antbig 0:ad97421fb1fb 632 break;
antbig 0:ad97421fb1fb 633
antbig 0:ad97421fb1fb 634
antbig 5:dcd817534b57 635 case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s
antbig 12:14729d584500 636 if(timeoutWarning.read_ms() >= BALISE_TIMEOUT)//ça fait plus de 2s, il faut changer de stratégie
antbig 5:dcd817534b57 637 {
antbig 5:dcd817534b57 638 gameEtat = ETAT_WARNING_SWITCH_STRATEGIE;
antbig 5:dcd817534b57 639 }
antbig 5:dcd817534b57 640 break;
antbig 5:dcd817534b57 641 case ETAT_WARING_END_BALISE_WAIT://Attente d'une seconde apres la fin d'un End Balise pour etre sur que c'est bon
antbig 12:14729d584500 642 if(timeoutWarningWaitEnd.read_ms() >= 1000) {//c'est bon, on repart
antbig 12:14729d584500 643 //actual_instruction = instruction.nextLineError;
antbig 12:14729d584500 644 gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
antbig 12:14729d584500 645 }
antbig 5:dcd817534b57 646 break;
antbig 5:dcd817534b57 647 case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours
ClementBreteau 25:f140c93a8666 648 /*
antbig 12:14729d584500 649 #ifdef ROBOT_BIG
ClementBreteau 15:c2fc239e85df 650 actual_instruction = instruction.nextLineError;// 2 //Modification directe... c'est pas bien mais ça marchait pour le match 5
antbig 12:14729d584500 651 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
ClementBreteau 20:de595e4ff01d 652 #else
antbig 12:14729d584500 653 actual_instruction = instruction.nextLineError;
antbig 12:14729d584500 654 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
ClementBreteau 20:de595e4ff01d 655 #endif
ClementBreteau 25:f140c93a8666 656 gameEtat = ETAT_END;*/
ClementBreteau 21:590cdacb6a35 657
ClementBreteau 25:f140c93a8666 658 switch(actionPrecedente){
ClementBreteau 25:f140c93a8666 659 case MV_LINE:
antbig 27:76ead555a63d 660
ClementBreteau 25:f140c93a8666 661 if(instruction.direction == BACKWARD) {
ClementBreteau 25:f140c93a8666 662 localData1 = -1;
ClementBreteau 25:f140c93a8666 663 } else {
ClementBreteau 25:f140c93a8666 664 localData1 = 1;
ClementBreteau 25:f140c93a8666 665 }
antbig 27:76ead555a63d 666
ClementBreteau 25:f140c93a8666 667 GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
antbig 27:76ead555a63d 668 debugXYTTarget(target_x_robot,target_y_robot,target_theta_robot);
antbig 27:76ead555a63d 669 waitingAckID = ASSERVISSEMENT_XYT;
antbig 27:76ead555a63d 670 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 27:76ead555a63d 671 gameEtat = ETAT_GAME_WAIT_ACK;
antbig 27:76ead555a63d 672 instruction.order = MV_XYT;
antbig 27:76ead555a63d 673 return;
ClementBreteau 25:f140c93a8666 674
ClementBreteau 25:f140c93a8666 675 case MV_XYT:
antbig 27:76ead555a63d 676 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
ClementBreteau 25:f140c93a8666 677 break;
ClementBreteau 25:f140c93a8666 678
ClementBreteau 26:2f4fcc2354f3 679 case MV_COURBURE:
ClementBreteau 26:2f4fcc2354f3 680 if(instruction.direction == LEFT){
ClementBreteau 26:2f4fcc2354f3 681 target_theta_robot = target_theta_robot - theta_robot;
ClementBreteau 26:2f4fcc2354f3 682 }else{
ClementBreteau 26:2f4fcc2354f3 683 target_theta_robot = target_theta_robot + theta_robot;
ClementBreteau 26:2f4fcc2354f3 684 }
ClementBreteau 26:2f4fcc2354f3 685 BendRadius(target_theta_robot, localData2, localData1, localData5);
ClementBreteau 25:f140c93a8666 686 break;
antbig 27:76ead555a63d 687 default:
antbig 27:76ead555a63d 688 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 27:76ead555a63d 689 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 27:76ead555a63d 690 break;
ClementBreteau 25:f140c93a8666 691 }
ClementBreteau 25:f140c93a8666 692
antbig 27:76ead555a63d 693 //actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 27:76ead555a63d 694 //gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 5:dcd817534b57 695 break;
antbig 5:dcd817534b57 696 case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur
antbig 5:dcd817534b57 697 actual_instruction = instruction.nextLineError;
antbig 5:dcd817534b57 698 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 12:14729d584500 699 ingnorBaliseOnce = 1;
antbig 5:dcd817534b57 700 break;
ClementBreteau 20:de595e4ff01d 701
ClementBreteau 20:de595e4ff01d 702
ClementBreteau 20:de595e4ff01d 703
ClementBreteau 14:c8fc06c4887f 704 case ETAT_END:
ClementBreteau 14:c8fc06c4887f 705 if (ModeDemo){
ClementBreteau 14:c8fc06c4887f 706 gameEtat = ETAT_CHECK_CARTE_SCREEN;
ClementBreteau 14:c8fc06c4887f 707 ModeDemo = 1;
ClementBreteau 14:c8fc06c4887f 708 } else {
ClementBreteau 14:c8fc06c4887f 709 gameEtat = ETAT_END_LOOP;
ClementBreteau 14:c8fc06c4887f 710 }
antbig 0:ad97421fb1fb 711 break;
antbig 0:ad97421fb1fb 712 case ETAT_END_LOOP:
antbig 0:ad97421fb1fb 713 //Rien, on tourne en rond
ClementBreteau 18:cc5fec34ed9c 714
antbig 0:ad97421fb1fb 715 break;
antbig 0:ad97421fb1fb 716 default:
ClementBreteau 20:de595e4ff01d 717
antbig 0:ad97421fb1fb 718 break;
ClementBreteau 20:de595e4ff01d 719 }
ClementBreteau 20:de595e4ff01d 720 }
ClementBreteau 20:de595e4ff01d 721
antbig 4:88431b537477 722 /****************************************************************************************/
antbig 4:88431b537477 723 /* FUNCTION NAME: canProcessRx */
antbig 4:88431b537477 724 /* DESCRIPTION : Fonction de traitement des messages CAN */
antbig 4:88431b537477 725 /****************************************************************************************/
antbig 0:ad97421fb1fb 726 void canProcessRx(void)
ClementBreteau 20:de595e4ff01d 727 {
antbig 0:ad97421fb1fb 728 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
antbig 4:88431b537477 729 CANMessage msgTx=CANMessage();
antbig 0:ad97421fb1fb 730 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
antbig 0:ad97421fb1fb 731 if(FIFO_occupation<0)
antbig 0:ad97421fb1fb 732 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
antbig 0:ad97421fb1fb 733 if(FIFO_max_occupation<FIFO_occupation)
antbig 0:ad97421fb1fb 734 FIFO_max_occupation=FIFO_occupation;
antbig 0:ad97421fb1fb 735 if(FIFO_occupation!=0) {
antbig 0:ad97421fb1fb 736
antbig 0:ad97421fb1fb 737 switch(msgRxBuffer[FIFO_lecture].id) {
antbig 4:88431b537477 738 case DEBUG_FAKE_JAKE://Permet de lancer le match à distance
ClementBreteau 23:ab87d308eaf9 739 case GLOBAL_JACK:
antbig 1:116040d14164 740 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
antbig 1:116040d14164 741 gameEtat = ETAT_GAME_START;
ClementBreteau 23:ab87d308eaf9 742 SendRawId(ACKNOWLEDGE_JACK);
antbig 1:116040d14164 743 }
antbig 1:116040d14164 744 break;
antbig 1:116040d14164 745
antbig 0:ad97421fb1fb 746 case ALIVE_BALISE:
antbig 0:ad97421fb1fb 747 case ALIVE_MOTEUR:
antbig 0:ad97421fb1fb 748 case ALIVE_IHM:
ClementBreteau 23:ab87d308eaf9 749 case ALIVE_ACTIONNEURS_AVANT:
ClementBreteau 23:ab87d308eaf9 750 case ALIVE_ACTIONNEURS_ARRIERE:
antbig 12:14729d584500 751 case ALIVE_AX12:
antbig 0:ad97421fb1fb 752 case ECRAN_ALL_CHECK:
antbig 0:ad97421fb1fb 753 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
antbig 0:ad97421fb1fb 754 waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
antbig 0:ad97421fb1fb 755 }
ClementBreteau 15:c2fc239e85df 756 break;
antbig 0:ad97421fb1fb 757
antbig 0:ad97421fb1fb 758 case ACKNOWLEDGE_BALISE:
antbig 0:ad97421fb1fb 759 case ACKNOWLEDGE_MOTEUR:
antbig 0:ad97421fb1fb 760 case ACKNOWLEDGE_IHM:
ClementBreteau 14:c8fc06c4887f 761 case ACKNOWLEDGE_TELEMETRE:
ClementBreteau 15:c2fc239e85df 762 case ACKNOWLEDGE_AX12:
antbig 0:ad97421fb1fb 763 case INSTRUCTION_END_BALISE:
antbig 0:ad97421fb1fb 764 case INSTRUCTION_END_MOTEUR:
antbig 0:ad97421fb1fb 765 case INSTRUCTION_END_IHM:
ClementBreteau 15:c2fc239e85df 766 case INSTRUCTION_END_AX12:
ClementBreteau 18:cc5fec34ed9c 767
antbig 11:ed13a480ddca 768 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) {
antbig 0:ad97421fb1fb 769 waitingAckFrom = 0;
antbig 0:ad97421fb1fb 770 waitingAckID = 0;
antbig 0:ad97421fb1fb 771 }
antbig 0:ad97421fb1fb 772 break;
antbig 1:116040d14164 773 #ifdef ROBOT_BIG
antbig 0:ad97421fb1fb 774 case ODOMETRIE_BIG_POSITION:
antbig 0:ad97421fb1fb 775 #else
antbig 0:ad97421fb1fb 776 case ODOMETRIE_SMALL_POSITION:
antbig 0:ad97421fb1fb 777 #endif
antbig 0:ad97421fb1fb 778 x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
antbig 0:ad97421fb1fb 779 y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
antbig 0:ad97421fb1fb 780 theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
antbig 0:ad97421fb1fb 781 break;
antbig 0:ad97421fb1fb 782
antbig 0:ad97421fb1fb 783 case ECRAN_START_MATCH:
antbig 0:ad97421fb1fb 784 if(gameEtat == ETAT_CONFIG) {
antbig 1:116040d14164 785 gameEtat = ETAT_GAME_INIT;
antbig 0:ad97421fb1fb 786 }
antbig 0:ad97421fb1fb 787 break;
antbig 0:ad97421fb1fb 788 case SERVO_AX12_SETGOAL:
antbig 9:d0042422d95a 789 //SendAck(0x114, SERVO_AX12_SETGOAL);
ClementBreteau 14:c8fc06c4887f 790 //if(AX12_isLocal(msgRxBuffer[FIFO_lecture].data[0]))
ClementBreteau 14:c8fc06c4887f 791 //AX12_setGoal(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[2])<<8), msgRxBuffer[FIFO_lecture].data[3]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[4])<<8));
antbig 5:dcd817534b57 792
antbig 0:ad97421fb1fb 793 break;
ClementBreteau 14:c8fc06c4887f 794
antbig 0:ad97421fb1fb 795 case SERVO_AX12_PROCESS:
ClementBreteau 14:c8fc06c4887f 796 SendAck(0x114, SERVO_AX12_PROCESS);
ClementBreteau 14:c8fc06c4887f 797 //AX12_processChange(1);
antbig 0:ad97421fb1fb 798 break;
ClementBreteau 14:c8fc06c4887f 799
antbig 1:116040d14164 800 case SERVO_AX12_DONE:
ClementBreteau 15:c2fc239e85df 801 SendRawId(POMPE_PWM);
ClementBreteau 14:c8fc06c4887f 802 /*//SendAck(0x114, SERVO_AX12_DONE);
antbig 1:116040d14164 803 AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0])));
ClementBreteau 14:c8fc06c4887f 804
ClementBreteau 15:c2fc239e85df 805 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
ClementBreteau 14:c8fc06c4887f 806 waitingAckFrom = 0;
ClementBreteau 14:c8fc06c4887f 807 waitingAckID = 0;*/
ClementBreteau 14:c8fc06c4887f 808
antbig 1:116040d14164 809 break;
antbig 2:8d8e2cf798a3 810 case ECRAN_CHOICE_COLOR://Choix de la couleur
antbig 4:88431b537477 811 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
antbig 2:8d8e2cf798a3 812 if(msgRxBuffer[FIFO_lecture].data[0] == 0)
antbig 2:8d8e2cf798a3 813 InversStrat = 0;//Pas d'inversion de la couleur
antbig 2:8d8e2cf798a3 814 else
antbig 2:8d8e2cf798a3 815 InversStrat = 1;//Inversion de la couleur
antbig 4:88431b537477 816
antbig 4:88431b537477 817 msgTx.id=ECRAN_ACK_COLOR; // tx ack de la couleur
antbig 4:88431b537477 818 msgTx.len=1;
antbig 4:88431b537477 819 msgTx.format=CANStandard;
antbig 4:88431b537477 820 msgTx.type=CANData;
antbig 4:88431b537477 821 // couleur sur 1 octet
antbig 4:88431b537477 822 msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0];
antbig 4:88431b537477 823 can1.write(msgTx);
antbig 9:d0042422d95a 824
antbig 2:8d8e2cf798a3 825 }
antbig 2:8d8e2cf798a3 826 break;
ClementBreteau 14:c8fc06c4887f 827
antbig 2:8d8e2cf798a3 828 case ECRAN_CHOICE_STRAT://Choix du fichier de stratégie à utiliser
antbig 4:88431b537477 829 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
antbig 4:88431b537477 830 msgTx.id=ECRAN_ACK_STRAT; // tx ack de la couleur
antbig 4:88431b537477 831 msgTx.len=1;
antbig 4:88431b537477 832 msgTx.format=CANStandard;
antbig 4:88431b537477 833 msgTx.type=CANData;
antbig 4:88431b537477 834 if(SelectStrategy(msgRxBuffer[FIFO_lecture].data[0])) {
antbig 4:88431b537477 835 // id de la stratégie sur 1 octet
ClementBreteau 16:7321fb3bb396 836 if (msgRxBuffer[FIFO_lecture].data[0] < 0x10){ // Si la strat est une strat de match, on desactive le mode demo
ClementBreteau 14:c8fc06c4887f 837 ModeDemo = 0;
ClementBreteau 16:7321fb3bb396 838 } else { // sinon, on active le mode demo, utile pour la fin de la demo
ClementBreteau 14:c8fc06c4887f 839 ModeDemo = 1;
ClementBreteau 14:c8fc06c4887f 840 }
ClementBreteau 14:c8fc06c4887f 841
antbig 4:88431b537477 842 msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0];
antbig 4:88431b537477 843 } else {
antbig 4:88431b537477 844 //erreur sur 1 octet
antbig 4:88431b537477 845 msgTx.data[0]=0;
antbig 4:88431b537477 846 }
antbig 4:88431b537477 847 can1.write(msgTx);
antbig 9:d0042422d95a 848 wait_ms(10);
antbig 27:76ead555a63d 849 //setAsservissementEtat(0);//Désactivation de l'asservissement pour repositionner le robot dans le zone de départ
antbig 12:14729d584500 850 tactile_printf("Strat %d, Asser desactive",msgTx.data[0]);
antbig 2:8d8e2cf798a3 851 }
antbig 2:8d8e2cf798a3 852 break;
ClementBreteau 16:7321fb3bb396 853 case BALISE_DANGER :
ClementBreteau 16:7321fb3bb396 854 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
ClementBreteau 16:7321fb3bb396 855 break;
ClementBreteau 16:7321fb3bb396 856
antbig 5:dcd817534b57 857 case BALISE_STOP:
antbig 5:dcd817534b57 858 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
antbig 27:76ead555a63d 859 //if (instruction[actual_instruction].order != MV_TURN && instruction[actual_instruction].order != ACTION ){ //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot,
ClementBreteau 16:7321fb3bb396 860 if(needToStop() != 0 && ingnorBaliseOnce ==0) {
ClementBreteau 16:7321fb3bb396 861 if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
ClementBreteau 16:7321fb3bb396 862 {
ClementBreteau 16:7321fb3bb396 863 SendRawId(ASSERVISSEMENT_STOP);
ClementBreteau 26:2f4fcc2354f3 864 //while(1); // ligne à décommenter si on est en homologation
ClementBreteau 16:7321fb3bb396 865 if(gameEtat != ETAT_WARING_END_BALISE_WAIT) {
ClementBreteau 16:7321fb3bb396 866 timeoutWarning.reset();
ClementBreteau 16:7321fb3bb396 867 timeoutWarning.start();//Reset du timer utiliser par le timeout
ClementBreteau 16:7321fb3bb396 868 }
antbig 27:76ead555a63d 869 gameEtat = ETAT_WARNING_TIMEOUT;
antbig 12:14729d584500 870 }
antbig 12:14729d584500 871 }
ClementBreteau 18:cc5fec34ed9c 872 //}
antbig 12:14729d584500 873 ingnorBaliseOnce = 0;
antbig 5:dcd817534b57 874 break;
ClementBreteau 16:7321fb3bb396 875
antbig 5:dcd817534b57 876 case BALISE_END_DANGER:
ClementBreteau 16:7321fb3bb396 877 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
antbig 5:dcd817534b57 878 if(gameEtat == ETAT_WARNING_TIMEOUT) {
antbig 5:dcd817534b57 879 timeoutWarningWaitEnd.reset();
antbig 5:dcd817534b57 880 timeoutWarningWaitEnd.start();
antbig 5:dcd817534b57 881 gameEtat = ETAT_WARING_END_BALISE_WAIT;
antbig 5:dcd817534b57 882 }
antbig 5:dcd817534b57 883 break;
antbig 12:14729d584500 884
antbig 12:14729d584500 885 case ECRAN_CHOICE_START_ACTION:
antbig 12:14729d584500 886 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
antbig 12:14729d584500 887 if(msgRxBuffer[FIFO_lecture].data[0] == 1) {
antbig 12:14729d584500 888 runRobotTest();
antbig 12:14729d584500 889 } else {
antbig 12:14729d584500 890 initRobotActionneur();
antbig 12:14729d584500 891 }
antbig 12:14729d584500 892 wait_ms(500);
antbig 12:14729d584500 893 SendRawId(ECRAN_ACK_CHOICE_START_ACTION);
antbig 12:14729d584500 894 }
antbig 12:14729d584500 895 break;
ClementBreteau 14:c8fc06c4887f 896
ClementBreteau 14:c8fc06c4887f 897 case OBJET_SUR_TABLE:
ClementBreteau 15:c2fc239e85df 898 if (msgRxBuffer[FIFO_lecture].data[1] == 0xff){
ClementBreteau 15:c2fc239e85df 899
ClementBreteau 15:c2fc239e85df 900 gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
ClementBreteau 15:c2fc239e85df 901 }
ClementBreteau 15:c2fc239e85df 902 else{
ClementBreteau 15:c2fc239e85df 903
ClementBreteau 15:c2fc239e85df 904 waitingAckFrom = 0;
ClementBreteau 15:c2fc239e85df 905 waitingAckID = 0;
ClementBreteau 15:c2fc239e85df 906
ClementBreteau 15:c2fc239e85df 907 strat_instructions[actual_instruction+1].arg1 = returnX(strat_instructions[actual_instruction].arg2);
ClementBreteau 15:c2fc239e85df 908 strat_instructions[actual_instruction+1].arg2 = returnY(strat_instructions[actual_instruction].arg2);
ClementBreteau 15:c2fc239e85df 909 }
ClementBreteau 15:c2fc239e85df 910 modeTelemetre = 0;
ClementBreteau 14:c8fc06c4887f 911 break;
antbig 0:ad97421fb1fb 912 }
antbig 0:ad97421fb1fb 913
antbig 0:ad97421fb1fb 914 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
antbig 0:ad97421fb1fb 915 }
antbig 0:ad97421fb1fb 916 }