CRAC Team
Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Strategie/Strategie.cpp
- Revision:
- 36:6dd30780bd8e
- Parent:
- 35:742dc6b200b0
- Child:
- 37:fca332b64b42
--- a/Strategie/Strategie.cpp Tue May 01 13:25:42 2018 +0000
+++ b/Strategie/Strategie.cpp Wed May 02 20:40:57 2018 +0000
@@ -779,7 +779,14 @@
}
else if(LANCEUR_ON.Touched()){
while (LANCEUR_ON.Touched());
- SendRawId(LANCEMENT_MOTEUR_TIR_ON);
+ CANMessage msgTx=CANMessage();
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.id=LANCEMENT_MOTEUR_TIR_ON;
+
+ msgTx.len=1;
+ msgTx.data[0]=0;
+ can2.write(msgTx);
break;
}
else if(LANCEUR_OFF.Touched()){
@@ -960,29 +967,19 @@
break;
case COMPTEUR:
- cptf=gameTimer.read();
+ /*cptf=gameTimer.read();
lcd.SetTextColor(LCD_COLOR_BLACK);
cpt=int(cptf);
- /*if(cpt != cpt1){
+ if(cpt != cpt1){
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 ",angleRecalage);
-
- 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;
case FIN :
@@ -1189,7 +1186,7 @@
Traitement de l'instruction, envoie de la trame CAN
*/
//debug_Instruction(instruction);
-
+ affichage_debug(gameEtat);
actionPrecedente = instruction.order;
switch(instruction.order)
{
@@ -1334,7 +1331,6 @@
break;
case MV_RECALAGE:
- wait(0.2);
if(instruction.nextActionType == MECANIQUE)
{
instruction.nextActionType = WAIT;
@@ -1358,21 +1354,7 @@
GoStraight(localData2, localData5, localData3, 0);
}
else //CAPTEUR
- {
- unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 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 distanceRecalage = 0;
-
+ {
SendRawId(DATA_RECALAGE);
waitingAckID = RECEPTION_RECALAGE;
waitingAckFrom = ACKNOWLEDGE_TELEMETRE;
@@ -1382,215 +1364,18 @@
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)
{
- 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))?0:3000) + (((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))?0:3000) + (((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)
- {
- nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
- moyennageTelemetre += telemetreDistance_avant_gauche;
- }
- if(telemetreDistance_avant_droite >= y_robot-instruction.arg1 && telemetreDistance_avant_droite <= y_robot+instruction.arg1)
- {
- nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
- moyennageTelemetre += telemetreDistance_avant_droite;
- }
- if(telemetreDistance_arriere_gauche >= y_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= y_robot+instruction.arg1)
- {
- nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
- moyennageTelemetre += telemetreDistance_arriere_gauche;
- }
- if(telemetreDistance_arriere_droite >= y_robot-instruction.arg1 && telemetreDistance_arriere_droite <= y_robot+instruction.arg1)
- {
- nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
- moyennageTelemetre += telemetreDistance_arriere_droite;
- }
-
- moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
-
- if(nombresDeMesuresAuxTelemetresQuiSontCoherentes)
- SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot);
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, recalageDistanceY(), 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)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
- telemetreDistance_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((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)
- {
- nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
- moyennageTelemetre += telemetreDistance_avant_gauche;
- }
- if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1)
- {
- nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
- moyennageTelemetre += telemetreDistance_avant_droite;
- }
- if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1)
- {
- nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
- moyennageTelemetre += telemetreDistance_arriere_gauche;
- }
- if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1)
- {
- nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
- moyennageTelemetre += telemetreDistance_arriere_droite;
- }
-
- moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
-
- if(nombresDeMesuresAuxTelemetresQuiSontCoherentes)
- SetOdometrie(ODOMETRIE_SMALL_POSITION, moyennageTelemetre, y_robot, theta_robot);
-
-
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, recalageDistanceX(), 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))?0:3000) + (((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))?0:3000) + (((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)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
- position_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((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 * atan2((double)(telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) , (double)ESPACE_INTER_TELEMETRE ))/M_PI;
- else
- angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
-
- nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
- moyennageTelemetre += angleArriere;
- }
- }
- }
- else if(orientationArrondie == 0 || orientationArrondie == 180) // Si il est en axe X
- {
- if(position_arriere_droite >= x_robot-instruction.arg1 && position_arriere_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
- {
- if(position_arriere_gauche >= x_robot-instruction.arg1 && position_arriere_gauche <= x_robot+instruction.arg1)
- {
- if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
- angleArriere =900+(1800 * atan2( (double) (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) , (double) ESPACE_INTER_TELEMETRE ))/M_PI;
- else
- angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
-
- nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
- moyennageTelemetre += angleArriere;
- }
- }
- }
-
- 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 = (1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
- else
- angleAvant = 900 + (1800 * atan2( (double)( telemetreDistance_avant_gauche-telemetreDistance_avant_droite) ,(double) ESPACE_INTER_TELEMETRE ))/M_PI;
-
- nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
- moyennageTelemetre += angleAvant;
- }
- }
- }
- else if(orientationArrondie == 0 || orientationArrondie == 180) // Si il est en axe X
- {
- if(position_avant_droite >= x_robot-instruction.arg1 && position_avant_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
- {
- if(position_avant_gauche >= x_robot-instruction.arg1 && position_avant_gauche <= x_robot+instruction.arg1)
- {
- if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
- angleAvant = (1800 * atan2((double) ESPACE_INTER_TELEMETRE, (double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
- else
- angleAvant = 900 + (1800 * atan2( (double) (telemetreDistance_avant_gauche-telemetreDistance_avant_droite) ,(double) ESPACE_INTER_TELEMETRE ))/M_PI;
-
- nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
- moyennageTelemetre += angleAvant;
- }
- }
- }
-
- angleRecalage = moyennageTelemetre/nombresDeMesuresAuxTelemetresQuiSontCoherentes;
-
-
-
- if(nombresDeMesuresAuxTelemetresQuiSontCoherentes)
- {
- if(orientationArrondie == 0)
- {
- angleRecalage -= 900;
-
- /*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 += 900;
- }
- else if(orientationArrondie == 270)
- {
- angleRecalage += 1800;
- }
- }
-
-
- lcd.Clear(VERT);
- affichage_var(nombresDeMesuresAuxTelemetresQuiSontCoherentes);
- affichage_var(orientationArrondie);
- affichage_var(angleAvant);
- affichage_var(angleArriere);
- affichage_var(angleRecalage);
- affichage_var(telemetreDistance_avant_gauche);
- affichage_var(telemetreDistance_avant_droite);
-
- if(nombresDeMesuresAuxTelemetresQuiSontCoherentes && (angleAvant-angleArriere<80 && angleAvant-angleArriere>-80)){
- SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, angleRecalage);
- lcd.DisplayStringAt(120, LINE(18),(uint8_t *) "recalement theta", LEFT_MODE);
- }
-
-
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, recalageAngulaireCapteur() );
}
}
break;
@@ -1648,7 +1433,8 @@
//AX12_processChange();//Il faut lancer le déplacement des AX12
//AX12_enchainement = 0;
}
- } else {//C'est un enchainement
+ }
+ else {//C'est un enchainement
if(instruction.order == MV_LINE){
gameEtat = ETAT_GAME_WAIT_ACK;
}
@@ -1660,9 +1446,7 @@
break;
case ETAT_GAME_WAIT_ACK:
- /*
- 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) {
@@ -1728,7 +1512,7 @@
actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
}
}
- else if(cartesCheker.read_ms () > 500){
+ 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
actual_instruction = instruction.nextLineError;
@@ -1736,7 +1520,6 @@
}
else {
gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
- //gameEtat=ETAT_GAME_WAIT_END_INSTRUCTION;
}
}
break;
@@ -1791,16 +1574,7 @@
break;
case ETAT_GAME_WAIT_END_INSTRUCTION:
- canProcessRx();
- /*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
- }
- } */
+ canProcessRx();
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
@@ -1999,7 +1773,7 @@
case INSTRUCTION_END_MOTEUR:
case ACK_FIN_ACTION:
if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) {
- //SendRawId(waitingAckID);
+ lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"ACK OK",LEFT_MODE);
waitingAckFrom = 0;
waitingAckID = 0;
}
@@ -2431,3 +2205,214 @@
lcd.FillRect(240,365,120,25);
lcd.FillRect(240,220,120,25);// G
}
+
+short recalageAngulaireCapteur(void)
+{
+ unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 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;
+
+ 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))?0:3000) + (((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))?0:3000) + (((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)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
+ position_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((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 * atan2((double)(telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) , (double)ESPACE_INTER_TELEMETRE ))/M_PI;
+ else
+ angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
+
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += angleArriere;
+ }
+ }
+ }
+ else if(orientationArrondie == 0 || orientationArrondie == 180) // Si il est en axe X
+ {
+ if(position_arriere_droite >= x_robot-instruction.arg1 && position_arriere_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
+ {
+ if(position_arriere_gauche >= x_robot-instruction.arg1 && position_arriere_gauche <= x_robot+instruction.arg1)
+ {
+ if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
+ angleArriere =900+(1800 * atan2( (double) (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) , (double) ESPACE_INTER_TELEMETRE ))/M_PI;
+ else
+ angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
+
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += angleArriere;
+ }
+ }
+ }
+
+ 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 = (1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
+ else
+ angleAvant = 900 + (1800 * atan2( (double)( telemetreDistance_avant_gauche-telemetreDistance_avant_droite) ,(double) ESPACE_INTER_TELEMETRE ))/M_PI;
+
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += angleAvant;
+ }
+ }
+ }
+ else if(orientationArrondie == 0 || orientationArrondie == 180) // Si il est en axe X
+ {
+ if(position_avant_droite >= x_robot-instruction.arg1 && position_avant_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
+ {
+ if(position_avant_gauche >= x_robot-instruction.arg1 && position_avant_gauche <= x_robot+instruction.arg1)
+ {
+ if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
+ angleAvant = (1800 * atan2((double) ESPACE_INTER_TELEMETRE, (double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
+ else
+ angleAvant = 900 + (1800 * atan2( (double) (telemetreDistance_avant_gauche-telemetreDistance_avant_droite) ,(double) ESPACE_INTER_TELEMETRE ))/M_PI;
+
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += angleAvant;
+ }
+ }
+ }
+
+ angleRecalage = moyennageTelemetre/nombresDeMesuresAuxTelemetresQuiSontCoherentes;
+
+
+
+ if(nombresDeMesuresAuxTelemetresQuiSontCoherentes)
+ {
+ if(orientationArrondie == 0)
+ {
+ angleRecalage -= 900;
+
+ /*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 += 900;
+ }
+ else if(orientationArrondie == 270)
+ {
+ angleRecalage += 1800;
+ }
+ }
+
+ return (nombresDeMesuresAuxTelemetresQuiSontCoherentes && (angleAvant-angleArriere<80 && angleAvant-angleArriere>-80)) ? angleRecalage : theta_robot;
+}
+
+short recalageDistanceX(void)
+{
+ unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
+ unsigned int moyennageTelemetre = 0;
+
+ 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)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
+ telemetreDistance_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((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)
+ {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_avant_gauche;
+ }
+ if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_avant_droite;
+ }
+ if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_arriere_gauche;
+ }
+ if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_arriere_droite;
+ }
+
+ moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
+
+ return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : x_robot; //SetOdometrie(ODOMETRIE_SMALL_POSITION, moyennageTelemetre, y_robot, theta_robot);
+}
+
+short recalageDistanceY(void)
+{
+ unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
+ unsigned int moyennageTelemetre = 0;
+
+ 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))?0:3000) + (((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))?0:3000) + (((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)
+ {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_avant_gauche;
+ }
+ if(telemetreDistance_avant_droite >= y_robot-instruction.arg1 && telemetreDistance_avant_droite <= y_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_avant_droite;
+ }
+ if(telemetreDistance_arriere_gauche >= y_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= y_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_arriere_gauche;
+ }
+ if(telemetreDistance_arriere_droite >= y_robot-instruction.arg1 && telemetreDistance_arriere_droite <= y_robot+instruction.arg1)
+ {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_arriere_droite;
+ }
+
+ moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
+
+ return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : y_robot ; // SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot);
+}
+