CRAC Team / Mbed 2 deprecated carte_strategie_2019

carte_strategie_2019

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@28:acd18776ed2d, 2017-05-27 (annotated)

Committer:
antbig
Date:
Sat May 27 05:40:26 2017 +0000
Revision:
28:acd18776ed2d
Parent:
27:76ead555a63d
Child:
29:41e02746041d
version pour du 27/05 07h40

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