CRAC Team
code de start qui marche a la fin du premier match, base pour la suite
Fork of
CRAC-Strat_2017_homologation_petit_rob
by 
CRAC Team
Embed:
(wiki syntax)
Show/hide line numbers
Strategie.cpp
00001 #include "Strategie.h" 00002 00003 E_stratGameEtat gameEtat = ETAT_CHECK_CARTE_SCREEN; 00004 E_stratGameEtat lastEtat = ETAT_CHECK_CARTE_SCREEN; 00005 unsigned char screenChecktry = 0; 00006 Timer cartesCheker;//Le timer pour le timeout de la vérification des cartes 00007 Timer fakeJack; 00008 Timer gameTimer; 00009 Timer debugetatTimer; 00010 Timer timeoutWarning; 00011 Timer timeoutWarningWaitEnd; 00012 Timeout chronoEnd;//permet d'envoyer la trame CAN pour la fin 00013 00014 unsigned short waitingAckID = 0;//L'id du ack attendu 00015 unsigned short waitingAckFrom = 0;//La provenance du ack attendu 00016 char modeTelemetre; // Si à 1, indique que l'on attend une reponse du telemetre 00017 unsigned short telemetreDistance = 0; 00018 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN 00019 00020 signed short x_robot,y_robot,theta_robot;//La position du robot 00021 00022 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 00023 00024 #ifdef ROBOT_BIG 00025 //unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS,CHECK_AX12,CHECK_POMPES}; 00026 //unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS,ALIVE_AX12,ALIVE_POMPES}; 00027 00028 unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_MOTEUR}; 00029 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_MOTEUR}; 00030 00031 InterruptIn jack(p25); // entrée analogique en interruption pour le jack 00032 #else 00033 //unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS}; 00034 //unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS}; 00035 00036 unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR}; 00037 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR}; 00038 00039 InterruptIn jack(p25); // entrée analogique en interruption pour le jack 00040 #endif 00041 unsigned char checkCurrent = 0; 00042 unsigned char countAliveCard = 0; 00043 00044 unsigned char InversStrat = 1;//Si à 1, indique que l'on part de l'autre cote de la table(inversion des Y) 00045 00046 unsigned char ModeDemo = 0; // Si à 1, indique que l'on est dans le mode demo 00047 00048 unsigned char countRobotNear = 0;//Le nombre de robot à proximité 00049 00050 unsigned char ingnorBaliseOnce = 0; 00051 00052 00053 /****************************************************************************************/ 00054 /* FUNCTION NAME: chronometre_ISR */ 00055 /* DESCRIPTION : Interruption à la fin des 90s du match */ 00056 /****************************************************************************************/ 00057 void chronometre_ISR (void) 00058 { 00059 SendRawId(ASSERVISSEMENT_STOP);//On stope les moteurs 00060 SendRawId(GLOBAL_GAME_END);//Indication fin de match 00061 gameTimer.stop();//Arret du timer 00062 00063 #ifdef ROBOT_BIG 00064 wait_ms(2000); 00065 doFunnyAction(); 00066 #endif 00067 00068 while(1);//On bloque la programme dans l'interruption 00069 } 00070 00071 /****************************************************************************************/ 00072 /* FUNCTION NAME: jack_ISR */ 00073 /* DESCRIPTION : Interruption en changement d'état sur le Jack */ 00074 /****************************************************************************************/ 00075 void jack_ISR (void) 00076 { 00077 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) { 00078 led4=1; 00079 gameEtat = ETAT_GAME_START;//On débute le match 00080 } 00081 } 00082 00083 /****************************************************************************************/ 00084 /* FUNCTION NAME: automate_process */ 00085 /* DESCRIPTION : Automate de gestion de la stratégie du robot */ 00086 /****************************************************************************************/ 00087 void automate_process(void) 00088 { 00089 static struct S_Instruction instruction; 00090 static unsigned char AX12_enchainement = 0; 00091 static unsigned char MV_enchainement = 0; 00092 signed char localData1 = 0; 00093 signed short localData2 = 0; 00094 unsigned short localData3 = 0; 00095 //signed short localData4 = 0; 00096 unsigned char localData5 = 0; 00097 00098 if(gameTimer.read_ms() >= 89000) {//Fin du match (On autorise 2s pour déposer des éléments 00099 gameTimer.stop(); 00100 gameTimer.reset(); 00101 gameEtat = ETAT_END;//Fin du temps 00102 } 00103 00104 if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) { 00105 lastEtat = gameEtat; 00106 debugetatTimer.reset(); 00107 sendStratEtat((unsigned char)gameEtat, (unsigned char)actual_instruction); 00108 } 00109 00110 switch(gameEtat) 00111 { 00112 case ETAT_CHECK_CARTE_SCREEN: 00113 /* 00114 Verification de l'état de la carte ecran 00115 */ 00116 waitingAckFrom = ALIVE_IHM;//On indique que l'on attend un ack de la carte IHM 00117 SendRawId(CHECK_IHM);//On demande à la carte IHM d'insiquer ça présence 00118 00119 screenChecktry++;//On incrèment le conteur de tentative de 1 00120 cartesCheker.reset();//On reset le timeOut 00121 cartesCheker.start();//On lance le timer pour le timeout 00122 gameEtat = ETAT_CHECK_CARTE_SCREEN_WAIT_ACK; 00123 00124 //gameEtat = ETAT_GAME_START; 00125 00126 break; 00127 case ETAT_CHECK_CARTE_SCREEN_WAIT_ACK: 00128 /* 00129 Attente du ALIVE de la carte écran. 00130 00131 Si la carte ne répond pas apres 10ms, on retoune dans l'etat ETAT_CHECK_CARTE_SCREEN 00132 maximum 3 tentatives 00133 Si pas de réponse, clignotement de toutes les leds possible 00134 */ 00135 if(waitingAckFrom == 0) {//C'est bon la carte est en ligne 00136 cartesCheker.stop(); 00137 screenChecktry = 0; 00138 gameEtat = ETAT_CHECK_CARTES; 00139 } else if(cartesCheker.read_ms () > 100) { 00140 cartesCheker.stop(); 00141 if(screenChecktry >=3) { 00142 errorLoop();//Erreur La carte IHM n'est pas en ligne 00143 } else { 00144 gameEtat = ETAT_CHECK_CARTE_SCREEN; 00145 } 00146 } 00147 break; 00148 case ETAT_CHECK_CARTES: 00149 /* 00150 Il faut faire une boucle pour verifier toutes les cartes les une apres les autres 00151 */ 00152 waitingAckFrom = id_alive[checkCurrent];//On indique que l'on attend un ack de la carte IHM 00153 SendRawId(id_check[checkCurrent]);//On demande à la carte d'indiquer ça présence 00154 00155 screenChecktry++;//On incrèment le conteur de tentative de 1 00156 cartesCheker.reset();//On reset le timeOut 00157 cartesCheker.start();//On lance le timer pour le timeout 00158 gameEtat = ETAT_CHECK_CARTES_WAIT_ACK; 00159 00160 00161 00162 00163 break; 00164 case ETAT_CHECK_CARTES_WAIT_ACK: 00165 /* 00166 On attend l'ack de la carte en cours de vérification 00167 */ 00168 //printf("cartesCheker = %d waitingAckFrom = %d\n",cartesCheker.read_ms(), waitingAckFrom); 00169 if(waitingAckFrom == 0) {//C'est bon la carte est en ligne 00170 cartesCheker.stop(); 00171 screenChecktry = 0; 00172 countAliveCard++; 00173 checkCurrent++; 00174 if(checkCurrent >= NOMBRE_CARTES) { 00175 //printf("all card check, missing %d cards\n",(NOMBRE_CARTES-countAliveCard)); 00176 if(countAliveCard >= NOMBRE_CARTES) { 00177 gameEtat = ETAT_CONFIG; 00178 SendRawId(ECRAN_ALL_CHECK);//On dit à l'IHM que toutes les cartes sont en ligne 00179 tactile_printf("Selection couleur et strategie"); 00180 } else { 00181 gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement 00182 waitingAckFrom = ECRAN_ALL_CHECK; 00183 } 00184 } else { 00185 gameEtat = ETAT_CHECK_CARTES; 00186 } 00187 } else if(cartesCheker.read_ms () > 100) { 00188 cartesCheker.stop(); 00189 if(screenChecktry >=3) { 00190 //printf("missing card %d\n",id_check[checkCurrent]); 00191 screenChecktry = 0; 00192 checkCurrent++; 00193 00194 if(checkCurrent >= NOMBRE_CARTES) { 00195 if(countAliveCard == NOMBRE_CARTES) { 00196 gameEtat = ETAT_CONFIG; 00197 SendRawId(ECRAN_ALL_CHECK);//On dit à l'IHM que toutes les cartes sont en ligne 00198 } else { 00199 gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement 00200 waitingAckFrom = ECRAN_ALL_CHECK; 00201 } 00202 } else { 00203 gameEtat = ETAT_CHECK_CARTES; 00204 } 00205 } else { 00206 gameEtat = ETAT_CHECK_CARTES; 00207 } 00208 } 00209 break; 00210 case ETAT_WAIT_FORCE: 00211 /* 00212 Attente du forçage de la part de la carte IHM 00213 */ 00214 if(waitingAckFrom == 0) { 00215 gameEtat = ETAT_CONFIG; 00216 } 00217 break; 00218 case ETAT_CONFIG: 00219 /* 00220 Attente de l'odre de choix de mode, 00221 Il est possible de modifier la couleur et l'id de la stratégie 00222 Il est aussi possible d'envoyer les ordres de debug 00223 */ 00224 modeTelemetre = 0; 00225 break; 00226 case ETAT_GAME_INIT: 00227 //On charge la liste des instructions 00228 loadAllInstruction();//Mise en cache de toute les instructions 00229 00230 gameEtat = ETAT_GAME_WAIT_FOR_JACK; 00231 SendRawId(ECRAN_ACK_START_MATCH); 00232 tactile_printf("Attente du JACK."); 00233 setAsservissementEtat(1);//On réactive l'asservissement 00234 jack.mode(PullDown); // désactivation de la résistance interne du jack 00235 jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack 00236 00237 #ifdef ROBOT_BIG //le gros robot n'a pas de recalage bordure pour ce placer au début, on lui envoit donc ça position 00238 localData2 = POSITION_DEBUT_T; 00239 localData3 = POSITION_DEBUT_Y; 00240 if(InversStrat == 1) { 00241 localData2 = -localData2;//Inversion theta 00242 localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y 00243 } 00244 SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2); 00245 #endif 00246 #ifdef ROBOT_SMALL 00247 localData2 = POSITION_DEBUT_T; 00248 localData3 = POSITION_DEBUT_Y; 00249 if(InversStrat == 1) { 00250 localData2 = -localData2;//Inversion theta 00251 localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y 00252 } 00253 SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,localData3,localData2); 00254 #endif 00255 break; 00256 case ETAT_GAME_WAIT_FOR_JACK: 00257 //On attend le jack 00258 break; 00259 case ETAT_GAME_START: 00260 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00261 00262 if (ModeDemo == 0){ 00263 chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s 00264 gameTimer.start(); 00265 } 00266 gameTimer.reset(); 00267 jack.fall(NULL);//On désactive l'interruption du jack 00268 SendRawId(GLOBAL_START); 00269 tactile_printf("Start");//Pas vraiment util mais bon 00270 break; 00271 case ETAT_GAME_LOAD_NEXT_INSTRUCTION: 00272 /* 00273 Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction 00274 */ 00275 //printf("load next instruction\n"); 00276 00277 if(actual_instruction >= nb_instructions || actual_instruction == 255) { 00278 gameEtat = ETAT_END; 00279 //Il n'y a plus d'instruction, fin du jeu 00280 } else { 00281 instruction = strat_instructions[actual_instruction]; 00282 //On effectue le traitement de l'instruction 00283 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION; 00284 } 00285 screenChecktry = 0; 00286 break; 00287 case ETAT_GAME_PROCESS_INSTRUCTION: 00288 /* 00289 Traitement de l'instruction, envoie de la trame CAN 00290 */ 00291 //debug_Instruction(instruction); 00292 switch(instruction.order) 00293 { 00294 case MV_COURBURE://C'est un rayon de courbure 00295 waitingAckID = ASSERVISSEMENT_COURBURE; 00296 waitingAckFrom = ACKNOWLEDGE_MOTEUR; 00297 if(instruction.nextActionType == ENCHAINEMENT) { 00298 MV_enchainement++; 00299 localData5 = 1; 00300 } else { 00301 if(MV_enchainement > 0) { 00302 localData5 = 2; 00303 MV_enchainement = 0; 00304 } else { 00305 localData5 = 0; 00306 } 00307 } 00308 localData1 = ((instruction.direction == LEFT)?1:-1); 00309 if(InversStrat == 1) 00310 { 00311 localData1 = -localData1;//Inversion de la direction 00312 } 00313 00314 BendRadius(instruction.arg1, instruction.arg3, localData1, localData5); 00315 break; 00316 case MV_LINE://Ligne droite 00317 waitingAckID = ASSERVISSEMENT_RECALAGE; 00318 waitingAckFrom = ACKNOWLEDGE_MOTEUR; 00319 if(instruction.nextActionType == ENCHAINEMENT) { 00320 MV_enchainement++; 00321 localData5 = 1; 00322 } else { 00323 if(MV_enchainement > 0) {//Utilisé en cas d'enchainement, 00324 localData5 = 2; 00325 MV_enchainement = 0; 00326 } else { 00327 localData5 = 0; 00328 } 00329 } 00330 #ifdef ROBOT_BIG 00331 if(InversStrat == 1) { 00332 /*if (instruction.direction == FORWARD) instruction.direction = BACKWARD; 00333 else instruction.direction = FORWARD;*/ 00334 instruction.direction = ((instruction.direction == FORWARD)?BACKWARD:FORWARD); 00335 } 00336 #endif 00337 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); 00338 GoStraight(localData2, 0, 0, localData5); 00339 00340 break; 00341 case MV_TURN: //Rotation sur place 00342 if(instruction.direction == RELATIVE) { 00343 localData2 = instruction.arg3; 00344 } else {//C'est un rotation absolu, il faut la convertir en relative 00345 localData2 = instruction.arg3; 00346 00347 localData2 = (localData2 - theta_robot)%3600; 00348 if(localData2 > 1800) { 00349 localData2 = localData2-3600; 00350 } 00351 00352 } 00353 00354 if(InversStrat == 1) { 00355 localData2 = -localData2; 00356 } 00357 Rotate(localData2); 00358 waitingAckID = ASSERVISSEMENT_ROTATION; 00359 waitingAckFrom = ACKNOWLEDGE_MOTEUR; 00360 break; 00361 case MV_XYT: 00362 if(instruction.direction == BACKWARD) { 00363 localData1 = -1; 00364 } else { 00365 localData1 = 1; 00366 } 00367 00368 if(InversStrat == 1) { 00369 localData2 = -instruction.arg3; 00370 localData3 = 3000 - instruction.arg2;//Inversion du Y 00371 } else { 00372 localData3 = instruction.arg2; 00373 localData2 = instruction.arg3; 00374 } 00375 GoToPosition(instruction.arg1,localData3,localData2,localData1); 00376 waitingAckID = ASSERVISSEMENT_XYT; 00377 waitingAckFrom = ACKNOWLEDGE_MOTEUR; 00378 break; 00379 case MV_RECALAGE: 00380 waitingAckID = ASSERVISSEMENT_RECALAGE; 00381 waitingAckFrom = ACKNOWLEDGE_MOTEUR; 00382 instruction.nextActionType = WAIT; 00383 localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler 00384 00385 if(instruction.precision == RECALAGE_Y) { 00386 localData5 = 2; 00387 if(InversStrat == 1) { 00388 localData3 = 3000 - instruction.arg1;//Inversion du Y 00389 } else { 00390 localData3 = instruction.arg1; 00391 } 00392 } else { 00393 localData5 = 1; 00394 localData3 = instruction.arg1; 00395 } 00396 00397 GoStraight(localData2, localData5, localData3, 0); 00398 break; 00399 case ACTION: 00400 int tempo = 0; 00401 waitingAckID= SERVO_AX12_ACTION; 00402 waitingAckFrom = ACKNOWLEDGE_AX12; 00403 tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3); 00404 if(tempo == 1){ 00405 //L'action est spécifique 00406 if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) { 00407 00408 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00409 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00410 } else { 00411 gameEtat = ETAT_GAME_WAIT_ACK; 00412 } 00413 #ifdef ROBOT_SMALL 00414 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00415 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00416 #endif 00417 return; 00418 #ifdef ROBOT_SMALL 00419 } else if (tempo == 2) { 00420 // on est dans le cas de l'avance selon le telemetre 00421 waitingAckID = ASSERVISSEMENT_RECALAGE; 00422 waitingAckFrom = ACKNOWLEDGE_MOTEUR; 00423 00424 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); 00425 GoStraight(telemetreDistance, 0, 0, 0); 00426 // on reset la distance du telemetre à 0 00427 telemetreDistance = 5000; 00428 #endif 00429 }else{ 00430 //C'est un AX12 qu'il faut bouger 00431 //AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2); 00432 //AX12_enchainement++; 00433 00434 } 00435 break; 00436 default: 00437 //Instruction inconnue, on l'ignore 00438 break; 00439 } 00440 00441 00442 00443 if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) { 00444 gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge 00445 screenChecktry++;//On incrèment le conteur de tentative de 1 00446 cartesCheker.reset();//On reset le timeOut 00447 cartesCheker.start(); 00448 if(AX12_enchainement > 0) { 00449 //AX12_processChange();//Il faut lancer le déplacement des AX12 00450 //AX12_enchainement = 0; 00451 } 00452 } else {//C'est un enchainement 00453 if(instruction.order == MV_LINE){ 00454 gameEtat = ETAT_GAME_WAIT_ACK; 00455 00456 }else{ 00457 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00458 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante 00459 } 00460 } 00461 00462 break; 00463 case ETAT_GAME_WAIT_ACK: 00464 /* 00465 Attente de l'ack de l'instruction 00466 */ 00467 if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continu 00468 //if(true) { 00469 cartesCheker.stop(); 00470 if(instruction.nextActionType == JUMP) { 00471 if(instruction.jumpAction == JUMP_POSITION) { 00472 gameEtat = ETAT_GAME_JUMP_POSITION; 00473 } else {//Pour eviter les erreurs, on dit que c'est par défaut un jump time 00474 gameEtat = ETAT_GAME_JUMP_TIME; 00475 cartesCheker.reset();//On reset le timeOut 00476 cartesCheker.start(); 00477 } 00478 } else if(instruction.nextActionType == WAIT) { 00479 gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION; 00480 switch(instruction.order) 00481 { 00482 case MV_COURBURE: 00483 waitingAckID = ASSERVISSEMENT_COURBURE; 00484 waitingAckFrom = INSTRUCTION_END_MOTEUR; 00485 break; 00486 case MV_LINE: 00487 waitingAckID = ASSERVISSEMENT_RECALAGE; 00488 waitingAckFrom = INSTRUCTION_END_MOTEUR; 00489 break; 00490 case MV_TURN: 00491 waitingAckID = ASSERVISSEMENT_ROTATION; 00492 waitingAckFrom = INSTRUCTION_END_MOTEUR; 00493 break; 00494 case MV_XYT: 00495 waitingAckID = ASSERVISSEMENT_XYT; 00496 waitingAckFrom = INSTRUCTION_END_MOTEUR; 00497 break; 00498 case MV_RECALAGE: 00499 waitingAckID = ASSERVISSEMENT_RECALAGE; 00500 waitingAckFrom = INSTRUCTION_END_MOTEUR; 00501 break; 00502 case ACTION: 00503 00504 if (modeTelemetre == 0){ 00505 if (telemetreDistance == 0){ 00506 waitingAckID = SERVO_AX12_ACTION;// instruction.arg1; 00507 waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE; 00508 }else if(telemetreDistance == 5000){ 00509 // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre 00510 waitingAckID = ASSERVISSEMENT_RECALAGE; 00511 waitingAckFrom = INSTRUCTION_END_MOTEUR; 00512 telemetreDistance = 0; 00513 } 00514 }else{ // si on attend la reponse du telemetre 00515 //modeTelemetre = 1; 00516 waitingAckID = OBJET_SUR_TABLE; 00517 waitingAckFrom = 0; 00518 } 00519 break; 00520 default: 00521 break; 00522 } 00523 } else { 00524 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00525 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00526 } 00527 } else if(cartesCheker.read_ms () > 50){ 00528 cartesCheker.stop(); 00529 if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur 00530 actual_instruction = instruction.nextLineError; 00531 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00532 } else { 00533 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction 00534 } 00535 } 00536 break; 00537 00538 case ETAT_GAME_JUMP_TIME: 00539 if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) { 00540 cartesCheker.stop();//On arrete le timer 00541 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00542 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante 00543 } 00544 break; 00545 00546 case ETAT_GAME_JUMP_CONFIG: 00547 signed int depasX = 1, depasY = 1; // servent à indiquer le sens de dépassement des coordonnées 00548 // 1 si l'instruction est plus grande que la position du robot 00549 // -1 si l'instruction est plus petite que la position du robot 00550 // 0 si l'instruction et position du robot sont proche de moins de 1cm 00551 if (abs(x_robot-instruction.JumpTimeOrX)<10){ 00552 depasX = 0; 00553 }else if(x_robot > instruction.JumpTimeOrX){ 00554 depasX = -1; 00555 } 00556 00557 if(abs(y_robot-instruction.JumpY)<10){ 00558 depasY = 0; 00559 }else if(y_robot > instruction.JumpY){ 00560 depasY = -1; 00561 } 00562 00563 gameEtat = ETAT_GAME_JUMP_POSITION; 00564 break; 00565 case ETAT_GAME_JUMP_POSITION: 00566 bool Xok = false, Yok = false; 00567 00568 if (depasX == 0){ 00569 Xok = true; 00570 }else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5){ 00571 Xok = true; 00572 } 00573 00574 if (depasY == 0){ 00575 Yok = true; 00576 }else if ((instruction.JumpY - y_robot)*depasY < -5){ 00577 Yok = true; 00578 } 00579 00580 // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante 00581 if (Xok && Yok){ 00582 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00583 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante 00584 } 00585 00586 break; 00587 case ETAT_GAME_WAIT_END_INSTRUCTION: 00588 if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée 00589 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00590 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante 00591 } 00592 break; 00593 00594 00595 case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s 00596 if(timeoutWarning.read_ms() >= BALISE_TIMEOUT)//ça fait plus de 2s, il faut changer de stratégie 00597 { 00598 gameEtat = ETAT_WARNING_SWITCH_STRATEGIE; 00599 } 00600 break; 00601 case ETAT_WARING_END_BALISE_WAIT://Attente d'une seconde apres la fin d'un End Balise pour etre sur que c'est bon 00602 if(timeoutWarningWaitEnd.read_ms() >= 1000) {//c'est bon, on repart 00603 //actual_instruction = instruction.nextLineError; 00604 gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION; 00605 } 00606 break; 00607 case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours 00608 #ifdef ROBOT_BIG 00609 actual_instruction = instruction.nextLineError;// 2 //Modification directe... c'est pas bien mais ça marchait pour le match 5 00610 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00611 #else 00612 actual_instruction = instruction.nextLineError; 00613 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00614 #endif 00615 gameEtat = ETAT_END; 00616 00617 break; 00618 case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur 00619 actual_instruction = instruction.nextLineError; 00620 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00621 ingnorBaliseOnce = 1; 00622 break; 00623 00624 00625 00626 case ETAT_END: 00627 if (ModeDemo){ 00628 gameEtat = ETAT_CHECK_CARTE_SCREEN; 00629 ModeDemo = 1; 00630 } else { 00631 gameEtat = ETAT_END_LOOP; 00632 } 00633 break; 00634 case ETAT_END_LOOP: 00635 //Rien, on tourne en rond 00636 00637 break; 00638 default: 00639 00640 break; 00641 } 00642 } 00643 00644 /****************************************************************************************/ 00645 /* FUNCTION NAME: canProcessRx */ 00646 /* DESCRIPTION : Fonction de traitement des messages CAN */ 00647 /****************************************************************************************/ 00648 void canProcessRx(void) 00649 { 00650 static signed char FIFO_occupation=0,FIFO_max_occupation=0; 00651 CANMessage msgTx=CANMessage(); 00652 FIFO_occupation=FIFO_ecriture-FIFO_lecture; 00653 if(FIFO_occupation<0) 00654 FIFO_occupation=FIFO_occupation+SIZE_FIFO; 00655 if(FIFO_max_occupation<FIFO_occupation) 00656 FIFO_max_occupation=FIFO_occupation; 00657 if(FIFO_occupation!=0) { 00658 00659 switch(msgRxBuffer[FIFO_lecture].id) { 00660 case DEBUG_FAKE_JAKE://Permet de lancer le match à distance 00661 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) { 00662 gameEtat = ETAT_GAME_START; 00663 } 00664 break; 00665 00666 case ALIVE_BALISE: 00667 case ALIVE_MOTEUR: 00668 case ALIVE_IHM: 00669 case ALIVE_ACTIONNEURS: 00670 case ALIVE_POMPES: 00671 case ALIVE_AX12: 00672 case ECRAN_ALL_CHECK: 00673 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) { 00674 waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne 00675 } 00676 break; 00677 00678 case ACKNOWLEDGE_BALISE: 00679 case ACKNOWLEDGE_MOTEUR: 00680 case ACKNOWLEDGE_IHM: 00681 case ACKNOWLEDGE_TELEMETRE: 00682 case ACKNOWLEDGE_AX12: 00683 case INSTRUCTION_END_BALISE: 00684 case INSTRUCTION_END_MOTEUR: 00685 case INSTRUCTION_END_IHM: 00686 case INSTRUCTION_END_AX12: 00687 00688 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) { 00689 waitingAckFrom = 0; 00690 waitingAckID = 0; 00691 } 00692 break; 00693 #ifdef ROBOT_BIG 00694 case ODOMETRIE_BIG_POSITION: 00695 #else 00696 case ODOMETRIE_SMALL_POSITION: 00697 #endif 00698 x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8); 00699 y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8); 00700 theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8); 00701 break; 00702 00703 case ECRAN_START_MATCH: 00704 if(gameEtat == ETAT_CONFIG) { 00705 gameEtat = ETAT_GAME_INIT; 00706 } 00707 break; 00708 case SERVO_AX12_SETGOAL: 00709 //SendAck(0x114, SERVO_AX12_SETGOAL); 00710 //if(AX12_isLocal(msgRxBuffer[FIFO_lecture].data[0])) 00711 //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)); 00712 00713 break; 00714 00715 case SERVO_AX12_PROCESS: 00716 SendAck(0x114, SERVO_AX12_PROCESS); 00717 //AX12_processChange(1); 00718 break; 00719 00720 case SERVO_AX12_DONE: 00721 SendRawId(POMPE_PWM); 00722 /*//SendAck(0x114, SERVO_AX12_DONE); 00723 AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0]))); 00724 00725 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00726 waitingAckFrom = 0; 00727 waitingAckID = 0;*/ 00728 00729 break; 00730 case ECRAN_CHOICE_COLOR://Choix de la couleur 00731 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config 00732 if(msgRxBuffer[FIFO_lecture].data[0] == 0) 00733 InversStrat = 0;//Pas d'inversion de la couleur 00734 else 00735 InversStrat = 1;//Inversion de la couleur 00736 00737 msgTx.id=ECRAN_ACK_COLOR; // tx ack de la couleur 00738 msgTx.len=1; 00739 msgTx.format=CANStandard; 00740 msgTx.type=CANData; 00741 // couleur sur 1 octet 00742 msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0]; 00743 can1.write(msgTx); 00744 00745 } 00746 break; 00747 00748 case ECRAN_CHOICE_STRAT://Choix du fichier de stratégie à utiliser 00749 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config 00750 msgTx.id=ECRAN_ACK_STRAT; // tx ack de la couleur 00751 msgTx.len=1; 00752 msgTx.format=CANStandard; 00753 msgTx.type=CANData; 00754 if(SelectStrategy(msgRxBuffer[FIFO_lecture].data[0])) { 00755 // id de la stratégie sur 1 octet 00756 if (msgRxBuffer[FIFO_lecture].data[0] < 0x10){ // Si la strat est une strat de match, on desactive le mode demo 00757 ModeDemo = 0; 00758 } else { // sinon, on active le mode demo, utile pour la fin de la demo 00759 ModeDemo = 1; 00760 } 00761 00762 msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0]; 00763 } else { 00764 //erreur sur 1 octet 00765 msgTx.data[0]=0; 00766 } 00767 can1.write(msgTx); 00768 wait_ms(10); 00769 setAsservissementEtat(0);//Désactivation de l'asservissement pour repositionner le robot dans le zone de départ 00770 tactile_printf("Strat %d, Asser desactive",msgTx.data[0]); 00771 } 00772 break; 00773 case BALISE_DANGER : 00774 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER); 00775 break; 00776 00777 case BALISE_STOP: 00778 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP); 00779 //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, 00780 if(needToStop() != 0 && ingnorBaliseOnce ==0) { 00781 if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT) 00782 { 00783 SendRawId(ASSERVISSEMENT_STOP); 00784 while(1); // ligne à décommenter si on est en homologation 00785 gameEtat = ETAT_WARNING_TIMEOUT; 00786 if(gameEtat != ETAT_WARING_END_BALISE_WAIT) { 00787 timeoutWarning.reset(); 00788 timeoutWarning.start();//Reset du timer utiliser par le timeout 00789 } 00790 } 00791 } 00792 //} 00793 ingnorBaliseOnce = 0; 00794 break; 00795 00796 case BALISE_END_DANGER: 00797 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER); 00798 if(gameEtat == ETAT_WARNING_TIMEOUT) { 00799 timeoutWarningWaitEnd.reset(); 00800 timeoutWarningWaitEnd.start(); 00801 gameEtat = ETAT_WARING_END_BALISE_WAIT; 00802 } 00803 break; 00804 00805 case ECRAN_CHOICE_START_ACTION: 00806 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config 00807 if(msgRxBuffer[FIFO_lecture].data[0] == 1) { 00808 runRobotTest(); 00809 } else { 00810 initRobotActionneur(); 00811 } 00812 wait_ms(500); 00813 SendRawId(ECRAN_ACK_CHOICE_START_ACTION); 00814 } 00815 break; 00816 00817 case OBJET_SUR_TABLE: 00818 if (msgRxBuffer[FIFO_lecture].data[1] == 0xff){ 00819 00820 gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION; 00821 } 00822 else{ 00823 00824 waitingAckFrom = 0; 00825 waitingAckID = 0; 00826 00827 strat_instructions[actual_instruction+1].arg1 = returnX(strat_instructions[actual_instruction].arg2); 00828 strat_instructions[actual_instruction+1].arg2 = returnY(strat_instructions[actual_instruction].arg2); 00829 } 00830 modeTelemetre = 0; 00831 break; 00832 } 00833 00834 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; 00835 } 00836 }
Generated on Wed Jul 13 2022 02:05:42 by
1.7.2