CRAC Team
carte_strategie_2019
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Strategie/Strategie.cpp
- Revision:
- 34:6aa4b46b102e
- Parent:
- 33:388aa0bf6af4
- Child:
- 35:742dc6b200b0
--- a/Strategie/Strategie.cpp Wed Apr 25 12:42:50 2018 +0000
+++ b/Strategie/Strategie.cpp Thu Apr 26 20:14:18 2018 +0000
@@ -6,9 +6,9 @@
#define M_PI 3.14159265358979323846
-#define BLEU 0xFF1467E5
+#define VERT 0xFF1467E5
#define ROUGE 0xFFFF0000
-#define VERT 0xFF00FF00
+#define BLEU 0xFF00FF00
#define JAUNE 0xFFFEFE00
#define BLANC 0xFF000000
#define ORANGE 0xFFFFA500
@@ -42,7 +42,8 @@
"End_loop",
};
-
+int waitingAckID_FIN;
+int waitingAckFrom_FIN;
Ticker ticker;
TS_DISCO_F469NI ts;
@@ -78,7 +79,7 @@
signed short start_move_x,start_move_y,start_move_theta;//La position du robot lors du début d'un mouvement, utilisé pour reprendre le mouvement apres stop balise
//unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
int flagSendCan=1;
-unsigned char Cote = 0; //0 -> bleu | 1 -> jaune
+unsigned char Cote = 0; //0 -> VERT | 1 -> jaune
unsigned char checkCurrent = 0;
unsigned char countAliveCard = 0;
@@ -106,8 +107,8 @@
/////////////////DEFINITION DES BOUTONS////////////////////
- Button COTE_BLEU(0, 25, 400, 300, "Bleu");
- Button COTE_JAUNE(0, 350, 400, 300, "Jaune");
+ Button COTE_VERT(0, 25, 400, 300, "VERT");
+ Button COTE_ORANGE(0, 350, 400, 300, "ORANGE");
Button RETOUR (0, 680, 400, 110, "--Precedent--");
Button LANCER (0, 200, 400, 200, "--LANCER--");
Button CHECK (0, 420, 400, 200, "Valider");
@@ -137,8 +138,8 @@
Button SUIVANT(0,380,200,100,"Suivant");
Button COLOR_ORANGE (0, 230, 190, 110,"");
Button COLOR_JAUNE (210, 230, 190, 110,"");
- Button COLOR_BLEU (0, 350, 190, 110,"");
- Button COLOR_VERT (210, 350, 190, 110,"");
+ Button COLOR_VERT (0, 350, 190, 110,"");
+ Button COLOR_BLEU (210, 350, 190, 110,"");
Button COLOR_NOIR (105, 470, 190, 110,"");
////////////////////////////////////////////////////////////
@@ -153,9 +154,10 @@
void effacer_segment(long couleur);
unsigned short telemetreDistance=0;
-unsigned short telemetreDistance1=0;
-unsigned short telemetreDistance2=0;
-unsigned short telemetreDistance3=0;
+unsigned short telemetreDistance_avant_gauche=0;
+unsigned short telemetreDistance_avant_droite=0;
+unsigned short telemetreDistance_arriere_gauche=0;
+unsigned short telemetreDistance_arriere_droite=0;
#ifdef ROBOT_BIG
@@ -292,7 +294,7 @@
}
strcpy(tableau_aff[9],tableau_etat[conv]);
for(i=0;i<10;i++){
- lcd.SetBackColor(BLEU);
+ lcd.SetBackColor(VERT);
lcd.DisplayStringAt(0, LINE(20+i), (uint8_t *)tableau_aff[i], LEFT_MODE);
}
/*while(!ack_bluetooth){
@@ -397,12 +399,12 @@
case DEMO :
lcd.Clear(LCD_COLOR_WHITE);
RETOUR.Draw(0xFFFF0000, 0);
- TEST_HERKULEX.Draw(BLEU, 0);
- TEST_LASER.Draw(BLEU, 0);
- TEST_COULEURS.Draw(BLEU, 0);
- TEST_TIR_BALLE.Draw(BLEU, 0);
- TEST_IMMEUBLE.Draw(BLEU,0);
- TEST_TRIEUR.Draw(BLEU,0);
+ TEST_HERKULEX.Draw(VERT, 0);
+ TEST_LASER.Draw(VERT, 0);
+ TEST_COULEURS.Draw(VERT, 0);
+ TEST_TIR_BALLE.Draw(VERT, 0);
+ TEST_IMMEUBLE.Draw(VERT,0);
+ TEST_TRIEUR.Draw(VERT,0);
if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config //
InversStrat = 0;//Pas d'inversion de la couleur // A changer , discussion avec l'ihm
}
@@ -553,8 +555,8 @@
COLOR_NOIR.Draw(NOIR,1);
COLOR_ORANGE.Draw(ORANGE,0);
COLOR_JAUNE.Draw(JAUNE,0);
- COLOR_BLEU.Draw(BLEU,0);
COLOR_VERT.Draw(VERT,0);
+ COLOR_BLEU.Draw(VERT,0);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(NOIR);
@@ -809,15 +811,15 @@
lcd.SetTextColor(LCD_COLOR_BLACK);
lcd.DisplayStringAt(70, LINE(0), (uint8_t *)"Choisir le cote", LEFT_MODE);
- COTE_BLEU.Draw(BLEU, 0);
- COTE_JAUNE.Draw(JAUNE, 0);
+ COTE_VERT.Draw(VERT, 0);
+ COTE_ORANGE.Draw(ORANGE, 0);
RETOUR.Draw(LCD_COLOR_RED, 0);
while (etat == SELECT_SIDE)
{
canProcessRx();
- if(COTE_BLEU.Touched())
+ if(COTE_VERT.Touched())
{
liaison_Tx.envoyer(0x30,3,"123");
Cote = 0x0;
@@ -830,11 +832,11 @@
trame_Tx.id=CHOICE_COLOR;
trame_Tx.data[0]=Cote;
can2.write(trame_Tx);
- while(COTE_BLEU.Touched());
+ while(COTE_VERT.Touched());
}
- if(COTE_JAUNE.Touched())
+ if(COTE_ORANGE.Touched())
{
Cote = 0x1;
InversStrat= Cote;
@@ -846,7 +848,7 @@
trame_Tx.id=CHOICE_COLOR;
trame_Tx.data[0]=Cote;
can2.write(trame_Tx);
- while(COTE_JAUNE.Touched());
+ while(COTE_ORANGE.Touched());
}
if(RETOUR.Touched())
@@ -860,16 +862,16 @@
case TACTIQUE :
if (Cote == 0){
- lcd.Clear(BLEU);
- lcd.SetBackColor(BLEU);
+ lcd.Clear(VERT);
+ lcd.SetBackColor(VERT);
}
else if (Cote == 1){
lcd.Clear(JAUNE);
lcd.SetBackColor(JAUNE);
}
else {
- lcd.Clear(BLEU);
- lcd.SetBackColor(BLEU);
+ lcd.Clear(VERT);
+ lcd.SetBackColor(VERT);
}
lcd.SetTextColor(LCD_COLOR_BLACK);
@@ -927,16 +929,16 @@
lcd.SetTextColor(LCD_COLOR_BLACK);
if (Cote == 0){
- lcd.Clear(BLEU);
- lcd.SetBackColor(BLEU);
+ lcd.Clear(VERT);
+ lcd.SetBackColor(VERT);
}
else if (Cote == 1){
lcd.Clear(JAUNE);
lcd.SetBackColor(JAUNE);
}
else {
- lcd.Clear(BLEU);
- lcd.SetBackColor(BLEU);
+ lcd.Clear(VERT);
+ lcd.SetBackColor(VERT);
}
canProcessRx();
lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"En attente du Jack", CENTER_MODE);
@@ -951,14 +953,24 @@
lcd.SetTextColor(LCD_COLOR_BLACK);
cpt=int(cptf);
if(cpt != cpt1){
- lcd.Clear(BLEU);
- affichage_compteur(90-cpt);
+ lcd.Clear(VERT);
+ affichage_compteur(100-cpt);
}
cpt1=cpt;
flag_timer=0;
- affichage_debug(gameEtat);
+ //affichage_debug(gameEtat);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ char position_x[10]="toto";
+ char position_y[10]="toto";
+ char position_theta[10]="toto";
+ sprintf(position_x,"%d ",x_robot);
+ sprintf(position_y,"%d",y_robot);
+ sprintf(position_theta,"%d ",theta_robot);
+ lcd.DisplayStringAt(120, LINE(18), (uint8_t *)position_x, LEFT_MODE);
+ lcd.DisplayStringAt(120, LINE(20), (uint8_t *)position_y, LEFT_MODE);
+ lcd.DisplayStringAt(120, LINE(22), (uint8_t *)position_theta, LEFT_MODE);
break;
@@ -990,7 +1002,7 @@
//signed short localData4 = 0;
unsigned char localData5 = 0;
- if(gameTimer.read_ms() >= 89000) {//Fin du match (On autorise 2s pour déposer des éléments
+ if(gameTimer.read_ms() >= 99000) {//Fin du match (On autorise 2s pour déposer des éléments
gameTimer.stop();
gameTimer.reset();
gameEtat = ETAT_END;//Fin du temps
@@ -1135,12 +1147,12 @@
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
if (ModeDemo == 0){
- chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s
+ chronoEnd.attach(&chronometre_ISR,100);//On lance le chrono de 90s
gameTimer.start();
}
gameTimer.reset();
jack.fall(NULL);//On désactive l'interruption du jack
- SendRawId(GLOBAL_START);
+ //SendRawId(GLOBAL_START);
Jack=0; //à envoyer sur le CAN et en direct pour l'automate de l'ihm ou sur CANV
//tactile_printf("Start");//Pas vraiment utile mais bon
break;
@@ -1271,9 +1283,10 @@
}
}
#endif
- Rotate(localData2);
waitingAckID = ASSERVISSEMENT_ROTATION;
waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ Rotate(localData2);
+
break;
case MV_XYT:
@@ -1310,25 +1323,242 @@
break;
case MV_RECALAGE:
- waitingAckID = ASSERVISSEMENT_RECALAGE;
- waitingAckFrom = ACKNOWLEDGE_MOTEUR;
- instruction.nextActionType = WAIT;
- localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//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
+ if(instruction.nextActionType == MECANIQUE)
+ {
+ instruction.nextActionType = WAIT;
+
+ waitingAckID = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+
+ localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//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);
}
-
- GoStraight(localData2, localData5, localData3, 0);
+ else //CAPTEUR
+ {
+ unsigned char nombresDeMesuresAuxTelemtresQuiSontCoherentes = 0;
+ unsigned int moyennageTelemetre = 0;
+ unsigned short angleAvant = 0;
+ unsigned short angleArriere = 0;
+ unsigned short orientationArrondie = 0;
+
+ unsigned short position_avant_gauche=0;
+ unsigned short position_avant_droite=0;
+ unsigned short position_arriere_gauche=0;
+ unsigned short position_arriere_droite=0;
+
+ unsigned short angleRecalage = 0;
+ unsigned short distanceRecalage = 0;
+
+ SendRawId(DATA_RECALAGE);
+ waitingAckID = RECEPTION_RECALAGE;
+ waitingAckFrom = ACKNOWLEDGE_TELEMETRE;
+
+ // On attend que les variables soient actualisé
+ while(!(waitingAckID == 0 && waitingAckFrom == 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)
+ {
+ telemetreDistance_avant_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_gauche;
+ telemetreDistance_avant_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_droite;
+ telemetreDistance_arriere_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_arriere_gauche;
+ telemetreDistance_arriere_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_arriere_droite;
+
+ if(telemetreDistance_avant_gauche >= y_robot-instruction.arg1 && telemetreDistance_avant_gauche <= y_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_avant_gauche;
+ }
+ if(telemetreDistance_avant_droite >= y_robot-instruction.arg1 && telemetreDistance_avant_droite <= y_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_avant_droite;
+ }
+ if(telemetreDistance_arriere_gauche >= y_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= y_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_arriere_gauche;
+ }
+ if(telemetreDistance_arriere_droite >= y_robot-instruction.arg1 && telemetreDistance_arriere_droite <= y_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_arriere_droite;
+ }
+
+ moyennageTelemetre /= nombresDeMesuresAuxTelemtresQuiSontCoherentes;
+
+ if(nombresDeMesuresAuxTelemtresQuiSontCoherentes)
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot);
+ }
+ else if(instruction.precision == RECALAGE_X)
+ {
+ telemetreDistance_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
+ telemetreDistance_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
+ telemetreDistance_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_arriere_gauche;
+ telemetreDistance_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_arriere_droite;
+
+ if(telemetreDistance_avant_gauche >= x_robot-instruction.arg1 && telemetreDistance_avant_gauche <= x_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_avant_gauche;
+ }
+ if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_avant_droite;
+ }
+ if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_arriere_gauche;
+ }
+ if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_arriere_droite;
+ }
+
+ moyennageTelemetre /= nombresDeMesuresAuxTelemtresQuiSontCoherentes;
+
+ if(nombresDeMesuresAuxTelemtresQuiSontCoherentes)
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, moyennageTelemetre, y_robot, theta_robot);
+ }
+ /*else if(instruction.precision == RECALAGE_T)
+ {
+ // On arrondie l'angle pour simplifier les conditions
+ if(theta_robot >= 450 && theta_robot <= 1350)
+ orientationArrondie = 90;
+ else if(theta_robot <= -450 && theta_robot >= -1350)
+ orientationArrondie = 270;
+ else if(theta_robot <= 450 && theta_robot >= -450)
+ orientationArrondie = 0;
+ else if(theta_robot >= 1350 && theta_robot <= -1350)
+ orientationArrondie = 180;
+
+ // Calcul de position pour faire la vérification de cohérence
+ if(orientationArrondie == 90 || orientationArrondie == 270)
+ {
+ position_avant_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_gauche;
+ position_avant_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_droite;
+ position_arriere_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_arriere_gauche;
+ position_arriere_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_arriere_droite;
+
+ }
+ else if(orientationArrondie == 0 || orientationArrondie == 180)
+ {
+ position_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
+ position_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
+ position_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_arriere_gauche;
+ position_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_arriere_droite;
+ }
+
+
+ if(orientationArrondie == 90 || orientationArrondie == 270) // Si il est en axe Y
+ {
+ if(position_arriere_droite >= y_robot-instruction.arg1 && position_arriere_droite <= y_robot+instruction.arg1) // Et que les mesures sont cohérentes
+ {
+ if(position_arriere_gauche >= y_robot-instruction.arg1 && position_arriere_gauche <= y_robot+instruction.arg1)
+ {
+ if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
+ angleArriere = 900 + (1800 * atan( (double)( (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ else
+ angleArriere = 900 + (1800 * atan( (double)( (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ }
+ }
+ }
+ else if(orientationArrondie == 0 || orientationArrondie == 180) // Si il est en axe X
+ {
+ if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
+ {
+ if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1)
+ {
+ if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
+ angleArriere = 900 + (1800 * atan( (double)( (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ else
+ angleArriere = 900 + (1800 * atan( (double)( (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ }
+ }
+ }
+
+ if(orientationArrondie == 90 || orientationArrondie == 270) // Si il est en axe Y
+ {
+ if(position_avant_droite >= y_robot-instruction.arg1 && position_avant_droite <= y_robot+instruction.arg1) // Et que les mesures sont cohérentes
+ {
+ if(position_avant_gauche >= y_robot-instruction.arg1 && position_avant_gauche <= y_robot+instruction.arg1)
+ {
+ if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
+ angleAvant = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ else
+ angleAvant = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+
+ nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ moyennageTelemetre += angleAvant;
+ }
+ }
+ }
+ else if(orientationArrondie == 0 || orientationArrondie == 180) // Si il est en axe X
+ {
+ if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
+ {
+ if(telemetreDistance_avant_gauche >= x_robot-instruction.arg1 && telemetreDistance_avant_gauche <= x_robot+instruction.arg1)
+ {
+ if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
+ angleAvant = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ else
+ angleAvant = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+
+ nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ moyennageTelemetre += angleArriere;
+ }
+ }
+ }
+
+ angleRecalage = moyennageTelemetre/nombresDeMesuresAuxTelemtresQuiSontCoherentes;
+
+ if(nombresDeMesuresAuxTelemtresQuiSontCoherentes)
+ {
+ if(orientationArrondie == 0)
+ {
+ angleRecalage -= 90;
+
+ if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
+ distanceRecalage = *);
+ else
+ distanceRecalage = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ }
+ else if(orientationArrondie == 90)
+ {
+ angleRecalage += 0;
+ }
+ else if(orientationArrondie == 180)
+ {
+ angleRecalage += 90;
+ }
+ else if(orientationArrondie == 270)
+ {
+ angleRecalage += 180;
+ }
+ }
+
+
+ if(nombresDeMesuresAuxTelemtresQuiSontCoherentes)
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, moyennageTelemetre);
+ } */
+ }
break;
+
case ACTION:
int tempo = 0;
waitingAckID= ACK_ACTION; //On veut un ack de type action
@@ -1336,7 +1566,7 @@
tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
if(tempo == 1){
//L'action est spécifique
- if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) {
+ if((waitingAckFrom == 0 && waitingAckID == 0) && instruction.nextActionType == ENCHAINEMENT) {
actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
@@ -1345,8 +1575,8 @@
gameEtat = ETAT_GAME_WAIT_ACK;
}
#ifdef ROBOT_SMALL
- actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
- gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+ /*actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;*/
#endif
return;
#ifdef ROBOT_SMALL
@@ -1398,7 +1628,7 @@
/*
Attente de l'ack de l'instruction
*/
- canProcessRx();
+
if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continue
//if(true) {
cartesCheker.stop();
@@ -1413,45 +1643,45 @@
}
}
else if(instruction.nextActionType == WAIT) { ///Actualisation des waiting ack afin d'attendre la fin des actions
- gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
+ gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
switch(instruction.order)
{
case MV_COURBURE:
- waitingAckID = ASSERVISSEMENT_COURBURE;
- waitingAckFrom = INSTRUCTION_END_MOTEUR;
+ waitingAckID_FIN = ASSERVISSEMENT_COURBURE;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
break;
case MV_LINE:
- waitingAckID = ASSERVISSEMENT_RECALAGE;
- waitingAckFrom = INSTRUCTION_END_MOTEUR;
+ waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
break;
case MV_TURN:
- waitingAckID = ASSERVISSEMENT_ROTATION;
- waitingAckFrom = INSTRUCTION_END_MOTEUR;
+ waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
break;
case MV_XYT:
- waitingAckID = ASSERVISSEMENT_XYT;
- waitingAckFrom = INSTRUCTION_END_MOTEUR;
+ waitingAckID_FIN = ASSERVISSEMENT_XYT;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
break;
case MV_RECALAGE:
- waitingAckID = ASSERVISSEMENT_RECALAGE;
- waitingAckFrom = INSTRUCTION_END_MOTEUR;
+ waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
break;
case ACTION:
if (modeTelemetre == 0){
if (telemetreDistance == 0){
- waitingAckID = ACK_FIN_ACTION;// ack de type action
- waitingAckFrom = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs
+ waitingAckID_FIN = ACK_FIN_ACTION;// ack de type action
+ waitingAckFrom_FIN = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs
}else if(telemetreDistance == 5000){
// on est dans le cas ou l'on fait une ligne suivant la distance du telemetre
- waitingAckID = ASSERVISSEMENT_RECALAGE;
- waitingAckFrom = INSTRUCTION_END_MOTEUR;
+ waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
telemetreDistance = 0;
}
}else{ // si on attend la reponse du telemetre
//modeTelemetre = 1;
- waitingAckID = OBJET_SUR_TABLE;
- waitingAckFrom = 0;
+ waitingAckID_FIN = OBJET_SUR_TABLE;
+ waitingAckFrom_FIN = 0;
}
break;
default:
@@ -1463,15 +1693,15 @@
actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
}
}
- else if(cartesCheker.read_ms () > 50){
+ else if(cartesCheker.read_ms () > 500){
cartesCheker.stop();
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 {
- //gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
- gameEtat=ETAT_GAME_WAIT_END_INSTRUCTION;
+ gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
+ //gameEtat=ETAT_GAME_WAIT_END_INSTRUCTION;
}
}
break;
@@ -1527,10 +1757,20 @@
break;
case ETAT_GAME_WAIT_END_INSTRUCTION:
canProcessRx();
- if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée
+ if(instruction.order==MV_XYT && (waitingAckID_FIN == 0 && waitingAckFrom_FIN ==0)){
+ if((x_robot<=target_x_robot-2 || x_robot>=target_x_robot+2)||(y_robot<=target_y_robot-2 || y_robot>=target_y_robot+2)||(theta_robot<=target_theta_robot-2 || theta_robot>=target_theta_robot+2)){
+ gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+ }
+ else {
+ actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
+ }
+ }
+ else if(waitingAckID_FIN == 0 && waitingAckFrom_FIN ==0) {//On attend que la carte nous indique que l'instruction est terminée
actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
}
+
break;
@@ -1671,10 +1911,10 @@
if (etat == ATT) {
lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
- lcd.FillRect(0,75,400,150); //carte moteur
+ lcd.FillRect(0,400,400,150);
lcd.SetTextColor(LCD_COLOR_BLACK);
lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
- lcd.DisplayStringAt(80, 135, (uint8_t *)"Carte Moteur", LEFT_MODE);
+ lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE);
}
break;
@@ -1682,10 +1922,10 @@
if (etat == ATT) {
lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
- lcd.FillRect(0,250,400,150); //carte AX12
+ lcd.FillRect(0,600,400,150); //carte AX12
lcd.SetTextColor(LCD_COLOR_BLACK);
lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
- lcd.DisplayStringAt(110, 310, (uint8_t *)"Balise", LEFT_MODE);
+ lcd.DisplayStringAt(110, 650, (uint8_t *)"Balise", LEFT_MODE);
}
break;
@@ -1716,9 +1956,10 @@
/////////////////////////////////////Acknowledges de Reception de la demande d'action////////////////////////////////////////
case ACKNOWLEDGE_HERKULEX:
case ACKNOWLEDGE_BALISE: //pas de break donc passe directement dans INSTRUCTION_END_AX12 mais conserve l'ident initial
- case ACKNOWLEDGE_MOTEUR:
+
case ACKNOWLEDGE_TELEMETRE:
/////////////////////////////////////////////Acknowledges de la fin d'action/////////////////////////////////////////////////
+ case ACKNOWLEDGE_MOTEUR:
case INSTRUCTION_END_BALISE:
case INSTRUCTION_END_MOTEUR:
case ACK_FIN_ACTION:
@@ -1727,6 +1968,11 @@
waitingAckFrom = 0;
waitingAckID = 0;
}
+ if(waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID_FIN)) {
+ //SendRawId(waitingAckID);
+ waitingAckFrom_FIN = 0;
+ waitingAckID_FIN = 0;
+ }
break;
#ifdef ROBOT_BIG
@@ -1824,33 +2070,34 @@
break;
case RECEPTION_RECALAGE:
- telemetreDistance=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]);
- telemetreDistance1=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[2], msgRxBuffer[FIFO_lecture].data[3]);
- telemetreDistance2=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[4], msgRxBuffer[FIFO_lecture].data[5]);
- telemetreDistance3=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]);
- waitingAckFrom = 0;
- waitingAckID = 0;
- if(ModeDemo==1){
- sprintf(message,"%04d mm",telemetreDistance);
+ telemetreDistance_arriere_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]); //
+ telemetreDistance_avant_droite = 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_avant_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]);
+
+
+
+ if(ModeDemo==1)
+ {
+ sprintf(message,"%04d mm",telemetreDistance_arriere_droite);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(10),(unsigned char *)"LASER ARD : ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(10),(unsigned char *)message, LEFT_MODE);
- sprintf(message1,"%04d mm",telemetreDistance1);
+ sprintf(message1,"%04d mm",telemetreDistance_avant_droite);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(12),(unsigned char *)"LASER AVD : ",LEFT_MODE);
lcd.DisplayStringAt(200, LINE(12),(unsigned char *)message1, LEFT_MODE);
- sprintf(message2,"%04d mm",telemetreDistance2);
+ 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);
- sprintf(message3,"%04d mm",telemetreDistance3);
+ sprintf(message3,"%04d mm",telemetreDistance_avant_gauche);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"LASER AVG : ",LEFT_MODE);
- lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message3, LEFT_MODE);
-
+ lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message3, LEFT_MODE);
}
break;