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