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