CRAC Team / CRAC-Strat_2017_homologation_gros_rob_testAck

homologation gros robot et test avec les ack de la carte a tout faire

Fork of CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT by CRAC Team

Strategie/Strategie.cpp@12:14729d584500, 2016-05-09 (annotated)

Committer:
antbig
Date:
Mon May 09 09:10:17 2016 +0000
Revision:
12:14729d584500
Parent:
11:ed13a480ddca
Child:
14:c8fc06c4887f
1Version utilis? lors du match 5

Who changed what in which revision?

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