CRAC Team
code petit robot pour homologation
Fork of
CRAC-Strat_2017_V2
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 SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T); 00248 #endif 00249 break; 00250 case ETAT_GAME_WAIT_FOR_JACK: 00251 //On attend le jack 00252 break; 00253 case ETAT_GAME_START: 00254 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00255 00256 if (ModeDemo == 0){ 00257 chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s 00258 gameTimer.start(); 00259 } 00260 gameTimer.reset(); 00261 jack.fall(NULL);//On désactive l'interruption du jack 00262 SendRawId(GLOBAL_START); 00263 tactile_printf("Start");//Pas vraiment util mais bon 00264 break; 00265 case ETAT_GAME_LOAD_NEXT_INSTRUCTION: 00266 /* 00267 Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction 00268 */ 00269 //printf("load next instruction\n"); 00270 00271 if(actual_instruction >= nb_instructions || actual_instruction == 255) { 00272 gameEtat = ETAT_END; 00273 //Il n'y a plus d'instruction, fin du jeu 00274 } else { 00275 instruction = strat_instructions[actual_instruction]; 00276 //On effectue le traitement de l'instruction 00277 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION; 00278 } 00279 screenChecktry = 0; 00280 break; 00281 case ETAT_GAME_PROCESS_INSTRUCTION: 00282 /* 00283 Traitement de l'instruction, envoie de la trame CAN 00284 */ 00285 //debug_Instruction(instruction); 00286 switch(instruction.order) 00287 { 00288 case MV_COURBURE://C'est un rayon de courbure 00289 waitingAckID = ASSERVISSEMENT_COURBURE; 00290 waitingAckFrom = ACKNOWLEDGE_MOTEUR; 00291 if(instruction.nextActionType == ENCHAINEMENT) { 00292 MV_enchainement++; 00293 localData5 = 1; 00294 } else { 00295 if(MV_enchainement > 0) { 00296 localData5 = 2; 00297 MV_enchainement = 0; 00298 } else { 00299 localData5 = 0; 00300 } 00301 } 00302 localData1 = ((instruction.direction == LEFT)?1:-1); 00303 if(InversStrat == 1) 00304 { 00305 localData1 = -localData1;//Inversion de la direction 00306 } 00307 00308 BendRadius(instruction.arg1, instruction.arg3, localData1, localData5); 00309 break; 00310 case MV_LINE://Ligne droite 00311 waitingAckID = ASSERVISSEMENT_RECALAGE; 00312 waitingAckFrom = ACKNOWLEDGE_MOTEUR; 00313 if(instruction.nextActionType == ENCHAINEMENT) { 00314 MV_enchainement++; 00315 localData5 = 1; 00316 } else { 00317 if(MV_enchainement > 0) {//Utilisé en cas d'enchainement, 00318 localData5 = 2; 00319 MV_enchainement = 0; 00320 } else { 00321 localData5 = 0; 00322 } 00323 } 00324 #ifdef ROBOT_BIG 00325 if(InversStrat == 1) { 00326 /*if (instruction.direction == FORWARD) instruction.direction = BACKWARD; 00327 else instruction.direction = FORWARD;*/ 00328 instruction.direction = ((instruction.direction == FORWARD)?BACKWARD:FORWARD); 00329 } 00330 #endif 00331 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); 00332 GoStraight(localData2, 0, 0, localData5); 00333 00334 break; 00335 case MV_TURN: //Rotation sur place 00336 if(instruction.direction == RELATIVE) { 00337 localData2 = instruction.arg3; 00338 } else {//C'est un rotation absolu, il faut la convertir en relative 00339 localData2 = instruction.arg3; 00340 00341 if(InversStrat == 1) { 00342 localData2 = -localData2; 00343 } 00344 00345 localData2 = (localData2 - theta_robot)%3600; 00346 if(localData2 > 1800) { 00347 localData2 = localData2-3600; 00348 } 00349 00350 } 00351 00352 Rotate(localData2); 00353 waitingAckID = ASSERVISSEMENT_ROTATION; 00354 waitingAckFrom = ACKNOWLEDGE_MOTEUR; 00355 break; 00356 case MV_XYT: 00357 if(instruction.direction == BACKWARD) { 00358 localData1 = -1; 00359 } else { 00360 localData1 = 1; 00361 } 00362 00363 if(InversStrat == 1) { 00364 localData2 = -instruction.arg3; 00365 localData3 = 3000 - instruction.arg2;//Inversion du Y 00366 } else { 00367 localData3 = instruction.arg2; 00368 localData2 = instruction.arg3; 00369 } 00370 GoToPosition(instruction.arg1,localData3,localData2,localData1); 00371 waitingAckID = ASSERVISSEMENT_XYT; 00372 waitingAckFrom = ACKNOWLEDGE_MOTEUR; 00373 break; 00374 case MV_RECALAGE: 00375 waitingAckID = ASSERVISSEMENT_RECALAGE; 00376 waitingAckFrom = ACKNOWLEDGE_MOTEUR; 00377 instruction.nextActionType = WAIT; 00378 localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler 00379 00380 if(instruction.precision == RECALAGE_Y) { 00381 localData5 = 2; 00382 if(InversStrat == 1) { 00383 localData3 = 3000 - instruction.arg1;//Inversion du Y 00384 } else { 00385 localData3 = instruction.arg1; 00386 } 00387 } else { 00388 localData5 = 1; 00389 localData3 = instruction.arg1; 00390 } 00391 00392 GoStraight(localData2, localData5, localData3, 0); 00393 break; 00394 case ACTION: 00395 int tempo = 0; 00396 waitingAckID= SERVO_AX12_ACTION; 00397 waitingAckFrom = ACKNOWLEDGE_AX12; 00398 tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3); 00399 if(tempo == 1){ 00400 //L'action est spécifique 00401 if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) { 00402 00403 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00404 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00405 } else { 00406 gameEtat = ETAT_GAME_WAIT_ACK; 00407 } 00408 #ifdef ROBOT_SMALL 00409 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00410 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00411 #endif 00412 return; 00413 #ifdef ROBOT_SMALL 00414 } else if (tempo == 2) { 00415 // on est dans le cas de l'avance selon le telemetre 00416 waitingAckID = ASSERVISSEMENT_RECALAGE; 00417 waitingAckFrom = ACKNOWLEDGE_MOTEUR; 00418 00419 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); 00420 GoStraight(telemetreDistance, 0, 0, 0); 00421 // on reset la distance du telemetre à 0 00422 telemetreDistance = 5000; 00423 #endif 00424 }else{ 00425 //C'est un AX12 qu'il faut bouger 00426 //AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2); 00427 //AX12_enchainement++; 00428 00429 } 00430 break; 00431 default: 00432 //Instruction inconnue, on l'ignore 00433 break; 00434 } 00435 00436 00437 00438 if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) { 00439 gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge 00440 screenChecktry++;//On incrèment le conteur de tentative de 1 00441 cartesCheker.reset();//On reset le timeOut 00442 cartesCheker.start(); 00443 if(AX12_enchainement > 0) { 00444 //AX12_processChange();//Il faut lancer le déplacement des AX12 00445 //AX12_enchainement = 0; 00446 } 00447 } else {//C'est un enchainement 00448 if(instruction.order == MV_LINE){ 00449 gameEtat = ETAT_GAME_WAIT_ACK; 00450 00451 }else{ 00452 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00453 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante 00454 } 00455 } 00456 00457 break; 00458 case ETAT_GAME_WAIT_ACK: 00459 /* 00460 Attente de l'ack de l'instruction 00461 */ 00462 if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continu 00463 //if(true) { 00464 cartesCheker.stop(); 00465 if(instruction.nextActionType == JUMP) { 00466 if(instruction.jumpAction == JUMP_POSITION) { 00467 gameEtat = ETAT_GAME_JUMP_POSITION; 00468 } else {//Pour eviter les erreurs, on dit que c'est par défaut un jump time 00469 gameEtat = ETAT_GAME_JUMP_TIME; 00470 cartesCheker.reset();//On reset le timeOut 00471 cartesCheker.start(); 00472 } 00473 } else if(instruction.nextActionType == WAIT) { 00474 gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION; 00475 switch(instruction.order) 00476 { 00477 case MV_COURBURE: 00478 waitingAckID = ASSERVISSEMENT_COURBURE; 00479 waitingAckFrom = INSTRUCTION_END_MOTEUR; 00480 break; 00481 case MV_LINE: 00482 waitingAckID = ASSERVISSEMENT_RECALAGE; 00483 waitingAckFrom = INSTRUCTION_END_MOTEUR; 00484 break; 00485 case MV_TURN: 00486 waitingAckID = ASSERVISSEMENT_ROTATION; 00487 waitingAckFrom = INSTRUCTION_END_MOTEUR; 00488 break; 00489 case MV_XYT: 00490 waitingAckID = ASSERVISSEMENT_XYT; 00491 waitingAckFrom = INSTRUCTION_END_MOTEUR; 00492 break; 00493 case MV_RECALAGE: 00494 waitingAckID = ASSERVISSEMENT_RECALAGE; 00495 waitingAckFrom = INSTRUCTION_END_MOTEUR; 00496 break; 00497 case ACTION: 00498 00499 if (modeTelemetre == 0){ 00500 if (telemetreDistance == 0){ 00501 waitingAckID = SERVO_AX12_ACTION;// instruction.arg1; 00502 waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE; 00503 }else if(telemetreDistance == 5000){ 00504 // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre 00505 waitingAckID = ASSERVISSEMENT_RECALAGE; 00506 waitingAckFrom = INSTRUCTION_END_MOTEUR; 00507 telemetreDistance = 0; 00508 } 00509 }else{ // si on attend la reponse du telemetre 00510 //modeTelemetre = 1; 00511 waitingAckID = OBJET_SUR_TABLE; 00512 waitingAckFrom = 0; 00513 } 00514 break; 00515 default: 00516 break; 00517 } 00518 } else { 00519 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00520 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00521 } 00522 } else if(cartesCheker.read_ms () > 50){ 00523 cartesCheker.stop(); 00524 if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur 00525 actual_instruction = instruction.nextLineError; 00526 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00527 } else { 00528 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction 00529 } 00530 } 00531 break; 00532 00533 case ETAT_GAME_JUMP_TIME: 00534 if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) { 00535 cartesCheker.stop();//On arrete le timer 00536 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00537 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante 00538 } 00539 break; 00540 00541 case ETAT_GAME_JUMP_CONFIG: 00542 signed int depasX = 1, depasY = 1; // servent à indiquer le sens de dépassement des coordonnées 00543 // 1 si l'instruction est plus grande que la position du robot 00544 // -1 si l'instruction est plus petite que la position du robot 00545 // 0 si l'instruction et position du robot sont proche de moins de 1cm 00546 if (abs(x_robot-instruction.JumpTimeOrX)<10){ 00547 depasX = 0; 00548 }else if(x_robot > instruction.JumpTimeOrX){ 00549 depasX = -1; 00550 } 00551 00552 if(abs(y_robot-instruction.JumpY)<10){ 00553 depasY = 0; 00554 }else if(y_robot > instruction.JumpY){ 00555 depasY = -1; 00556 } 00557 00558 gameEtat = ETAT_GAME_JUMP_POSITION; 00559 break; 00560 case ETAT_GAME_JUMP_POSITION: 00561 bool Xok = false, Yok = false; 00562 00563 if (depasX == 0){ 00564 Xok = true; 00565 }else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5){ 00566 Xok = true; 00567 } 00568 00569 if (depasY == 0){ 00570 Yok = true; 00571 }else if ((instruction.JumpY - y_robot)*depasY < -5){ 00572 Yok = true; 00573 } 00574 00575 // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante 00576 if (Xok && Yok){ 00577 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00578 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante 00579 } 00580 00581 break; 00582 case ETAT_GAME_WAIT_END_INSTRUCTION: 00583 if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée 00584 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante 00585 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante 00586 } 00587 break; 00588 00589 00590 case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s 00591 if(timeoutWarning.read_ms() >= BALISE_TIMEOUT)//ça fait plus de 2s, il faut changer de stratégie 00592 { 00593 gameEtat = ETAT_WARNING_SWITCH_STRATEGIE; 00594 } 00595 break; 00596 case ETAT_WARING_END_BALISE_WAIT://Attente d'une seconde apres la fin d'un End Balise pour etre sur que c'est bon 00597 if(timeoutWarningWaitEnd.read_ms() >= 1000) {//c'est bon, on repart 00598 //actual_instruction = instruction.nextLineError; 00599 gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION; 00600 } 00601 break; 00602 case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours 00603 #ifdef ROBOT_BIG 00604 actual_instruction = instruction.nextLineError;// 2 //Modification directe... c'est pas bien mais ça marchait pour le match 5 00605 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00606 #else 00607 actual_instruction = instruction.nextLineError; 00608 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00609 #endif 00610 gameEtat = ETAT_END; 00611 break; 00612 case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur 00613 actual_instruction = instruction.nextLineError; 00614 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00615 ingnorBaliseOnce = 1; 00616 break; 00617 00618 00619 00620 case ETAT_END: 00621 if (ModeDemo){ 00622 gameEtat = ETAT_CHECK_CARTE_SCREEN; 00623 ModeDemo = 1; 00624 } else { 00625 gameEtat = ETAT_END_LOOP; 00626 } 00627 break; 00628 case ETAT_END_LOOP: 00629 //Rien, on tourne en rond 00630 00631 break; 00632 default: 00633 00634 break; 00635 } 00636 } 00637 00638 /****************************************************************************************/ 00639 /* FUNCTION NAME: canProcessRx */ 00640 /* DESCRIPTION : Fonction de traitement des messages CAN */ 00641 /****************************************************************************************/ 00642 void canProcessRx(void) 00643 { 00644 static signed char FIFO_occupation=0,FIFO_max_occupation=0; 00645 CANMessage msgTx=CANMessage(); 00646 FIFO_occupation=FIFO_ecriture-FIFO_lecture; 00647 if(FIFO_occupation<0) 00648 FIFO_occupation=FIFO_occupation+SIZE_FIFO; 00649 if(FIFO_max_occupation<FIFO_occupation) 00650 FIFO_max_occupation=FIFO_occupation; 00651 if(FIFO_occupation!=0) { 00652 00653 switch(msgRxBuffer[FIFO_lecture].id) { 00654 case DEBUG_FAKE_JAKE://Permet de lancer le match à distance 00655 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) { 00656 gameEtat = ETAT_GAME_START; 00657 } 00658 break; 00659 00660 case ALIVE_BALISE: 00661 case ALIVE_MOTEUR: 00662 case ALIVE_IHM: 00663 case ALIVE_ACTIONNEURS: 00664 case ALIVE_POMPES: 00665 case ALIVE_AX12: 00666 case ECRAN_ALL_CHECK: 00667 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) { 00668 waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne 00669 } 00670 break; 00671 00672 case ACKNOWLEDGE_BALISE: 00673 case ACKNOWLEDGE_MOTEUR: 00674 case ACKNOWLEDGE_IHM: 00675 case ACKNOWLEDGE_TELEMETRE: 00676 case ACKNOWLEDGE_AX12: 00677 case INSTRUCTION_END_BALISE: 00678 case INSTRUCTION_END_MOTEUR: 00679 case INSTRUCTION_END_IHM: 00680 case INSTRUCTION_END_AX12: 00681 00682 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) { 00683 waitingAckFrom = 0; 00684 waitingAckID = 0; 00685 } 00686 break; 00687 #ifdef ROBOT_BIG 00688 case ODOMETRIE_BIG_POSITION: 00689 #else 00690 case ODOMETRIE_SMALL_POSITION: 00691 #endif 00692 x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8); 00693 y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8); 00694 theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8); 00695 break; 00696 00697 case ECRAN_START_MATCH: 00698 if(gameEtat == ETAT_CONFIG) { 00699 gameEtat = ETAT_GAME_INIT; 00700 } 00701 break; 00702 case SERVO_AX12_SETGOAL: 00703 //SendAck(0x114, SERVO_AX12_SETGOAL); 00704 //if(AX12_isLocal(msgRxBuffer[FIFO_lecture].data[0])) 00705 //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)); 00706 00707 break; 00708 00709 case SERVO_AX12_PROCESS: 00710 SendAck(0x114, SERVO_AX12_PROCESS); 00711 //AX12_processChange(1); 00712 break; 00713 00714 case SERVO_AX12_DONE: 00715 SendRawId(POMPE_PWM); 00716 /*//SendAck(0x114, SERVO_AX12_DONE); 00717 AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0]))); 00718 00719 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 00720 waitingAckFrom = 0; 00721 waitingAckID = 0;*/ 00722 00723 break; 00724 case ECRAN_CHOICE_COLOR://Choix de la couleur 00725 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config 00726 if(msgRxBuffer[FIFO_lecture].data[0] == 0) 00727 InversStrat = 0;//Pas d'inversion de la couleur 00728 else 00729 InversStrat = 1;//Inversion de la couleur 00730 00731 msgTx.id=ECRAN_ACK_COLOR; // tx ack de la couleur 00732 msgTx.len=1; 00733 msgTx.format=CANStandard; 00734 msgTx.type=CANData; 00735 // couleur sur 1 octet 00736 msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0]; 00737 can1.write(msgTx); 00738 00739 } 00740 break; 00741 00742 case ECRAN_CHOICE_STRAT://Choix du fichier de stratégie à utiliser 00743 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config 00744 msgTx.id=ECRAN_ACK_STRAT; // tx ack de la couleur 00745 msgTx.len=1; 00746 msgTx.format=CANStandard; 00747 msgTx.type=CANData; 00748 if(SelectStrategy(msgRxBuffer[FIFO_lecture].data[0])) { 00749 // id de la stratégie sur 1 octet 00750 if (msgRxBuffer[FIFO_lecture].data[0] < 0x10){ // Si la strat est une strat de match, on desactive le mode demo 00751 ModeDemo = 0; 00752 } else { // sinon, on active le mode demo, utile pour la fin de la demo 00753 ModeDemo = 1; 00754 } 00755 00756 msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0]; 00757 } else { 00758 //erreur sur 1 octet 00759 msgTx.data[0]=0; 00760 } 00761 can1.write(msgTx); 00762 wait_ms(10); 00763 setAsservissementEtat(0);//Désactivation de l'asservissement pour repositionner le robot dans le zone de départ 00764 tactile_printf("Strat %d, Asser desactive",msgTx.data[0]); 00765 } 00766 break; 00767 case BALISE_DANGER : 00768 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER); 00769 break; 00770 00771 case BALISE_STOP: 00772 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP); 00773 //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, 00774 if(needToStop() != 0 && ingnorBaliseOnce ==0) { 00775 if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT) 00776 { 00777 SendRawId(ASSERVISSEMENT_STOP); 00778 //while(1); 00779 gameEtat = ETAT_WARNING_TIMEOUT; 00780 if(gameEtat != ETAT_WARING_END_BALISE_WAIT) { 00781 timeoutWarning.reset(); 00782 timeoutWarning.start();//Reset du timer utiliser par le timeout 00783 } 00784 } 00785 } 00786 //} 00787 ingnorBaliseOnce = 0; 00788 break; 00789 00790 case BALISE_END_DANGER: 00791 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER); 00792 if(gameEtat == ETAT_WARNING_TIMEOUT) { 00793 timeoutWarningWaitEnd.reset(); 00794 timeoutWarningWaitEnd.start(); 00795 gameEtat = ETAT_WARING_END_BALISE_WAIT; 00796 } 00797 break; 00798 00799 case ECRAN_CHOICE_START_ACTION: 00800 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config 00801 if(msgRxBuffer[FIFO_lecture].data[0] == 1) { 00802 runRobotTest(); 00803 } else { 00804 initRobotActionneur(); 00805 } 00806 wait_ms(500); 00807 SendRawId(ECRAN_ACK_CHOICE_START_ACTION); 00808 } 00809 break; 00810 00811 case OBJET_SUR_TABLE: 00812 if (msgRxBuffer[FIFO_lecture].data[1] == 0xff){ 00813 00814 gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION; 00815 } 00816 else{ 00817 00818 waitingAckFrom = 0; 00819 waitingAckID = 0; 00820 00821 strat_instructions[actual_instruction+1].arg1 = returnX(strat_instructions[actual_instruction].arg2); 00822 strat_instructions[actual_instruction+1].arg2 = returnY(strat_instructions[actual_instruction].arg2); 00823 } 00824 modeTelemetre = 0; 00825 break; 00826 } 00827 00828 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; 00829 } 00830 }
Generated on Tue Jul 12 2022 21:45:50 by
1.7.2