CRAC Team
Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Strategie/Strategie.cpp
- Revision:
- 50:a5361ffeefc8
- Parent:
- 49:d83a4851a257
- Child:
- 51:aa6e09f2cfec
diff -r d83a4851a257 -r a5361ffeefc8 Strategie/Strategie.cpp
--- a/Strategie/Strategie.cpp Thu May 16 12:08:07 2019 +0000
+++ b/Strategie/Strategie.cpp Fri May 17 21:06:46 2019 +0000
@@ -179,13 +179,13 @@
unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE};
-InterruptIn jack(PG_11); // entrée analogique en interruption pour le jack
+InterruptIn jack(PG_11); // entrée numerique en interruption pour le jack
#else
unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE};
-InterruptIn jack(PG_11); // entrée analogique en interruption pour le jack
+InterruptIn jack(PG_11); // entrée numerique en interruption pour le jack
#endif
@@ -770,7 +770,7 @@
RETOUR.Draw(0xFFFF0000, 0);
while(etat==TEST_TELEMETRE) {
SendRawId(DATA_RECALAGE);
- wait(0.1);
+ wait_ms(100);
canProcessRx();
if(RETOUR.Touched()) {
while( RETOUR.Touched());
@@ -1698,56 +1698,38 @@
break;
case ETAT_EVITEMENT :
-
- /*disposition des waypoint sur le terrain
- on considère le terrain comme un rectangle de 3m par 1m50 , on exclus la zone de la rampe
- ______________
- | | format(x,y)en mm
- Y| x x | (1000, 2250) (1500 , 2250)
- | |
- | x x | (1000 , 1500) (1500 , 1500)
- | |
- | x x | (1000 , 750) (1500 , 750)
- | |
- |______________|
- X-->
- */
+ char message[10]="toto";
+ char message1[10]="toto";
+ char message2[10]="toto";
+ char message3[10]="toto";
+
static short x_terrain=3000;
static short y_terrain=1500;
static float x_cote_droit[3]= {0};
static float y_cote_droit[3]= {0};
+
static float x_cote_gauche[3]= {0};
static float y_cote_gauche[3]= {0};
static short cote=0;
-//---------------------------
- /* short x_robot=500;
- short y_robot=500;
- short theta_robot=0;
- //---------------------------
- short target_x_robot=2000;
- short target_y_robot=1000;
- short target_theta_robot=1000;*/
//--------------------------
- float dist_robot_adversaire=650;//distance à laquelle on s'arrete grace à la balise
+ static float dist_robot_adversaire=650;//distance à laquelle on s'arrete grace à la balise
int proxy=400;//distance entre point de controle et obstacle/adversaire
int proximity=400;//distance entre l'objectif et obstacle/adversaire
short taille_petit=150;// distance proxymité max mur
//---------------------------*
static unsigned short distance=50000;//valeur impossible
static unsigned short distance_prev=50000;
- static unsigned short theta1,theta2,theta_adversaire;
+ static signed short theta_adversaire;
switch(EvitEtat) {
case 0:
- /*SendRawId(DATA_RECALAGE);
- wait(0.1);
- canProcessRx();
- unsigned short distance_adverse=telemetreDistance_arriere_gauche; //on sauvegarde notre distance au robot
- */
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(0, LINE(2),(unsigned char *)"EVITEMENT ",LEFT_MODE);
+
ingnorBalise=1;
- Rotate(300); //on tourne a gauche pour scanner
+ Rotate(450); //on tourne a gauche pour scanner
waitingAckID = ASSERVISSEMENT_ROTATION;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
while(waitingAckID !=0 && waitingAckFrom !=0)
@@ -1755,46 +1737,73 @@
waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
- while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0) {
+ while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
canProcessRx();
- SendRawId(DATA_RECALAGE);
- distance=telemetreDistance_arriere_gauche; //on sauvegarde notre distance au robot
- if(distance<=distance_prev) {
- distance_prev=distance;
- theta1=theta_robot;
- }
- }
- Rotate(-600);//on tourne a droite pour scanner
- waitingAckID = ASSERVISSEMENT_ROTATION;
+
+ SendSpeed(50,1500,1500);
+ waitingAckID = ASSERVISSEMENT_CONFIG;
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(300);//on revien au centre
+
+ Rotate(-900);//on tourne a droite pour scanner
waitingAckID = ASSERVISSEMENT_ROTATION;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ wait_us(150);
while(waitingAckID !=0 && waitingAckFrom !=0)
canProcessRx();
waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ waitingId = RECEPTION_RECALAGE;
+ SendRawId(DATA_RECALAGE);
+ //wait_us(150);
while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0) {
canProcessRx();
- SendRawId(DATA_RECALAGE);
- distance=telemetreDistance_arriere_gauche; //on sauvegarde notre distance au robot
- if(distance<=distance_prev) {
- distance_prev=distance;
- theta2=theta_robot;
+ // On attend que les variables soient actualise
+ wait_ms(1);
+ if(waitingId == 0) {
+ distance=telemetreDistance_avant_droite; //on sauvegarde notre distance au robot
+ if(distance<=distance_prev) {
+ /*unsigned char datatemp[2];
+ memcpy(datatemp, &distance, 2);
+ SendMsgCan(0x750, datatemp,2);
+ wait_us(150);*/
+ distance_prev=distance;
+ theta_adversaire=theta_robot;
+ dist_robot_adversaire=distance;
+ }
+ waitingId = RECEPTION_RECALAGE;
+ wait_ms(1);
+ SendRawId(DATA_RECALAGE);
+ wait_us(150);
}
}
- theta_adversaire=(theta1+theta2)/2;//version avec telemetre
- //theta_adversaire=theta_robot;//version sans telemetre (robot en face
-
+ /*
+ SendSpeed(600,5000,5000);//vitesse inintiale
+ waitingAckID = ASSERVISSEMENT_CONFIG;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ while(waitingAckID !=0 && waitingAckFrom !=0)
+ canProcessRx();
+
+ Rotate(theta_adversaire);//Rotate(450);on revient au centre
+ 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=10;
+
+ break;
+ //theta_adversaire=(theta1+theta2)/2;//version avec telemetre
+ //theta_adversaire=theta_robot;//version sans telemetre (robot en face
+
+ case 1:
short ang_target = (short)((atan2((float)(target_y_robot - y_robot), (float)(target_x_robot - x_robot)) * 1800 / M_PI) - theta_robot + 7200) % 3600;
// On passe le résultat entre -1800 et 1800
if (ang_target > 1800) ang_target = (ang_target- 3600);
@@ -1870,11 +1879,11 @@
if ((x_robot_adversaire >= target_x_robot-proximity && x_robot_adversaire <= target_x_robot+proximity)&&(y_robot_adversaire >= target_y_robot-proximity && y_robot_adversaire <= target_y_robot+proximity)) cote=0;
- EvitEtat = 1;
+ EvitEtat = 2;
break;
- case 1 ://on attend la fin de la première rotation pour activer la balise
+ case 2 ://on attend la fin de la première rotation pour activer la balise
//ingnorBalise=1;
SendRawId(0x0D1);//init evitement
if(cote!=0) {
@@ -1921,7 +1930,7 @@
EvitEtat=2;
break;
- case 2:
+ case 3:
SendRawId(0x0D2);
GoToPosition(target_x_robot,target_y_robot,target_theta_robot,1);
waitingAckID = ASSERVISSEMENT_XYT;
@@ -1938,13 +1947,27 @@
EvitEtat=3;
break;
- case 3: //on charge l'instruction suivante et sort de l'evitement
+ case 4: //on charge l'instruction suivante et sort de l'evitement
actual_instruction++;//on charge l'instruction suivante
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
EvitEtat = 0;
ingnorBalise=0;
Fevitement=0;
break;
+
+ case 10:
+ wait(0.2);
+
+ sprintf(message,"%d ",theta_adversaire);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(0, LINE(8),(unsigned char *)"Theta_adv : ",LEFT_MODE);
+ lcd.DisplayStringAt(200, LINE(8),(unsigned char *)message, LEFT_MODE);
+
+ sprintf(message1,"%04d mm",(short)dist_robot_adversaire);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(0, LINE(12),(unsigned char *)"Dist_adv : ",LEFT_MODE);
+ lcd.DisplayStringAt(200, LINE(12),(unsigned char *)message1, LEFT_MODE);
+ break;
}
break;
@@ -1983,12 +2006,19 @@
char message2[10]="toto";
char message3[10]="toto";
FIFO_occupation=FIFO_ecriture-FIFO_lecture;
+
if(FIFO_occupation<0)
FIFO_occupation=FIFO_occupation+SIZE_FIFO;
- if(FIFO_max_occupation<FIFO_occupation)
+
+ if(FIFO_max_occupation<FIFO_occupation) {
FIFO_max_occupation=FIFO_occupation;
+ //SendRawId(
+ }
+
if(FIFO_occupation!=0) {
int identifiant=msgRxBuffer[FIFO_lecture].id;
+
+ if (waitingId == identifiant) waitingId = 0;
switch(identifiant) {
case ALIVE_MOTEUR:
@@ -2050,8 +2080,6 @@
case INSTRUCTION_END_MOTEUR:
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);
- Send2Short(0x5D7,waitingAckFrom_FIN, waitingAckID_FIN);
- Send2Short(0x5D8,msgRxBuffer[FIFO_lecture].id, recieveAckID);
if( waitingAckFrom == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID ) {
waitingAckFrom = 0;
@@ -2147,8 +2175,13 @@
if(needToStop() != 0 && ingnorBaliseOnce ==0 && ingnorBalise==0) {
if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT) {
- SendRawId(0x0D9);//balise_stop
SendRawId(ASSERVISSEMENT_STOP);
+
+ /*waitingAckID_FIN = ASSERVISSEMENT_STOP;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();*/
+
//while(1); // ligne à décommenter si on est en homologation
if(gameEtat != ETAT_WARING_END_BALISE_WAIT) {
timeoutWarning.reset();
@@ -2158,10 +2191,6 @@
gameEtat = ETAT_WARNING_TIMEOUT;
}
}
-
-
-
-
}
ingnorBaliseOnce = 0;
break;
@@ -2199,33 +2228,35 @@
break;
case RECEPTION_RECALAGE:
- telemetreDistance_avant_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]); //on récupère la distance traité par l'autre micro
+ SendRawId(0x750);
+ wait_us(150);
+ // telemetreDistance_avant_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]); //on récupère la distance traité par l'autre micro
+ memcpy(&telemetreDistance_avant_droite,msgRxBuffer[FIFO_lecture].data,2);
telemetreDistance_avant_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[2], msgRxBuffer[FIFO_lecture].data[3]);
telemetreDistance_arriere_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[4], msgRxBuffer[FIFO_lecture].data[5]);
telemetreDistance_arriere_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]);
-
if(ModeDemo==1) {
sprintf(message,"%04d mm",telemetreDistance_avant_droite);
lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.DisplayStringAt(0, LINE(10),(unsigned char *)"LASER AVD : ",LEFT_MODE);
- lcd.DisplayStringAt(200, LINE(10),(unsigned char *)message, LEFT_MODE);
+ 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);
lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.DisplayStringAt(0, LINE(12),(unsigned char *)"LASER AVG : ",LEFT_MODE);
- lcd.DisplayStringAt(200, LINE(12),(unsigned char *)message1, LEFT_MODE);
+ lcd.DisplayStringAt(0, LINE(10),(unsigned char *)"LASER AVG : ",LEFT_MODE);
+ lcd.DisplayStringAt(200, LINE(10),(unsigned char *)message1, LEFT_MODE);
sprintf(message2,"%04d mm",telemetreDistance_arriere_gauche);
lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.DisplayStringAt(0, LINE(14),(unsigned char *)"LASER ARG : ",LEFT_MODE);
- lcd.DisplayStringAt(200, LINE(14),(unsigned char *)message2, LEFT_MODE);
+ 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);
lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"LASER ARD : ",LEFT_MODE);
- lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message3, LEFT_MODE);
+ lcd.DisplayStringAt(0, LINE(18),(unsigned char *)"LASER ARD : ",LEFT_MODE);
+ lcd.DisplayStringAt(200, LINE(18),(unsigned char *)message3, LEFT_MODE);
}
break;