carte_strategie_2019
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
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?
User | Revision | Line number | New 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 | } |