pour trouver l'automate de reception can
Fork of CRAC-Strat_copy by
Diff: Strategie/Strategie.cpp
- Revision:
- 12:14729d584500
- Parent:
- 11:ed13a480ddca
- Child:
- 14:c8fc06c4887f
--- a/Strategie/Strategie.cpp Mon May 02 19:40:59 2016 +0000 +++ b/Strategie/Strategie.cpp Mon May 09 09:10:17 2016 +0000 @@ -18,10 +18,12 @@ signed short x_robot,y_robot,theta_robot;//La position du robot +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 + #ifdef ROBOT_BIG -unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS}; -unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS}; -InterruptIn jack(p24); // entrée analogique en interruption pour le jack +unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS,CHECK_AX12,CHECK_POMPES}; +unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS,ALIVE_AX12,ALIVE_POMPES}; +InterruptIn jack(p25); // entrée analogique en interruption pour le jack #else unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS}; unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS}; @@ -34,6 +36,8 @@ unsigned char countRobotNear = 0;//Le nombre de robot à proximité +unsigned char ingnorBaliseOnce = 0; + /****************************************************************************************/ /* FUNCTION NAME: chronometre_ISR */ /* DESCRIPTION : Interruption à la fin des 90s du match */ @@ -45,6 +49,7 @@ gameTimer.stop();//Arret du timer #ifdef ROBOT_BIG + wait_ms(2000); doFunnyAction(); #endif @@ -78,7 +83,7 @@ signed short localData4 = 0; unsigned char localData5 = 0; - if(gameTimer.read_ms() >= 88000) {//Fin du match (On autorise 2s pour déposer des éléments + if(gameTimer.read_ms() >= 89000) {//Fin du match (On autorise 2s pour déposer des éléments gameTimer.stop(); gameTimer.reset(); gameEtat = ETAT_END;//Fin du temps @@ -149,6 +154,7 @@ countAliveCard++; checkCurrent++; if(checkCurrent >= NOMBRE_CARTES) { + //printf("all card check, missing %d cards\n",(NOMBRE_CARTES-countAliveCard)); if(countAliveCard >= NOMBRE_CARTES) { gameEtat = ETAT_CONFIG; SendRawId(ECRAN_ALL_CHECK);//On dit à l'IHM que toutes les cartes sont en ligne @@ -163,8 +169,10 @@ } else if(cartesCheker.read_ms () > 100) { cartesCheker.stop(); if(screenChecktry >=3) { + //printf("missing card %d\n",id_check[checkCurrent]); screenChecktry = 0; checkCurrent++; + if(checkCurrent >= NOMBRE_CARTES) { if(countAliveCard == NOMBRE_CARTES) { gameEtat = ETAT_CONFIG; @@ -198,29 +206,37 @@ break; case ETAT_GAME_INIT: //On charge la liste des instructions - //strcpy(cheminFileStart,"/local/test.txt");//On ouvre le fichier test.txt loadAllInstruction();//Mise en cache de toute les instructions gameEtat = ETAT_GAME_WAIT_FOR_JACK; SendRawId(ECRAN_ACK_START_MATCH); tactile_printf("Attente du JACK."); + setAsservissementEtat(1);//On réactive l'asservissement + jack.mode(PullDown); // désactivation de la résistance interne du jack jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack - /* -#ifdef ROBOT_BIG - SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T); -#else + +#ifdef ROBOT_BIG //le gros robot n'a pas de recalage bordure pour ce placer au début, on lui envoit donc ça position + localData2 = POSITION_DEBUT_T; + localData3 = POSITION_DEBUT_Y; + if(InversStrat == 1) { + localData2 = -localData2;//Inversion theta + localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y + } + SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2); +#endif /* SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T); #endif*/ break; case ETAT_GAME_WAIT_FOR_JACK: - //TODO Attendre le jack + //On attend le jack break; case ETAT_GAME_START: - chronoEnd.attach(&chronometre_ISR,90); + chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; gameTimer.reset(); gameTimer.start(); - jack.fall(NULL); + jack.fall(NULL);//On désactive l'interruption du jack SendRawId(GLOBAL_START); + tactile_printf("Start");//Pas vraiment util mais bon break; case ETAT_GAME_LOAD_NEXT_INSTRUCTION: /* @@ -234,10 +250,8 @@ instruction = strat_instructions[actual_instruction]; //On effectue le traitement de l'instruction gameEtat = ETAT_GAME_PROCESS_INSTRUCTION; - //actual_instruction++; } screenChecktry = 0; - wait_ms(100); break; case ETAT_GAME_PROCESS_INSTRUCTION: /* @@ -246,7 +260,7 @@ //debug_Instruction(instruction); switch(instruction.order) { - case MV_COURBURE: + case MV_COURBURE://C'est un rayon de courbure waitingAckID = ASSERVISSEMENT_COURBURE; waitingAckFrom = ACKNOWLEDGE_MOTEUR; if(instruction.nextActionType == ENCHAINEMENT) { @@ -261,16 +275,20 @@ } } localData1 = ((instruction.direction == LEFT)?1:-1); + if(InversStrat == 1) + { + localData1 = -localData1;//Inversion de la direction + } BendRadius(instruction.arg1, instruction.arg3, localData1, localData5); break; - case MV_LINE: + case MV_LINE://Ligne droite waitingAckID = ASSERVISSEMENT_RECALAGE; waitingAckFrom = ACKNOWLEDGE_MOTEUR; if(instruction.nextActionType == ENCHAINEMENT) { MV_enchainement++; localData5 = 1; } else { - if(MV_enchainement > 0) { + if(MV_enchainement > 0) {//Utilisé en cas d'enchainement, localData5 = 2; MV_enchainement = 0; } else { @@ -280,19 +298,23 @@ localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); GoStraight(localData2, 0, 0, localData5); break; - case MV_TURN: + case MV_TURN: //Rotation sur place if(instruction.direction == RELATIVE) { localData2 = instruction.arg3; - } else { - if(abs(instruction.arg3 - theta_robot) > 180) { - localData2 = 360 - instruction.arg3 - theta_robot; - } else { - localData2 = instruction.arg3 - theta_robot; + } else {//C'est un rotation absolu, il faut la convertir en relative + localData2 = instruction.arg3; + + if(InversStrat == 1) { + localData2 = -localData2; } + + localData2 = (localData2 - theta_robot)%3600; + if(localData2 > 1800) { + localData2 = localData2-3600; + } + } - if(InversStrat == 1) { - localData2 = -localData2; - } + Rotate(localData2); waitingAckID = ASSERVISSEMENT_ROTATION; waitingAckFrom = ACKNOWLEDGE_MOTEUR; @@ -305,12 +327,13 @@ } if(InversStrat == 1) { - //localData2 = 360 - instruction.arg3 + localData2 = -instruction.arg3; localData3 = 3000 - instruction.arg2;//Inversion du Y } else { localData3 = instruction.arg2; + localData2 = instruction.arg3; } - GoToPosition(instruction.arg1,localData3,instruction.arg3,localData1); + GoToPosition(instruction.arg1,localData3,localData2,localData1); waitingAckID = ASSERVISSEMENT_XYT; waitingAckFrom = ACKNOWLEDGE_MOTEUR; break; @@ -318,7 +341,7 @@ waitingAckID = ASSERVISSEMENT_RECALAGE; waitingAckFrom = ACKNOWLEDGE_MOTEUR; instruction.nextActionType = WAIT; - localData2 = (((instruction.direction == FORWARD)?1:-1)*3000); + localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler if(instruction.precision == RECALAGE_Y) { localData5 = 2; @@ -454,20 +477,30 @@ case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s - if(timeoutWarning.read_ms() >= 2000)//ça fait plus de 2s, il faut changer de stratégie + if(timeoutWarning.read_ms() >= BALISE_TIMEOUT)//ça fait plus de 2s, il faut changer de stratégie { gameEtat = ETAT_WARNING_SWITCH_STRATEGIE; } break; case ETAT_WARING_END_BALISE_WAIT://Attente d'une seconde apres la fin d'un End Balise pour etre sur que c'est bon - + if(timeoutWarningWaitEnd.read_ms() >= 1000) {//c'est bon, on repart + //actual_instruction = instruction.nextLineError; + gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION; + } break; case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours - +#ifdef ROBOT_BIG + actual_instruction = 2;//Modification directe... c'est pas bien mais ça marchait pour le match 5 + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; +#else + actual_instruction = instruction.nextLineError; + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; +#endif break; case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur actual_instruction = instruction.nextLineError; gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; + ingnorBaliseOnce = 1; break; @@ -512,6 +545,7 @@ case ALIVE_IHM: case ALIVE_ACTIONNEURS: case ALIVE_POMPES: + case ALIVE_AX12: case ECRAN_ALL_CHECK: if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) { waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne @@ -590,19 +624,26 @@ } can1.write(msgTx); wait_ms(10); - tactile_printf("Strat %d selectionne",msgTx.data[0]); + setAsservissementEtat(0);//Désactivation de l'asservissement pour repositionner le robot dans le zone de départ + tactile_printf("Strat %d, Asser desactive",msgTx.data[0]); } break; case BALISE_STOP: SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP); - if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT) - { - wait_ms(10); - SendRawId(ASSERVISSEMENT_STOP); - gameEtat = ETAT_WARNING_TIMEOUT; - timeoutWarning.reset(); - timeoutWarning.start();//Reset du timer utiliser par le timeout + + if(needToStop() != 0 && ingnorBaliseOnce ==0) { + if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT) + { + SendRawId(ASSERVISSEMENT_STOP); + //while(1); + gameEtat = ETAT_WARNING_TIMEOUT; + if(gameEtat != ETAT_WARING_END_BALISE_WAIT) { + timeoutWarning.reset(); + timeoutWarning.start();//Reset du timer utiliser par le timeout + } + } } + ingnorBaliseOnce = 0; break; case BALISE_END_DANGER: if(gameEtat == ETAT_WARNING_TIMEOUT) { @@ -611,6 +652,18 @@ gameEtat = ETAT_WARING_END_BALISE_WAIT; } break; + + case ECRAN_CHOICE_START_ACTION: + if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config + if(msgRxBuffer[FIFO_lecture].data[0] == 1) { + runRobotTest(); + } else { + initRobotActionneur(); + } + wait_ms(500); + SendRawId(ECRAN_ACK_CHOICE_START_ACTION); + } + break; } FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;