CRAC Team / CRAC-Strat_2017

pour trouver l'automate de reception can

Fork of CRAC-Strat_copy by Clement Breteau

Strategie/Strategie.cpp@1:116040d14164, 2016-04-15 (annotated)

Committer:
antbig
Date:
Fri Apr 15 10:49:40 2016 +0000
Revision:
1:116040d14164
Parent:
0:ad97421fb1fb
Child:
2:8d8e2cf798a3
Premier test fonctionnel avec le petit robot,

Who changed what in which revision?

UserRevisionLine numberNew contents of line
antbig 0:ad97421fb1fb 1 #include "Strategie.h"
antbig 0:ad97421fb1fb 2
antbig 0:ad97421fb1fb 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 0:ad97421fb1fb 10 Timeout chronoEnd;//permet d'envoyer la trame CAN pour la fin
antbig 0:ad97421fb1fb 11
antbig 0:ad97421fb1fb 12 unsigned short waitingAckID = 0;//L'id du ack attendu
antbig 0:ad97421fb1fb 13 unsigned short waitingAckFrom = 0;//La provenance du ack attendu
antbig 0:ad97421fb1fb 14
antbig 0:ad97421fb1fb 15 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
antbig 0:ad97421fb1fb 16
antbig 0:ad97421fb1fb 17 signed short x_robot,y_robot,theta_robot;//La position du robot
antbig 0:ad97421fb1fb 18
antbig 0:ad97421fb1fb 19 unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR};
antbig 0:ad97421fb1fb 20 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR};
antbig 0:ad97421fb1fb 21 unsigned char checkCurrent = 0;
antbig 0:ad97421fb1fb 22 unsigned char countAliveCard = 0;
antbig 0:ad97421fb1fb 23
antbig 1:116040d14164 24 unsigned char InversStrat = 0;//Si à 1, indique que l'on part de l'autre cote de la table(inversion des Y)
antbig 1:116040d14164 25
antbig 0:ad97421fb1fb 26
antbig 0:ad97421fb1fb 27 void chronometre_ISR (void)
antbig 0:ad97421fb1fb 28 {
antbig 0:ad97421fb1fb 29 SendRawId(ASSERVISSEMENT_STOP);//On stope les moteurs
antbig 0:ad97421fb1fb 30 SendRawId(GLOBAL_GAME_END);//Indication fin de match
antbig 0:ad97421fb1fb 31 gameTimer.stop();//Arret du timer
antbig 0:ad97421fb1fb 32
antbig 1:116040d14164 33 #ifdef ROBOT_BIG
antbig 0:ad97421fb1fb 34 doFunnyAction();
antbig 0:ad97421fb1fb 35 #endif
antbig 0:ad97421fb1fb 36
antbig 1:116040d14164 37 while(1);//On bloque la programme dans l'interruption
antbig 0:ad97421fb1fb 38 }
antbig 0:ad97421fb1fb 39
antbig 0:ad97421fb1fb 40 void automate_process(void)
antbig 0:ad97421fb1fb 41 {
antbig 0:ad97421fb1fb 42 static struct S_Instruction instruction;
antbig 1:116040d14164 43 static unsigned char AX12_enchainement = 0;
antbig 1:116040d14164 44 static unsigned char MV_enchainement = 0;
antbig 0:ad97421fb1fb 45 signed char localData1 = 0;
antbig 0:ad97421fb1fb 46 signed short localData2 = 0;
antbig 0:ad97421fb1fb 47 unsigned short localData3 = 0;
antbig 0:ad97421fb1fb 48 signed short localData4 = 0;
antbig 1:116040d14164 49 unsigned char localData5 = 0;
antbig 0:ad97421fb1fb 50
antbig 0:ad97421fb1fb 51 if(gameTimer.read_ms() >= 88000) {//Fin du match (On autorise 2s pour déposer des éléments
antbig 0:ad97421fb1fb 52 gameTimer.stop();
antbig 0:ad97421fb1fb 53 gameTimer.reset();
antbig 0:ad97421fb1fb 54 gameEtat = ETAT_END;//Fin du temps
antbig 0:ad97421fb1fb 55 }
antbig 0:ad97421fb1fb 56
antbig 0:ad97421fb1fb 57 if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) {
antbig 0:ad97421fb1fb 58 lastEtat = gameEtat;
antbig 0:ad97421fb1fb 59 debugetatTimer.reset();
antbig 0:ad97421fb1fb 60 sendStratEtat((unsigned char)gameEtat);
antbig 0:ad97421fb1fb 61 }
antbig 0:ad97421fb1fb 62
antbig 0:ad97421fb1fb 63 switch(gameEtat)
antbig 0:ad97421fb1fb 64 {
antbig 0:ad97421fb1fb 65 case ETAT_CHECK_CARTE_SCREEN:
antbig 0:ad97421fb1fb 66 /*
antbig 0:ad97421fb1fb 67 Verification de l'état de la carte ecran
antbig 0:ad97421fb1fb 68 */
antbig 0:ad97421fb1fb 69 waitingAckFrom = ALIVE_IHM;//On indique que l'on attend un ack de la carte IHM
antbig 0:ad97421fb1fb 70 SendRawId(CHECK_IHM);//On demande à la carte IHM d'insiquer ça présence
antbig 0:ad97421fb1fb 71
antbig 0:ad97421fb1fb 72 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 73 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 74 cartesCheker.start();//On lance le timer pour le timeout
antbig 0:ad97421fb1fb 75 gameEtat = ETAT_CHECK_CARTE_SCREEN_WAIT_ACK;
antbig 0:ad97421fb1fb 76
antbig 0:ad97421fb1fb 77 break;
antbig 0:ad97421fb1fb 78 case ETAT_CHECK_CARTE_SCREEN_WAIT_ACK:
antbig 0:ad97421fb1fb 79 /*
antbig 0:ad97421fb1fb 80 Attente du ALIVE de la carte écran.
antbig 0:ad97421fb1fb 81
antbig 0:ad97421fb1fb 82 Si la carte ne répond pas apres 10ms, on retoune dans l'etat ETAT_CHECK_CARTE_SCREEN
antbig 0:ad97421fb1fb 83 maximum 3 tentatives
antbig 0:ad97421fb1fb 84 Si pas de réponse, clignotement de toutes les leds possible
antbig 0:ad97421fb1fb 85 */
antbig 0:ad97421fb1fb 86 if(waitingAckFrom == 0) {//C'est bon la carte est en ligne
antbig 0:ad97421fb1fb 87 cartesCheker.stop();
antbig 0:ad97421fb1fb 88 screenChecktry = 0;
antbig 0:ad97421fb1fb 89 gameEtat = ETAT_CHECK_CARTES;
antbig 0:ad97421fb1fb 90 } else if(cartesCheker.read_ms () > 100) {
antbig 0:ad97421fb1fb 91 cartesCheker.stop();
antbig 0:ad97421fb1fb 92 if(screenChecktry >=3) {
antbig 0:ad97421fb1fb 93 errorLoop();//Erreur La carte IHM n'est pas en ligne
antbig 0:ad97421fb1fb 94 } else {
antbig 0:ad97421fb1fb 95 gameEtat = ETAT_CHECK_CARTE_SCREEN;
antbig 0:ad97421fb1fb 96 }
antbig 0:ad97421fb1fb 97 }
antbig 0:ad97421fb1fb 98 break;
antbig 0:ad97421fb1fb 99 case ETAT_CHECK_CARTES:
antbig 0:ad97421fb1fb 100 /*
antbig 0:ad97421fb1fb 101 Il faut faire une boucle pour verifier toutes les cartes les une apres les autres
antbig 0:ad97421fb1fb 102 */
antbig 0:ad97421fb1fb 103 waitingAckFrom = id_alive[checkCurrent];//On indique que l'on attend un ack de la carte IHM
antbig 0:ad97421fb1fb 104 SendRawId(id_check[checkCurrent]);//On demande à la carte IHM d'insiquer ça présence
antbig 0:ad97421fb1fb 105
antbig 0:ad97421fb1fb 106 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 107 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 108 cartesCheker.start();//On lance le timer pour le timeout
antbig 0:ad97421fb1fb 109 gameEtat = ETAT_CHECK_CARTES_WAIT_ACK;
antbig 0:ad97421fb1fb 110 break;
antbig 0:ad97421fb1fb 111 case ETAT_CHECK_CARTES_WAIT_ACK:
antbig 0:ad97421fb1fb 112 /*
antbig 0:ad97421fb1fb 113 On attend l'ack de la carte en cours de vérification
antbig 0:ad97421fb1fb 114 */
antbig 0:ad97421fb1fb 115 //printf("cartesCheker = %d waitingAckFrom = %d\n",cartesCheker.read_ms(), waitingAckFrom);
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 countAliveCard++;
antbig 0:ad97421fb1fb 120 if(checkCurrent >= NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 121 if(countAliveCard >= NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 122 gameEtat = ETAT_CONFIG;
antbig 0:ad97421fb1fb 123 SendRawId(ECRAN_ALL_CHECK);//On dit à l'IHM que toutes les cartes sont en ligne
antbig 0:ad97421fb1fb 124 } else {
antbig 0:ad97421fb1fb 125 gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
antbig 0:ad97421fb1fb 126 waitingAckFrom = ECRAN_ALL_CHECK;
antbig 0:ad97421fb1fb 127 }
antbig 0:ad97421fb1fb 128 } else {
antbig 0:ad97421fb1fb 129 checkCurrent++;
antbig 0:ad97421fb1fb 130 gameEtat = ETAT_CHECK_CARTES;
antbig 0:ad97421fb1fb 131 }
antbig 0:ad97421fb1fb 132 } else if(cartesCheker.read_ms () > 100) {
antbig 0:ad97421fb1fb 133 cartesCheker.stop();
antbig 0:ad97421fb1fb 134 if(screenChecktry >=3) {
antbig 0:ad97421fb1fb 135 screenChecktry = 0;
antbig 0:ad97421fb1fb 136 if(checkCurrent >= NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 137 if(countAliveCard == NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 138 gameEtat = ETAT_CONFIG;
antbig 0:ad97421fb1fb 139 SendRawId(ECRAN_ALL_CHECK);//On dit à l'IHM que toutes les cartes sont en ligne
antbig 0:ad97421fb1fb 140 } else {
antbig 0:ad97421fb1fb 141 gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
antbig 0:ad97421fb1fb 142 waitingAckFrom = ECRAN_ALL_CHECK;
antbig 0:ad97421fb1fb 143 }
antbig 0:ad97421fb1fb 144 } else {
antbig 0:ad97421fb1fb 145 checkCurrent++;
antbig 0:ad97421fb1fb 146 gameEtat = ETAT_CHECK_CARTES;
antbig 0:ad97421fb1fb 147 }
antbig 0:ad97421fb1fb 148 } else {
antbig 0:ad97421fb1fb 149 gameEtat = ETAT_CHECK_CARTES;
antbig 0:ad97421fb1fb 150 }
antbig 0:ad97421fb1fb 151 }
antbig 0:ad97421fb1fb 152 break;
antbig 0:ad97421fb1fb 153 case ETAT_WAIT_FORCE:
antbig 0:ad97421fb1fb 154 /*
antbig 0:ad97421fb1fb 155 Attente du forçage de la part de la carte IHM
antbig 0:ad97421fb1fb 156 */
antbig 0:ad97421fb1fb 157 if(waitingAckFrom == 0) {
antbig 0:ad97421fb1fb 158 gameEtat = ETAT_CONFIG;
antbig 0:ad97421fb1fb 159 }
antbig 0:ad97421fb1fb 160 break;
antbig 0:ad97421fb1fb 161 case ETAT_CONFIG:
antbig 0:ad97421fb1fb 162 /*
antbig 0:ad97421fb1fb 163 Attente de l'odre de choix de mode,
antbig 0:ad97421fb1fb 164 Il est possible de modifier la couleur et l'id de la stratégie
antbig 0:ad97421fb1fb 165 Il est aussi possible d'envoyer les ordres de debug
antbig 0:ad97421fb1fb 166 */
antbig 0:ad97421fb1fb 167 break;
antbig 1:116040d14164 168 case ETAT_GAME_INIT:
antbig 0:ad97421fb1fb 169 //On charge la liste des instructions
antbig 0:ad97421fb1fb 170 strcpy(cheminFileStart,"/local/test.txt");//On ouvre le fichier test.txt
antbig 0:ad97421fb1fb 171 loadAllInstruction();//Mise en cache de toute les instructions
antbig 0:ad97421fb1fb 172 gameEtat = ETAT_GAME_WAIT_FOR_JACK;
antbig 0:ad97421fb1fb 173 SendRawId(ECRAN_ACK_START_MATCH);
antbig 0:ad97421fb1fb 174 tactile_printf("Attente du JACK.");
antbig 0:ad97421fb1fb 175 break;
antbig 0:ad97421fb1fb 176 case ETAT_GAME_WAIT_FOR_JACK:
antbig 0:ad97421fb1fb 177 //TODO Attendre le jack
antbig 1:116040d14164 178 break;
antbig 1:116040d14164 179 case ETAT_GAME_START:
antbig 1:116040d14164 180 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 1:116040d14164 181 gameTimer.reset();
antbig 1:116040d14164 182 gameTimer.start();
antbig 1:116040d14164 183 chronoEnd.attach(&chronometre_ISR,90);
antbig 0:ad97421fb1fb 184 break;
antbig 0:ad97421fb1fb 185 case ETAT_GAME_LOAD_NEXT_INSTRUCTION:
antbig 0:ad97421fb1fb 186 /*
antbig 0:ad97421fb1fb 187 Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction
antbig 0:ad97421fb1fb 188 */
antbig 0:ad97421fb1fb 189 //printf("load next instruction\n");
antbig 0:ad97421fb1fb 190 if(actual_instruction >= nb_instructions || actual_instruction == 255) {
antbig 0:ad97421fb1fb 191 gameEtat = ETAT_END;
antbig 0:ad97421fb1fb 192 //Il n'y a plus d'instruction, fin du jeu
antbig 0:ad97421fb1fb 193 } else {
antbig 0:ad97421fb1fb 194 instruction = strat_instructions[actual_instruction];
antbig 0:ad97421fb1fb 195 //On effectue le traitement de l'instruction
antbig 0:ad97421fb1fb 196 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
antbig 0:ad97421fb1fb 197 //actual_instruction++;
antbig 0:ad97421fb1fb 198 }
antbig 0:ad97421fb1fb 199 screenChecktry = 0;
antbig 1:116040d14164 200 wait_ms(100);
antbig 0:ad97421fb1fb 201 break;
antbig 0:ad97421fb1fb 202 case ETAT_GAME_PROCESS_INSTRUCTION:
antbig 0:ad97421fb1fb 203 /*
antbig 0:ad97421fb1fb 204 Traitement de l'instruction, envoie de la trame CAN
antbig 0:ad97421fb1fb 205 */
antbig 0:ad97421fb1fb 206 //debug_Instruction(instruction);
antbig 0:ad97421fb1fb 207 switch(instruction.order)
antbig 0:ad97421fb1fb 208 {
antbig 0:ad97421fb1fb 209 case MV_COURBURE:
antbig 0:ad97421fb1fb 210 //TODO - mettre l'ACK à jour
antbig 0:ad97421fb1fb 211 if(instruction.nextActionType == ENCHAINEMENT) {
antbig 0:ad97421fb1fb 212 //TODO - flag dans le trame pour indiquer l'enchainement
antbig 0:ad97421fb1fb 213 }
antbig 0:ad97421fb1fb 214 break;
antbig 0:ad97421fb1fb 215 case MV_LINE:
antbig 0:ad97421fb1fb 216 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 217 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 218 if(instruction.nextActionType == ENCHAINEMENT) {
antbig 1:116040d14164 219 MV_enchainement++;
antbig 1:116040d14164 220 localData5 = 1;
antbig 1:116040d14164 221 } else {
antbig 1:116040d14164 222 if(MV_enchainement > 0) {
antbig 1:116040d14164 223 localData5 = 2;
antbig 1:116040d14164 224 MV_enchainement = 0;
antbig 1:116040d14164 225 } else {
antbig 1:116040d14164 226 localData5 = 0;
antbig 1:116040d14164 227 }
antbig 0:ad97421fb1fb 228 }
antbig 1:116040d14164 229 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
antbig 1:116040d14164 230 GoStraight(localData2, 0, 0, localData5);
antbig 0:ad97421fb1fb 231 break;
antbig 0:ad97421fb1fb 232 case MV_TURN:
antbig 0:ad97421fb1fb 233 if(instruction.direction == RELATIVE) {
antbig 0:ad97421fb1fb 234 localData2 = instruction.arg3;
antbig 0:ad97421fb1fb 235 } else {
antbig 0:ad97421fb1fb 236 if(abs(instruction.arg3 - theta_robot) > 180) {
antbig 0:ad97421fb1fb 237 localData2 = 360 - instruction.arg3 - theta_robot;
antbig 0:ad97421fb1fb 238 } else {
antbig 0:ad97421fb1fb 239 localData2 = instruction.arg3 - theta_robot;
antbig 0:ad97421fb1fb 240 }
antbig 0:ad97421fb1fb 241 }
antbig 0:ad97421fb1fb 242 Rotate(localData2);
antbig 0:ad97421fb1fb 243 waitingAckID = ASSERVISSEMENT_ROTATION;
antbig 0:ad97421fb1fb 244 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 245 break;
antbig 0:ad97421fb1fb 246 case MV_XYT:
antbig 0:ad97421fb1fb 247 if(instruction.direction == BACKWARD) {
antbig 0:ad97421fb1fb 248 localData1 = -1;
antbig 0:ad97421fb1fb 249 } else {
antbig 0:ad97421fb1fb 250 localData1 = 1;
antbig 0:ad97421fb1fb 251 }
antbig 0:ad97421fb1fb 252 GoToPosition(instruction.arg1,instruction.arg2,instruction.arg3,localData1);
antbig 0:ad97421fb1fb 253 waitingAckID = ASSERVISSEMENT_XYT;
antbig 0:ad97421fb1fb 254 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 255 break;
antbig 0:ad97421fb1fb 256 case MV_RECALAGE:
antbig 0:ad97421fb1fb 257 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 258 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 259 //TODO - mettre l'ACK à jour
antbig 0:ad97421fb1fb 260 break;
antbig 0:ad97421fb1fb 261 case ACTION:
antbig 0:ad97421fb1fb 262 if(doAction(instruction.arg1,instruction.arg2,instruction.arg3)) {
antbig 0:ad97421fb1fb 263 //L'action est spécifique
antbig 0:ad97421fb1fb 264 } else {
antbig 0:ad97421fb1fb 265 //C'est un AX12 qu'il faut bouger
antbig 1:116040d14164 266 AX12_setGoal(instruction.arg1,instruction.arg3,instruction.arg2);
antbig 1:116040d14164 267 AX12_enchainement++;
antbig 0:ad97421fb1fb 268 }
antbig 1:116040d14164 269 waitingAckFrom = 0;
antbig 1:116040d14164 270 waitingAckID = 0;
antbig 0:ad97421fb1fb 271 break;
antbig 0:ad97421fb1fb 272 default:
antbig 0:ad97421fb1fb 273 //Instruction inconnue, on l'ignore
antbig 0:ad97421fb1fb 274 break;
antbig 0:ad97421fb1fb 275 }
antbig 0:ad97421fb1fb 276
antbig 0:ad97421fb1fb 277
antbig 0:ad97421fb1fb 278
antbig 0:ad97421fb1fb 279 if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) {
antbig 0:ad97421fb1fb 280 gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge
antbig 0:ad97421fb1fb 281 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 282 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 283 cartesCheker.start();
antbig 1:116040d14164 284 if(AX12_enchainement > 0) {
antbig 1:116040d14164 285 AX12_processChange();//Il faut lancer le déplacement des AX12
antbig 1:116040d14164 286 AX12_enchainement = 0;
antbig 1:116040d14164 287 }
antbig 0:ad97421fb1fb 288 } else {//C'est un enchainement
antbig 0:ad97421fb1fb 289 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 290 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
antbig 0:ad97421fb1fb 291 }
antbig 0:ad97421fb1fb 292
antbig 0:ad97421fb1fb 293 break;
antbig 0:ad97421fb1fb 294 case ETAT_GAME_WAIT_ACK:
antbig 0:ad97421fb1fb 295 /*
antbig 0:ad97421fb1fb 296 Attente de l'ack de l'instruction
antbig 0:ad97421fb1fb 297 */
antbig 0:ad97421fb1fb 298 if(waitingAckID == 0 && waitingAckFrom ==0) {//Les ack ont été reset, c'est bon on continu
antbig 0:ad97421fb1fb 299 //if(true) {
antbig 0:ad97421fb1fb 300 cartesCheker.stop();
antbig 0:ad97421fb1fb 301 if(instruction.nextActionType == JUMP) {
antbig 0:ad97421fb1fb 302 if(instruction.jumpAction == JUMP_TIME) {
antbig 0:ad97421fb1fb 303 gameEtat = ETAT_GAME_JUMP_TIME;
antbig 0:ad97421fb1fb 304 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 305 cartesCheker.start();
antbig 0:ad97421fb1fb 306 } else if(instruction.jumpAction == JUMP_POSITION) {
antbig 0:ad97421fb1fb 307 gameEtat = ETAT_GAME_JUMP_POSITION;
antbig 0:ad97421fb1fb 308 } else {
antbig 0:ad97421fb1fb 309 //ERROR
antbig 0:ad97421fb1fb 310 }
antbig 0:ad97421fb1fb 311 } else if(instruction.nextActionType == WAIT) {
antbig 0:ad97421fb1fb 312 gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
antbig 0:ad97421fb1fb 313 switch(instruction.order)
antbig 0:ad97421fb1fb 314 {
antbig 0:ad97421fb1fb 315 case MV_COURBURE:
antbig 0:ad97421fb1fb 316 waitingAckID = ASSERVISSEMENT_COURBURE;
antbig 0:ad97421fb1fb 317 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 318 break;
antbig 0:ad97421fb1fb 319 case MV_LINE:
antbig 0:ad97421fb1fb 320 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 321 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 322 break;
antbig 0:ad97421fb1fb 323 case MV_TURN:
antbig 0:ad97421fb1fb 324 waitingAckID = ASSERVISSEMENT_ROTATION;
antbig 0:ad97421fb1fb 325 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 326 break;
antbig 0:ad97421fb1fb 327 case MV_XYT:
antbig 0:ad97421fb1fb 328 waitingAckID = ASSERVISSEMENT_XYT;
antbig 0:ad97421fb1fb 329 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 330 break;
antbig 0:ad97421fb1fb 331 case MV_RECALAGE:
antbig 0:ad97421fb1fb 332 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 333 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 334 break;
antbig 0:ad97421fb1fb 335 case ACTION:
antbig 1:116040d14164 336 waitingAckFrom = SERVO_AX12_DONE;
antbig 1:116040d14164 337 waitingAckID = instruction.arg1;
antbig 0:ad97421fb1fb 338 break;
antbig 0:ad97421fb1fb 339 default:
antbig 0:ad97421fb1fb 340 break;
antbig 0:ad97421fb1fb 341 }
antbig 0:ad97421fb1fb 342 } else {
antbig 0:ad97421fb1fb 343 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 0:ad97421fb1fb 344 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 345 }
antbig 0:ad97421fb1fb 346 } else if(cartesCheker.read_ms () > 50){
antbig 0:ad97421fb1fb 347 cartesCheker.stop();
antbig 0:ad97421fb1fb 348 if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur
antbig 0:ad97421fb1fb 349 actual_instruction = instruction.nextLineError;
antbig 0:ad97421fb1fb 350 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 0:ad97421fb1fb 351 } else {
antbig 0:ad97421fb1fb 352 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
antbig 0:ad97421fb1fb 353 }
antbig 0:ad97421fb1fb 354 }
antbig 0:ad97421fb1fb 355 break;
antbig 0:ad97421fb1fb 356
antbig 0:ad97421fb1fb 357 case ETAT_GAME_JUMP_TIME:
antbig 0:ad97421fb1fb 358 if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) {
antbig 0:ad97421fb1fb 359 cartesCheker.stop();//On arrete le timer
antbig 0:ad97421fb1fb 360 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 361 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
antbig 0:ad97421fb1fb 362 }
antbig 0:ad97421fb1fb 363 break;
antbig 0:ad97421fb1fb 364 case ETAT_GAME_JUMP_POSITION:
antbig 0:ad97421fb1fb 365
antbig 0:ad97421fb1fb 366 break;
antbig 0:ad97421fb1fb 367 case ETAT_GAME_WAIT_END_INSTRUCTION:
antbig 0:ad97421fb1fb 368 if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée
antbig 0:ad97421fb1fb 369 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 370 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
antbig 0:ad97421fb1fb 371 }
antbig 0:ad97421fb1fb 372 break;
antbig 0:ad97421fb1fb 373
antbig 0:ad97421fb1fb 374
antbig 0:ad97421fb1fb 375
antbig 0:ad97421fb1fb 376 case ETAT_END:
antbig 0:ad97421fb1fb 377
antbig 0:ad97421fb1fb 378 gameEtat = ETAT_END_LOOP;
antbig 0:ad97421fb1fb 379 break;
antbig 0:ad97421fb1fb 380 case ETAT_END_LOOP:
antbig 0:ad97421fb1fb 381 //Rien, on tourne en rond
antbig 0:ad97421fb1fb 382 break;
antbig 0:ad97421fb1fb 383 default:
antbig 0:ad97421fb1fb 384
antbig 0:ad97421fb1fb 385 break;
antbig 0:ad97421fb1fb 386 }
antbig 0:ad97421fb1fb 387 }
antbig 0:ad97421fb1fb 388
antbig 0:ad97421fb1fb 389 void canProcessRx(void)
antbig 0:ad97421fb1fb 390 {
antbig 0:ad97421fb1fb 391 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
antbig 0:ad97421fb1fb 392
antbig 0:ad97421fb1fb 393 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
antbig 0:ad97421fb1fb 394 if(FIFO_occupation<0)
antbig 0:ad97421fb1fb 395 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
antbig 0:ad97421fb1fb 396 if(FIFO_max_occupation<FIFO_occupation)
antbig 0:ad97421fb1fb 397 FIFO_max_occupation=FIFO_occupation;
antbig 0:ad97421fb1fb 398 if(FIFO_occupation!=0) {
antbig 0:ad97421fb1fb 399
antbig 0:ad97421fb1fb 400 switch(msgRxBuffer[FIFO_lecture].id) {
antbig 0:ad97421fb1fb 401 case ASSERVISSEMENT_STOP:
antbig 0:ad97421fb1fb 402
antbig 0:ad97421fb1fb 403 break;
antbig 1:116040d14164 404
antbig 1:116040d14164 405 case DEBUG_FAKE_JAKE:
antbig 1:116040d14164 406 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
antbig 1:116040d14164 407 gameEtat = ETAT_GAME_START;
antbig 1:116040d14164 408 }
antbig 1:116040d14164 409 break;
antbig 1:116040d14164 410
antbig 0:ad97421fb1fb 411 case ALIVE_BALISE:
antbig 0:ad97421fb1fb 412 case ALIVE_MOTEUR:
antbig 0:ad97421fb1fb 413 case ALIVE_IHM:
antbig 0:ad97421fb1fb 414 case ECRAN_ALL_CHECK:
antbig 0:ad97421fb1fb 415 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
antbig 0:ad97421fb1fb 416 waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
antbig 0:ad97421fb1fb 417 }
antbig 0:ad97421fb1fb 418 break;
antbig 0:ad97421fb1fb 419
antbig 0:ad97421fb1fb 420 case ACKNOWLEDGE_BALISE:
antbig 0:ad97421fb1fb 421 case ACKNOWLEDGE_MOTEUR:
antbig 0:ad97421fb1fb 422 case ACKNOWLEDGE_IHM:
antbig 0:ad97421fb1fb 423 case INSTRUCTION_END_BALISE:
antbig 0:ad97421fb1fb 424 case INSTRUCTION_END_MOTEUR:
antbig 0:ad97421fb1fb 425 case INSTRUCTION_END_IHM:
antbig 0:ad97421fb1fb 426 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && (msgRxBuffer[FIFO_lecture].data[0]|((unsigned int)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) {
antbig 0:ad97421fb1fb 427 waitingAckFrom = 0;
antbig 0:ad97421fb1fb 428 waitingAckID = 0;
antbig 0:ad97421fb1fb 429 }
antbig 0:ad97421fb1fb 430 break;
antbig 1:116040d14164 431 #ifdef ROBOT_BIG
antbig 0:ad97421fb1fb 432 case ODOMETRIE_BIG_POSITION:
antbig 0:ad97421fb1fb 433 #else
antbig 0:ad97421fb1fb 434 case ODOMETRIE_SMALL_POSITION:
antbig 0:ad97421fb1fb 435 #endif
antbig 0:ad97421fb1fb 436 x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
antbig 0:ad97421fb1fb 437 y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
antbig 0:ad97421fb1fb 438 theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
antbig 0:ad97421fb1fb 439 break;
antbig 0:ad97421fb1fb 440
antbig 0:ad97421fb1fb 441 case ECRAN_START_MATCH:
antbig 0:ad97421fb1fb 442 if(gameEtat == ETAT_CONFIG) {
antbig 1:116040d14164 443 gameEtat = ETAT_GAME_INIT;
antbig 0:ad97421fb1fb 444 }
antbig 0:ad97421fb1fb 445 break;
antbig 0:ad97421fb1fb 446 case SERVO_AX12_SETGOAL:
antbig 0:ad97421fb1fb 447 if(AX12_isLocal(msgRxBuffer[FIFO_lecture].data[0]))
antbig 0:ad97421fb1fb 448 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 0:ad97421fb1fb 449
antbig 0:ad97421fb1fb 450 break;
antbig 0:ad97421fb1fb 451 case SERVO_AX12_PROCESS:
antbig 0:ad97421fb1fb 452 AX12_processChange();
antbig 0:ad97421fb1fb 453 break;
antbig 1:116040d14164 454 case SERVO_AX12_DONE:
antbig 1:116040d14164 455 AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0])));
antbig 1:116040d14164 456 break;
antbig 0:ad97421fb1fb 457 }
antbig 0:ad97421fb1fb 458
antbig 0:ad97421fb1fb 459 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
antbig 0:ad97421fb1fb 460 }
antbig 0:ad97421fb1fb 461 }