CRAC Team / CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of CRAC-Strat_2017_V2 by CRAC Team

Strategie/Strategie.cpp@9:d0042422d95a, 2016-04-28 (annotated)

Committer:
antbig
Date:
Thu Apr 28 08:11:36 2016 +0000
Revision:
9:d0042422d95a
Parent:
8:0edc7dfb7f7e
Child:
10:a788d9cf60f2
ajout message choix id strat

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