CRAC Team / CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT

code petit robot pour homologation

Fork of CRAC-Strat_2017_V2 by CRAC Team

Strategie/Strategie.cpp@6:eddfa414fd11, 2016-04-25 (annotated)

Committer:
antbig
Date:
Mon Apr 25 10:59:22 2016 +0000
Revision:
6:eddfa414fd11
Parent:
5:dcd817534b57
Child:
8:0edc7dfb7f7e
Correction check carte petit robot (carte actionneurs non fonctionnel)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
antbig 0:ad97421fb1fb 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
antbig 0:ad97421fb1fb 16
antbig 0:ad97421fb1fb 17 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
antbig 0:ad97421fb1fb 18
antbig 0:ad97421fb1fb 19 signed short x_robot,y_robot,theta_robot;//La position du robot
antbig 0:ad97421fb1fb 20
antbig 5:dcd817534b57 21 #ifdef ROBOT_BIG
antbig 0:ad97421fb1fb 22 unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR};
antbig 0:ad97421fb1fb 23 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR};
antbig 5:dcd817534b57 24 #else
antbig 6:eddfa414fd11 25 unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR};
antbig 6:eddfa414fd11 26 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR};
antbig 5:dcd817534b57 27 #endif
antbig 0:ad97421fb1fb 28 unsigned char checkCurrent = 0;
antbig 0:ad97421fb1fb 29 unsigned char countAliveCard = 0;
antbig 0:ad97421fb1fb 30
antbig 2:8d8e2cf798a3 31 unsigned char InversStrat = 1;//Si à 1, indique que l'on part de l'autre cote de la table(inversion des Y)
antbig 1:116040d14164 32
antbig 5:dcd817534b57 33 unsigned char countRobotNear = 0;//Le nombre de robot à proximité
antbig 5:dcd817534b57 34
antbig 4:88431b537477 35 /****************************************************************************************/
antbig 4:88431b537477 36 /* FUNCTION NAME: chronometre_ISR */
antbig 4:88431b537477 37 /* DESCRIPTION : Interruption à la fin des 90s du match */
antbig 4:88431b537477 38 /****************************************************************************************/
antbig 0:ad97421fb1fb 39 void chronometre_ISR (void)
antbig 0:ad97421fb1fb 40 {
antbig 0:ad97421fb1fb 41 SendRawId(ASSERVISSEMENT_STOP);//On stope les moteurs
antbig 0:ad97421fb1fb 42 SendRawId(GLOBAL_GAME_END);//Indication fin de match
antbig 0:ad97421fb1fb 43 gameTimer.stop();//Arret du timer
antbig 0:ad97421fb1fb 44
antbig 1:116040d14164 45 #ifdef ROBOT_BIG
antbig 0:ad97421fb1fb 46 doFunnyAction();
antbig 0:ad97421fb1fb 47 #endif
antbig 0:ad97421fb1fb 48
antbig 1:116040d14164 49 while(1);//On bloque la programme dans l'interruption
antbig 0:ad97421fb1fb 50 }
antbig 0:ad97421fb1fb 51
antbig 4:88431b537477 52 /****************************************************************************************/
antbig 4:88431b537477 53 /* FUNCTION NAME: automate_process */
antbig 4:88431b537477 54 /* DESCRIPTION : Automate de gestion de la stratégie du robot */
antbig 4:88431b537477 55 /****************************************************************************************/
antbig 0:ad97421fb1fb 56 void automate_process(void)
antbig 0:ad97421fb1fb 57 {
antbig 0:ad97421fb1fb 58 static struct S_Instruction instruction;
antbig 1:116040d14164 59 static unsigned char AX12_enchainement = 0;
antbig 1:116040d14164 60 static unsigned char MV_enchainement = 0;
antbig 0:ad97421fb1fb 61 signed char localData1 = 0;
antbig 0:ad97421fb1fb 62 signed short localData2 = 0;
antbig 0:ad97421fb1fb 63 unsigned short localData3 = 0;
antbig 0:ad97421fb1fb 64 signed short localData4 = 0;
antbig 1:116040d14164 65 unsigned char localData5 = 0;
antbig 0:ad97421fb1fb 66
antbig 0:ad97421fb1fb 67 if(gameTimer.read_ms() >= 88000) {//Fin du match (On autorise 2s pour déposer des éléments
antbig 0:ad97421fb1fb 68 gameTimer.stop();
antbig 0:ad97421fb1fb 69 gameTimer.reset();
antbig 0:ad97421fb1fb 70 gameEtat = ETAT_END;//Fin du temps
antbig 0:ad97421fb1fb 71 }
antbig 0:ad97421fb1fb 72
antbig 0:ad97421fb1fb 73 if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) {
antbig 0:ad97421fb1fb 74 lastEtat = gameEtat;
antbig 0:ad97421fb1fb 75 debugetatTimer.reset();
antbig 0:ad97421fb1fb 76 sendStratEtat((unsigned char)gameEtat);
antbig 0:ad97421fb1fb 77 }
antbig 0:ad97421fb1fb 78
antbig 0:ad97421fb1fb 79 switch(gameEtat)
antbig 0:ad97421fb1fb 80 {
antbig 0:ad97421fb1fb 81 case ETAT_CHECK_CARTE_SCREEN:
antbig 0:ad97421fb1fb 82 /*
antbig 0:ad97421fb1fb 83 Verification de l'état de la carte ecran
antbig 0:ad97421fb1fb 84 */
antbig 0:ad97421fb1fb 85 waitingAckFrom = ALIVE_IHM;//On indique que l'on attend un ack de la carte IHM
antbig 0:ad97421fb1fb 86 SendRawId(CHECK_IHM);//On demande à la carte IHM d'insiquer ça présence
antbig 0:ad97421fb1fb 87
antbig 0:ad97421fb1fb 88 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 89 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 90 cartesCheker.start();//On lance le timer pour le timeout
antbig 0:ad97421fb1fb 91 gameEtat = ETAT_CHECK_CARTE_SCREEN_WAIT_ACK;
antbig 0:ad97421fb1fb 92
antbig 0:ad97421fb1fb 93 break;
antbig 0:ad97421fb1fb 94 case ETAT_CHECK_CARTE_SCREEN_WAIT_ACK:
antbig 0:ad97421fb1fb 95 /*
antbig 0:ad97421fb1fb 96 Attente du ALIVE de la carte écran.
antbig 0:ad97421fb1fb 97
antbig 0:ad97421fb1fb 98 Si la carte ne répond pas apres 10ms, on retoune dans l'etat ETAT_CHECK_CARTE_SCREEN
antbig 0:ad97421fb1fb 99 maximum 3 tentatives
antbig 0:ad97421fb1fb 100 Si pas de réponse, clignotement de toutes les leds possible
antbig 0:ad97421fb1fb 101 */
antbig 0:ad97421fb1fb 102 if(waitingAckFrom == 0) {//C'est bon la carte est en ligne
antbig 0:ad97421fb1fb 103 cartesCheker.stop();
antbig 0:ad97421fb1fb 104 screenChecktry = 0;
antbig 0:ad97421fb1fb 105 gameEtat = ETAT_CHECK_CARTES;
antbig 0:ad97421fb1fb 106 } else if(cartesCheker.read_ms () > 100) {
antbig 0:ad97421fb1fb 107 cartesCheker.stop();
antbig 0:ad97421fb1fb 108 if(screenChecktry >=3) {
antbig 0:ad97421fb1fb 109 errorLoop();//Erreur La carte IHM n'est pas en ligne
antbig 0:ad97421fb1fb 110 } else {
antbig 0:ad97421fb1fb 111 gameEtat = ETAT_CHECK_CARTE_SCREEN;
antbig 0:ad97421fb1fb 112 }
antbig 0:ad97421fb1fb 113 }
antbig 0:ad97421fb1fb 114 break;
antbig 0:ad97421fb1fb 115 case ETAT_CHECK_CARTES:
antbig 0:ad97421fb1fb 116 /*
antbig 0:ad97421fb1fb 117 Il faut faire une boucle pour verifier toutes les cartes les une apres les autres
antbig 0:ad97421fb1fb 118 */
antbig 0:ad97421fb1fb 119 waitingAckFrom = id_alive[checkCurrent];//On indique que l'on attend un ack de la carte IHM
antbig 0:ad97421fb1fb 120 SendRawId(id_check[checkCurrent]);//On demande à la carte IHM d'insiquer ça présence
antbig 0:ad97421fb1fb 121
antbig 0:ad97421fb1fb 122 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 123 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 124 cartesCheker.start();//On lance le timer pour le timeout
antbig 0:ad97421fb1fb 125 gameEtat = ETAT_CHECK_CARTES_WAIT_ACK;
antbig 0:ad97421fb1fb 126 break;
antbig 0:ad97421fb1fb 127 case ETAT_CHECK_CARTES_WAIT_ACK:
antbig 0:ad97421fb1fb 128 /*
antbig 0:ad97421fb1fb 129 On attend l'ack de la carte en cours de vérification
antbig 0:ad97421fb1fb 130 */
antbig 0:ad97421fb1fb 131 //printf("cartesCheker = %d waitingAckFrom = %d\n",cartesCheker.read_ms(), waitingAckFrom);
antbig 0:ad97421fb1fb 132 if(waitingAckFrom == 0) {//C'est bon la carte est en ligne
antbig 0:ad97421fb1fb 133 cartesCheker.stop();
antbig 0:ad97421fb1fb 134 screenChecktry = 0;
antbig 0:ad97421fb1fb 135 countAliveCard++;
antbig 0:ad97421fb1fb 136 if(checkCurrent >= NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 137 if(countAliveCard >= NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 138 gameEtat = ETAT_CONFIG;
antbig 0:ad97421fb1fb 139 SendRawId(ECRAN_ALL_CHECK);//On dit à l'IHM que toutes les cartes sont en ligne
antbig 5:dcd817534b57 140 tactile_printf("Selection couleur et strategie");
antbig 0:ad97421fb1fb 141 } else {
antbig 0:ad97421fb1fb 142 gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
antbig 0:ad97421fb1fb 143 waitingAckFrom = ECRAN_ALL_CHECK;
antbig 0:ad97421fb1fb 144 }
antbig 0:ad97421fb1fb 145 } else {
antbig 0:ad97421fb1fb 146 checkCurrent++;
antbig 0:ad97421fb1fb 147 gameEtat = ETAT_CHECK_CARTES;
antbig 0:ad97421fb1fb 148 }
antbig 0:ad97421fb1fb 149 } else if(cartesCheker.read_ms () > 100) {
antbig 0:ad97421fb1fb 150 cartesCheker.stop();
antbig 0:ad97421fb1fb 151 if(screenChecktry >=3) {
antbig 0:ad97421fb1fb 152 screenChecktry = 0;
antbig 0:ad97421fb1fb 153 if(checkCurrent >= NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 154 if(countAliveCard == NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 155 gameEtat = ETAT_CONFIG;
antbig 0:ad97421fb1fb 156 SendRawId(ECRAN_ALL_CHECK);//On dit à l'IHM que toutes les cartes sont en ligne
antbig 0:ad97421fb1fb 157 } else {
antbig 0:ad97421fb1fb 158 gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
antbig 0:ad97421fb1fb 159 waitingAckFrom = ECRAN_ALL_CHECK;
antbig 0:ad97421fb1fb 160 }
antbig 0:ad97421fb1fb 161 } else {
antbig 0:ad97421fb1fb 162 checkCurrent++;
antbig 0:ad97421fb1fb 163 gameEtat = ETAT_CHECK_CARTES;
antbig 0:ad97421fb1fb 164 }
antbig 0:ad97421fb1fb 165 } else {
antbig 0:ad97421fb1fb 166 gameEtat = ETAT_CHECK_CARTES;
antbig 0:ad97421fb1fb 167 }
antbig 0:ad97421fb1fb 168 }
antbig 0:ad97421fb1fb 169 break;
antbig 0:ad97421fb1fb 170 case ETAT_WAIT_FORCE:
antbig 0:ad97421fb1fb 171 /*
antbig 0:ad97421fb1fb 172 Attente du forçage de la part de la carte IHM
antbig 0:ad97421fb1fb 173 */
antbig 0:ad97421fb1fb 174 if(waitingAckFrom == 0) {
antbig 0:ad97421fb1fb 175 gameEtat = ETAT_CONFIG;
antbig 0:ad97421fb1fb 176 }
antbig 0:ad97421fb1fb 177 break;
antbig 0:ad97421fb1fb 178 case ETAT_CONFIG:
antbig 0:ad97421fb1fb 179 /*
antbig 0:ad97421fb1fb 180 Attente de l'odre de choix de mode,
antbig 0:ad97421fb1fb 181 Il est possible de modifier la couleur et l'id de la stratégie
antbig 0:ad97421fb1fb 182 Il est aussi possible d'envoyer les ordres de debug
antbig 0:ad97421fb1fb 183 */
antbig 0:ad97421fb1fb 184 break;
antbig 1:116040d14164 185 case ETAT_GAME_INIT:
antbig 0:ad97421fb1fb 186 //On charge la liste des instructions
antbig 4:88431b537477 187 //strcpy(cheminFileStart,"/local/test.txt");//On ouvre le fichier test.txt
antbig 0:ad97421fb1fb 188 loadAllInstruction();//Mise en cache de toute les instructions
antbig 0:ad97421fb1fb 189 gameEtat = ETAT_GAME_WAIT_FOR_JACK;
antbig 0:ad97421fb1fb 190 SendRawId(ECRAN_ACK_START_MATCH);
antbig 0:ad97421fb1fb 191 tactile_printf("Attente du JACK.");
antbig 0:ad97421fb1fb 192 break;
antbig 0:ad97421fb1fb 193 case ETAT_GAME_WAIT_FOR_JACK:
antbig 0:ad97421fb1fb 194 //TODO Attendre le jack
antbig 1:116040d14164 195 break;
antbig 1:116040d14164 196 case ETAT_GAME_START:
antbig 1:116040d14164 197 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 1:116040d14164 198 gameTimer.reset();
antbig 1:116040d14164 199 gameTimer.start();
antbig 1:116040d14164 200 chronoEnd.attach(&chronometre_ISR,90);
antbig 0:ad97421fb1fb 201 break;
antbig 0:ad97421fb1fb 202 case ETAT_GAME_LOAD_NEXT_INSTRUCTION:
antbig 0:ad97421fb1fb 203 /*
antbig 0:ad97421fb1fb 204 Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction
antbig 0:ad97421fb1fb 205 */
antbig 0:ad97421fb1fb 206 //printf("load next instruction\n");
antbig 0:ad97421fb1fb 207 if(actual_instruction >= nb_instructions || actual_instruction == 255) {
antbig 0:ad97421fb1fb 208 gameEtat = ETAT_END;
antbig 0:ad97421fb1fb 209 //Il n'y a plus d'instruction, fin du jeu
antbig 0:ad97421fb1fb 210 } else {
antbig 0:ad97421fb1fb 211 instruction = strat_instructions[actual_instruction];
antbig 0:ad97421fb1fb 212 //On effectue le traitement de l'instruction
antbig 0:ad97421fb1fb 213 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
antbig 0:ad97421fb1fb 214 //actual_instruction++;
antbig 0:ad97421fb1fb 215 }
antbig 0:ad97421fb1fb 216 screenChecktry = 0;
antbig 1:116040d14164 217 wait_ms(100);
antbig 0:ad97421fb1fb 218 break;
antbig 0:ad97421fb1fb 219 case ETAT_GAME_PROCESS_INSTRUCTION:
antbig 0:ad97421fb1fb 220 /*
antbig 0:ad97421fb1fb 221 Traitement de l'instruction, envoie de la trame CAN
antbig 0:ad97421fb1fb 222 */
antbig 0:ad97421fb1fb 223 //debug_Instruction(instruction);
antbig 0:ad97421fb1fb 224 switch(instruction.order)
antbig 0:ad97421fb1fb 225 {
antbig 0:ad97421fb1fb 226 case MV_COURBURE:
antbig 6:eddfa414fd11 227 waitingAckID = ASSERVISSEMENT_COURBURE;
antbig 6:eddfa414fd11 228 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 229 if(instruction.nextActionType == ENCHAINEMENT) {
antbig 6:eddfa414fd11 230 MV_enchainement++;
antbig 6:eddfa414fd11 231 localData5 = 1;
antbig 6:eddfa414fd11 232 } else {
antbig 6:eddfa414fd11 233 if(MV_enchainement > 0) {
antbig 6:eddfa414fd11 234 localData5 = 2;
antbig 6:eddfa414fd11 235 MV_enchainement = 0;
antbig 6:eddfa414fd11 236 } else {
antbig 6:eddfa414fd11 237 localData5 = 0;
antbig 6:eddfa414fd11 238 }
antbig 0:ad97421fb1fb 239 }
antbig 6:eddfa414fd11 240
antbig 6:eddfa414fd11 241 localData1 = ((instruction.direction == FORWARD)?1:-1);
antbig 6:eddfa414fd11 242 BendRadius (instruction.arg1, instruction.arg3, localData1, localData5);
antbig 0:ad97421fb1fb 243 break;
antbig 0:ad97421fb1fb 244 case MV_LINE:
antbig 0:ad97421fb1fb 245 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 246 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 247 if(instruction.nextActionType == ENCHAINEMENT) {
antbig 1:116040d14164 248 MV_enchainement++;
antbig 1:116040d14164 249 localData5 = 1;
antbig 1:116040d14164 250 } else {
antbig 1:116040d14164 251 if(MV_enchainement > 0) {
antbig 1:116040d14164 252 localData5 = 2;
antbig 1:116040d14164 253 MV_enchainement = 0;
antbig 1:116040d14164 254 } else {
antbig 1:116040d14164 255 localData5 = 0;
antbig 1:116040d14164 256 }
antbig 0:ad97421fb1fb 257 }
antbig 1:116040d14164 258 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
antbig 1:116040d14164 259 GoStraight(localData2, 0, 0, localData5);
antbig 0:ad97421fb1fb 260 break;
antbig 0:ad97421fb1fb 261 case MV_TURN:
antbig 0:ad97421fb1fb 262 if(instruction.direction == RELATIVE) {
antbig 0:ad97421fb1fb 263 localData2 = instruction.arg3;
antbig 0:ad97421fb1fb 264 } else {
antbig 0:ad97421fb1fb 265 if(abs(instruction.arg3 - theta_robot) > 180) {
antbig 0:ad97421fb1fb 266 localData2 = 360 - instruction.arg3 - theta_robot;
antbig 0:ad97421fb1fb 267 } else {
antbig 0:ad97421fb1fb 268 localData2 = instruction.arg3 - theta_robot;
antbig 0:ad97421fb1fb 269 }
antbig 0:ad97421fb1fb 270 }
antbig 2:8d8e2cf798a3 271 if(InversStrat == 1) {
antbig 2:8d8e2cf798a3 272 localData2 = -localData2;
antbig 2:8d8e2cf798a3 273 }
antbig 0:ad97421fb1fb 274 Rotate(localData2);
antbig 0:ad97421fb1fb 275 waitingAckID = ASSERVISSEMENT_ROTATION;
antbig 0:ad97421fb1fb 276 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 277 break;
antbig 0:ad97421fb1fb 278 case MV_XYT:
antbig 0:ad97421fb1fb 279 if(instruction.direction == BACKWARD) {
antbig 0:ad97421fb1fb 280 localData1 = -1;
antbig 0:ad97421fb1fb 281 } else {
antbig 0:ad97421fb1fb 282 localData1 = 1;
antbig 0:ad97421fb1fb 283 }
antbig 2:8d8e2cf798a3 284
antbig 2:8d8e2cf798a3 285 if(InversStrat == 1) {
antbig 2:8d8e2cf798a3 286 //localData2 = 360 - instruction.arg3
antbig 2:8d8e2cf798a3 287 localData3 = 3000 - instruction.arg2;//Inversion du Y
antbig 2:8d8e2cf798a3 288 } else {
antbig 2:8d8e2cf798a3 289 localData3 = instruction.arg2;
antbig 2:8d8e2cf798a3 290 }
antbig 2:8d8e2cf798a3 291 GoToPosition(instruction.arg1,localData3,instruction.arg3,localData1);
antbig 0:ad97421fb1fb 292 waitingAckID = ASSERVISSEMENT_XYT;
antbig 0:ad97421fb1fb 293 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 294 break;
antbig 0:ad97421fb1fb 295 case MV_RECALAGE:
antbig 0:ad97421fb1fb 296 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 297 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 298 //TODO - mettre l'ACK à jour
antbig 0:ad97421fb1fb 299 break;
antbig 0:ad97421fb1fb 300 case ACTION:
antbig 0:ad97421fb1fb 301 if(doAction(instruction.arg1,instruction.arg2,instruction.arg3)) {
antbig 0:ad97421fb1fb 302 //L'action est spécifique
antbig 0:ad97421fb1fb 303 } else {
antbig 0:ad97421fb1fb 304 //C'est un AX12 qu'il faut bouger
antbig 2:8d8e2cf798a3 305 AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
antbig 1:116040d14164 306 AX12_enchainement++;
antbig 0:ad97421fb1fb 307 }
antbig 1:116040d14164 308 waitingAckFrom = 0;
antbig 1:116040d14164 309 waitingAckID = 0;
antbig 0:ad97421fb1fb 310 break;
antbig 0:ad97421fb1fb 311 default:
antbig 0:ad97421fb1fb 312 //Instruction inconnue, on l'ignore
antbig 0:ad97421fb1fb 313 break;
antbig 0:ad97421fb1fb 314 }
antbig 0:ad97421fb1fb 315
antbig 0:ad97421fb1fb 316
antbig 0:ad97421fb1fb 317
antbig 0:ad97421fb1fb 318 if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) {
antbig 0:ad97421fb1fb 319 gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge
antbig 0:ad97421fb1fb 320 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 321 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 322 cartesCheker.start();
antbig 1:116040d14164 323 if(AX12_enchainement > 0) {
antbig 1:116040d14164 324 AX12_processChange();//Il faut lancer le déplacement des AX12
antbig 1:116040d14164 325 AX12_enchainement = 0;
antbig 1:116040d14164 326 }
antbig 0:ad97421fb1fb 327 } else {//C'est un enchainement
antbig 0:ad97421fb1fb 328 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 329 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
antbig 0:ad97421fb1fb 330 }
antbig 0:ad97421fb1fb 331
antbig 0:ad97421fb1fb 332 break;
antbig 0:ad97421fb1fb 333 case ETAT_GAME_WAIT_ACK:
antbig 0:ad97421fb1fb 334 /*
antbig 0:ad97421fb1fb 335 Attente de l'ack de l'instruction
antbig 0:ad97421fb1fb 336 */
antbig 0:ad97421fb1fb 337 if(waitingAckID == 0 && waitingAckFrom ==0) {//Les ack ont été reset, c'est bon on continu
antbig 0:ad97421fb1fb 338 //if(true) {
antbig 0:ad97421fb1fb 339 cartesCheker.stop();
antbig 0:ad97421fb1fb 340 if(instruction.nextActionType == JUMP) {
antbig 0:ad97421fb1fb 341 if(instruction.jumpAction == JUMP_TIME) {
antbig 0:ad97421fb1fb 342 gameEtat = ETAT_GAME_JUMP_TIME;
antbig 0:ad97421fb1fb 343 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 344 cartesCheker.start();
antbig 0:ad97421fb1fb 345 } else if(instruction.jumpAction == JUMP_POSITION) {
antbig 0:ad97421fb1fb 346 gameEtat = ETAT_GAME_JUMP_POSITION;
antbig 0:ad97421fb1fb 347 } else {
antbig 0:ad97421fb1fb 348 //ERROR
antbig 0:ad97421fb1fb 349 }
antbig 0:ad97421fb1fb 350 } else if(instruction.nextActionType == WAIT) {
antbig 0:ad97421fb1fb 351 gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
antbig 0:ad97421fb1fb 352 switch(instruction.order)
antbig 0:ad97421fb1fb 353 {
antbig 0:ad97421fb1fb 354 case MV_COURBURE:
antbig 0:ad97421fb1fb 355 waitingAckID = ASSERVISSEMENT_COURBURE;
antbig 0:ad97421fb1fb 356 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 357 break;
antbig 0:ad97421fb1fb 358 case MV_LINE:
antbig 0:ad97421fb1fb 359 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 360 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 361 break;
antbig 0:ad97421fb1fb 362 case MV_TURN:
antbig 0:ad97421fb1fb 363 waitingAckID = ASSERVISSEMENT_ROTATION;
antbig 0:ad97421fb1fb 364 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 365 break;
antbig 0:ad97421fb1fb 366 case MV_XYT:
antbig 0:ad97421fb1fb 367 waitingAckID = ASSERVISSEMENT_XYT;
antbig 0:ad97421fb1fb 368 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 369 break;
antbig 0:ad97421fb1fb 370 case MV_RECALAGE:
antbig 0:ad97421fb1fb 371 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 372 waitingAckFrom = INSTRUCTION_END_MOTEUR;
antbig 0:ad97421fb1fb 373 break;
antbig 0:ad97421fb1fb 374 case ACTION:
antbig 1:116040d14164 375 waitingAckFrom = SERVO_AX12_DONE;
antbig 1:116040d14164 376 waitingAckID = instruction.arg1;
antbig 0:ad97421fb1fb 377 break;
antbig 0:ad97421fb1fb 378 default:
antbig 0:ad97421fb1fb 379 break;
antbig 0:ad97421fb1fb 380 }
antbig 0:ad97421fb1fb 381 } else {
antbig 0:ad97421fb1fb 382 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 0:ad97421fb1fb 383 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 384 }
antbig 0:ad97421fb1fb 385 } else if(cartesCheker.read_ms () > 50){
antbig 0:ad97421fb1fb 386 cartesCheker.stop();
antbig 0:ad97421fb1fb 387 if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur
antbig 0:ad97421fb1fb 388 actual_instruction = instruction.nextLineError;
antbig 0:ad97421fb1fb 389 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 0:ad97421fb1fb 390 } else {
antbig 0:ad97421fb1fb 391 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
antbig 0:ad97421fb1fb 392 }
antbig 0:ad97421fb1fb 393 }
antbig 0:ad97421fb1fb 394 break;
antbig 0:ad97421fb1fb 395
antbig 0:ad97421fb1fb 396 case ETAT_GAME_JUMP_TIME:
antbig 0:ad97421fb1fb 397 if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) {
antbig 0:ad97421fb1fb 398 cartesCheker.stop();//On arrete le timer
antbig 0:ad97421fb1fb 399 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 400 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
antbig 0:ad97421fb1fb 401 }
antbig 0:ad97421fb1fb 402 break;
antbig 0:ad97421fb1fb 403 case ETAT_GAME_JUMP_POSITION:
antbig 0:ad97421fb1fb 404
antbig 0:ad97421fb1fb 405 break;
antbig 0:ad97421fb1fb 406 case ETAT_GAME_WAIT_END_INSTRUCTION:
antbig 0:ad97421fb1fb 407 if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée
antbig 0:ad97421fb1fb 408 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 409 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
antbig 0:ad97421fb1fb 410 }
antbig 0:ad97421fb1fb 411 break;
antbig 0:ad97421fb1fb 412
antbig 0:ad97421fb1fb 413
antbig 5:dcd817534b57 414 case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s
antbig 5:dcd817534b57 415 if(timeoutWarning.read_ms() >= 2000)//ça fait plus de 2s, il faut changer de stratégie
antbig 5:dcd817534b57 416 {
antbig 5:dcd817534b57 417 gameEtat = ETAT_WARNING_SWITCH_STRATEGIE;
antbig 5:dcd817534b57 418 }
antbig 5:dcd817534b57 419 break;
antbig 5:dcd817534b57 420 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 5:dcd817534b57 421
antbig 5:dcd817534b57 422 break;
antbig 5:dcd817534b57 423 case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours
antbig 5:dcd817534b57 424
antbig 5:dcd817534b57 425 break;
antbig 5:dcd817534b57 426 case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur
antbig 5:dcd817534b57 427 actual_instruction = instruction.nextLineError;
antbig 5:dcd817534b57 428 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 5:dcd817534b57 429 break;
antbig 5:dcd817534b57 430
antbig 5:dcd817534b57 431
antbig 0:ad97421fb1fb 432
antbig 0:ad97421fb1fb 433 case ETAT_END:
antbig 0:ad97421fb1fb 434
antbig 0:ad97421fb1fb 435 gameEtat = ETAT_END_LOOP;
antbig 0:ad97421fb1fb 436 break;
antbig 0:ad97421fb1fb 437 case ETAT_END_LOOP:
antbig 0:ad97421fb1fb 438 //Rien, on tourne en rond
antbig 0:ad97421fb1fb 439 break;
antbig 0:ad97421fb1fb 440 default:
antbig 0:ad97421fb1fb 441
antbig 0:ad97421fb1fb 442 break;
antbig 0:ad97421fb1fb 443 }
antbig 0:ad97421fb1fb 444 }
antbig 0:ad97421fb1fb 445
antbig 4:88431b537477 446 /****************************************************************************************/
antbig 4:88431b537477 447 /* FUNCTION NAME: canProcessRx */
antbig 4:88431b537477 448 /* DESCRIPTION : Fonction de traitement des messages CAN */
antbig 4:88431b537477 449 /****************************************************************************************/
antbig 0:ad97421fb1fb 450 void canProcessRx(void)
antbig 0:ad97421fb1fb 451 {
antbig 0:ad97421fb1fb 452 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
antbig 4:88431b537477 453 CANMessage msgTx=CANMessage();
antbig 0:ad97421fb1fb 454 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
antbig 0:ad97421fb1fb 455 if(FIFO_occupation<0)
antbig 0:ad97421fb1fb 456 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
antbig 0:ad97421fb1fb 457 if(FIFO_max_occupation<FIFO_occupation)
antbig 0:ad97421fb1fb 458 FIFO_max_occupation=FIFO_occupation;
antbig 0:ad97421fb1fb 459 if(FIFO_occupation!=0) {
antbig 0:ad97421fb1fb 460
antbig 0:ad97421fb1fb 461 switch(msgRxBuffer[FIFO_lecture].id) {
antbig 4:88431b537477 462 case DEBUG_FAKE_JAKE://Permet de lancer le match à distance
antbig 1:116040d14164 463 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
antbig 1:116040d14164 464 gameEtat = ETAT_GAME_START;
antbig 1:116040d14164 465 }
antbig 1:116040d14164 466 break;
antbig 1:116040d14164 467
antbig 0:ad97421fb1fb 468 case ALIVE_BALISE:
antbig 0:ad97421fb1fb 469 case ALIVE_MOTEUR:
antbig 0:ad97421fb1fb 470 case ALIVE_IHM:
antbig 0:ad97421fb1fb 471 case ECRAN_ALL_CHECK:
antbig 0:ad97421fb1fb 472 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
antbig 0:ad97421fb1fb 473 waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
antbig 0:ad97421fb1fb 474 }
antbig 0:ad97421fb1fb 475 break;
antbig 0:ad97421fb1fb 476
antbig 0:ad97421fb1fb 477 case ACKNOWLEDGE_BALISE:
antbig 0:ad97421fb1fb 478 case ACKNOWLEDGE_MOTEUR:
antbig 0:ad97421fb1fb 479 case ACKNOWLEDGE_IHM:
antbig 0:ad97421fb1fb 480 case INSTRUCTION_END_BALISE:
antbig 0:ad97421fb1fb 481 case INSTRUCTION_END_MOTEUR:
antbig 0:ad97421fb1fb 482 case INSTRUCTION_END_IHM:
antbig 0:ad97421fb1fb 483 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && (msgRxBuffer[FIFO_lecture].data[0]|((unsigned int)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) {
antbig 0:ad97421fb1fb 484 waitingAckFrom = 0;
antbig 0:ad97421fb1fb 485 waitingAckID = 0;
antbig 0:ad97421fb1fb 486 }
antbig 0:ad97421fb1fb 487 break;
antbig 1:116040d14164 488 #ifdef ROBOT_BIG
antbig 0:ad97421fb1fb 489 case ODOMETRIE_BIG_POSITION:
antbig 0:ad97421fb1fb 490 #else
antbig 0:ad97421fb1fb 491 case ODOMETRIE_SMALL_POSITION:
antbig 0:ad97421fb1fb 492 #endif
antbig 0:ad97421fb1fb 493 x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
antbig 0:ad97421fb1fb 494 y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
antbig 0:ad97421fb1fb 495 theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
antbig 0:ad97421fb1fb 496 break;
antbig 0:ad97421fb1fb 497
antbig 0:ad97421fb1fb 498 case ECRAN_START_MATCH:
antbig 0:ad97421fb1fb 499 if(gameEtat == ETAT_CONFIG) {
antbig 1:116040d14164 500 gameEtat = ETAT_GAME_INIT;
antbig 0:ad97421fb1fb 501 }
antbig 0:ad97421fb1fb 502 break;
antbig 0:ad97421fb1fb 503 case SERVO_AX12_SETGOAL:
antbig 0:ad97421fb1fb 504 if(AX12_isLocal(msgRxBuffer[FIFO_lecture].data[0]))
antbig 0:ad97421fb1fb 505 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 506
antbig 0:ad97421fb1fb 507 break;
antbig 0:ad97421fb1fb 508 case SERVO_AX12_PROCESS:
antbig 5:dcd817534b57 509 AX12_processChange(1);
antbig 0:ad97421fb1fb 510 break;
antbig 1:116040d14164 511 case SERVO_AX12_DONE:
antbig 1:116040d14164 512 AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0])));
antbig 1:116040d14164 513 break;
antbig 2:8d8e2cf798a3 514 case ECRAN_CHOICE_COLOR://Choix de la couleur
antbig 4:88431b537477 515 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
antbig 2:8d8e2cf798a3 516 if(msgRxBuffer[FIFO_lecture].data[0] == 0)
antbig 2:8d8e2cf798a3 517 InversStrat = 0;//Pas d'inversion de la couleur
antbig 2:8d8e2cf798a3 518 else
antbig 2:8d8e2cf798a3 519 InversStrat = 1;//Inversion de la couleur
antbig 4:88431b537477 520
antbig 4:88431b537477 521 msgTx.id=ECRAN_ACK_COLOR; // tx ack de la couleur
antbig 4:88431b537477 522 msgTx.len=1;
antbig 4:88431b537477 523 msgTx.format=CANStandard;
antbig 4:88431b537477 524 msgTx.type=CANData;
antbig 4:88431b537477 525 // couleur sur 1 octet
antbig 4:88431b537477 526 msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0];
antbig 4:88431b537477 527 can1.write(msgTx);
antbig 2:8d8e2cf798a3 528 }
antbig 2:8d8e2cf798a3 529 break;
antbig 2:8d8e2cf798a3 530 case ECRAN_CHOICE_STRAT://Choix du fichier de stratégie à utiliser
antbig 4:88431b537477 531 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
antbig 4:88431b537477 532 msgTx.id=ECRAN_ACK_STRAT; // tx ack de la couleur
antbig 4:88431b537477 533 msgTx.len=1;
antbig 4:88431b537477 534 msgTx.format=CANStandard;
antbig 4:88431b537477 535 msgTx.type=CANData;
antbig 4:88431b537477 536 if(SelectStrategy(msgRxBuffer[FIFO_lecture].data[0])) {
antbig 4:88431b537477 537 // id de la stratégie sur 1 octet
antbig 4:88431b537477 538 msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0];
antbig 4:88431b537477 539 } else {
antbig 4:88431b537477 540 //erreur sur 1 octet
antbig 4:88431b537477 541 msgTx.data[0]=0;
antbig 4:88431b537477 542 }
antbig 4:88431b537477 543 can1.write(msgTx);
antbig 2:8d8e2cf798a3 544 }
antbig 2:8d8e2cf798a3 545 break;
antbig 5:dcd817534b57 546 case BALISE_STOP:
antbig 5:dcd817534b57 547 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
antbig 6:eddfa414fd11 548 if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
antbig 6:eddfa414fd11 549 {
antbig 6:eddfa414fd11 550 wait_ms(10);
antbig 6:eddfa414fd11 551 SendRawId(ASSERVISSEMENT_STOP);
antbig 6:eddfa414fd11 552 gameEtat = ETAT_WARNING_TIMEOUT;
antbig 6:eddfa414fd11 553 timeoutWarning.reset();
antbig 6:eddfa414fd11 554 timeoutWarning.start();//Reset du timer utiliser par le timeout
antbig 6:eddfa414fd11 555 }
antbig 5:dcd817534b57 556 break;
antbig 5:dcd817534b57 557 case BALISE_END_DANGER:
antbig 5:dcd817534b57 558 if(gameEtat == ETAT_WARNING_TIMEOUT) {
antbig 5:dcd817534b57 559 timeoutWarningWaitEnd.reset();
antbig 5:dcd817534b57 560 timeoutWarningWaitEnd.start();
antbig 5:dcd817534b57 561 gameEtat = ETAT_WARING_END_BALISE_WAIT;
antbig 5:dcd817534b57 562 }
antbig 5:dcd817534b57 563 break;
antbig 0:ad97421fb1fb 564 }
antbig 0:ad97421fb1fb 565
antbig 0:ad97421fb1fb 566 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
antbig 0:ad97421fb1fb 567 }
antbig 0:ad97421fb1fb 568 }