CRAC Team / CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT

code petit robot pour homologation

Fork of CRAC-Strat_2017_V2 by CRAC Team

Strategie/Strategie.cpp@14:c8fc06c4887f, 2017-03-31 (annotated)

Committer:
ClementBreteau
Date:
Fri Mar 31 16:20:26 2017 +0000
Revision:
14:c8fc06c4887f
Parent:
12:14729d584500
Child:
15:c2fc239e85df
code strategie du robot, version 31 mars 2017

Who changed what in which revision?

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