CRAC Team
carte_strategie_2019
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Strategie/Strategie.cpp
- Revision:
- 62:c4863b4b2543
- Parent:
- 61:4046a91e1b0e
- Child:
- 63:bcfe62e3f1d2
--- a/Strategie/Strategie.cpp Fri May 24 21:18:26 2019 +0000
+++ b/Strategie/Strategie.cpp Sat May 25 14:56:55 2019 +0000
@@ -177,6 +177,12 @@
unsigned short telemetreDistance_arriere_gauche=0;
unsigned short telemetreDistance_arriere_droite=0;
+unsigned char DT_AVD_interrupt=0;
+unsigned char DT_AVG_interrupt=0;
+unsigned char DT_ARD_interrupt=0;
+unsigned char DT_ARG_interrupt=0;
+
+
#ifdef ROBOT_BIG
@@ -454,19 +460,19 @@
else if (TEST_TIR_BALLE.Touched()) {
while(TEST_TIR_BALLE.Touched());
- while(1){
- TEST_TIR_BALLE.Draw(0xFFF0F0F0, 0);
- //etat =TEST_TIR ;
- //lcd.Clear(LCD_COLOR_WHITE);
- /* CANMessage trame_Tx = CANMessage();
- trame_Tx.len = 1;
- trame_Tx.format = CANStandard;
- trame_Tx.type = CANData;
- trame_Tx.id=CHOICE_COLOR;
- trame_Tx.data[0]=0x2;
- can2.write(trame_Tx);*/
- rn42_Tx.printf("A");
- pc.printf("electron\r");
+ while(1) {
+ TEST_TIR_BALLE.Draw(0xFFF0F0F0, 0);
+ //etat =TEST_TIR ;
+ //lcd.Clear(LCD_COLOR_WHITE);
+ /* CANMessage trame_Tx = CANMessage();
+ trame_Tx.len = 1;
+ trame_Tx.format = CANStandard;
+ trame_Tx.type = CANData;
+ trame_Tx.id=CHOICE_COLOR;
+ trame_Tx.data[0]=0x2;
+ can2.write(trame_Tx);*/
+ rn42_Tx.printf("A");
+ pc.printf("electron\r");
}
//ModeDemo=1;
} else if(TEST_IMMEUBLE.Touched()) {
@@ -490,200 +496,200 @@
}
}
break;
- /* ///////////////////////////////TESTE LES SERVOS LIES AU TRI DES BALLES///////////////////////////////
- case DEMO_TRIEUR:
- lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
- TRI.Draw(VERT, 0);
- AIGUILLEUR_D.Draw(VERT, 0);
- AIGUILLEUR_G.Draw(VERT, 0);
- AIGUILLEUR_CTRE.Draw(VERT, 0);
- while(etat==DEMO_TRIEUR) {
- if(RETOUR.Touched()) {
- while (RETOUR.Touched());
- etat=DEMO;
- } else if(TRI.Touched()) {
- while (TRI.Touched());
- SendRawId(AIGUILLEUR_CENTRE);
- wait(0.5);
- SendRawId(AIGUILLEUR_DROITE);
- wait(0.5);
- SendRawId(AIGUILLEUR_GAUCHE);
- wait(0.5);
- SendRawId(AIGUILLEUR_CENTRE);
-
- break;
- } else if(AIGUILLEUR_D.Touched()) {
- while (AIGUILLEUR_D.Touched());
- SendRawId(AIGUILLEUR_DROITE);
- break;
- } else if(AIGUILLEUR_G.Touched()) {
- while (AIGUILLEUR_G.Touched());
- SendRawId(AIGUILLEUR_GAUCHE);
- break;
-
- } else if(BRAS_ABEILLE_OFF.Touched()) {
- while (BRAS_ABEILLE_OFF.Touched());
- SendRawId(BRAS_ABEILLE_DOWN);
- break;
- } else if(AIGUILLEUR_CTRE.Touched()) {
- while (AIGUILLEUR_CTRE.Touched());
- SendRawId(AIGUILLEUR_CENTRE);
- break;
- }
-
- }
- break;
- case DEMO_IMMEUBLE: //TESTE LE MONTE IMMEUBLE SUIVANT UN CODE COULEUR CHOISI
- int color=0;
- lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"Choix du code couleur", LEFT_MODE);
-
- CANMessage msgTx=CANMessage();
- msgTx.id=MONTER_IMMEUBLE; // Monter immeuble
- msgTx.len=3;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
-
-
- while(etat==DEMO_IMMEUBLE) {
- switch(color) {
- case 0:
-
- RETOUR.Draw(ROUGE,0);
- COLOR_NOIR.Draw(NOIR,1);
- COLOR_ORANGE.Draw(ORANGE,0);
- COLOR_JAUNE.Draw(JAUNE,0);
- COLOR_VERT.Draw(VERT,0);
- COLOR_BLEU.Draw(BLEU,0);
-
- lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.SetTextColor(NOIR);
- lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 1", LEFT_MODE);
- while(color==0) {
- if(COLOR_ORANGE.Touched()) {
- while(COLOR_ORANGE.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=1;
- color++;
- } else if (COLOR_NOIR.Touched()) {
- while(COLOR_NOIR.Touched());
- COLOR_NOIR.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=2;
- color++;
- } else if (COLOR_VERT.Touched()) {
- while(COLOR_VERT.Touched());
- COLOR_VERT.Draw(LCD_COLOR_WHITE);
-
- msgTx.data[color]=3;
- color++;
- } else if (COLOR_JAUNE.Touched()) {
- while(COLOR_JAUNE.Touched());
- COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=4;
- color++;
- } else if (COLOR_BLEU.Touched()) {
- while(COLOR_BLEU.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=5;
- color++;
- }
- }
- break;
-
- case 1:
- lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.SetTextColor(NOIR);
- lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 2", LEFT_MODE);
- if(COLOR_ORANGE.Touched()) {
- while(COLOR_ORANGE.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=1;
- color++;
- } else if (COLOR_NOIR.Touched()) {
- while(COLOR_NOIR.Touched());
- COLOR_NOIR.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=2;
- color++;
- } else if (COLOR_VERT.Touched()) {
- while(COLOR_VERT.Touched());
- COLOR_VERT.Draw(LCD_COLOR_WHITE);
-
- msgTx.data[color]=3;
- color++;
- } else if (COLOR_JAUNE.Touched()) {
- while(COLOR_JAUNE.Touched());
- COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=4;
- color++;
- } else if (COLOR_BLEU.Touched()) {
- while(COLOR_BLEU.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=5;
- color++;
- }
- break;
-
- case 2:
- lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.SetTextColor(NOIR);
- lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 3", LEFT_MODE);
- if(COLOR_ORANGE.Touched()) {
- while(COLOR_ORANGE.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=1;
- color++;
- } else if (COLOR_NOIR.Touched()) {
- while(COLOR_NOIR.Touched());
- COLOR_NOIR.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=2;
- color++;
- } else if (COLOR_VERT.Touched()) {
- while(COLOR_VERT.Touched());
- COLOR_VERT.Draw(LCD_COLOR_WHITE);
-
- msgTx.data[color]=3;
- color++;
- } else if (COLOR_JAUNE.Touched()) {
- while(COLOR_JAUNE.Touched());
- COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=4;
- color++;
- } else if (COLOR_BLEU.Touched()) {
- while(COLOR_BLEU.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=5;
- color++;
- }
- break;
- case 3:
- lcd.Clear(LCD_COLOR_WHITE);
- lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.SetTextColor(NOIR);
-
- lcd.DisplayStringAt(0, LINE(4), (uint8_t *)"Immeuble en construction", LEFT_MODE);
- RETOUR.Draw(ROUGE,0);
- can2.write(msgTx);
- color++;
- break;
-
- case 4:
- if(RETOUR.Touched()) {
- while(RETOUR.Touched());
- etat=DEMO;
- }
- break;
- }
- if(RETOUR.Touched()) {
- while(RETOUR.Touched());
- etat=DEMO;
- }
- }
- break;
-
-
-
-*/
+ /* ///////////////////////////////TESTE LES SERVOS LIES AU TRI DES BALLES///////////////////////////////
+ case DEMO_TRIEUR:
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
+ TRI.Draw(VERT, 0);
+ AIGUILLEUR_D.Draw(VERT, 0);
+ AIGUILLEUR_G.Draw(VERT, 0);
+ AIGUILLEUR_CTRE.Draw(VERT, 0);
+ while(etat==DEMO_TRIEUR) {
+ if(RETOUR.Touched()) {
+ while (RETOUR.Touched());
+ etat=DEMO;
+ } else if(TRI.Touched()) {
+ while (TRI.Touched());
+ SendRawId(AIGUILLEUR_CENTRE);
+ wait(0.5);
+ SendRawId(AIGUILLEUR_DROITE);
+ wait(0.5);
+ SendRawId(AIGUILLEUR_GAUCHE);
+ wait(0.5);
+ SendRawId(AIGUILLEUR_CENTRE);
+
+ break;
+ } else if(AIGUILLEUR_D.Touched()) {
+ while (AIGUILLEUR_D.Touched());
+ SendRawId(AIGUILLEUR_DROITE);
+ break;
+ } else if(AIGUILLEUR_G.Touched()) {
+ while (AIGUILLEUR_G.Touched());
+ SendRawId(AIGUILLEUR_GAUCHE);
+ break;
+
+ } else if(BRAS_ABEILLE_OFF.Touched()) {
+ while (BRAS_ABEILLE_OFF.Touched());
+ SendRawId(BRAS_ABEILLE_DOWN);
+ break;
+ } else if(AIGUILLEUR_CTRE.Touched()) {
+ while (AIGUILLEUR_CTRE.Touched());
+ SendRawId(AIGUILLEUR_CENTRE);
+ break;
+ }
+
+ }
+ break;
+ case DEMO_IMMEUBLE: //TESTE LE MONTE IMMEUBLE SUIVANT UN CODE COULEUR CHOISI
+ int color=0;
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"Choix du code couleur", LEFT_MODE);
+
+ CANMessage msgTx=CANMessage();
+ msgTx.id=MONTER_IMMEUBLE; // Monter immeuble
+ msgTx.len=3;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+
+
+ while(etat==DEMO_IMMEUBLE) {
+ switch(color) {
+ case 0:
+
+ RETOUR.Draw(ROUGE,0);
+ COLOR_NOIR.Draw(NOIR,1);
+ COLOR_ORANGE.Draw(ORANGE,0);
+ COLOR_JAUNE.Draw(JAUNE,0);
+ COLOR_VERT.Draw(VERT,0);
+ COLOR_BLEU.Draw(BLEU,0);
+
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.SetTextColor(NOIR);
+ lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 1", LEFT_MODE);
+ while(color==0) {
+ if(COLOR_ORANGE.Touched()) {
+ while(COLOR_ORANGE.Touched());
+ COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=1;
+ color++;
+ } else if (COLOR_NOIR.Touched()) {
+ while(COLOR_NOIR.Touched());
+ COLOR_NOIR.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=2;
+ color++;
+ } else if (COLOR_VERT.Touched()) {
+ while(COLOR_VERT.Touched());
+ COLOR_VERT.Draw(LCD_COLOR_WHITE);
+
+ msgTx.data[color]=3;
+ color++;
+ } else if (COLOR_JAUNE.Touched()) {
+ while(COLOR_JAUNE.Touched());
+ COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=4;
+ color++;
+ } else if (COLOR_BLEU.Touched()) {
+ while(COLOR_BLEU.Touched());
+ COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=5;
+ color++;
+ }
+ }
+ break;
+
+ case 1:
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.SetTextColor(NOIR);
+ lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 2", LEFT_MODE);
+ if(COLOR_ORANGE.Touched()) {
+ while(COLOR_ORANGE.Touched());
+ COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=1;
+ color++;
+ } else if (COLOR_NOIR.Touched()) {
+ while(COLOR_NOIR.Touched());
+ COLOR_NOIR.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=2;
+ color++;
+ } else if (COLOR_VERT.Touched()) {
+ while(COLOR_VERT.Touched());
+ COLOR_VERT.Draw(LCD_COLOR_WHITE);
+
+ msgTx.data[color]=3;
+ color++;
+ } else if (COLOR_JAUNE.Touched()) {
+ while(COLOR_JAUNE.Touched());
+ COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=4;
+ color++;
+ } else if (COLOR_BLEU.Touched()) {
+ while(COLOR_BLEU.Touched());
+ COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=5;
+ color++;
+ }
+ break;
+
+ case 2:
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.SetTextColor(NOIR);
+ lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 3", LEFT_MODE);
+ if(COLOR_ORANGE.Touched()) {
+ while(COLOR_ORANGE.Touched());
+ COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=1;
+ color++;
+ } else if (COLOR_NOIR.Touched()) {
+ while(COLOR_NOIR.Touched());
+ COLOR_NOIR.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=2;
+ color++;
+ } else if (COLOR_VERT.Touched()) {
+ while(COLOR_VERT.Touched());
+ COLOR_VERT.Draw(LCD_COLOR_WHITE);
+
+ msgTx.data[color]=3;
+ color++;
+ } else if (COLOR_JAUNE.Touched()) {
+ while(COLOR_JAUNE.Touched());
+ COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=4;
+ color++;
+ } else if (COLOR_BLEU.Touched()) {
+ while(COLOR_BLEU.Touched());
+ COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+ msgTx.data[color]=5;
+ color++;
+ }
+ break;
+ case 3:
+ lcd.Clear(LCD_COLOR_WHITE);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.SetTextColor(NOIR);
+
+ lcd.DisplayStringAt(0, LINE(4), (uint8_t *)"Immeuble en construction", LEFT_MODE);
+ RETOUR.Draw(ROUGE,0);
+ can2.write(msgTx);
+ color++;
+ break;
+
+ case 4:
+ if(RETOUR.Touched()) {
+ while(RETOUR.Touched());
+ etat=DEMO;
+ }
+ break;
+ }
+ if(RETOUR.Touched()) {
+ while(RETOUR.Touched());
+ etat=DEMO;
+ }
+ }
+ break;
+
+
+
+ */
case TEST_SERVO: //TEST DU RESTE DES SERVOS DISPO HORS TIR
lcd.SetBackColor(LCD_COLOR_WHITE);
@@ -736,8 +742,8 @@
LANCEUR_ON.Draw(VERT, 0);
LANCEUR_OFF.Draw(VERT, 0);
RETOUR.Draw(ROUGE, 0);
-
- etat=DEMO;/*
+
+ etat=DEMO;/*
while(etat==TEST_TIR) {
if(TIR_CHATEAU.Touched()) {
while (TIR_CHATEAU.Touched());
@@ -780,6 +786,7 @@
RETOUR.Draw(0xFFFF0000, 0);
while(etat==TEST_TELEMETRE) {
SendRawId(DATA_RECALAGE);
+ SendRawId(DATA_TELEMETRE_LOGIQUE);
wait_ms(100);
canProcessRx();
if(RETOUR.Touched()) {
@@ -797,8 +804,8 @@
lcd.SetTextColor(LCD_COLOR_BLACK);
lcd.DisplayStringAt(70, LINE(0), (uint8_t *)"Choisir le cote", LEFT_MODE);
- COTE_JAUNE.Draw(JAUNE , 0);
- COTE_VIOLET.Draw(VIOLET , 0);
+ COTE_JAUNE.Draw(JAUNE, 0);
+ COTE_VIOLET.Draw(VIOLET, 0);
RETOUR.Draw(LCD_COLOR_RED, 0);
@@ -1229,8 +1236,8 @@
break;
case ETAT_GAME_START:
-
-
+
+
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
if (ModeDemo == 0) {
@@ -1266,7 +1273,7 @@
*/
//debug_Instruction(instruction);
//affichage_debug(gameEtat);
- rn42_Tx.printf("A");
+ rn42_Tx.printf("A");//lance l'electron
actionPrecedente = instruction.order;
switch(instruction.order) {
case MV_COURBURE://C'est un rayon de courbure
@@ -1328,7 +1335,7 @@
localData2 = instruction.arg3;
} else {//C'est un rotation absolu, il faut la convertir en relative
localData2 = instruction.arg3;
-
+
localData2 = (localData2 - theta_robot)%3600;
if(localData2 > 1800) {
localData2 = localData2-3600;
@@ -1340,25 +1347,24 @@
if(InversStrat == 1 && ingnorInversionOnce == 0) {
localData2 = -localData2;
}*/
-
-
- localData2 = instruction.arg3;
-
- if(InversStrat == 1 && ingnorInversionOnce == 0)
- {
+
+
+ localData2 = instruction.arg3;
+
+ if(InversStrat == 1 && ingnorInversionOnce == 0) {
localData2 = -localData2;
}
-
- if(instruction.direction == ABSOLUTE)
- {//C'est un rotation absolu, il faut la convertir en relative
-
+
+ if(instruction.direction == ABSOLUTE) {
+ //C'est un rotation absolu, il faut la convertir en relative
+
localData2 = (localData2 - theta_robot)%3600;
if(localData2 > 1800) localData2 = localData2-3600;
-
+
else if(localData2 <-1800) localData2 = localData2+3600;
}
-
-
+
+
waitingAckID = ASSERVISSEMENT_ROTATION;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Rotate(localData2);
@@ -1635,21 +1641,21 @@
case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s
if(timeoutWarning.read_ms() >= BALISE_TIMEOUT) { //ça fait plus de 2s, il faut changer de stratégie
- gameEtat = ETAT_EVITEMENT;
- /* if(Fevitement==1) {
- EvitEtat= 0;
- Fevitement=0;
- }*/
-
- /*-------------------------------------
- code origine
-
- if(instruction.nextLineOK != instruction.nextLineError)
- {
- actual_instruction = instruction.nextLineError;
- gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
- }----------------------------------------*/
-
+ if( needToStop()==2) {
+ gameEtat = ETAT_EVITEMENT;
+ /* if(Fevitement==1) {
+ EvitEtat=0;
+ Fevitement=0;
+ }*/
+ }
+ if( needToStop()==1) {
+ // code origine
+ if(instruction.nextLineOK != instruction.nextLineError) {
+ actual_instruction = instruction.nextLineError;
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+ }
+ //--------------------------------------------
+ }
}
break;
@@ -1731,19 +1737,19 @@
break;
case ETAT_EVITEMENT :
- /*
-
- 90°
- |
- |Violet
- |
- |
- |
- |
- |Jaune
- |
- |________________ 0° */
-
+ /*
+
+ 90°
+ |
+ |Violet
+ |
+ |
+ |
+ |
+ |Jaune
+ |
+ |________________ 0° */
+
char message[10]="toto";
char message1[10]="toto";
char message2[10]="toto";
@@ -1752,7 +1758,7 @@
static short x_terrain=3000;
static short y_terrain=1500;
*/
-
+
static short y_terrain=3000;
static short x_terrain=1500;
@@ -1771,11 +1777,11 @@
static unsigned short distance=50000;//valeur impossible
static unsigned short distance_prev=50000;
static signed short theta_adversaire;
-
+
switch(EvitEtat) {
case 0:
-
-
+
+
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(2),(unsigned char *)"EVITEMENT ",LEFT_MODE);
@@ -1831,17 +1837,17 @@
wait_us(150);
while(waitingAckID !=0 && waitingAckFrom !=0)
canProcessRx();
-
- /* Rotate(theta_adversaire); //on tourne a gauche pour scanner
- waitingAckID = ASSERVISSEMENT_ROTATION;
- waitingAckFrom = ACKNOWLEDGE_MOTEUR;
- while(waitingAckID !=0 && waitingAckFrom !=0)
- canProcessRx();
-
- waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
- waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
- while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
- canProcessRx();*/
+
+ /* Rotate(theta_adversaire); //on tourne a gauche pour scanner
+ waitingAckID = ASSERVISSEMENT_ROTATION;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ while(waitingAckID !=0 && waitingAckFrom !=0)
+ canProcessRx();
+
+ waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();*/
EvitEtat=1;
@@ -1853,28 +1859,28 @@
if (ang_target > 1800) ang_target = (ang_target - 3600);
// float dist_target = (short)sqrt((target_x_robot - x_robot)*(target_x_robot - x_robot)+(target_y_robot - y_robot)*(target_y_robot - y_robot));
-/*
+ /*
+ float x_robot_adversaire = x_robot + (dist_robot_adversaire)*cos((float)(theta_adversaire)* M_PI/1800);
+ float y_robot_adversaire = y_robot + (dist_robot_adversaire)*sin((float)(theta_adversaire)*M_PI/1800);
+
+ x_cote_droit[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
+ y_cote_droit[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
+ x_cote_gauche[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
+ y_cote_gauche[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
+
+ x_cote_droit[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+900)*M_PI/1800);
+ y_cote_droit[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+900)*M_PI/1800);
+ x_cote_gauche[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-900)*M_PI/1800);
+ y_cote_gauche[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-900)*M_PI/1800);
+
+ x_cote_droit[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+500)*M_PI/1800);
+ y_cote_droit[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+500)*M_PI/1800);
+ x_cote_gauche[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-500)*M_PI/1800);
+ y_cote_gauche[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-500)*M_PI/1800);
+ */
float x_robot_adversaire = x_robot + (dist_robot_adversaire)*cos((float)(theta_adversaire)* M_PI/1800);
float y_robot_adversaire = y_robot + (dist_robot_adversaire)*sin((float)(theta_adversaire)*M_PI/1800);
- x_cote_droit[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
- y_cote_droit[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
- x_cote_gauche[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
- y_cote_gauche[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
-
- x_cote_droit[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+900)*M_PI/1800);
- y_cote_droit[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+900)*M_PI/1800);
- x_cote_gauche[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-900)*M_PI/1800);
- y_cote_gauche[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-900)*M_PI/1800);
-
- x_cote_droit[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+500)*M_PI/1800);
- y_cote_droit[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+500)*M_PI/1800);
- x_cote_gauche[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-500)*M_PI/1800);
- y_cote_gauche[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-500)*M_PI/1800);
-*/
- float x_robot_adversaire = x_robot + (dist_robot_adversaire)*cos((float)(theta_adversaire)* M_PI/1800);
- float y_robot_adversaire = y_robot + (dist_robot_adversaire)*sin((float)(theta_adversaire)*M_PI/1800);
-
x_cote_droit[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-1300.0)*M_PI/1800);
y_cote_droit[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
x_cote_gauche[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
@@ -2122,9 +2128,6 @@
flag=1;
break;
-
-
-
/////////////////////////////////////Acknowledges de Reception de la demande d'action////////////////////////////////////////
case ACKNOWLEDGE_HERKULEX:
case ACKNOWLEDGE_BALISE: //pas de break donc passe directement dans ACK_FIN_ACTION mais conserve l'ident initial
@@ -2138,12 +2141,14 @@
unsigned short recieveAckID;// = (unsigned short)msgRxBuffer[FIFO_lecture].data[0] | ( ((unsigned short)msgRxBuffer[FIFO_lecture].data[1]) <<8);
memcpy(&recieveAckID, msgRxBuffer[FIFO_lecture].data, 2);
+ if(msgRxBuffer[FIFO_lecture].id==ACKNOWLEDGE_MOTEUR && ASSERVISSEMENT_XYT==recieveAckID)ingnorBalise=1;
+ if(msgRxBuffer[FIFO_lecture].id==INSTRUCTION_END_MOTEUR && ASSERVISSEMENT_XYT_ROTATE==recieveAckID)ingnorBalise=0;
+
if( waitingAckFrom == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID ) {
waitingAckFrom = 0;
waitingAckID = 0;
}
if( waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID_FIN ) {
- //SendRawId(0x5D9);
waitingAckFrom_FIN = 0;
waitingAckID_FIN = 0;
}
@@ -2169,7 +2174,7 @@
case ODOMETRIE_BIG_POSITION:
case ODOMETRIE_SMALL_POSITION:
-
+
x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
@@ -2192,9 +2197,6 @@
signed char fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
signed char debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
-
-
-
if(debut_angle_detection > fin_angle_detection) {
angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0f-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0f;
if(angle_moyen_balise_IR > 15.0f)
@@ -2288,35 +2290,42 @@
if(ModeDemo==1) {
- sprintf(message,"%04d mm",telemetreDistance_avant_droite);
+ sprintf(message,"%04dmm L:%d",telemetreDistance_avant_droite,DT_AVD_interrupt);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(8),(unsigned char *)"LASER AVD : ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(8),(unsigned char *)message, LEFT_MODE);
- sprintf(message1,"%04d mm",telemetreDistance_avant_gauche);
+ sprintf(message1,"%04dmm L:%d",telemetreDistance_avant_gauche,DT_AVG_interrupt);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(10),(unsigned char *)"LASER AVG : ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(10),(unsigned char *)message1, LEFT_MODE);
-
-
- sprintf(message4,"%04d ",theta_robot);
+
+
+ sprintf(message4,"%04d",theta_robot);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(13),(unsigned char *)"THETA: ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(13),(unsigned char *)message4, LEFT_MODE);
- sprintf(message2,"%04d mm",telemetreDistance_arriere_gauche);
+ sprintf(message2,"%04dmm L:%d",telemetreDistance_arriere_gauche,DT_ARG_interrupt);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"LASER ARG : ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message2, LEFT_MODE);
- sprintf(message3,"%04d mm",telemetreDistance_arriere_droite);
+ sprintf(message3,"%04dmm L:%d",telemetreDistance_arriere_droite,DT_ARD_interrupt);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(18),(unsigned char *)"LASER ARD : ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(18),(unsigned char *)message3, LEFT_MODE);
}
break;
-
+ case RECEPTION_TELEMETRE_LOGIQUE:
+
+ DT_AVD_interrupt=msgRxBuffer[FIFO_lecture].data[0];
+ DT_AVG_interrupt=msgRxBuffer[FIFO_lecture].data[1];
+ DT_ARG_interrupt=msgRxBuffer[FIFO_lecture].data[2];
+ DT_ARD_interrupt=msgRxBuffer[FIFO_lecture].data[3];
+
+ break;
case RECEPTION_COULEUR:
if (blocage_balise==0) {
couleur1=msgRxBuffer[FIFO_lecture].data[0];
@@ -2329,14 +2338,14 @@
}
break;
-/*
- case NO_BLOC: //il n'y a pas de bloc, on saute les étapes liées à l'attrape bloc
- actual_instruction = instruction.nextLineError;
- gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
- // waitingAckID_FIN=0;
- // waitingAckFrom_FIN=0;
- SendRawId(0x40);
- break;*/
+ /*
+ case NO_BLOC: //il n'y a pas de bloc, on saute les étapes liées à l'attrape bloc
+ actual_instruction = instruction.nextLineError;
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+ // waitingAckID_FIN=0;
+ // waitingAckFrom_FIN=0;
+ SendRawId(0x40);
+ break;*/
}
FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
}