Approche Gradin + amélioration du Wifi

L'algorythme d'approche du gradin prend forme. Les capteurs sont bien lus et interprétés.
Côté Wifi :
- Les identifiants sont renseignés dans un fichier qui n'est pas suivi par Git.
- Début d'une page statu pour le cerveau

Cerveau/.gitignore

@@ -0,0 +1 @@
+Wifi_settings.ino

Cerveau/Cerveau.ino

@@ -3,8 +3,9 @@
 #include <WiFi.h>
 #include <math.h>
 
-#include "Communication_chassis.h"
-#include "Communication_detection_adversaire.h"
+#include "Com_chassis.h"
+#include "Com_detection_adversaire.h"
+#include "Com_gradins.h"
 #include "ServerWeb.h"
 
 
@@ -21,16 +22,21 @@
 #define VITESSE_STANDARD 1000
 #define ACCELERATION_STANDARD 500
 
+#define COULEUR_BLEU 0
+#define COULEUR_JAUNE 1
+
 #define gst_server;
 
+extern const char *ssid;
+extern const char *password;
+
+
 struct triangulation_reception_t {
   int pos_x_mm, pos_y_mm;
   float angle_rad;
   bool validite;
 };
 
-const char* ssid = "riombotique";
-const char* password = "password";
 // adresse du >Pi qui gere le serveur : 192.168.99.100
 IPAddress local_IP(192, 168, 99, 101);
 IPAddress gateway(192, 168, 99, 1);
@@ -41,7 +47,8 @@ IPAddress subnet(255, 255, 255, 0);
 // IPAddress gateway(192, 168, 1, 1);
 // IPAddress subnet(255, 255, 255, 0);
 
-
+const unsigned char configuration_match_jaune = 0;
+const unsigned char configuration_match_bleu = 1;
 
 const int16_t I2C_MASTER = 0x42;
 const int16_t I2C_SLAVE_trian = 0x30;
@@ -95,7 +102,7 @@ int tolerance_position =100;
 float tolerance_orientation =0.03; // 2°
 
 
-char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position", "Deplacement absolu"};
+char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position", "Deplacement absolu", "Approche gradin"};
 char* statu[] = {"/..","./.","../"};
 int index_statu=0;
 
@@ -182,7 +189,7 @@ void Initialisation_wifi(){
   //Initialisation wifi
   //------------WIFI------------
   //Initialisation wifi
-  WiFi.config(local_IP, gateway, subnet);
+  //WiFi.config(local_IP, gateway, subnet);
   WiFi.begin(ssid, password);
   int test_wifi = 0;
   while (WiFi.status() != WL_CONNECTED){
@@ -200,9 +207,13 @@ void Initialisation_wifi(){
     M5.Lcd.print("Connecte au reseau    ;-)");
     M5.Lcd.setCursor(10,30);
     M5.Lcd.print(ssid); 
+    M5.Lcd.setCursor(10,50);
+    M5.Lcd.print(WiFi.localIP());
+    delay(3000);
+    return;
   }
-  else {
   // le routeur riombotique n'a pas été trouvé - création d'un point d'accès
+  WiFi.config(local_IP, gateway, subnet);
   WiFi.mode(WIFI_OFF);
   if (!WiFi.softAP("cerveau", "ilestsecret")) {
     log_e("Soft AP creation failed.");
@@ -216,7 +227,7 @@ void Initialisation_wifi(){
   M5.Lcd.print("cerveau 192.168.4.1"); 
   M5.Lcd.setCursor(10,50);
   M5.Lcd.print("mdp : ilestsecret"); 
-  }
+
   delay(1500);
 }
 
@@ -263,8 +274,11 @@ void gestion_match(){
   struct chassis_reception_t chassis_reception;
   struct chassis_emission_t chassis_emission;
   struct triangulation_reception_t triangulation_reception;
+  struct detect_gradin_t detect_gradin;
+  enum etat_action_t etat_action;
   static int translation_x_mm, translation_y_mm;
   static float rotation_rad;
+  static int couleur;
 
   enum etat_strategie{
     ATTENTE_ORDRE=0,
@@ -272,6 +286,7 @@ void gestion_match(){
     DEPLACEMENT_RELATIF=2,
     MATCH_EN_COURS=3,
     TEST_DEPLACEMENT_ABSOLU=4,
+    TEST_APPROCHE_GRADIN=5
   };
 
   switch(index_Maitre){
@@ -302,8 +317,7 @@ void gestion_match(){
         translation_y_mm = 0;
         rotation_rad = 0;
         
-        //index_Maitre = DEPLACEMENT_RELATIF;
-        Scan_Triangulation(&triangulation_reception);
+        index_Maitre = TEST_APPROCHE_GRADIN;      
       }
       if(M5.BtnB.read() == 1){
         Serial.println("BtnB");
@@ -319,6 +333,11 @@ void gestion_match(){
         translation_y_mm = 0;
         rotation_rad = 100;
         Triangulation_send_immobile(1);
+        while(M5.BtnC.read()){
+          M5.update();
+        }
+        delay(200);
+        IHM_attente_match(&couleur);
 
         index_Maitre = MATCH_EN_COURS;
         
@@ -345,7 +364,7 @@ void gestion_match(){
       break;
 
     case MATCH_EN_COURS:
-      if(Strategie() == ACTION_TERMINEE){
+      if(Strategie(couleur) == ACTION_TERMINEE){
         index_Maitre = ATTENTE_ORDRE;
       }
       break;
@@ -355,10 +374,56 @@ void gestion_match(){
         index_Maitre = ATTENTE_ORDRE;
       }
       break;
+
+    case TEST_APPROCHE_GRADIN:
+      if(gradin_approche() != ACTION_EN_COURS){
+        index_Maitre = ATTENTE_ORDRE;
+        affichage_standard_init();
+      }
+
+    /*
+      do{
+        char chaine[200];
+        Detect_gradin(&detect_gradin);
+        sprintf(chaine, "I2C OK\nStatus:%d\nCentre X:%4d\nCentre Y:%4d\nAngle:%2.2f\n", detect_gradin.status, 
+          detect_gradin.centre_x_mm, detect_gradin.centre_y_mm, detect_gradin.angle_rad / M_PI * 180);
+        affiche_msg("Detect gradin", chaine);
+        // if(detect_gradin.status == 2){
+        //   while(deplacement_relatif(0, 0, - detect_gradin.angle_rad, 0) != ACTION_TERMINEE);
+        // }
+      }while(fabs(detect_gradin.angle_rad / M_PI * 180) > 0.5);
+      //index_Maitre = ATTENTE_ORDRE;
+      //affichage_standard_init();
+      */
+      break;
+    
   }
 }
 
-enum etat_action_t Strategie(void){
+void IHM_attente_match(int * couleur){
+  int pret = 0;
+  int m_couleur = COULEUR_BLEU;
+  affiche_msg("Choix couleur", "En attente");
+  while(!pret){
+    M5.update();
+    if(M5.BtnA.read() == 1){
+      affiche_msg("Couleur", "  BLEU     ");
+      Triangulation_send_config(configuration_match_bleu);
+      m_couleur = COULEUR_BLEU;
+    }
+    if(M5.BtnB.read() == 1){
+      affiche_msg("Couleur", "  JAUNE    ");
+      Triangulation_send_config(configuration_match_jaune);
+      m_couleur = COULEUR_JAUNE;
+    }
+    if(M5.BtnC.read() == 1){
+      pret = 1;
+      *couleur = m_couleur;
+    }
+  }
+}
+
+enum etat_action_t Strategie(int couleur){
   static enum {
     STRAT_RECULE_BANDEROLE, // Deplacement relatif
     STRAT_ALLER_GRADINS_1_A,  // Déplacement absolu
@@ -378,14 +443,18 @@ enum etat_action_t Strategie(void){
       translation_x_mm = -450;
       translation_y_mm = 0;
       rotation_rad = 0;
-      etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 0);
+      etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
       if(etat_action == ACTION_TERMINEE){
         etat_strategie = STRAT_ALLER_GRADINS_1_A;
       }
       break;
 
     case STRAT_ALLER_GRADINS_1_A:
-      etat_action = deplacement_absolu(800, 800, -M_PI/2., 0);
+      if(couleur == COULEUR_JAUNE){
+        etat_action = deplacement_absolu(800, 800, -M_PI/2., 1);
+      }else{
+        etat_action = deplacement_absolu(3000 - 800, 800, -M_PI/2., 1);
+      }
       if(etat_action == ACTION_TERMINEE){
         etat_strategie = STRAT_ALLER_GRADINS_1_B;
       }
@@ -406,7 +475,11 @@ enum etat_action_t Strategie(void){
       break;
 
     case STRAT_ALLER_PREPA_BACKSTAGE:
+      if(couleur == COULEUR_JAUNE){
         etat_action = deplacement_absolu(550, 1150, M_PI/2., 0);
+      }else{
+        etat_action = deplacement_absolu(3000 - 550, 1150, M_PI/2., 0);
+      }
       if(etat_action == ACTION_TERMINEE){
         etat_strategie = STRAT_ALLER_BACKSTAGE;
       }
@@ -425,6 +498,107 @@ enum etat_action_t Strategie(void){
 
 }
 
+enum etat_action_t gradin_approche(void){
+  static enum{
+    GA_INIT,
+    GA_CHERCHE_GAUCHE,
+    GA_CHERCHE_DROIT,
+    GA_GOTO_LARGE,
+    GA_TOURNE_LARGE,
+    GA_GOTO_PROCHE,
+    GA_GOTO_PREND
+  } statu_approche_gradin = GA_INIT;
+  static float angle_parcouru, angle_mem;
+  static int nb_erreur;
+  int translation_x, translation_y;
+  struct detect_gradin_t detect_gradin;
+
+  Detect_gradin(&detect_gradin);
+  char chaine[200];
+  sprintf(chaine, "I2C OK\nStatus:%d\nCentre X:%4d\nCentre Y:%4d\nAngle:%2.2f\n", detect_gradin.status, 
+    detect_gradin.centre_x_mm, detect_gradin.centre_y_mm, detect_gradin.angle_rad / M_PI * 180);
+  affiche_msg("Detect gradin", chaine);
+
+  switch(statu_approche_gradin){
+    case GA_INIT:
+      angle_parcouru = 0;
+      statu_approche_gradin = GA_CHERCHE_GAUCHE;
+      break;
+
+    case GA_CHERCHE_GAUCHE:
+      if(detect_gradin.status == 2){
+        // On a trouvé !
+        statu_approche_gradin = GA_GOTO_LARGE;
+        angle_mem = detect_gradin.angle_rad;
+        nb_erreur = 0;
+      }else if(detect_gradin.status == 0){
+        // On a perdu la détection
+        while(deplacement_relatif(0, 0, -3. * M_PI / 180., 0) == ACTION_EN_COURS);
+        statu_approche_gradin = GA_CHERCHE_DROIT;
+      }else{
+        // On tourne à gauche de quelques degrés
+        while(deplacement_relatif(0, 0, 3. * M_PI / 180., 0) == ACTION_EN_COURS);
+      }
+      break;
+
+    case GA_CHERCHE_DROIT:
+      if(detect_gradin.status == 2){
+        // On a trouvé !
+        statu_approche_gradin = GA_GOTO_LARGE;
+        angle_mem = detect_gradin.angle_rad;
+        nb_erreur = 0;
+      }else if(detect_gradin.status == 0){
+        // On a perdu la détection
+        statu_approche_gradin = GA_INIT;
+        return ACTION_ECHEC;
+      }else{
+        // On tourne à gauche de quelques degrés
+        while(deplacement_relatif(0, 0, -3. * M_PI / 180., 0) == ACTION_EN_COURS);
+      }
+      break;
+
+    case GA_GOTO_LARGE:
+      Detect_gradin(&detect_gradin);
+      if(detect_gradin.status != 2){
+        nb_erreur++;
+        if(nb_erreur > 100){
+          affiche_erreur("Gradin Approche", "GA_GOTO_LARGE\n Status != 2");
+          while(1);
+        }
+      }
+      translation_x = (detect_gradin.centre_y_mm + 150) * tan(detect_gradin.angle_rad);
+      translation_y = -(detect_gradin.centre_y_mm + 150) * sin(detect_gradin.angle_rad) - detect_gradin.centre_x_mm;
+
+      if(deplacement_relatif(translation_x, translation_y, 0, 0) == ACTION_TERMINEE){
+        statu_approche_gradin = GA_TOURNE_LARGE;
+      }
+      break;
+
+    case GA_TOURNE_LARGE:
+      if(deplacement_relatif(0, 0, -angle_mem, 0) == ACTION_TERMINEE){
+        Detect_gradin(&detect_gradin);
+        if(detect_gradin.status != 2 || fabs(detect_gradin.angle_rad) > 0.009){
+          statu_approche_gradin = GA_CHERCHE_GAUCHE;
+        }else{
+          statu_approche_gradin = GA_GOTO_PROCHE;
+        }
+
+      }
+      break;
+
+    case GA_GOTO_PROCHE:
+      Detect_gradin(&detect_gradin);
+      if(deplacement_relatif((detect_gradin.centre_y_mm - 20), 0, 0, 0) == ACTION_TERMINEE){
+        statu_approche_gradin = GA_INIT;
+        return ACTION_TERMINEE;
+      }
+      break;
+
+  }
+  return ACTION_EN_COURS;
+
+}
+
 
 /// @brief : compare la position actuelle et la position lue par la balise
 /// Note : Pour l'instant, on ne déclenche un mouvment qu'en cas d'ecart sur la distance, pas sur l'orientation.
@@ -532,6 +706,39 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, floa
   return ACTION_EN_COURS;
 }
 
+/// @brief Récupère les données de la carte de détection de l'advesraire et renoi 1 s'il y aun adversaire
+/// Test les capteurs dans la direction d'avancement
+/// @param angle_deplacement direction dans laquelle avance le robot, dans le référentiel du robot
+int detection_adversaire(float angle_deplacement){
+  int capteur_central, capteur_precedant, capteur_suivant;
+  struct detect_adv_reception_t detect_adv_reception;
+  // On ramène l'angle entre 0 et 2 PI.
+  while(angle_deplacement < 0){
+    angle_deplacement += 2 * M_PI;
+  }
+  while(angle_deplacement > 2 * M_PI){
+    angle_deplacement -= 2 * M_PI;
+  }
+  // On obtient le capteur central.
+  capteur_central = angle_deplacement / (M_PI * 2) * 12;
+  capteur_precedant = capteur_central - 1;
+  if(capteur_precedant < 0){
+    capteur_precedant = 11;
+  }
+  capteur_suivant = capteur_central + 1;
+  if(capteur_suivant > 11){
+    capteur_suivant = 0;
+  }
+  Detect_adv_lire(&detect_adv_reception);
+
+  if(detect_adv_reception.distance_cm[capteur_central] < 50 || 
+      detect_adv_reception.distance_cm[capteur_precedant] < 50 ||
+      detect_adv_reception.distance_cm[capteur_suivant] < 50 ){
+    return 1;
+  }
+  return 0;
+}
+
 /// @brief Deplacement dans le repère du robot, pouvant prendre en compte la detection de l'adversaire
 /// evitement : 1 pour s'arreter si adversaire detecté, 0 pour ignorer l'adversaire
 enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, float rotation_rad, int evitement){
@@ -563,11 +770,19 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, flo
         // On lit les capteurs
         Detect_adv_lire(&detect_adv_reception);
         // On analyse les valeurs - TODO : créer une fonction plus évoluée
+        if(distance_x_mm > 0){
           if(detect_adv_reception.distance_cm[0] < 50){
             // Arrêt du mouvement
             chassis_emission.status = MOUVEMENT_INTERRUPTION;
             send_Chassis(&chassis_emission);
           }
+        }else if(distance_x_mm < 0){
+          if(detect_adv_reception.distance_cm[6] < 50){
+            // Arrêt du mouvement
+            chassis_emission.status = MOUVEMENT_INTERRUPTION;
+            send_Chassis(&chassis_emission);
+          }
+        }
       }
 
       Scan_chassis(&chassis_reception);

Cerveau/Com_chassis.h

@@ -14,7 +14,7 @@ struct chassis_emission_t {
   int vitesse, acceleration;
 };
 
-void Scan_chassis(struct chassis_reception_t * chassis_reception);
+int Scan_chassis(struct chassis_reception_t * chassis_reception);
 void send_Chassis(struct chassis_emission_t * chassis_emission);
 void send_Chassis_RAZ(void);
 

Cerveau/Com_chassis.ino

@@ -4,7 +4,7 @@
 
 
 /// @brief Lit l'état du chassis en I2C
-void Scan_chassis(struct chassis_reception_t * chassis_reception){
+int Scan_chassis(struct chassis_reception_t * chassis_reception){
   unsigned char tampon2[14];
   //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
   error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);
@@ -19,6 +19,7 @@ void Scan_chassis(struct chassis_reception_t * chassis_reception){
     Mvt_finit = (MOUVEMENT_FINI == tampon2[0]);
     chassis_reception->status = tampon2[0];
   }
+  return error;
 }
 
 void send_Chassis(struct chassis_emission_t * chassis_emission){

Cerveau/Com_gradins.h

@@ -0,0 +1,12 @@
+#ifndef COM_GRADINS_H
+#define COM_GRADINS_H
+
+#define I2C_SLAVE_detect_gradin 0x19
+
+struct detect_gradin_t{
+  char status;
+  int centre_x_mm, centre_y_mm;
+  float angle_rad;
+};
+
+#endif

Cerveau/Com_gradins.ino

@@ -0,0 +1,28 @@
+//#include "Chassis.h"
+#include <Arduino.h>
+#include <HardwareSerial.h>
+#include "Com_gradins.h"
+
+
+/// @brief Lit les capteurs VL53L1X
+void Detect_gradin(struct detect_gradin_t * detect_gradin){
+  unsigned char tampon[14];
+  char chaine[200];
+  int angle_mrad;
+  //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
+  error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0, tampon, 13);
+  if (error !=0){
+    affiche_erreur("Detect_gradin", "Erreur I2C");
+    while(1);
+  }else{
+    
+    detect_gradin->status = tampon[0];
+    detect_gradin->centre_x_mm = tampon[1] << 24 | tampon[2] << 16 | tampon[3] << 8 | tampon[4];
+    detect_gradin->centre_y_mm = tampon[5] << 24 | tampon[6] << 16 | tampon[7] << 8 | tampon[8];
+    angle_mrad = tampon[9] << 24 | tampon[10] << 16 | tampon[11] << 8 | tampon[12];
+    detect_gradin->angle_rad = angle_mrad / 1000.;
+    
+
+    
+  }
+}

Cerveau/Com_triangulation.ino

@@ -48,3 +48,12 @@ void Triangulation_send_immobile(int immobile){
     while(1);
   }
 }
+
+
+void Triangulation_send_config(unsigned char configuration){
+  error = I2C_ecrire_registre(I2C_SLAVE_trian, 14, &configuration, 1);    // si errror != de 0 alors erreur de communication
+  if (error !=0){
+    affiche_erreur("Send_Triangulation", "Erreur I2C");
+    while(1);
+  }
+}

Cerveau/ServerWeb.ino

@@ -57,6 +57,7 @@ WebServer server(80);
 void Web_init(){
   server.begin();
   server.on("/form", handleForm);
+  server.on("/statu", showStatus);
 }
 
 bool Web_nouveau_message(){
@@ -99,3 +100,27 @@ void handleForm() {
   Web_nouvelle_entree=1;
 }
 
+void showStatus() {
+  char chassis[50], gradin[50], triangulation[50], detection_adversaire[50];
+  char message_statu[500];
+  const char message_statu_tmplt[] = "<!DOCTYPE html><html><head><meta charset=\"UTF-8\"><title>A simple form</title></head>\
+  <body>\n<h1>Statu du robot</h1>\n\
+  <p>Chassis: %s</p><p>Gradin: %d</p><p>Triangulation: %d</p><p>Detection_adversaire: %d</p>";
+  struct chassis_reception_t chassis_reception;
+
+  statu_to_string(chassis, !Scan_chassis(&chassis_reception) );
+
+  sprintf(message_statu, message_statu_tmplt, chassis, 0, 0, 0);
+  server.send(200, "text/html", message_statu);
+  
+}
+
+/// @brief convertir un statu boolean en message web.
+/// @param statu 0 NOK, 1 OK
+void statu_to_string(char * statu_str, bool statu){
+  if(!statu){
+    strcpy(statu_str, "<span style=\"color:red;\">NOK</span>");
+  }else{
+    strcpy(statu_str, "<span style=\"color:green;\">OK</span>");
+  }
+}

Cerveau/Wifi_settings.ino.example

@@ -0,0 +1,3 @@
+const char *ssid = "Mon SSID";
+const char *password = "Ma clé WiFi";
+

Readme.md

@@ -1,3 +1,14 @@
+WiFi
+====
+
+Pour compiler le code du "Cerveau", il est nécessaire de copier le fichier Wifi_settings.ino.example en Wifi_settings.ino.
+Vous pouvez ensuite éditer le fichier Wifi_settings pour indiquer les identifiants Wifi.
+
+Communication I2C
+=================
+
+La documentation de notre protocole I2C est [disponible sous forme de PDF ici](Doc/Communication%20I2C.pdf).
+
 Déplacement fonctionnel 
 =======================
 
@@ -10,7 +21,3 @@ Vidéo réalisée le 24 février 2024, paramètres :
 - vitesse : 2000
 - acceleration : 1500
  
-Communication I2C
-=================
-
-La documentation de notre protocole I2C est [disponible sous forme de PDF ici](Doc/Communication%20I2C.pdf).

Triangulation/Triangulation.ino

@@ -20,7 +20,7 @@ String Lecture;
 #define pi2 6.283185307179586
 #define ALIGNEMENT_RAD (245. / 180. *M_PI)
 
-uint8_t * data_i2C;
+uint8_t * data_i2c;
 int Balise[4][2];               // 4 balises potentielles i de 0 a 3           
                                 // Frequence modulation individuelle balises
                                 // Balise[i][0] est la frequence de la balise
@@ -51,6 +51,8 @@ int Frequence_0 = 5000; // frequence balise 0
 int Frequence_1 = 6000; // frequence balise 1
 int Frequence_2 = 4000; // frequence balise 2
 int Bande_P = 200;      // bande passante balises
+
+// Attention, les absisces des balises sont redéfines plus bas (dans loop)
 int X1 = -90;             // abcisse balise 0
 int X2 = -90;           // abcisse balise 1
 int X3 = 3090;           // abcisse balise 2
@@ -194,8 +196,8 @@ void setup() {
   WiFi.begin(ssid, password);
 
   // Pour accéder aux données de l'I2C
-  data_i2C = get_i2c_data();
-  data_i2C[13] = 1; // On dit que le robot est immobile.
+  data_i2c = get_i2c_data();
+  data_i2c[13] = 1; // On dit que le robot est immobile.
 
   int test_wifi = 0;
   
@@ -277,8 +279,9 @@ void setup() {
 }
 
 void loop() {
-  //Effectue a chaque tour ........................
-  if(data_i2C[13] != 1){
+  
+  // On invalide les balises si le robot bouge
+  if(data_i2c[13] != 1){
     Nb_Balises = 0; 
     Balise_0 = false;
     Balise_1 = false;
@@ -287,6 +290,35 @@ void loop() {
     uint8_t etat_balises = (Balise_0 | Balise_1 <<1 | Balise_2 <<2 | Calcul_Valide <<3);
     I2C_envoi_8bits(etat_balises,0);
   }
+
+  // Configuration des balises
+  if(data_i2c[14] == 0){
+    // Match côté Jaune
+    X1 = -90;             // abcisse balise 0
+    X2 = -90;           // abcisse balise 1
+    X3 = 3090;           // abcisse balise 2
+    Y1 = 1950;             // ordonnee balise 0
+    Y2 = 50;             // ordonnee balise 1
+    Y3 = 1000;            // ordonnee balise 2
+    Xp1 = X1 - X2;
+    Yp1 = Y1 - Y2;
+    Xp3 = X3 - X2;
+    Yp3 = Y3 - Y2;
+  }else if(data_i2c[14] == 1){
+    // Match côté Jaune
+    X1 = 3090;             // abcisse balise 0
+    X2 = 3090;           // abcisse balise 1
+    X3 = -90;           // abcisse balise 2
+    Y1 = 1950;             // ordonnee balise 0
+    Y2 = 50;             // ordonnee balise 1
+    Y3 = 1000;            // ordonnee balise 2
+    Xp1 = X1 - X2;
+    Yp1 = Y1 - Y2;
+    Xp3 = X3 - X2;
+    Yp3 = Y3 - Y2;
+  }
+
+  //Effectue a chaque tour ........................
   if ((Old_Nb_tours != Nb_tours) && (Trigger_Balises)){
     Old_Nb_tours = Nb_tours;
     if (!Balise_Valide){