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