CRAC Team / Mbed 2 deprecated carte_straegie_2019

Programme carte strategie (disco)

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@25:f140c93a8666, 2017-05-26 (annotated)

Committer:
ClementBreteau
Date:
Fri May 26 01:27:49 2017 +0000
Revision:
25:f140c93a8666
Parent:
24:4a28a72781a6
Child:
26:2f4fcc2354f3
code de strat deuxi?me nuit, rajout de l?inversion bien, et de la reprise apr?s une balise

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