CRAC Team / CRAC-Strat_2017-19-05-2017

strat des robots

Fork of CRAC-Strat_2017 by CRAC Team

Strategie/Strategie.cpp@17:d1594579eec6, 2017-05-19 (annotated)

Committer:
ClementBreteau
Date:
Fri May 19 17:14:07 2017 +0000
Revision:
17:d1594579eec6
Parent:
16:7321fb3bb396 
strat du robot, 19-05-2017, 19h

Who changed what in which revision?

UserRevisionLine numberNew 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 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
antbig 8:0edc7dfb7f7e 39 InterruptIn jack(p24); // 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);
antbig 12:14729d584500 245 #endif /*
antbig 9:d0042422d95a 246 SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T);
antbig 11:ed13a480ddca 247 #endif*/
antbig 0:ad97421fb1fb 248 break;
antbig 0:ad97421fb1fb 249 case ETAT_GAME_WAIT_FOR_JACK:
antbig 12:14729d584500 250 //On attend le jack
antbig 1:116040d14164 251 break;
antbig 1:116040d14164 252 case ETAT_GAME_START:
antbig 1:116040d14164 253 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
ClementBreteau 14:c8fc06c4887f 254
ClementBreteau 14:c8fc06c4887f 255 if (ModeDemo == 0){
ClementBreteau 14:c8fc06c4887f 256 chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s
ClementBreteau 14:c8fc06c4887f 257 gameTimer.start();
ClementBreteau 14:c8fc06c4887f 258 }
antbig 1:116040d14164 259 gameTimer.reset();
antbig 12:14729d584500 260 jack.fall(NULL);//On désactive l'interruption du jack
antbig 10:a788d9cf60f2 261 SendRawId(GLOBAL_START);
antbig 12:14729d584500 262 tactile_printf("Start");//Pas vraiment util mais bon
antbig 0:ad97421fb1fb 263 break;
antbig 0:ad97421fb1fb 264 case ETAT_GAME_LOAD_NEXT_INSTRUCTION:
antbig 0:ad97421fb1fb 265 /*
antbig 0:ad97421fb1fb 266 Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction
antbig 0:ad97421fb1fb 267 */
antbig 0:ad97421fb1fb 268 //printf("load next instruction\n");
ClementBreteau 14:c8fc06c4887f 269
antbig 0:ad97421fb1fb 270 if(actual_instruction >= nb_instructions || actual_instruction == 255) {
antbig 0:ad97421fb1fb 271 gameEtat = ETAT_END;
antbig 0:ad97421fb1fb 272 //Il n'y a plus d'instruction, fin du jeu
antbig 0:ad97421fb1fb 273 } else {
antbig 0:ad97421fb1fb 274 instruction = strat_instructions[actual_instruction];
antbig 0:ad97421fb1fb 275 //On effectue le traitement de l'instruction
antbig 0:ad97421fb1fb 276 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
antbig 0:ad97421fb1fb 277 }
antbig 0:ad97421fb1fb 278 screenChecktry = 0;
antbig 0:ad97421fb1fb 279 break;
antbig 0:ad97421fb1fb 280 case ETAT_GAME_PROCESS_INSTRUCTION:
antbig 0:ad97421fb1fb 281 /*
antbig 0:ad97421fb1fb 282 Traitement de l'instruction, envoie de la trame CAN
antbig 0:ad97421fb1fb 283 */
antbig 0:ad97421fb1fb 284 //debug_Instruction(instruction);
antbig 0:ad97421fb1fb 285 switch(instruction.order)
antbig 0:ad97421fb1fb 286 {
antbig 12:14729d584500 287 case MV_COURBURE://C'est un rayon de courbure
antbig 6:eddfa414fd11 288 waitingAckID = ASSERVISSEMENT_COURBURE;
antbig 6:eddfa414fd11 289 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 290 if(instruction.nextActionType == ENCHAINEMENT) {
antbig 6:eddfa414fd11 291 MV_enchainement++;
antbig 6:eddfa414fd11 292 localData5 = 1;
antbig 6:eddfa414fd11 293 } else {
antbig 6:eddfa414fd11 294 if(MV_enchainement > 0) {
antbig 6:eddfa414fd11 295 localData5 = 2;
antbig 6:eddfa414fd11 296 MV_enchainement = 0;
antbig 6:eddfa414fd11 297 } else {
antbig 6:eddfa414fd11 298 localData5 = 0;
antbig 6:eddfa414fd11 299 }
antbig 0:ad97421fb1fb 300 }
antbig 11:ed13a480ddca 301 localData1 = ((instruction.direction == LEFT)?1:-1);
antbig 12:14729d584500 302 if(InversStrat == 1)
antbig 12:14729d584500 303 {
antbig 12:14729d584500 304 localData1 = -localData1;//Inversion de la direction
antbig 12:14729d584500 305 }
antbig 9:d0042422d95a 306 BendRadius(instruction.arg1, instruction.arg3, localData1, localData5);
antbig 0:ad97421fb1fb 307 break;
antbig 12:14729d584500 308 case MV_LINE://Ligne droite
antbig 0:ad97421fb1fb 309 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 310 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 311 if(instruction.nextActionType == ENCHAINEMENT) {
antbig 1:116040d14164 312 MV_enchainement++;
antbig 1:116040d14164 313 localData5 = 1;
antbig 1:116040d14164 314 } else {
antbig 12:14729d584500 315 if(MV_enchainement > 0) {//Utilisé en cas d'enchainement,
antbig 1:116040d14164 316 localData5 = 2;
antbig 1:116040d14164 317 MV_enchainement = 0;
antbig 1:116040d14164 318 } else {
antbig 1:116040d14164 319 localData5 = 0;
antbig 1:116040d14164 320 }
antbig 0:ad97421fb1fb 321 }
ClementBreteau 16:7321fb3bb396 322 #ifdef ROBOT_BIG
ClementBreteau 15:c2fc239e85df 323 if(InversStrat == 1) {
ClementBreteau 15:c2fc239e85df 324 /*if (instruction.direction == FORWARD) instruction.direction = BACKWARD;
ClementBreteau 15:c2fc239e85df 325 else instruction.direction = FORWARD;*/
ClementBreteau 15:c2fc239e85df 326 instruction.direction = ((instruction.direction == FORWARD)?BACKWARD:FORWARD);
ClementBreteau 15:c2fc239e85df 327 }
ClementBreteau 16:7321fb3bb396 328 #endif
antbig 1:116040d14164 329 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
antbig 1:116040d14164 330 GoStraight(localData2, 0, 0, localData5);
ClementBreteau 14:c8fc06c4887f 331
antbig 0:ad97421fb1fb 332 break;
antbig 12:14729d584500 333 case MV_TURN: //Rotation sur place
antbig 0:ad97421fb1fb 334 if(instruction.direction == RELATIVE) {
antbig 0:ad97421fb1fb 335 localData2 = instruction.arg3;
antbig 12:14729d584500 336 } else {//C'est un rotation absolu, il faut la convertir en relative
antbig 12:14729d584500 337 localData2 = instruction.arg3;
antbig 12:14729d584500 338
antbig 12:14729d584500 339 if(InversStrat == 1) {
antbig 12:14729d584500 340 localData2 = -localData2;
antbig 0:ad97421fb1fb 341 }
antbig 12:14729d584500 342
antbig 12:14729d584500 343 localData2 = (localData2 - theta_robot)%3600;
antbig 12:14729d584500 344 if(localData2 > 1800) {
antbig 12:14729d584500 345 localData2 = localData2-3600;
antbig 12:14729d584500 346 }
antbig 12:14729d584500 347
antbig 0:ad97421fb1fb 348 }
antbig 12:14729d584500 349
antbig 0:ad97421fb1fb 350 Rotate(localData2);
antbig 0:ad97421fb1fb 351 waitingAckID = ASSERVISSEMENT_ROTATION;
antbig 0:ad97421fb1fb 352 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 353 break;
antbig 0:ad97421fb1fb 354 case MV_XYT:
antbig 0:ad97421fb1fb 355 if(instruction.direction == BACKWARD) {
antbig 0:ad97421fb1fb 356 localData1 = -1;
antbig 0:ad97421fb1fb 357 } else {
antbig 0:ad97421fb1fb 358 localData1 = 1;
antbig 0:ad97421fb1fb 359 }
antbig 2:8d8e2cf798a3 360
antbig 2:8d8e2cf798a3 361 if(InversStrat == 1) {
antbig 12:14729d584500 362 localData2 = -instruction.arg3;
antbig 2:8d8e2cf798a3 363 localData3 = 3000 - instruction.arg2;//Inversion du Y
antbig 2:8d8e2cf798a3 364 } else {
antbig 2:8d8e2cf798a3 365 localData3 = instruction.arg2;
antbig 12:14729d584500 366 localData2 = instruction.arg3;
antbig 2:8d8e2cf798a3 367 }
antbig 12:14729d584500 368 GoToPosition(instruction.arg1,localData3,localData2,localData1);
antbig 0:ad97421fb1fb 369 waitingAckID = ASSERVISSEMENT_XYT;
antbig 0:ad97421fb1fb 370 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 371 break;
antbig 0:ad97421fb1fb 372 case MV_RECALAGE:
antbig 0:ad97421fb1fb 373 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 374 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 11:ed13a480ddca 375 instruction.nextActionType = WAIT;
antbig 12:14729d584500 376 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 377
antbig 11:ed13a480ddca 378 if(instruction.precision == RECALAGE_Y) {
antbig 11:ed13a480ddca 379 localData5 = 2;
antbig 11:ed13a480ddca 380 if(InversStrat == 1) {
antbig 11:ed13a480ddca 381 localData3 = 3000 - instruction.arg1;//Inversion du Y
antbig 11:ed13a480ddca 382 } else {
antbig 11:ed13a480ddca 383 localData3 = instruction.arg1;
antbig 11:ed13a480ddca 384 }
antbig 11:ed13a480ddca 385 } else {
antbig 11:ed13a480ddca 386 localData5 = 1;
antbig 11:ed13a480ddca 387 localData3 = instruction.arg1;
antbig 11:ed13a480ddca 388 }
antbig 11:ed13a480ddca 389
antbig 11:ed13a480ddca 390 GoStraight(localData2, localData5, localData3, 0);
antbig 0:ad97421fb1fb 391 break;
antbig 0:ad97421fb1fb 392 case ACTION:
ClementBreteau 15:c2fc239e85df 393
ClementBreteau 15:c2fc239e85df 394 waitingAckID = SERVO_AX12_ACTION;
ClementBreteau 15:c2fc239e85df 395 waitingAckFrom = ACKNOWLEDGE_AX12;
antbig 0:ad97421fb1fb 396 if(doAction(instruction.arg1,instruction.arg2,instruction.arg3)) {
antbig 0:ad97421fb1fb 397 //L'action est spécifique
antbig 11:ed13a480ddca 398 if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) {
ClementBreteau 15:c2fc239e85df 399 wait_us(200);
antbig 11:ed13a480ddca 400 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 11:ed13a480ddca 401 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 11:ed13a480ddca 402 } else {
ClementBreteau 15:c2fc239e85df 403 gameEtat = ETAT_GAME_WAIT_ACK;
antbig 11:ed13a480ddca 404 }
antbig 11:ed13a480ddca 405 return;
antbig 0:ad97421fb1fb 406 } else {
antbig 0:ad97421fb1fb 407 //C'est un AX12 qu'il faut bouger
ClementBreteau 15:c2fc239e85df 408 //AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
ClementBreteau 15:c2fc239e85df 409 //AX12_enchainement++;
ClementBreteau 14:c8fc06c4887f 410
antbig 0:ad97421fb1fb 411 }
antbig 0:ad97421fb1fb 412 break;
antbig 0:ad97421fb1fb 413 default:
antbig 0:ad97421fb1fb 414 //Instruction inconnue, on l'ignore
antbig 0:ad97421fb1fb 415 break;
antbig 0:ad97421fb1fb 416 }
antbig 0:ad97421fb1fb 417
antbig 0:ad97421fb1fb 418
antbig 0:ad97421fb1fb 419
antbig 0:ad97421fb1fb 420 if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) {
antbig 0:ad97421fb1fb 421 gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge
antbig 0:ad97421fb1fb 422 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 423 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 424 cartesCheker.start();
antbig 1:116040d14164 425 if(AX12_enchainement > 0) {
ClementBreteau 15:c2fc239e85df 426 //AX12_processChange();//Il faut lancer le déplacement des AX12
ClementBreteau 15:c2fc239e85df 427 //AX12_enchainement = 0;
antbig 1:116040d14164 428 }
antbig 0:ad97421fb1fb 429 } else {//C'est un enchainement
ClementBreteau 15:c2fc239e85df 430 if(instruction.order == MV_LINE){
ClementBreteau 15:c2fc239e85df 431 gameEtat = ETAT_GAME_WAIT_ACK;
ClementBreteau 15:c2fc239e85df 432
ClementBreteau 15:c2fc239e85df 433 }else{
ClementBreteau 15:c2fc239e85df 434 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
ClementBreteau 15:c2fc239e85df 435 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
ClementBreteau 15:c2fc239e85df 436 }
antbig 0:ad97421fb1fb 437 }
antbig 0:ad97421fb1fb 438
antbig 0:ad97421fb1fb 439 break;
antbig 0:ad97421fb1fb 440 case ETAT_GAME_WAIT_ACK:
antbig 0:ad97421fb1fb 441 /*
antbig 0:ad97421fb1fb 442 Attente de l'ack de l'instruction
antbig 0:ad97421fb1fb 443 */
ClementBreteau 14:c8fc06c4887f 444 if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continu
antbig 0:ad97421fb1fb 445 //if(true) {
antbig 0:ad97421fb1fb 446 cartesCheker.stop();
antbig 0:ad97421fb1fb 447 if(instruction.nextActionType == JUMP) {
antbig 11:ed13a480ddca 448 if(instruction.jumpAction == JUMP_POSITION) {
antbig 11:ed13a480ddca 449 gameEtat = ETAT_GAME_JUMP_POSITION;
antbig 11:ed13a480ddca 450 } else {//Pour eviter les erreurs, on dit que c'est par défaut un jump time
antbig 0:ad97421fb1fb 451 gameEtat = ETAT_GAME_JUMP_TIME;
antbig 0:ad97421fb1fb 452 cartesCheker.reset();//On reset le timeOut
antbig 11:ed13a480ddca 453 cartesCheker.start();
antbig 0:ad97421fb1fb 454 }
antbig 0:ad97421fb1fb 455 } else if(instruction.nextActionType == WAIT) {
antbig 0:ad97421fb1fb 456 gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
antbig 0:ad97421fb1fb 457 switch(instruction.order)
antbig 0:ad97421fb1fb 458 {
antbig 0:ad97421fb1fb 459 case MV_COURBURE:
antbig 0:ad97421fb1fb 460 waitingAckID = ASSERVISSEMENT_COURBURE;
antbig 0:ad97421fb1fb 461 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 462 break;
antbig 0:ad97421fb1fb 463 case MV_LINE:
antbig 0:ad97421fb1fb 464 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 465 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 466 break;
antbig 0:ad97421fb1fb 467 case MV_TURN:
antbig 0:ad97421fb1fb 468 waitingAckID = ASSERVISSEMENT_ROTATION;
antbig 0:ad97421fb1fb 469 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 470 break;
antbig 0:ad97421fb1fb 471 case MV_XYT:
antbig 0:ad97421fb1fb 472 waitingAckID = ASSERVISSEMENT_XYT;
antbig 0:ad97421fb1fb 473 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 474 break;
antbig 0:ad97421fb1fb 475 case MV_RECALAGE:
antbig 0:ad97421fb1fb 476 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 477 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 478 break;
antbig 0:ad97421fb1fb 479 case ACTION:
ClementBreteau 15:c2fc239e85df 480
ClementBreteau 15:c2fc239e85df 481 if (modeTelemetre == 0){
ClementBreteau 15:c2fc239e85df 482 waitingAckID = SERVO_AX12_ACTION;// instruction.arg1;
ClementBreteau 15:c2fc239e85df 483 waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE;
ClementBreteau 15:c2fc239e85df 484 }else{ // si on attend la reponse du telemetre
ClementBreteau 15:c2fc239e85df 485 //modeTelemetre = 1;
ClementBreteau 15:c2fc239e85df 486 waitingAckID = OBJET_SUR_TABLE;
ClementBreteau 15:c2fc239e85df 487 waitingAckFrom = 0;
ClementBreteau 15:c2fc239e85df 488 }
antbig 0:ad97421fb1fb 489 break;
antbig 0:ad97421fb1fb 490 default:
antbig 0:ad97421fb1fb 491 break;
antbig 0:ad97421fb1fb 492 }
antbig 0:ad97421fb1fb 493 } else {
antbig 0:ad97421fb1fb 494 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 0:ad97421fb1fb 495 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 496 }
antbig 0:ad97421fb1fb 497 } else if(cartesCheker.read_ms () > 50){
antbig 0:ad97421fb1fb 498 cartesCheker.stop();
antbig 0:ad97421fb1fb 499 if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur
antbig 0:ad97421fb1fb 500 actual_instruction = instruction.nextLineError;
antbig 0:ad97421fb1fb 501 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 0:ad97421fb1fb 502 } else {
antbig 0:ad97421fb1fb 503 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
antbig 0:ad97421fb1fb 504 }
antbig 0:ad97421fb1fb 505 }
antbig 0:ad97421fb1fb 506 break;
antbig 0:ad97421fb1fb 507
antbig 0:ad97421fb1fb 508 case ETAT_GAME_JUMP_TIME:
antbig 0:ad97421fb1fb 509 if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) {
antbig 0:ad97421fb1fb 510 cartesCheker.stop();//On arrete le timer
antbig 0:ad97421fb1fb 511 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 512 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
antbig 0:ad97421fb1fb 513 }
antbig 0:ad97421fb1fb 514 break;
ClementBreteau 15:c2fc239e85df 515
ClementBreteau 15:c2fc239e85df 516 case ETAT_GAME_JUMP_CONFIG:
ClementBreteau 15:c2fc239e85df 517 signed int depasX = 1, depasY = 1; // servent à indiquer le sens de dépassement des coordonnées
ClementBreteau 15:c2fc239e85df 518 // 1 si l'instruction est plus grande que la position du robot
ClementBreteau 15:c2fc239e85df 519 // -1 si l'instruction est plus petite que la position du robot
ClementBreteau 15:c2fc239e85df 520 // 0 si l'instruction et position du robot sont proche de moins de 1cm
ClementBreteau 15:c2fc239e85df 521 if (abs(x_robot-instruction.JumpTimeOrX)<10){
ClementBreteau 15:c2fc239e85df 522 depasX = 0;
ClementBreteau 15:c2fc239e85df 523 }else if(x_robot > instruction.JumpTimeOrX){
ClementBreteau 15:c2fc239e85df 524 depasX = -1;
ClementBreteau 15:c2fc239e85df 525 }
ClementBreteau 15:c2fc239e85df 526
ClementBreteau 15:c2fc239e85df 527 if(abs(y_robot-instruction.JumpY)<10){
ClementBreteau 15:c2fc239e85df 528 depasY = 0;
ClementBreteau 15:c2fc239e85df 529 }else if(y_robot > instruction.JumpY){
ClementBreteau 15:c2fc239e85df 530 depasY = -1;
ClementBreteau 15:c2fc239e85df 531 }
ClementBreteau 15:c2fc239e85df 532
ClementBreteau 15:c2fc239e85df 533 gameEtat = ETAT_GAME_JUMP_POSITION;
ClementBreteau 15:c2fc239e85df 534 break;
antbig 0:ad97421fb1fb 535 case ETAT_GAME_JUMP_POSITION:
ClementBreteau 15:c2fc239e85df 536 bool Xok = false, Yok = false;
ClementBreteau 15:c2fc239e85df 537
ClementBreteau 15:c2fc239e85df 538 if (depasX == 0){
ClementBreteau 15:c2fc239e85df 539 Xok = true;
ClementBreteau 15:c2fc239e85df 540 }else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5){
ClementBreteau 15:c2fc239e85df 541 Xok = true;
ClementBreteau 15:c2fc239e85df 542 }
ClementBreteau 15:c2fc239e85df 543
ClementBreteau 15:c2fc239e85df 544 if (depasY == 0){
ClementBreteau 15:c2fc239e85df 545 Yok = true;
ClementBreteau 15:c2fc239e85df 546 }else if ((instruction.JumpY - y_robot)*depasY < -5){
ClementBreteau 15:c2fc239e85df 547 Yok = true;
ClementBreteau 15:c2fc239e85df 548 }
ClementBreteau 15:c2fc239e85df 549
ClementBreteau 15:c2fc239e85df 550 // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante
ClementBreteau 15:c2fc239e85df 551 if (Xok && Yok){
ClementBreteau 15:c2fc239e85df 552 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
ClementBreteau 15:c2fc239e85df 553 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
ClementBreteau 15:c2fc239e85df 554 }
ClementBreteau 15:c2fc239e85df 555
antbig 0:ad97421fb1fb 556 break;
antbig 0:ad97421fb1fb 557 case ETAT_GAME_WAIT_END_INSTRUCTION:
antbig 0:ad97421fb1fb 558 if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée
antbig 0:ad97421fb1fb 559 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 560 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
antbig 0:ad97421fb1fb 561 }
antbig 0:ad97421fb1fb 562 break;
antbig 0:ad97421fb1fb 563
antbig 0:ad97421fb1fb 564
antbig 5:dcd817534b57 565 case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s
antbig 12:14729d584500 566 if(timeoutWarning.read_ms() >= BALISE_TIMEOUT)//ça fait plus de 2s, il faut changer de stratégie
antbig 5:dcd817534b57 567 {
antbig 5:dcd817534b57 568 gameEtat = ETAT_WARNING_SWITCH_STRATEGIE;
antbig 5:dcd817534b57 569 }
antbig 5:dcd817534b57 570 break;
antbig 5:dcd817534b57 571 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 572 if(timeoutWarningWaitEnd.read_ms() >= 1000) {//c'est bon, on repart
antbig 12:14729d584500 573 //actual_instruction = instruction.nextLineError;
antbig 12:14729d584500 574 gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
antbig 12:14729d584500 575 }
antbig 5:dcd817534b57 576 break;
antbig 5:dcd817534b57 577 case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours
antbig 12:14729d584500 578 #ifdef ROBOT_BIG
ClementBreteau 15:c2fc239e85df 579 actual_instruction = instruction.nextLineError;// 2 //Modification directe... c'est pas bien mais ça marchait pour le match 5
antbig 12:14729d584500 580 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 12:14729d584500 581 #else
antbig 12:14729d584500 582 actual_instruction = instruction.nextLineError;
antbig 12:14729d584500 583 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 12:14729d584500 584 #endif
antbig 5:dcd817534b57 585 break;
antbig 5:dcd817534b57 586 case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur
antbig 5:dcd817534b57 587 actual_instruction = instruction.nextLineError;
antbig 5:dcd817534b57 588 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 12:14729d584500 589 ingnorBaliseOnce = 1;
antbig 5:dcd817534b57 590 break;
antbig 5:dcd817534b57 591
antbig 5:dcd817534b57 592
antbig 0:ad97421fb1fb 593
ClementBreteau 14:c8fc06c4887f 594 case ETAT_END:
ClementBreteau 14:c8fc06c4887f 595 if (ModeDemo){
ClementBreteau 14:c8fc06c4887f 596 gameEtat = ETAT_CHECK_CARTE_SCREEN;
ClementBreteau 14:c8fc06c4887f 597 ModeDemo = 1;
ClementBreteau 14:c8fc06c4887f 598 } else {
ClementBreteau 14:c8fc06c4887f 599 gameEtat = ETAT_END_LOOP;
ClementBreteau 14:c8fc06c4887f 600 }
antbig 0:ad97421fb1fb 601 break;
antbig 0:ad97421fb1fb 602 case ETAT_END_LOOP:
antbig 0:ad97421fb1fb 603 //Rien, on tourne en rond
antbig 0:ad97421fb1fb 604 break;
antbig 0:ad97421fb1fb 605 default:
antbig 0:ad97421fb1fb 606
antbig 0:ad97421fb1fb 607 break;
antbig 0:ad97421fb1fb 608 }
antbig 0:ad97421fb1fb 609 }
antbig 0:ad97421fb1fb 610
antbig 4:88431b537477 611 /****************************************************************************************/
antbig 4:88431b537477 612 /* FUNCTION NAME: canProcessRx */
antbig 4:88431b537477 613 /* DESCRIPTION : Fonction de traitement des messages CAN */
antbig 4:88431b537477 614 /****************************************************************************************/
antbig 0:ad97421fb1fb 615 void canProcessRx(void)
antbig 0:ad97421fb1fb 616 {
antbig 0:ad97421fb1fb 617 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
antbig 4:88431b537477 618 CANMessage msgTx=CANMessage();
antbig 0:ad97421fb1fb 619 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
antbig 0:ad97421fb1fb 620 if(FIFO_occupation<0)
antbig 0:ad97421fb1fb 621 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
antbig 0:ad97421fb1fb 622 if(FIFO_max_occupation<FIFO_occupation)
antbig 0:ad97421fb1fb 623 FIFO_max_occupation=FIFO_occupation;
antbig 0:ad97421fb1fb 624 if(FIFO_occupation!=0) {
antbig 0:ad97421fb1fb 625
antbig 0:ad97421fb1fb 626 switch(msgRxBuffer[FIFO_lecture].id) {
antbig 4:88431b537477 627 case DEBUG_FAKE_JAKE://Permet de lancer le match à distance
antbig 1:116040d14164 628 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
antbig 1:116040d14164 629 gameEtat = ETAT_GAME_START;
antbig 1:116040d14164 630 }
antbig 1:116040d14164 631 break;
antbig 1:116040d14164 632
antbig 0:ad97421fb1fb 633 case ALIVE_BALISE:
antbig 0:ad97421fb1fb 634 case ALIVE_MOTEUR:
antbig 0:ad97421fb1fb 635 case ALIVE_IHM:
antbig 11:ed13a480ddca 636 case ALIVE_ACTIONNEURS:
antbig 11:ed13a480ddca 637 case ALIVE_POMPES:
antbig 12:14729d584500 638 case ALIVE_AX12:
antbig 0:ad97421fb1fb 639 case ECRAN_ALL_CHECK:
antbig 0:ad97421fb1fb 640 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
antbig 0:ad97421fb1fb 641 waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
antbig 0:ad97421fb1fb 642 }
ClementBreteau 15:c2fc239e85df 643 break;
antbig 0:ad97421fb1fb 644
antbig 0:ad97421fb1fb 645 case ACKNOWLEDGE_BALISE:
antbig 0:ad97421fb1fb 646 case ACKNOWLEDGE_MOTEUR:
antbig 0:ad97421fb1fb 647 case ACKNOWLEDGE_IHM:
ClementBreteau 14:c8fc06c4887f 648 case ACKNOWLEDGE_TELEMETRE:
ClementBreteau 15:c2fc239e85df 649 case ACKNOWLEDGE_AX12:
antbig 0:ad97421fb1fb 650 case INSTRUCTION_END_BALISE:
antbig 0:ad97421fb1fb 651 case INSTRUCTION_END_MOTEUR:
antbig 0:ad97421fb1fb 652 case INSTRUCTION_END_IHM:
ClementBreteau 15:c2fc239e85df 653 case INSTRUCTION_END_AX12:
antbig 11:ed13a480ddca 654 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 655 waitingAckFrom = 0;
antbig 0:ad97421fb1fb 656 waitingAckID = 0;
antbig 0:ad97421fb1fb 657 }
antbig 0:ad97421fb1fb 658 break;
antbig 1:116040d14164 659 #ifdef ROBOT_BIG
antbig 0:ad97421fb1fb 660 case ODOMETRIE_BIG_POSITION:
antbig 0:ad97421fb1fb 661 #else
antbig 0:ad97421fb1fb 662 case ODOMETRIE_SMALL_POSITION:
antbig 0:ad97421fb1fb 663 #endif
antbig 0:ad97421fb1fb 664 x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
antbig 0:ad97421fb1fb 665 y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
antbig 0:ad97421fb1fb 666 theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
antbig 0:ad97421fb1fb 667 break;
antbig 0:ad97421fb1fb 668
antbig 0:ad97421fb1fb 669 case ECRAN_START_MATCH:
antbig 0:ad97421fb1fb 670 if(gameEtat == ETAT_CONFIG) {
antbig 1:116040d14164 671 gameEtat = ETAT_GAME_INIT;
antbig 0:ad97421fb1fb 672 }
antbig 0:ad97421fb1fb 673 break;
antbig 0:ad97421fb1fb 674 case SERVO_AX12_SETGOAL:
antbig 9:d0042422d95a 675 //SendAck(0x114, SERVO_AX12_SETGOAL);
ClementBreteau 14:c8fc06c4887f 676 //if(AX12_isLocal(msgRxBuffer[FIFO_lecture].data[0]))
ClementBreteau 14:c8fc06c4887f 677 //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 678
antbig 0:ad97421fb1fb 679 break;
ClementBreteau 14:c8fc06c4887f 680
antbig 0:ad97421fb1fb 681 case SERVO_AX12_PROCESS:
ClementBreteau 14:c8fc06c4887f 682 SendAck(0x114, SERVO_AX12_PROCESS);
ClementBreteau 14:c8fc06c4887f 683 //AX12_processChange(1);
antbig 0:ad97421fb1fb 684 break;
ClementBreteau 14:c8fc06c4887f 685
antbig 1:116040d14164 686 case SERVO_AX12_DONE:
ClementBreteau 15:c2fc239e85df 687 SendRawId(POMPE_PWM);
ClementBreteau 14:c8fc06c4887f 688 /*//SendAck(0x114, SERVO_AX12_DONE);
antbig 1:116040d14164 689 AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0])));
ClementBreteau 14:c8fc06c4887f 690
ClementBreteau 15:c2fc239e85df 691 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
ClementBreteau 14:c8fc06c4887f 692 waitingAckFrom = 0;
ClementBreteau 14:c8fc06c4887f 693 waitingAckID = 0;*/
ClementBreteau 14:c8fc06c4887f 694
antbig 1:116040d14164 695 break;
antbig 2:8d8e2cf798a3 696 case ECRAN_CHOICE_COLOR://Choix de la couleur
antbig 4:88431b537477 697 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
antbig 2:8d8e2cf798a3 698 if(msgRxBuffer[FIFO_lecture].data[0] == 0)
antbig 2:8d8e2cf798a3 699 InversStrat = 0;//Pas d'inversion de la couleur
antbig 2:8d8e2cf798a3 700 else
antbig 2:8d8e2cf798a3 701 InversStrat = 1;//Inversion de la couleur
antbig 4:88431b537477 702
antbig 4:88431b537477 703 msgTx.id=ECRAN_ACK_COLOR; // tx ack de la couleur
antbig 4:88431b537477 704 msgTx.len=1;
antbig 4:88431b537477 705 msgTx.format=CANStandard;
antbig 4:88431b537477 706 msgTx.type=CANData;
antbig 4:88431b537477 707 // couleur sur 1 octet
antbig 4:88431b537477 708 msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0];
antbig 4:88431b537477 709 can1.write(msgTx);
antbig 9:d0042422d95a 710
antbig 2:8d8e2cf798a3 711 }
antbig 2:8d8e2cf798a3 712 break;
ClementBreteau 14:c8fc06c4887f 713
antbig 2:8d8e2cf798a3 714 case ECRAN_CHOICE_STRAT://Choix du fichier de stratégie à utiliser
antbig 4:88431b537477 715 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
antbig 4:88431b537477 716 msgTx.id=ECRAN_ACK_STRAT; // tx ack de la couleur
antbig 4:88431b537477 717 msgTx.len=1;
antbig 4:88431b537477 718 msgTx.format=CANStandard;
antbig 4:88431b537477 719 msgTx.type=CANData;
antbig 4:88431b537477 720 if(SelectStrategy(msgRxBuffer[FIFO_lecture].data[0])) {
antbig 4:88431b537477 721 // id de la stratégie sur 1 octet
ClementBreteau 16:7321fb3bb396 722 if (msgRxBuffer[FIFO_lecture].data[0] < 0x10){ // Si la strat est une strat de match, on desactive le mode demo
ClementBreteau 14:c8fc06c4887f 723 ModeDemo = 0;
ClementBreteau 16:7321fb3bb396 724 } else { // sinon, on active le mode demo, utile pour la fin de la demo
ClementBreteau 14:c8fc06c4887f 725 ModeDemo = 1;
ClementBreteau 14:c8fc06c4887f 726 }
ClementBreteau 14:c8fc06c4887f 727
antbig 4:88431b537477 728 msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0];
antbig 4:88431b537477 729 } else {
antbig 4:88431b537477 730 //erreur sur 1 octet
antbig 4:88431b537477 731 msgTx.data[0]=0;
antbig 4:88431b537477 732 }
antbig 4:88431b537477 733 can1.write(msgTx);
antbig 9:d0042422d95a 734 wait_ms(10);
antbig 12:14729d584500 735 setAsservissementEtat(0);//Désactivation de l'asservissement pour repositionner le robot dans le zone de départ
antbig 12:14729d584500 736 tactile_printf("Strat %d, Asser desactive",msgTx.data[0]);
antbig 2:8d8e2cf798a3 737 }
antbig 2:8d8e2cf798a3 738 break;
ClementBreteau 16:7321fb3bb396 739 case BALISE_DANGER :
ClementBreteau 16:7321fb3bb396 740 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
ClementBreteau 16:7321fb3bb396 741 break;
ClementBreteau 16:7321fb3bb396 742
antbig 5:dcd817534b57 743 case BALISE_STOP:
antbig 5:dcd817534b57 744 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
ClementBreteau 16:7321fb3bb396 745 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 746 if(needToStop() != 0 && ingnorBaliseOnce ==0) {
ClementBreteau 16:7321fb3bb396 747 if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
ClementBreteau 16:7321fb3bb396 748 {
ClementBreteau 16:7321fb3bb396 749 SendRawId(ASSERVISSEMENT_STOP);
ClementBreteau 16:7321fb3bb396 750 //while(1);
ClementBreteau 16:7321fb3bb396 751 gameEtat = ETAT_WARNING_TIMEOUT;
ClementBreteau 16:7321fb3bb396 752 if(gameEtat != ETAT_WARING_END_BALISE_WAIT) {
ClementBreteau 16:7321fb3bb396 753 timeoutWarning.reset();
ClementBreteau 16:7321fb3bb396 754 timeoutWarning.start();//Reset du timer utiliser par le timeout
ClementBreteau 16:7321fb3bb396 755 }
antbig 12:14729d584500 756 }
antbig 12:14729d584500 757 }
antbig 6:eddfa414fd11 758 }
antbig 12:14729d584500 759 ingnorBaliseOnce = 0;
antbig 5:dcd817534b57 760 break;
ClementBreteau 16:7321fb3bb396 761
antbig 5:dcd817534b57 762 case BALISE_END_DANGER:
ClementBreteau 16:7321fb3bb396 763 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
antbig 5:dcd817534b57 764 if(gameEtat == ETAT_WARNING_TIMEOUT) {
antbig 5:dcd817534b57 765 timeoutWarningWaitEnd.reset();
antbig 5:dcd817534b57 766 timeoutWarningWaitEnd.start();
antbig 5:dcd817534b57 767 gameEtat = ETAT_WARING_END_BALISE_WAIT;
antbig 5:dcd817534b57 768 }
antbig 5:dcd817534b57 769 break;
antbig 12:14729d584500 770
antbig 12:14729d584500 771 case ECRAN_CHOICE_START_ACTION:
antbig 12:14729d584500 772 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
antbig 12:14729d584500 773 if(msgRxBuffer[FIFO_lecture].data[0] == 1) {
antbig 12:14729d584500 774 runRobotTest();
antbig 12:14729d584500 775 } else {
antbig 12:14729d584500 776 initRobotActionneur();
antbig 12:14729d584500 777 }
antbig 12:14729d584500 778 wait_ms(500);
antbig 12:14729d584500 779 SendRawId(ECRAN_ACK_CHOICE_START_ACTION);
antbig 12:14729d584500 780 }
antbig 12:14729d584500 781 break;
ClementBreteau 14:c8fc06c4887f 782
ClementBreteau 14:c8fc06c4887f 783 case OBJET_SUR_TABLE:
ClementBreteau 15:c2fc239e85df 784 if (msgRxBuffer[FIFO_lecture].data[1] == 0xff){
ClementBreteau 15:c2fc239e85df 785
ClementBreteau 15:c2fc239e85df 786 gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
ClementBreteau 15:c2fc239e85df 787 }
ClementBreteau 15:c2fc239e85df 788 else{
ClementBreteau 15:c2fc239e85df 789
ClementBreteau 15:c2fc239e85df 790 waitingAckFrom = 0;
ClementBreteau 15:c2fc239e85df 791 waitingAckID = 0;
ClementBreteau 15:c2fc239e85df 792
ClementBreteau 15:c2fc239e85df 793 strat_instructions[actual_instruction+1].arg1 = returnX(strat_instructions[actual_instruction].arg2);
ClementBreteau 15:c2fc239e85df 794 strat_instructions[actual_instruction+1].arg2 = returnY(strat_instructions[actual_instruction].arg2);
ClementBreteau 15:c2fc239e85df 795 }
ClementBreteau 15:c2fc239e85df 796 modeTelemetre = 0;
ClementBreteau 14:c8fc06c4887f 797 break;
antbig 0:ad97421fb1fb 798 }
antbig 0:ad97421fb1fb 799
antbig 0:ad97421fb1fb 800 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
antbig 0:ad97421fb1fb 801 }
antbig 0:ad97421fb1fb 802 }