code strat avant match 2, strat inversée OK normalement
Fork of CRAC-Strat_2017_fin_premier_match by
Diff: Strategie/Strategie.cpp
- Revision:
- 11:ed13a480ddca
- Parent:
- 10:a788d9cf60f2
- Child:
- 12:14729d584500
diff -r a788d9cf60f2 -r ed13a480ddca Strategie/Strategie.cpp --- a/Strategie/Strategie.cpp Fri Apr 29 09:14:27 2016 +0000 +++ b/Strategie/Strategie.cpp Mon May 02 19:40:59 2016 +0000 @@ -19,12 +19,12 @@ signed short x_robot,y_robot,theta_robot;//La position du robot #ifdef ROBOT_BIG -unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR}; -unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR}; +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 #else -unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR}; -unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR}; +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 #endif unsigned char checkCurrent = 0; @@ -87,7 +87,7 @@ if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) { lastEtat = gameEtat; debugetatTimer.reset(); - sendStratEtat((unsigned char)gameEtat); + sendStratEtat((unsigned char)gameEtat, (unsigned char)actual_instruction); } switch(gameEtat) @@ -131,7 +131,7 @@ Il faut faire une boucle pour verifier toutes les cartes les une apres les autres */ waitingAckFrom = id_alive[checkCurrent];//On indique que l'on attend un ack de la carte IHM - SendRawId(id_check[checkCurrent]);//On demande à la carte IHM d'insiquer ça présence + SendRawId(id_check[checkCurrent]);//On demande à la carte d'indiquer ça présence screenChecktry++;//On incrèment le conteur de tentative de 1 cartesCheker.reset();//On reset le timeOut @@ -147,6 +147,7 @@ cartesCheker.stop(); screenChecktry = 0; countAliveCard++; + checkCurrent++; if(checkCurrent >= NOMBRE_CARTES) { if(countAliveCard >= NOMBRE_CARTES) { gameEtat = ETAT_CONFIG; @@ -157,13 +158,13 @@ waitingAckFrom = ECRAN_ALL_CHECK; } } else { - checkCurrent++; gameEtat = ETAT_CHECK_CARTES; } } else if(cartesCheker.read_ms () > 100) { cartesCheker.stop(); if(screenChecktry >=3) { screenChecktry = 0; + checkCurrent++; if(checkCurrent >= NOMBRE_CARTES) { if(countAliveCard == NOMBRE_CARTES) { gameEtat = ETAT_CONFIG; @@ -173,7 +174,6 @@ waitingAckFrom = ECRAN_ALL_CHECK; } } else { - checkCurrent++; gameEtat = ETAT_CHECK_CARTES; } } else { @@ -204,12 +204,12 @@ SendRawId(ECRAN_ACK_START_MATCH); tactile_printf("Attente 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 SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T); -#endif +#endif*/ break; case ETAT_GAME_WAIT_FOR_JACK: //TODO Attendre le jack @@ -260,8 +260,7 @@ localData5 = 0; } } - - localData1 = ((instruction.direction == FORWARD)?1:-1); + localData1 = ((instruction.direction == LEFT)?1:-1); BendRadius(instruction.arg1, instruction.arg3, localData1, localData5); break; case MV_LINE: @@ -318,21 +317,40 @@ case MV_RECALAGE: waitingAckID = ASSERVISSEMENT_RECALAGE; waitingAckFrom = ACKNOWLEDGE_MOTEUR; - //TODO - mettre l'ACK à jour + instruction.nextActionType = WAIT; + localData2 = (((instruction.direction == FORWARD)?1:-1)*3000); + + if(instruction.precision == RECALAGE_Y) { + localData5 = 2; + if(InversStrat == 1) { + localData3 = 3000 - instruction.arg1;//Inversion du Y + } else { + localData3 = instruction.arg1; + } + } else { + localData5 = 1; + localData3 = instruction.arg1; + } + + GoStraight(localData2, localData5, localData3, 0); break; case ACTION: + waitingAckFrom = 0; + waitingAckID = 0; if(doAction(instruction.arg1,instruction.arg2,instruction.arg3)) { //L'action est spécifique + if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) { + actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; + } else { + gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION; + } + return; } else { //C'est un AX12 qu'il faut bouger AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2); AX12_enchainement++; } - waitingAckFrom = 0; - waitingAckID = 0; - actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante - gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; - return; break; default: //Instruction inconnue, on l'ignore @@ -364,14 +382,12 @@ //if(true) { cartesCheker.stop(); if(instruction.nextActionType == JUMP) { - if(instruction.jumpAction == JUMP_TIME) { + if(instruction.jumpAction == JUMP_POSITION) { + gameEtat = ETAT_GAME_JUMP_POSITION; + } else {//Pour eviter les erreurs, on dit que c'est par défaut un jump time gameEtat = ETAT_GAME_JUMP_TIME; cartesCheker.reset();//On reset le timeOut - cartesCheker.start(); - } else if(instruction.jumpAction == JUMP_POSITION) { - gameEtat = ETAT_GAME_JUMP_POSITION; - } else { - //ERROR + cartesCheker.start(); } } else if(instruction.nextActionType == WAIT) { gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION; @@ -494,6 +510,8 @@ case ALIVE_BALISE: case ALIVE_MOTEUR: case ALIVE_IHM: + case ALIVE_ACTIONNEURS: + case ALIVE_POMPES: case ECRAN_ALL_CHECK: if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) { waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne @@ -506,7 +524,7 @@ case INSTRUCTION_END_BALISE: case INSTRUCTION_END_MOTEUR: case INSTRUCTION_END_IHM: - if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && (msgRxBuffer[FIFO_lecture].data[0]|((unsigned int)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) { + if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) { waitingAckFrom = 0; waitingAckID = 0; }