![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Strategie/Strategie.cpp@10:a788d9cf60f2, 2016-04-29 (annotated)
- Committer:
- antbig
- Date:
- Fri Apr 29 09:14:27 2016 +0000
- Revision:
- 10:a788d9cf60f2
- Parent:
- 9:d0042422d95a
- Child:
- 11:ed13a480ddca
Mise en place des angles pour les pinces XL320
Who changed what in which revision?
User | Revision | Line number | New 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 | 10:a788d9cf60f2 | 223 | SendRawId(GLOBAL_START); |
antbig | 0:ad97421fb1fb | 224 | break; |
antbig | 0:ad97421fb1fb | 225 | case ETAT_GAME_LOAD_NEXT_INSTRUCTION: |
antbig | 0:ad97421fb1fb | 226 | /* |
antbig | 0:ad97421fb1fb | 227 | Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction |
antbig | 0:ad97421fb1fb | 228 | */ |
antbig | 0:ad97421fb1fb | 229 | //printf("load next instruction\n"); |
antbig | 0:ad97421fb1fb | 230 | if(actual_instruction >= nb_instructions || actual_instruction == 255) { |
antbig | 0:ad97421fb1fb | 231 | gameEtat = ETAT_END; |
antbig | 0:ad97421fb1fb | 232 | //Il n'y a plus d'instruction, fin du jeu |
antbig | 0:ad97421fb1fb | 233 | } else { |
antbig | 0:ad97421fb1fb | 234 | instruction = strat_instructions[actual_instruction]; |
antbig | 0:ad97421fb1fb | 235 | //On effectue le traitement de l'instruction |
antbig | 0:ad97421fb1fb | 236 | gameEtat = ETAT_GAME_PROCESS_INSTRUCTION; |
antbig | 0:ad97421fb1fb | 237 | //actual_instruction++; |
antbig | 0:ad97421fb1fb | 238 | } |
antbig | 0:ad97421fb1fb | 239 | screenChecktry = 0; |
antbig | 1:116040d14164 | 240 | wait_ms(100); |
antbig | 0:ad97421fb1fb | 241 | break; |
antbig | 0:ad97421fb1fb | 242 | case ETAT_GAME_PROCESS_INSTRUCTION: |
antbig | 0:ad97421fb1fb | 243 | /* |
antbig | 0:ad97421fb1fb | 244 | Traitement de l'instruction, envoie de la trame CAN |
antbig | 0:ad97421fb1fb | 245 | */ |
antbig | 0:ad97421fb1fb | 246 | //debug_Instruction(instruction); |
antbig | 0:ad97421fb1fb | 247 | switch(instruction.order) |
antbig | 0:ad97421fb1fb | 248 | { |
antbig | 0:ad97421fb1fb | 249 | case MV_COURBURE: |
antbig | 6:eddfa414fd11 | 250 | waitingAckID = ASSERVISSEMENT_COURBURE; |
antbig | 6:eddfa414fd11 | 251 | waitingAckFrom = ACKNOWLEDGE_MOTEUR; |
antbig | 0:ad97421fb1fb | 252 | if(instruction.nextActionType == ENCHAINEMENT) { |
antbig | 6:eddfa414fd11 | 253 | MV_enchainement++; |
antbig | 6:eddfa414fd11 | 254 | localData5 = 1; |
antbig | 6:eddfa414fd11 | 255 | } else { |
antbig | 6:eddfa414fd11 | 256 | if(MV_enchainement > 0) { |
antbig | 6:eddfa414fd11 | 257 | localData5 = 2; |
antbig | 6:eddfa414fd11 | 258 | MV_enchainement = 0; |
antbig | 6:eddfa414fd11 | 259 | } else { |
antbig | 6:eddfa414fd11 | 260 | localData5 = 0; |
antbig | 6:eddfa414fd11 | 261 | } |
antbig | 0:ad97421fb1fb | 262 | } |
antbig | 6:eddfa414fd11 | 263 | |
antbig | 6:eddfa414fd11 | 264 | localData1 = ((instruction.direction == FORWARD)?1:-1); |
antbig | 9:d0042422d95a | 265 | BendRadius(instruction.arg1, instruction.arg3, localData1, localData5); |
antbig | 0:ad97421fb1fb | 266 | break; |
antbig | 0:ad97421fb1fb | 267 | case MV_LINE: |
antbig | 0:ad97421fb1fb | 268 | waitingAckID = ASSERVISSEMENT_RECALAGE; |
antbig | 0:ad97421fb1fb | 269 | waitingAckFrom = ACKNOWLEDGE_MOTEUR; |
antbig | 0:ad97421fb1fb | 270 | if(instruction.nextActionType == ENCHAINEMENT) { |
antbig | 1:116040d14164 | 271 | MV_enchainement++; |
antbig | 1:116040d14164 | 272 | localData5 = 1; |
antbig | 1:116040d14164 | 273 | } else { |
antbig | 1:116040d14164 | 274 | if(MV_enchainement > 0) { |
antbig | 1:116040d14164 | 275 | localData5 = 2; |
antbig | 1:116040d14164 | 276 | MV_enchainement = 0; |
antbig | 1:116040d14164 | 277 | } else { |
antbig | 1:116040d14164 | 278 | localData5 = 0; |
antbig | 1:116040d14164 | 279 | } |
antbig | 0:ad97421fb1fb | 280 | } |
antbig | 1:116040d14164 | 281 | localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); |
antbig | 1:116040d14164 | 282 | GoStraight(localData2, 0, 0, localData5); |
antbig | 0:ad97421fb1fb | 283 | break; |
antbig | 0:ad97421fb1fb | 284 | case MV_TURN: |
antbig | 0:ad97421fb1fb | 285 | if(instruction.direction == RELATIVE) { |
antbig | 0:ad97421fb1fb | 286 | localData2 = instruction.arg3; |
antbig | 0:ad97421fb1fb | 287 | } else { |
antbig | 0:ad97421fb1fb | 288 | if(abs(instruction.arg3 - theta_robot) > 180) { |
antbig | 0:ad97421fb1fb | 289 | localData2 = 360 - instruction.arg3 - theta_robot; |
antbig | 0:ad97421fb1fb | 290 | } else { |
antbig | 0:ad97421fb1fb | 291 | localData2 = instruction.arg3 - theta_robot; |
antbig | 0:ad97421fb1fb | 292 | } |
antbig | 0:ad97421fb1fb | 293 | } |
antbig | 2:8d8e2cf798a3 | 294 | if(InversStrat == 1) { |
antbig | 2:8d8e2cf798a3 | 295 | localData2 = -localData2; |
antbig | 2:8d8e2cf798a3 | 296 | } |
antbig | 0:ad97421fb1fb | 297 | Rotate(localData2); |
antbig | 0:ad97421fb1fb | 298 | waitingAckID = ASSERVISSEMENT_ROTATION; |
antbig | 0:ad97421fb1fb | 299 | waitingAckFrom = ACKNOWLEDGE_MOTEUR; |
antbig | 0:ad97421fb1fb | 300 | break; |
antbig | 0:ad97421fb1fb | 301 | case MV_XYT: |
antbig | 0:ad97421fb1fb | 302 | if(instruction.direction == BACKWARD) { |
antbig | 0:ad97421fb1fb | 303 | localData1 = -1; |
antbig | 0:ad97421fb1fb | 304 | } else { |
antbig | 0:ad97421fb1fb | 305 | localData1 = 1; |
antbig | 0:ad97421fb1fb | 306 | } |
antbig | 2:8d8e2cf798a3 | 307 | |
antbig | 2:8d8e2cf798a3 | 308 | if(InversStrat == 1) { |
antbig | 2:8d8e2cf798a3 | 309 | //localData2 = 360 - instruction.arg3 |
antbig | 2:8d8e2cf798a3 | 310 | localData3 = 3000 - instruction.arg2;//Inversion du Y |
antbig | 2:8d8e2cf798a3 | 311 | } else { |
antbig | 2:8d8e2cf798a3 | 312 | localData3 = instruction.arg2; |
antbig | 2:8d8e2cf798a3 | 313 | } |
antbig | 2:8d8e2cf798a3 | 314 | GoToPosition(instruction.arg1,localData3,instruction.arg3,localData1); |
antbig | 0:ad97421fb1fb | 315 | waitingAckID = ASSERVISSEMENT_XYT; |
antbig | 0:ad97421fb1fb | 316 | waitingAckFrom = ACKNOWLEDGE_MOTEUR; |
antbig | 0:ad97421fb1fb | 317 | break; |
antbig | 0:ad97421fb1fb | 318 | case MV_RECALAGE: |
antbig | 0:ad97421fb1fb | 319 | waitingAckID = ASSERVISSEMENT_RECALAGE; |
antbig | 0:ad97421fb1fb | 320 | waitingAckFrom = ACKNOWLEDGE_MOTEUR; |
antbig | 0:ad97421fb1fb | 321 | //TODO - mettre l'ACK à jour |
antbig | 0:ad97421fb1fb | 322 | break; |
antbig | 0:ad97421fb1fb | 323 | case ACTION: |
antbig | 0:ad97421fb1fb | 324 | if(doAction(instruction.arg1,instruction.arg2,instruction.arg3)) { |
antbig | 0:ad97421fb1fb | 325 | //L'action est spécifique |
antbig | 0:ad97421fb1fb | 326 | } else { |
antbig | 0:ad97421fb1fb | 327 | //C'est un AX12 qu'il faut bouger |
antbig | 2:8d8e2cf798a3 | 328 | AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2); |
antbig | 1:116040d14164 | 329 | AX12_enchainement++; |
antbig | 0:ad97421fb1fb | 330 | } |
antbig | 1:116040d14164 | 331 | waitingAckFrom = 0; |
antbig | 1:116040d14164 | 332 | waitingAckID = 0; |
antbig | 10:a788d9cf60f2 | 333 | actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante |
antbig | 10:a788d9cf60f2 | 334 | gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; |
antbig | 10:a788d9cf60f2 | 335 | return; |
antbig | 0:ad97421fb1fb | 336 | break; |
antbig | 0:ad97421fb1fb | 337 | default: |
antbig | 0:ad97421fb1fb | 338 | //Instruction inconnue, on l'ignore |
antbig | 0:ad97421fb1fb | 339 | break; |
antbig | 0:ad97421fb1fb | 340 | } |
antbig | 0:ad97421fb1fb | 341 | |
antbig | 0:ad97421fb1fb | 342 | |
antbig | 0:ad97421fb1fb | 343 | |
antbig | 0:ad97421fb1fb | 344 | if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) { |
antbig | 0:ad97421fb1fb | 345 | gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge |
antbig | 0:ad97421fb1fb | 346 | screenChecktry++;//On incrèment le conteur de tentative de 1 |
antbig | 0:ad97421fb1fb | 347 | cartesCheker.reset();//On reset le timeOut |
antbig | 0:ad97421fb1fb | 348 | cartesCheker.start(); |
antbig | 1:116040d14164 | 349 | if(AX12_enchainement > 0) { |
antbig | 1:116040d14164 | 350 | AX12_processChange();//Il faut lancer le déplacement des AX12 |
antbig | 1:116040d14164 | 351 | AX12_enchainement = 0; |
antbig | 1:116040d14164 | 352 | } |
antbig | 0:ad97421fb1fb | 353 | } else {//C'est un enchainement |
antbig | 0:ad97421fb1fb | 354 | actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante |
antbig | 0:ad97421fb1fb | 355 | gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante |
antbig | 0:ad97421fb1fb | 356 | } |
antbig | 0:ad97421fb1fb | 357 | |
antbig | 0:ad97421fb1fb | 358 | break; |
antbig | 0:ad97421fb1fb | 359 | case ETAT_GAME_WAIT_ACK: |
antbig | 0:ad97421fb1fb | 360 | /* |
antbig | 0:ad97421fb1fb | 361 | Attente de l'ack de l'instruction |
antbig | 0:ad97421fb1fb | 362 | */ |
antbig | 0:ad97421fb1fb | 363 | if(waitingAckID == 0 && waitingAckFrom ==0) {//Les ack ont été reset, c'est bon on continu |
antbig | 0:ad97421fb1fb | 364 | //if(true) { |
antbig | 0:ad97421fb1fb | 365 | cartesCheker.stop(); |
antbig | 0:ad97421fb1fb | 366 | if(instruction.nextActionType == JUMP) { |
antbig | 0:ad97421fb1fb | 367 | if(instruction.jumpAction == JUMP_TIME) { |
antbig | 0:ad97421fb1fb | 368 | gameEtat = ETAT_GAME_JUMP_TIME; |
antbig | 0:ad97421fb1fb | 369 | cartesCheker.reset();//On reset le timeOut |
antbig | 0:ad97421fb1fb | 370 | cartesCheker.start(); |
antbig | 0:ad97421fb1fb | 371 | } else if(instruction.jumpAction == JUMP_POSITION) { |
antbig | 0:ad97421fb1fb | 372 | gameEtat = ETAT_GAME_JUMP_POSITION; |
antbig | 0:ad97421fb1fb | 373 | } else { |
antbig | 0:ad97421fb1fb | 374 | //ERROR |
antbig | 0:ad97421fb1fb | 375 | } |
antbig | 0:ad97421fb1fb | 376 | } else if(instruction.nextActionType == WAIT) { |
antbig | 0:ad97421fb1fb | 377 | gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION; |
antbig | 0:ad97421fb1fb | 378 | switch(instruction.order) |
antbig | 0:ad97421fb1fb | 379 | { |
antbig | 0:ad97421fb1fb | 380 | case MV_COURBURE: |
antbig | 0:ad97421fb1fb | 381 | waitingAckID = ASSERVISSEMENT_COURBURE; |
antbig | 0:ad97421fb1fb | 382 | waitingAckFrom = INSTRUCTION_END_MOTEUR; |
antbig | 0:ad97421fb1fb | 383 | break; |
antbig | 0:ad97421fb1fb | 384 | case MV_LINE: |
antbig | 0:ad97421fb1fb | 385 | waitingAckID = ASSERVISSEMENT_RECALAGE; |
antbig | 0:ad97421fb1fb | 386 | waitingAckFrom = INSTRUCTION_END_MOTEUR; |
antbig | 0:ad97421fb1fb | 387 | break; |
antbig | 0:ad97421fb1fb | 388 | case MV_TURN: |
antbig | 0:ad97421fb1fb | 389 | waitingAckID = ASSERVISSEMENT_ROTATION; |
antbig | 0:ad97421fb1fb | 390 | waitingAckFrom = INSTRUCTION_END_MOTEUR; |
antbig | 0:ad97421fb1fb | 391 | break; |
antbig | 0:ad97421fb1fb | 392 | case MV_XYT: |
antbig | 0:ad97421fb1fb | 393 | waitingAckID = ASSERVISSEMENT_XYT; |
antbig | 0:ad97421fb1fb | 394 | waitingAckFrom = INSTRUCTION_END_MOTEUR; |
antbig | 0:ad97421fb1fb | 395 | break; |
antbig | 0:ad97421fb1fb | 396 | case MV_RECALAGE: |
antbig | 0:ad97421fb1fb | 397 | waitingAckID = ASSERVISSEMENT_RECALAGE; |
antbig | 0:ad97421fb1fb | 398 | waitingAckFrom = INSTRUCTION_END_MOTEUR; |
antbig | 0:ad97421fb1fb | 399 | break; |
antbig | 0:ad97421fb1fb | 400 | case ACTION: |
antbig | 9:d0042422d95a | 401 | waitingAckFrom = 0;//SERVO_AX12_DONE; |
antbig | 9:d0042422d95a | 402 | waitingAckID = 0;//instruction.arg1; |
antbig | 0:ad97421fb1fb | 403 | break; |
antbig | 0:ad97421fb1fb | 404 | default: |
antbig | 0:ad97421fb1fb | 405 | break; |
antbig | 0:ad97421fb1fb | 406 | } |
antbig | 0:ad97421fb1fb | 407 | } else { |
antbig | 0:ad97421fb1fb | 408 | gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; |
antbig | 0:ad97421fb1fb | 409 | actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante |
antbig | 0:ad97421fb1fb | 410 | } |
antbig | 0:ad97421fb1fb | 411 | } else if(cartesCheker.read_ms () > 50){ |
antbig | 0:ad97421fb1fb | 412 | cartesCheker.stop(); |
antbig | 0:ad97421fb1fb | 413 | if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur |
antbig | 0:ad97421fb1fb | 414 | actual_instruction = instruction.nextLineError; |
antbig | 0:ad97421fb1fb | 415 | gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; |
antbig | 0:ad97421fb1fb | 416 | } else { |
antbig | 0:ad97421fb1fb | 417 | gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction |
antbig | 0:ad97421fb1fb | 418 | } |
antbig | 0:ad97421fb1fb | 419 | } |
antbig | 0:ad97421fb1fb | 420 | break; |
antbig | 0:ad97421fb1fb | 421 | |
antbig | 0:ad97421fb1fb | 422 | case ETAT_GAME_JUMP_TIME: |
antbig | 0:ad97421fb1fb | 423 | if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) { |
antbig | 0:ad97421fb1fb | 424 | cartesCheker.stop();//On arrete le timer |
antbig | 0:ad97421fb1fb | 425 | actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante |
antbig | 0:ad97421fb1fb | 426 | gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante |
antbig | 0:ad97421fb1fb | 427 | } |
antbig | 0:ad97421fb1fb | 428 | break; |
antbig | 0:ad97421fb1fb | 429 | case ETAT_GAME_JUMP_POSITION: |
antbig | 0:ad97421fb1fb | 430 | |
antbig | 0:ad97421fb1fb | 431 | break; |
antbig | 0:ad97421fb1fb | 432 | case ETAT_GAME_WAIT_END_INSTRUCTION: |
antbig | 0:ad97421fb1fb | 433 | if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée |
antbig | 0:ad97421fb1fb | 434 | actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante |
antbig | 0:ad97421fb1fb | 435 | gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante |
antbig | 0:ad97421fb1fb | 436 | } |
antbig | 0:ad97421fb1fb | 437 | break; |
antbig | 0:ad97421fb1fb | 438 | |
antbig | 0:ad97421fb1fb | 439 | |
antbig | 5:dcd817534b57 | 440 | case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s |
antbig | 5:dcd817534b57 | 441 | if(timeoutWarning.read_ms() >= 2000)//ça fait plus de 2s, il faut changer de stratégie |
antbig | 5:dcd817534b57 | 442 | { |
antbig | 5:dcd817534b57 | 443 | gameEtat = ETAT_WARNING_SWITCH_STRATEGIE; |
antbig | 5:dcd817534b57 | 444 | } |
antbig | 5:dcd817534b57 | 445 | break; |
antbig | 5:dcd817534b57 | 446 | 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 | 447 | |
antbig | 5:dcd817534b57 | 448 | break; |
antbig | 5:dcd817534b57 | 449 | case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours |
antbig | 5:dcd817534b57 | 450 | |
antbig | 5:dcd817534b57 | 451 | break; |
antbig | 5:dcd817534b57 | 452 | case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur |
antbig | 5:dcd817534b57 | 453 | actual_instruction = instruction.nextLineError; |
antbig | 5:dcd817534b57 | 454 | gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; |
antbig | 5:dcd817534b57 | 455 | break; |
antbig | 5:dcd817534b57 | 456 | |
antbig | 5:dcd817534b57 | 457 | |
antbig | 0:ad97421fb1fb | 458 | |
antbig | 0:ad97421fb1fb | 459 | case ETAT_END: |
antbig | 0:ad97421fb1fb | 460 | |
antbig | 0:ad97421fb1fb | 461 | gameEtat = ETAT_END_LOOP; |
antbig | 0:ad97421fb1fb | 462 | break; |
antbig | 0:ad97421fb1fb | 463 | case ETAT_END_LOOP: |
antbig | 0:ad97421fb1fb | 464 | //Rien, on tourne en rond |
antbig | 0:ad97421fb1fb | 465 | break; |
antbig | 0:ad97421fb1fb | 466 | default: |
antbig | 0:ad97421fb1fb | 467 | |
antbig | 0:ad97421fb1fb | 468 | break; |
antbig | 0:ad97421fb1fb | 469 | } |
antbig | 0:ad97421fb1fb | 470 | } |
antbig | 0:ad97421fb1fb | 471 | |
antbig | 4:88431b537477 | 472 | /****************************************************************************************/ |
antbig | 4:88431b537477 | 473 | /* FUNCTION NAME: canProcessRx */ |
antbig | 4:88431b537477 | 474 | /* DESCRIPTION : Fonction de traitement des messages CAN */ |
antbig | 4:88431b537477 | 475 | /****************************************************************************************/ |
antbig | 0:ad97421fb1fb | 476 | void canProcessRx(void) |
antbig | 0:ad97421fb1fb | 477 | { |
antbig | 0:ad97421fb1fb | 478 | static signed char FIFO_occupation=0,FIFO_max_occupation=0; |
antbig | 4:88431b537477 | 479 | CANMessage msgTx=CANMessage(); |
antbig | 0:ad97421fb1fb | 480 | FIFO_occupation=FIFO_ecriture-FIFO_lecture; |
antbig | 0:ad97421fb1fb | 481 | if(FIFO_occupation<0) |
antbig | 0:ad97421fb1fb | 482 | FIFO_occupation=FIFO_occupation+SIZE_FIFO; |
antbig | 0:ad97421fb1fb | 483 | if(FIFO_max_occupation<FIFO_occupation) |
antbig | 0:ad97421fb1fb | 484 | FIFO_max_occupation=FIFO_occupation; |
antbig | 0:ad97421fb1fb | 485 | if(FIFO_occupation!=0) { |
antbig | 0:ad97421fb1fb | 486 | |
antbig | 0:ad97421fb1fb | 487 | switch(msgRxBuffer[FIFO_lecture].id) { |
antbig | 4:88431b537477 | 488 | case DEBUG_FAKE_JAKE://Permet de lancer le match à distance |
antbig | 1:116040d14164 | 489 | if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) { |
antbig | 1:116040d14164 | 490 | gameEtat = ETAT_GAME_START; |
antbig | 1:116040d14164 | 491 | } |
antbig | 1:116040d14164 | 492 | break; |
antbig | 1:116040d14164 | 493 | |
antbig | 0:ad97421fb1fb | 494 | case ALIVE_BALISE: |
antbig | 0:ad97421fb1fb | 495 | case ALIVE_MOTEUR: |
antbig | 0:ad97421fb1fb | 496 | case ALIVE_IHM: |
antbig | 0:ad97421fb1fb | 497 | case ECRAN_ALL_CHECK: |
antbig | 0:ad97421fb1fb | 498 | if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) { |
antbig | 0:ad97421fb1fb | 499 | waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne |
antbig | 0:ad97421fb1fb | 500 | } |
antbig | 0:ad97421fb1fb | 501 | break; |
antbig | 0:ad97421fb1fb | 502 | |
antbig | 0:ad97421fb1fb | 503 | case ACKNOWLEDGE_BALISE: |
antbig | 0:ad97421fb1fb | 504 | case ACKNOWLEDGE_MOTEUR: |
antbig | 0:ad97421fb1fb | 505 | case ACKNOWLEDGE_IHM: |
antbig | 0:ad97421fb1fb | 506 | case INSTRUCTION_END_BALISE: |
antbig | 0:ad97421fb1fb | 507 | case INSTRUCTION_END_MOTEUR: |
antbig | 0:ad97421fb1fb | 508 | case INSTRUCTION_END_IHM: |
antbig | 0:ad97421fb1fb | 509 | if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && (msgRxBuffer[FIFO_lecture].data[0]|((unsigned int)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) { |
antbig | 0:ad97421fb1fb | 510 | waitingAckFrom = 0; |
antbig | 0:ad97421fb1fb | 511 | waitingAckID = 0; |
antbig | 0:ad97421fb1fb | 512 | } |
antbig | 0:ad97421fb1fb | 513 | break; |
antbig | 1:116040d14164 | 514 | #ifdef ROBOT_BIG |
antbig | 0:ad97421fb1fb | 515 | case ODOMETRIE_BIG_POSITION: |
antbig | 0:ad97421fb1fb | 516 | #else |
antbig | 0:ad97421fb1fb | 517 | case ODOMETRIE_SMALL_POSITION: |
antbig | 0:ad97421fb1fb | 518 | #endif |
antbig | 0:ad97421fb1fb | 519 | x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8); |
antbig | 0:ad97421fb1fb | 520 | y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8); |
antbig | 0:ad97421fb1fb | 521 | theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8); |
antbig | 0:ad97421fb1fb | 522 | break; |
antbig | 0:ad97421fb1fb | 523 | |
antbig | 0:ad97421fb1fb | 524 | case ECRAN_START_MATCH: |
antbig | 0:ad97421fb1fb | 525 | if(gameEtat == ETAT_CONFIG) { |
antbig | 1:116040d14164 | 526 | gameEtat = ETAT_GAME_INIT; |
antbig | 0:ad97421fb1fb | 527 | } |
antbig | 0:ad97421fb1fb | 528 | break; |
antbig | 0:ad97421fb1fb | 529 | case SERVO_AX12_SETGOAL: |
antbig | 9:d0042422d95a | 530 | //SendAck(0x114, SERVO_AX12_SETGOAL); |
antbig | 0:ad97421fb1fb | 531 | if(AX12_isLocal(msgRxBuffer[FIFO_lecture].data[0])) |
antbig | 0:ad97421fb1fb | 532 | 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 | 533 | |
antbig | 0:ad97421fb1fb | 534 | break; |
antbig | 0:ad97421fb1fb | 535 | case SERVO_AX12_PROCESS: |
antbig | 9:d0042422d95a | 536 | //SendAck(0x114, SERVO_AX12_PROCESS); |
antbig | 5:dcd817534b57 | 537 | AX12_processChange(1); |
antbig | 0:ad97421fb1fb | 538 | break; |
antbig | 1:116040d14164 | 539 | case SERVO_AX12_DONE: |
antbig | 9:d0042422d95a | 540 | //SendAck(0x114, SERVO_AX12_DONE); |
antbig | 1:116040d14164 | 541 | AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0]))); |
antbig | 1:116040d14164 | 542 | break; |
antbig | 2:8d8e2cf798a3 | 543 | case ECRAN_CHOICE_COLOR://Choix de la couleur |
antbig | 4:88431b537477 | 544 | if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config |
antbig | 2:8d8e2cf798a3 | 545 | if(msgRxBuffer[FIFO_lecture].data[0] == 0) |
antbig | 2:8d8e2cf798a3 | 546 | InversStrat = 0;//Pas d'inversion de la couleur |
antbig | 2:8d8e2cf798a3 | 547 | else |
antbig | 2:8d8e2cf798a3 | 548 | InversStrat = 1;//Inversion de la couleur |
antbig | 4:88431b537477 | 549 | |
antbig | 4:88431b537477 | 550 | msgTx.id=ECRAN_ACK_COLOR; // tx ack de la couleur |
antbig | 4:88431b537477 | 551 | msgTx.len=1; |
antbig | 4:88431b537477 | 552 | msgTx.format=CANStandard; |
antbig | 4:88431b537477 | 553 | msgTx.type=CANData; |
antbig | 4:88431b537477 | 554 | // couleur sur 1 octet |
antbig | 4:88431b537477 | 555 | msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0]; |
antbig | 4:88431b537477 | 556 | can1.write(msgTx); |
antbig | 9:d0042422d95a | 557 | |
antbig | 2:8d8e2cf798a3 | 558 | } |
antbig | 2:8d8e2cf798a3 | 559 | break; |
antbig | 2:8d8e2cf798a3 | 560 | case ECRAN_CHOICE_STRAT://Choix du fichier de stratégie à utiliser |
antbig | 4:88431b537477 | 561 | if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config |
antbig | 4:88431b537477 | 562 | msgTx.id=ECRAN_ACK_STRAT; // tx ack de la couleur |
antbig | 4:88431b537477 | 563 | msgTx.len=1; |
antbig | 4:88431b537477 | 564 | msgTx.format=CANStandard; |
antbig | 4:88431b537477 | 565 | msgTx.type=CANData; |
antbig | 4:88431b537477 | 566 | if(SelectStrategy(msgRxBuffer[FIFO_lecture].data[0])) { |
antbig | 4:88431b537477 | 567 | // id de la stratégie sur 1 octet |
antbig | 4:88431b537477 | 568 | msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0]; |
antbig | 4:88431b537477 | 569 | } else { |
antbig | 4:88431b537477 | 570 | //erreur sur 1 octet |
antbig | 4:88431b537477 | 571 | msgTx.data[0]=0; |
antbig | 4:88431b537477 | 572 | } |
antbig | 4:88431b537477 | 573 | can1.write(msgTx); |
antbig | 9:d0042422d95a | 574 | wait_ms(10); |
antbig | 9:d0042422d95a | 575 | tactile_printf("Strat %d selectionne",msgTx.data[0]); |
antbig | 2:8d8e2cf798a3 | 576 | } |
antbig | 2:8d8e2cf798a3 | 577 | break; |
antbig | 5:dcd817534b57 | 578 | case BALISE_STOP: |
antbig | 5:dcd817534b57 | 579 | SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP); |
antbig | 6:eddfa414fd11 | 580 | if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT) |
antbig | 6:eddfa414fd11 | 581 | { |
antbig | 6:eddfa414fd11 | 582 | wait_ms(10); |
antbig | 6:eddfa414fd11 | 583 | SendRawId(ASSERVISSEMENT_STOP); |
antbig | 6:eddfa414fd11 | 584 | gameEtat = ETAT_WARNING_TIMEOUT; |
antbig | 6:eddfa414fd11 | 585 | timeoutWarning.reset(); |
antbig | 6:eddfa414fd11 | 586 | timeoutWarning.start();//Reset du timer utiliser par le timeout |
antbig | 6:eddfa414fd11 | 587 | } |
antbig | 5:dcd817534b57 | 588 | break; |
antbig | 5:dcd817534b57 | 589 | case BALISE_END_DANGER: |
antbig | 5:dcd817534b57 | 590 | if(gameEtat == ETAT_WARNING_TIMEOUT) { |
antbig | 5:dcd817534b57 | 591 | timeoutWarningWaitEnd.reset(); |
antbig | 5:dcd817534b57 | 592 | timeoutWarningWaitEnd.start(); |
antbig | 5:dcd817534b57 | 593 | gameEtat = ETAT_WARING_END_BALISE_WAIT; |
antbig | 5:dcd817534b57 | 594 | } |
antbig | 5:dcd817534b57 | 595 | break; |
antbig | 0:ad97421fb1fb | 596 | } |
antbig | 0:ad97421fb1fb | 597 | |
antbig | 0:ad97421fb1fb | 598 | FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; |
antbig | 0:ad97421fb1fb | 599 | } |
antbig | 0:ad97421fb1fb | 600 | } |