CRAC Team
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:
- 37:f1a8734c193d
- Parent:
- 36:c37dbe2be916
--- a/Strategie/Strategie.cpp Mon May 31 13:36:03 2021 +0000
+++ b/Strategie/Strategie.cpp Sat Jul 17 16:07:29 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};
+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,7 @@
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 ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois
@@ -138,7 +142,12 @@
}
}
-
+void wait_ISR(void)
+{
+ actual_instruction = instruction.nextLineOK;
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+ timer.detach();
+}
/****************************************************************************************/
/* FUNCTION NAME: Strategie */
@@ -153,8 +162,19 @@
unsigned short localData3 = 0;
//signed 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 +386,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 +515,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 +546,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 +612,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 +624,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,6 +649,7 @@
Rotate(localData2);
break;
+
case MV_XYT:
Debug_Audio(3,10);
if(instruction.direction == BACKWARD) {
@@ -665,8 +665,10 @@
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;
@@ -675,55 +677,122 @@
target_theta_robot = localData2;
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)
+ {
+ 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 +820,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 +851,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 +906,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 +931,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 +1194,7 @@
case BALISE_DANGER :
SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
- balise_danger();
+ balise_danger(FIFO_lecture);
break;
case BALISE_STOP:
@@ -1181,6 +1273,11 @@
}
break;
+
+ case VALEUR_GIROUETTE :
+ val_girou = msgRxBuffer[FIFO_lecture].data[0] ;
+ break ;
+
default:
break;
/*
@@ -1191,6 +1288,11 @@
// waitingAckFrom_FIN=0;
SendRawId(0x40);
break;*/
+
+
+
+
+
}
FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
}
@@ -1619,7 +1721,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 +1729,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 +1748,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 +1756,52 @@
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 : //lescture de la girouette
+ SendRawId(LECTURE_GIROUETTE) ;
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+
+ case 161 : //aller dans le bon port de fin de match
+ if(Flag_bon_port == 1)
+ {
+ arriver_bon_port();
+ Flag_bon_port = 0;
+ }
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ //faire la condition entre l'oddo et val_girou
+ break ;
+
+ case 162 : //attendre le temps indiqué en seconde
+ timer.attach(&wait_ISR, arg1);
+ break ;
+
default:
retour = 0;//L'action n'existe pas, il faut utiliser le CAN
break;