code de la carte IHM avant les bugs et avant le travail effectué avec Melchior
Dependencies: mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait
Diff: Strategie/Strategie.cpp
- Revision:
- 38:9d6a3ccc0582
- Parent:
- 36:c37dbe2be916
--- a/Strategie/Strategie.cpp Mon May 31 13:36:03 2021 +0000 +++ b/Strategie/Strategie.cpp Sat Jul 17 11:07:17 2021 +0000 @@ -14,7 +14,7 @@ Ticker chrono; Timeout AffTime; -Timer timer; +Ticker timer; Timer cartesCheker;//Le timer pour le timeout de la vérification des cartes Timer gameTimer; Timer debugetatTimer; @@ -25,6 +25,7 @@ unsigned char screenChecktry = 0; unsigned char test[32] = {32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32}; +signed char asser_stop_direction=0; char counter = 0; char check; char Jack = 1; @@ -40,6 +41,8 @@ unsigned short flag_check_carte = 0, flag_strat = 0, flag_timer; int flagReceptionTelemetres = 0, flagNonRepriseErrorMot = 0; +int Flag_Bras_Re = 0, Flag_Manche_Bas = 0, Flag_Manche_Moy = 0, Flag_pavillon = 0, Flag_bon_port = 0; //Flag utilisé pour compter le score des bras / manches / pavillon +unsigned short Flag_num_bras; signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN @@ -49,7 +52,7 @@ //unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN int flagSendCan=1; unsigned char Cote = 0; //0 -> JAUNE | 1 -> VIOLET -unsigned char Hauteur = 0; +unsigned char Hauteur = 0; //Robot du haut -> 1, du bas -> 0 unsigned short angleRecalage = 0; unsigned char checkCurrent = 0; unsigned char countAliveCard = 0; @@ -63,6 +66,9 @@ unsigned char ingnorBaliseOnce = 0;//une fois détecté réinitialise unsigned char ingnorBalise = 0;//0:balise ignore 1:on ecoute la balise short direction; +char val_girou ; + +unsigned char debug_bon_port = 0; unsigned char ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois @@ -138,7 +144,23 @@ } } +void wait_ISR(void) +{ + actual_instruction = instruction.nextLineOK; + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; + timer.detach(); +} +void girouette_ISR(void) +{ + actual_instruction = instruction.nextLineOK; + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; +} + +void isr_end_danger(void) +{ + balise_end_danger(&instruction,&dodgeq,target_x_robot,target_y_robot,target_theta_robot, theta_robot,x_robot,y_robot); +} /****************************************************************************************/ /* FUNCTION NAME: Strategie */ @@ -151,10 +173,21 @@ signed char localData1 = 0; signed short localData2 = 0; unsigned short localData3 = 0; - //signed short localData4 = 0; + unsigned short localData4 = 0; unsigned char localData5 = 0; + int unique = 0 ; - + if(gameTimer.read_ms() >= 95000) + { + int unique = 0 ; + if (unique == 0) + { + SendRawId(PAVILLON_DEPLOYE) ; + Flag_pavillon=1; + unique = 1; + } + } + if(gameTimer.read_ms() >= 99000) {//Fin du match (On autorise 2s pour déposer des éléments gameTimer.stop(); gameTimer.reset(); @@ -366,30 +399,82 @@ break; case GOTOPOS: - localData1 = -1; - - if(InversStrat == 1 && ingnorInversionOnce == 0) { - localData2 = -instruction.arg3; - localData3 = 3000 - instruction.arg2;//Inversion du Y - } else { - localData3 = instruction.arg2; - localData2 = instruction.arg3; + if(Hauteur == 0) + { + localData1 = -1; + + if(InversStrat == 1 && ingnorInversionOnce == 0) { + localData2 = -instruction.arg3; + localData3 = 3000 - instruction.arg2;//Inversion du Y + } else { + localData3 = instruction.arg2; + localData2 = instruction.arg3; + } + + GoToPosition(instruction.arg1,localData3,localData2,localData1); + waitingAckID = ASSERVISSEMENT_XYT; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + while(waitingAckID !=0 && waitingAckFrom !=0) + canProcessRx(); + waitingAckID_FIN=ASSERVISSEMENT_XYT; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0) + canProcessRx(); + etat_pos=FIN_POS; } - - GoToPosition(instruction.arg1,localData3,localData2,localData1); - waitingAckID = ASSERVISSEMENT_XYT; - waitingAckFrom = ACKNOWLEDGE_MOTEUR; - - while(waitingAckID !=0 && waitingAckFrom !=0) - canProcessRx(); - waitingAckID_FIN=ASSERVISSEMENT_XYT; - waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; - while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0) - canProcessRx(); - etat_pos=FIN_POS; + else if(Hauteur == 1) + { + localData1 = 1; + + if(InversStrat == 1 && ingnorInversionOnce == 0) { + localData2 = -instruction.arg3; + localData3 = 3000 - 600;//Inversion du Y + } else { + localData3 = 600; + localData2 = instruction.arg3; + } + + GoToPosition(instruction.arg1,localData3,localData2,localData1); + waitingAckID = ASSERVISSEMENT_XYT; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + while(waitingAckID !=0 && waitingAckFrom !=0) + canProcessRx(); + waitingAckID_FIN=ASSERVISSEMENT_XYT; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0) + canProcessRx(); + + localData1 = -1; + + if(InversStrat == 1 && ingnorInversionOnce == 0) { + localData2 = -instruction.arg3; + localData3 = 3000 - instruction.arg2;//Inversion du Y + } else { + localData3 = instruction.arg2; + localData2 = instruction.arg3; + } + + GoToPosition(instruction.arg1,localData3,localData2,localData1); + waitingAckID = ASSERVISSEMENT_XYT; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + while(waitingAckID !=0 && waitingAckFrom !=0) + canProcessRx(); + waitingAckID_FIN=ASSERVISSEMENT_XYT; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0) + canProcessRx(); + etat_pos=FIN_POS; + } + break; case FIN_POS: actual_instruction = instruction.nextLineOK; + target_x_robot=x_robot; + target_y_robot=y_robot; + target_theta_robot=theta_robot; break; } } @@ -443,13 +528,13 @@ Traitement de l'instruction, envoie de la trame CAN */ //debug_Instruction(instruction); - actionPrecedente = instruction.order; - switch(instruction.order) { + switch(instruction.order) + { case MV_COURBURE://C'est un rayon de courbure - Debug_Audio(3,6); +// Debug_Audio(3,6); float alpha=0, theta=0; - unsigned short alph=0; + short alph=0; actionPrecedente = MV_COURBURE; waitingAckID = ASSERVISSEMENT_COURBURE; waitingAckFrom = ACKNOWLEDGE_MOTEUR; @@ -474,134 +559,58 @@ /*if(InversStrat == 1 && ingnorInversionOnce == 0) { localData1 = -localData1;//Inversion de la direction }*/ - + enum E_InstructionDirection directionxyt; BendRadius(instruction.arg1, localData2, localData1, localData5); + alph=localData2; if(localData2>0) { direction=1; } else { direction=-1; } - if(localData2>0)alph=localData2; - else alph=-localData2; + if(localData2>0) + { + directionxyt=FORWARD; + asser_stop_direction=1; + } + else + { + directionxyt=BACKWARD; + asser_stop_direction=-1; + } alpha = localData2*M_PI/1800.0f; theta = theta_robot*M_PI/1800.0f; - - if(instruction.direction == LEFT) { //-------------LEFT - if(alph<450){ // 1 XYT - dodgeq.inst[0].order = MV_XYT; - dodgeq.inst[0].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X - dodgeq.inst[0].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y - dodgeq.inst[0].arg3 = theta_robot + alph;// T - } - else if(alph<900){ - for(int c=0;c<2;c++){ // 2 points de passages - dodgeq.inst[c].order = MV_XYT; - dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X - dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y - dodgeq.inst[c].arg3 = theta_robot + alph;// T - alpha-=alpha/2.0f; - alph-=alph/2; - } - } - else if(alph<1350){ - for(int c=0;c<3;c++){ // 3 points de passages - dodgeq.inst[c].order = MV_XYT; - dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X - dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y - dodgeq.inst[c].arg3 = theta_robot + alph;// T - alpha-=alpha/3.0f; - alph-=alph/3; - } - } - else if(alph<1800){ - for(int c=0;c<4;c++){ // 4 points de passages - dodgeq.inst[c].order = MV_XYT; - dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X - dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y - dodgeq.inst[c].arg3 = theta_robot + alph;// T - alpha-=alpha/4.0f; - alph-=alph/4; - } - } - else if(alph<2250){ - for(int c=0;c<5;c++){ // 5 points de passages - dodgeq.inst[c].order = MV_XYT; - dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X - dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y - dodgeq.inst[c].arg3 = theta_robot + alph;// T - alpha-=alpha/5.0f; - alph-=alph/5; - } + int nbre; + nbre=abs(alph)/450; + for(int c=0;c<nbre+1;c++) + { + dodgeq.inst[c].order = MV_XYT; + dodgeq.inst[c].direction=directionxyt; + if(instruction.direction == LEFT) //-------------LEFT + { + target_x_robot = x_robot + instruction.arg1*(sin(theta+alpha)-sin(theta));// X + target_y_robot = y_robot + instruction.arg1*(cos(theta)-cos(theta+alpha));// Y + target_theta_robot = theta_robot + alph;// T + dodgeq.inst[c].arg1 = target_x_robot;// X + dodgeq.inst[c].arg2 = target_y_robot;// Y + dodgeq.inst[c].arg3 = target_theta_robot;// T } - else { - for(int c=0;c<6;c++){ // 6 points de passages - dodgeq.inst[c].order = MV_XYT; - dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X - dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y - dodgeq.inst[c].arg3 = theta_robot + alph;// T - alpha-=alpha/6.0f; - alph-=alph/6; - } - } - } else { //-----------------------------------------RIGHT - if(alph<450){ // 1 XYT - dodgeq.inst[0].order = MV_XYT; - dodgeq.inst[0].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X - dodgeq.inst[0].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y - dodgeq.inst[0].arg3 = theta_robot - alph;// T - } - else if(alph<900){ - for(int c=0;c<2;c++){ // 2 points de passages - dodgeq.inst[c].order = MV_XYT; - dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X - dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y - dodgeq.inst[c].arg3 = theta_robot - alph;// T - alpha-=alpha/2.0f; - } + else + { + target_x_robot = x_robot + instruction.arg1*(sin(theta)-sin(theta-alpha));// X + target_y_robot = y_robot + instruction.arg1*(cos(theta-alpha)-cos(theta));// Y + target_theta_robot = theta_robot - alph;// T + dodgeq.inst[c].arg1 = target_x_robot;// X + dodgeq.inst[c].arg2 = target_y_robot;// Y + dodgeq.inst[c].arg3 = target_theta_robot;// T } - else if(alph<1350){ - for(int c=0;c<3;c++){ // 3 points de passages - dodgeq.inst[c].order = MV_XYT; - dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X - dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y - dodgeq.inst[c].arg3 = theta_robot - alph;// T - alpha-=alpha/3.0f; - } - } - else if(alph<1800){ - for(int c=0;c<4;c++){ // 4 points de passages - dodgeq.inst[c].order = MV_XYT; - dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X - dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y - dodgeq.inst[c].arg3 = theta_robot - alph;// T - alpha-=alpha/4.0f; - } - } - else if(alph<2250){ - for(int c=0;c<5;c++){ // 5 points de passages - dodgeq.inst[c].order = MV_XYT; - dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X - dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y - dodgeq.inst[c].arg3 = theta_robot - alph;// T - alpha-=alpha/5.0f; - } - } - else { - for(int c=0;c<6;c++){ // 6 points de passages - dodgeq.inst[c].order = MV_XYT; - dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X - dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y - dodgeq.inst[c].arg3 = theta_robot - alph;// T - alpha-=alpha/6.0f; - } - } + alpha-=alpha/((float)nbre); + alph-=alph/(nbre+1); } break; case MV_LINE://Ligne droite - Debug_Audio(3,8); waitingAckID = ASSERVISSEMENT_RECALAGE; waitingAckFrom = ACKNOWLEDGE_MOTEUR; if(instruction.nextActionType == ENCHAINEMENT) { @@ -616,6 +625,10 @@ } } localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); + if(instruction.direction == FORWARD) + asser_stop_direction=1; + else + asser_stop_direction=-1; GoStraight(localData2, 0, 0, localData5); target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800); @@ -624,7 +637,6 @@ break; case MV_TURN: //Rotation sur place - Debug_Audio(3,9); target_x_robot = x_robot; target_y_robot = y_robot; target_theta_robot = theta_robot + localData2; @@ -650,12 +662,61 @@ Rotate(localData2); break; + case MV_XYT: + + if(Flag_bon_port ==1) //XYT pour la rentrée dans le port + { Debug_Audio(3,10); if(instruction.direction == BACKWARD) { localData1 = -1; + asser_stop_direction=-1; } else { localData1 = 1; + asser_stop_direction=1; + } + + if(val_girou == 0) //Si c'est Sud on déplace le X + { + localData4 = 1300; + } + else + { + localData4 = instruction.arg1; + } + + if(InversStrat == 1 && ingnorInversionOnce == 0) + { + localData2 = -instruction.arg3; + localData3 = 3000 - instruction.arg2;//Inversion du Y + } + else + { + localData3 = instruction.arg2; + localData2 = instruction.arg3; + } + if((instruction.arg1<=0)||(localData3<=0)) + GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1); + else + GoToPosition(localData4,localData3,localData2,localData1); + waitingAckID = ASSERVISSEMENT_XYT; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + target_x_robot = instruction.arg1; + target_y_robot = localData3; + target_theta_robot = localData2; + + Flag_bon_port = 0; + } + else //Sinon on effectue XYT normalement selon les instructions + { + Debug_Audio(3,10); + if(instruction.direction == BACKWARD) { + localData1 = -1; + asser_stop_direction=-1; + } else { + localData1 = 1; + asser_stop_direction=1; } if(InversStrat == 1 && ingnorInversionOnce == 0) { @@ -665,65 +726,134 @@ localData3 = instruction.arg2; localData2 = instruction.arg3; } - - GoToPosition(instruction.arg1,localData3,localData2,localData1); + if((instruction.arg1<=0)||(localData3<=0)) + GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1); + else + GoToPosition(instruction.arg1,localData3,localData2,localData1); waitingAckID = ASSERVISSEMENT_XYT; waitingAckFrom = ACKNOWLEDGE_MOTEUR; target_x_robot = instruction.arg1; target_y_robot = localData3; target_theta_robot = localData2; - - break; + } + break; + case MV_RECALAGE: - if(instruction.nextActionType == MECANIQUE) { - instruction.nextActionType = WAIT; - - waitingAckID = ASSERVISSEMENT_RECALAGE; - waitingAckFrom = ACKNOWLEDGE_MOTEUR; - - localData2 = (((instruction.direction == FORWARD)?1:-1)*1000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler - - if(instruction.precision == RECALAGE_Y) { - localData5 = 2; - if(InversStrat == 1 && ingnorInversionOnce == 0) { - localData3 = 3000 - instruction.arg1;//Inversion du Y - } else { + //Effectuer un recalage si on a aucun gobelet ou au moins 2 + /* + if( ((instruction.direction == FORWARD)&&(etat_groupe[0]==5&&etat_groupe[1]==5) || (etat_groupe[0]==5&&etat_groupe[2]==5) || (etat_groupe[1]==5&&etat_groupe[2]==5)) || + ((instruction.direction == BACKWARD)&&(etat_groupe[3]==5&&etat_groupe[4]==5) || (etat_groupe[3]==5&&etat_groupe[5]==5) || (etat_groupe[4]==5&&etat_groupe[5]==5)) || + (etat_groupe[0]!=5 && etat_groupe[1]!=5 && etat_groupe[2]!=5 && etat_groupe[3]!=5 && etat_groupe[4]!=5 && etat_groupe[5]!=5)) + { + */ + if(instruction.nextActionType == MECANIQUE) + { + instruction.nextActionType = WAIT; + waitingAckID = ASSERVISSEMENT_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + localData2 = (((instruction.direction == FORWARD)?1:-1)*1000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler + if(instruction.precision == RECALAGE_Y) + { + localData5 = 2; + if(InversStrat == 1 && ingnorInversionOnce == 0) + { + localData3 = 3000 - instruction.arg1;//Inversion du Y + } + else + { + localData3 = instruction.arg1; + } + } + else + { + localData5 = 1; localData3 = instruction.arg1; } - } else { - localData5 = 1; - localData3 = instruction.arg1; + GoStraight(localData2, localData5, localData3, 0); + } + else + { //CAPTEUR + SendRawId(DATA_RECALAGE); + waitingAckID = RECEPTION_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_TELEMETRE; + + // On attend que les variables soient actualisé + while(!(waitingAckID == 0 && waitingAckFrom == 0)) + canProcessRx(); + while(!(waitingAckID_FIN==0 && waitingAckFrom_FIN==0)) + canProcessRx(); + + if(instruction.precision == RECALAGE_Y) // ((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600)) (theta_robot < 900 && theta_robot > -900) + { + SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, recalageDistanceY(), theta_robot); + } + else if(instruction.precision == RECALAGE_X) + { + SetOdometrie(ODOMETRIE_SMALL_POSITION, recalageDistanceX(), y_robot, theta_robot); + } + else if(instruction.precision == RECALAGE_T) + { + SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, recalageAngulaireCapteur() ); + } } - GoStraight(localData2, localData5, localData3, 0); - } else { //CAPTEUR - SendRawId(DATA_RECALAGE); - waitingAckID = RECEPTION_RECALAGE; - waitingAckFrom = ACKNOWLEDGE_TELEMETRE; - - // On attend que les variables soient actualisé - while(!(waitingAckID == 0 && waitingAckFrom == 0)) - canProcessRx(); - while(!(waitingAckID_FIN==0 && waitingAckFrom_FIN==0)) - canProcessRx(); - - if(instruction.precision == RECALAGE_Y) { // ((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600)) (theta_robot < 900 && theta_robot > -900) - SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, recalageDistanceY(), theta_robot); - } else if(instruction.precision == RECALAGE_X) { - SetOdometrie(ODOMETRIE_SMALL_POSITION, recalageDistanceX(), y_robot, theta_robot); - } else if(instruction.precision == RECALAGE_T) { - SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, recalageAngulaireCapteur() ); + /*} + //Ou effectuer une ligne droite jusqu'à la nouvelle valeur de recalage + + else + { + Debug_Audio(3,8); + instruction.nextActionType = WAIT; + waitingAckID = ASSERVISSEMENT_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + //Calcul de la distance à parcourir pour se retrouver au même point que si on avait fait un recalage + localData2 = instruction.arg1; + if(instruction.precision == RECALAGE_Y) + { + if(localData2 == 2832) localData2 = 2832 - y_robot; //Gob+ en y + else if(localData2 == 168) localData2 = y_robot - 168; //Gob- en y } + else if(instruction.precision == RECALAGE_X) + { + if(localData2 == 1832) localData2 = 1832 - x_robot; //Gob+ en x + else if(localData2 == 168) localData2 = x_robot - 168; //Gob- en x + } + localData2 = ((instruction.direction == FORWARD)?1:-1)*localData2; + GoStraight(localData2, 0, 0, 0);//x,x,x,localData5 + + target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800); + target_y_robot = y_robot + localData2*sin((double)theta_robot*M_PI/1800); + target_theta_robot = theta_robot; } + */ break; case ACTION: - waitingAckID_FIN = 0; waitingAckFrom_FIN = 0; int tempo = 0; waitingAckID= ACK_ACTION; //On veut un ack de type action waitingAckFrom = ACKNOWLEDGE_HERKULEX; //de la part des herkulex + if(InversStrat==1 && ingnorInversionOnce == 0) + { + if((instruction.arg1 >= 151 && instruction.arg1 <= 154) || (instruction.arg1 >= 163 && instruction.arg1 <= 164)) + { + if(instruction.arg2 == 0) instruction.arg2 = 2; + else if(instruction.arg2 == 2) instruction.arg2 = 0; + else if(instruction.arg2 == 10) instruction.arg2 = 21; + else if(instruction.arg2 == 21) instruction.arg2 = 10; + else if(instruction.arg2 == 3) instruction.arg2 = 5; + else if(instruction.arg2 == 5) instruction.arg2 = 3; + else if(instruction.arg2 == 43) instruction.arg2 = 54; + else if(instruction.arg2 == 54) instruction.arg2 = 43; + } + else if(instruction.arg1 >= 157 && instruction.arg1 <= 159) + { + if(instruction.arg2 == 0) instruction.arg2 = 1; + else if(instruction.arg2 == 1) instruction.arg2 = 0; + } + } tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3); // unsigned char test=(unsigned char) tempo; // SendMsgCan(0x5BD, &test,1); @@ -751,7 +881,10 @@ //AX12_enchainement++; } - break; + /*target_x_robot=x_robot; + target_y_robot=y_robot; + target_theta_robot=theta_robot; + */break; default: //Instruction inconnue, on l'ignore break; @@ -779,18 +912,25 @@ case ETAT_GAME_WAIT_ACK: canProcessRx(); //SendSpeed(200);//--------------------------------------------------MODE RALENTI !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continue + if(waitingAckID == 0 && waitingAckFrom == 0) //Les ack ont été reset, c'est bon on continue + { //if(true) { cartesCheker.stop(); - if(instruction.nextActionType == JUMP) { - if(instruction.jumpAction == JUMP_POSITION) { + if(instruction.nextActionType == JUMP) + { + 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 + } + 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.nextActionType == WAIT) { ///Actualisation des waiting ack afin d'attendre la fin des actions + } + else if(instruction.nextActionType == WAIT) ///Actualisation des waiting ack afin d'attendre la fin des actions + { /*wait_ms(200); #ifdef ROBOT_BIG SetOdometrie(ODOMETRIE_BIG_POSITION, x_robot, y_robot, theta_robot); @@ -827,17 +967,23 @@ break; case ACTION: - if (modeTelemetre == 0) { - if (telemetreDistance == 0) { + if (modeTelemetre == 0) + { + if (telemetreDistance == 0) + { waitingAckID_FIN = ACK_FIN_ACTION;// ack de type action waitingAckFrom_FIN = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs - } else if(telemetreDistance == 5000) { + } + else if(telemetreDistance == 5000) + { // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre //waitingAckID_FIN = ASSERVISSEMENT_RECALAGE; //waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; telemetreDistance = 0; } - } else { // si on attend la reponse du telemetre + } + else // si on attend la reponse du telemetre + { //modeTelemetre = 1; waitingAckID_FIN = OBJET_SUR_TABLE; waitingAckFrom_FIN = 0; @@ -846,16 +992,23 @@ default: break; } - } else { + } + else + { gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante } - } else if(cartesCheker.read_ms () > 1000) { + } + else if(cartesCheker.read_ms () > 1000) + { cartesCheker.stop(); - if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur + if(screenChecktry >=2) //La carte n'a pas reçus l'information, on passe à l'instruction d'erreur + { actual_instruction = instruction.nextLineError; gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; - } else { + } + else + { gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'strat_etat_s d'envois de l'instruction } } @@ -1102,7 +1255,7 @@ case BALISE_DANGER : SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER); - balise_danger(); + balise_danger(FIFO_lecture); break; case BALISE_STOP: @@ -1181,6 +1334,13 @@ } break; + + case VALEUR_GIROUETTE : + val_girou = msgRxBuffer[FIFO_lecture].data[0] ; + girouette_ISR(); + + break ; + default: break; /* @@ -1191,6 +1351,11 @@ // waitingAckFrom_FIN=0; SendRawId(0x40); break;*/ + + + + + } FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; } @@ -1619,7 +1784,7 @@ waitingAckID = 0; break; - case 151: + case 151: //Bras attraper unsigned char argu_at_bras = arg1; if(arg1 == 543) argu_at_bras = 66; SendMsgCan(BRAS_AT, &argu_at_bras,sizeof(arg1)); @@ -1627,43 +1792,18 @@ waitingAckID =0; break; - case 152: + case 152: //Bras relacher unsigned char argu_re_bras = arg1; if(arg1 == 543) argu_re_bras = 66; SendMsgCan(BRAS_RE, &argu_re_bras,sizeof(arg1)); - waitingAckFrom = 0; - waitingAckID =0; - break; -/* - case 153: - unsigned char argu_at_2_bras = arg1; - SendMsgCan(BRAS_AT_2, &argu_at_2_bras,sizeof(arg1)); - waitingAckFrom = 0; - waitingAckID =0; - break; - - case 154: - unsigned char argu_re_2_bras = arg1; - SendMsgCan(BRAS_RE_2, &argu_re_2_bras,sizeof(arg1)); + SendRawId(0x050); + Flag_Bras_Re = 1; + Flag_num_bras = argu_re_bras; waitingAckFrom = 0; waitingAckID =0; break; - case 155: - unsigned char argu_at_3_bras = arg1; - SendMsgCan(BRAS_AT_3, &argu_at_3_bras,sizeof(arg1)); - waitingAckFrom = 0; - waitingAckID =0; - break; - - case 156: - unsigned char argu_re_3_bras = arg1; - SendMsgCan(BRAS_RE_3, &argu_re_3_bras,sizeof(arg1)); - waitingAckFrom = 0; - waitingAckID =0; - break; -*/ - case 153: + case 153: //Ventouse attraper unsigned char argu_at_vent = arg1; if(arg1 == 543) argu_at_vent = 66; SendMsgCan(VENT_AT, &argu_at_vent,sizeof(arg1)); @@ -1671,7 +1811,7 @@ waitingAckID =0; break; - case 154: + case 154: //Ventouse Relacher unsigned char argu_re_vent = arg1; if(arg1 == 543) argu_re_vent = 66; SendMsgCan(VENT_RE, &argu_re_vent,sizeof(arg1)); @@ -1679,20 +1819,66 @@ waitingAckID =0; break; - case 157: + case 157: //Manche à air bas unsigned char argu_manche_bas = arg1; SendMsgCan(AUTOMATE_MANCHE_BAS, &argu_manche_bas,sizeof(arg1)); + Flag_Manche_Bas = 1; + Flag_Manche_Moy = 1; waitingAckFrom = 0; waitingAckID =0; break; - case 158: + case 158: //Manche à air moyen + unsigned char argu_manche_moy = arg1; + SendMsgCan(AUTOMATE_MANCHE_MOY,&argu_manche_moy,sizeof(arg1)); + Flag_Manche_Bas = 1; + Flag_Manche_Moy = 1; + waitingAckFrom = 0; + waitingAckID =0; + break; + + case 159: //Manche à air haut unsigned char argu_manche_haut = arg1; SendMsgCan(AUTOMATE_MANCHE_HAUT, &argu_manche_haut,sizeof(arg1)); waitingAckFrom = 0; waitingAckID =0; break; + case 160 : //lecture de la girouette + SendRawId(LECTURE_GIROUETTE) ; + debug_bon_port = 1; + wait_ms(100); + break; + + case 161 : //aller dans le bon port de fin de match + Flag_bon_port = 1; + waitingAckFrom = 0; + waitingAckID =0; + break ; + + case 162 : //attendre le temps indiqué en seconde + debug_bon_port = 2; + timer.attach(&wait_ISR, arg1); + break ; + + case 163 : //bras prépare à poser + unsigned char argu_prepa_bras = arg1; + if(arg1 == 543) argu_prepa_bras = 66; + SendMsgCan(BRAS_PREPA, &argu_prepa_bras,sizeof(arg1)); + waitingAckFrom = 0; + waitingAckID =0; + break ; + + case 164 : //bras pose les gobelets en préparation + unsigned char argu_pose_bras = arg1; + if(arg1 == 543) argu_pose_bras = 66; + SendMsgCan(BRAS_POSE, &argu_pose_bras,sizeof(arg1)); + Flag_Bras_Re = 1; + Flag_num_bras = argu_re_bras; + waitingAckFrom = 0; + waitingAckID =0; + break ; + default: retour = 0;//L'action n'existe pas, il faut utiliser le CAN break;