code de start qui marche a la fin du premier match, base pour la suite
Fork of CRAC-Strat_2017_homologation_petit_rob by
Diff: Strategie/Strategie.cpp
- Revision:
- 18:cc5fec34ed9c
- Parent:
- 16:7321fb3bb396
- Child:
- 20:de595e4ff01d
--- a/Strategie/Strategie.cpp Fri May 19 17:14:07 2017 +0000 +++ b/Strategie/Strategie.cpp Mon May 22 15:01:49 2017 +0000 @@ -14,7 +14,7 @@ unsigned short waitingAckID = 0;//L'id du ack attendu unsigned short waitingAckFrom = 0;//La provenance du ack attendu char modeTelemetre; // Si à 1, indique que l'on attend une reponse du telemetre - +//unsigned short telemetreDistance = 0; signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN signed short x_robot,y_robot,theta_robot;//La position du robot @@ -36,7 +36,7 @@ unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR}; unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR}; -InterruptIn jack(p24); // entrée analogique en interruption pour le jack +InterruptIn jack(p25); // entrée analogique en interruption pour le jack #endif unsigned char checkCurrent = 0; unsigned char countAliveCard = 0; @@ -242,9 +242,10 @@ localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y } SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2); -#endif /* +#endif +#ifdef ROBOT_SMALL SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T); -#endif*/ +#endif break; case ETAT_GAME_WAIT_FOR_JACK: //On attend le jack @@ -303,6 +304,7 @@ { localData1 = -localData1;//Inversion de la direction } + BendRadius(instruction.arg1, instruction.arg3, localData1, localData5); break; case MV_LINE://Ligne droite @@ -390,20 +392,36 @@ GoStraight(localData2, localData5, localData3, 0); break; case ACTION: - - waitingAckID = SERVO_AX12_ACTION; + int tempo = 0; + waitingAckID= SERVO_AX12_ACTION; waitingAckFrom = ACKNOWLEDGE_AX12; - if(doAction(instruction.arg1,instruction.arg2,instruction.arg3)) { + tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3); + if(tempo == 1){ //L'action est spécifique if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) { - wait_us(200); + 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_ACK; } + #ifdef ROBOT_SMALL + actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; + #endif return; - } else { + #ifdef ROBOT_SMALL + } else if (tempo == 2) { + // on est dans le cas de l'avance selon le telemetre + waitingAckID = ASSERVISSEMENT_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); + GoStraight(telemetreDistance, 0, 0, 0); + // on reset la distance du telemetre à 0 + telemetreDistance = 5000; + #endif + }else{ //C'est un AX12 qu'il faut bouger //AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2); //AX12_enchainement++; @@ -479,8 +497,15 @@ case ACTION: if (modeTelemetre == 0){ - waitingAckID = SERVO_AX12_ACTION;// instruction.arg1; - waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE; + if (telemetreDistance == 0){ + waitingAckID = SERVO_AX12_ACTION;// instruction.arg1; + waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE; + }else if(telemetreDistance == 5000){ + // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre + waitingAckID = ASSERVISSEMENT_RECALAGE; + waitingAckFrom = INSTRUCTION_END_MOTEUR; + telemetreDistance = 0; + } }else{ // si on attend la reponse du telemetre //modeTelemetre = 1; waitingAckID = OBJET_SUR_TABLE; @@ -601,6 +626,7 @@ break; case ETAT_END_LOOP: //Rien, on tourne en rond + break; default: @@ -651,6 +677,7 @@ case INSTRUCTION_END_MOTEUR: case INSTRUCTION_END_IHM: case INSTRUCTION_END_AX12: + 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; @@ -742,7 +769,7 @@ case BALISE_STOP: SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP); - 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, + //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, if(needToStop() != 0 && ingnorBaliseOnce ==0) { if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT) { @@ -755,7 +782,7 @@ } } } - } + //} ingnorBaliseOnce = 0; break;