CRAC Team
Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Strategie/Strategie.cpp
- Revision:
- 51:aa6e09f2cfec
- Parent:
- 50:a5361ffeefc8
- Child:
- 52:a47350923b5e
--- a/Strategie/Strategie.cpp Fri May 17 21:06:46 2019 +0000
+++ b/Strategie/Strategie.cpp Sat May 18 16:54:06 2019 +0000
@@ -1603,10 +1603,10 @@
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) {
+ /* if(Fevitement==1) {
EvitEtat= 0;
Fevitement=0;
- }
+ }*/
/*-------------------------------------
code origine
@@ -1698,6 +1698,19 @@
break;
case ETAT_EVITEMENT :
+ /*
+
+ 90°
+ |
+ |Violet
+ |
+ |
+ |
+ |
+ |Jaune
+ |
+ |________________ 0° */
+
char message[10]="toto";
char message1[10]="toto";
char message2[10]="toto";
@@ -1714,17 +1727,18 @@
static short cote=0;
//--------------------------
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
+ int proxy=300;//distance entre point de controle et obstacle/adversaire
+ int proximity=300;//distance entre l'objectif et obstacle/adversaire
+ short taille_petit=200;// distance proxymité max mur
//---------------------------*
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);
@@ -1757,59 +1771,52 @@
waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
waitingId = RECEPTION_RECALAGE;
SendRawId(DATA_RECALAGE);
- //wait_us(150);
+ wait_us(150);
while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0) {
canProcessRx();
- // On attend que les variables soient actualise
- wait_ms(1);
+ wait_ms(10);
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;
+ dist_robot_adversaire=distance+100;
theta_adversaire=theta_robot;
- dist_robot_adversaire=distance;
}
waitingId = RECEPTION_RECALAGE;
- wait_ms(1);
SendRawId(DATA_RECALAGE);
wait_us(150);
}
}
- /*
- 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;
+
+ SendSpeed(300,5000,5000);//vitesse inintiale SendSpeed(600,5000,5000)
+ waitingAckID = ASSERVISSEMENT_CONFIG;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ 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();*/
+
+ EvitEtat=1;
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);
+ 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);
@@ -1827,6 +1834,24 @@
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)*sin((float)(theta_adversaire)* M_PI/1800);
+ float y_robot_adversaire = y_robot + (dist_robot_adversaire)*cos((float)(theta_adversaire)*M_PI/1800);
+
+ x_cote_droit[0] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
+ y_cote_droit[0] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
+ x_cote_gauche[0] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
+ y_cote_gauche[0] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
+
+ x_cote_droit[1] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+900)*M_PI/1800);
+ y_cote_droit[1] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+900)*M_PI/1800);
+ x_cote_gauche[1] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-900)*M_PI/1800);
+ y_cote_gauche[1] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-900)*M_PI/1800);
+
+ x_cote_droit[2] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+500)*M_PI/1800);
+ y_cote_droit[2] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+500)*M_PI/1800);
+ x_cote_gauche[2] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-500)*M_PI/1800);
+ y_cote_gauche[2] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-500)*M_PI/1800);
SendRawId(0x0D0);//calcul
@@ -1836,7 +1861,6 @@
bool cote_droit=false, cote_gauche=false;
-
for (int i=0; i<3; i++) {
if (x_cote_droit[i]>taille_petit && x_cote_droit[i]<x_terrain-taille_petit && y_cote_droit[i] >taille_petit && y_cote_droit[i] < y_terrain-taille_petit) {
cote_droit=true;
@@ -1868,17 +1892,10 @@
}
if ( ang_target>600 || ang_target<-600)cote=0;
-
-
- if (!cote_droit && !cote_gauche) {
- cote=0;
- }
-
+ if (!cote_droit && !cote_gauche)cote=0;
//--------------------test target --------------------------------------
-
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 = 2;
break;
@@ -1889,10 +1906,12 @@
if(cote!=0) {
for(int i=0; i<3; i++) {
if(cote==-1) {
- GoToPosition(y_cote_droit[i],x_cote_droit[i],theta_robot,1);//
+ GoToPosition(x_cote_droit[i],y_cote_droit[i],theta_robot,1);
+ //GoToPosition(y_cote_droit[i],x_cote_droit[i],theta_robot,1);//
SendRawId(0x1D1);//evitement a gauche
} else if(cote==1) {
- GoToPosition(y_cote_gauche[i],x_cote_gauche[i],theta_robot,1);
+ GoToPosition(x_cote_gauche[i],y_cote_gauche[i],theta_robot,1);
+ //GoToPosition(y_cote_gauche[i],x_cote_gauche[i],theta_robot,1);
SendRawId(0x2D1);//evitement a droite
}
waitingAckID = ASSERVISSEMENT_XYT;
@@ -1927,10 +1946,10 @@
ingnorBalise=0;
Fevitement=0;
}
- EvitEtat=2;
+ EvitEtat=3;
break;
- case 3:
+ case 3://on va vers l'objectif initial
SendRawId(0x0D2);
GoToPosition(target_x_robot,target_y_robot,target_theta_robot,1);
waitingAckID = ASSERVISSEMENT_XYT;
@@ -1944,7 +1963,7 @@
while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
canProcessRx();
- EvitEtat=3;
+ EvitEtat=4;
break;
case 4: //on charge l'instruction suivante et sort de l'evitement
@@ -2005,6 +2024,7 @@
char message1[10]="toto";
char message2[10]="toto";
char message3[10]="toto";
+ char message4[10]="toto";
FIFO_occupation=FIFO_ecriture-FIFO_lecture;
if(FIFO_occupation<0)
@@ -2114,11 +2134,11 @@
-#ifdef ROBOT_BIG
+//#ifdef ROBOT_BIG
case ODOMETRIE_BIG_POSITION:
-#else
+//#else
case ODOMETRIE_SMALL_POSITION:
-#endif
+//#endif
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);
@@ -2228,7 +2248,6 @@
break;
case RECEPTION_RECALAGE:
- 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);
@@ -2247,6 +2266,13 @@
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);
+ 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);
lcd.SetBackColor(LCD_COLOR_WHITE);