From a7be11a7b1b9d3aac018117811d4116f131fd176 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Tue, 1 Apr 2025 18:37:28 +0200
Subject: [PATCH] Calcul de l'orientation du robot

---
 Cerveau/Cerveau.ino             |  2 +-
 Triangulation/Triangulation.ino | 38 +++++++++++++++++++++++++--------
 2 files changed, 30 insertions(+), 10 deletions(-)

diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino
index 968ece3..c0d73b6 100644
--- a/Cerveau/Cerveau.ino
+++ b/Cerveau/Cerveau.ino
@@ -419,7 +419,7 @@ void compar_cinematique(int consigne_x_mm, int consigne_y_mm, float consigne_ori
       if(abs(compar_rotation) > tolerance_orientation) {
         chassis_emission->translation_x_mm = 0;
         chassis_emission->translation_y_mm = 0;
-        chassis_emission->rotation_z_rad = compar_rotation;
+        chassis_emission->rotation_z_rad = -compar_rotation;
         chassis_emission->status = MOUVEMENT_EN_COURS;
       }else{
         chassis_emission->status = MOUVEMENT_FINI;
diff --git a/Triangulation/Triangulation.ino b/Triangulation/Triangulation.ino
index bf43e20..a5d3e59 100644
--- a/Triangulation/Triangulation.ino
+++ b/Triangulation/Triangulation.ino
@@ -18,6 +18,7 @@ String Lecture;
 #define CONV2 1.570796326794896619
 #define pi  3.141592653589793
 #define pi2 6.283185307179586
+#define ALIGNEMENT_RAD (245. / 180. *M_PI)
 
 uint8_t * data_i2C;
 int Balise[4][2];               // 4 balises potentielles i de 0 a 3           
@@ -292,7 +293,7 @@ void loop() {
       Balise_Valide = true;
       rapport();
     }
-
+    /*
     //_____________________________________________ gestion I2C et Data sent
     if(Xr>0){
   
@@ -309,18 +310,37 @@ void loop() {
       Angle_Robot_RAD = calc2/360*pi2;  // en radian
       Angle_Robot_Deg_int = calc2;      // arrondi des degres dans un entier
       Angle_Robot_Deg = calc2;          // angle d'orientation robot en degres (0 sur l'axe Y)
-    }
+    }*/
     uint8_t etat_balises = (Balise_0 | Balise_1 <<1 | Balise_2 <<2 | Calcul_Valide <<3);
+    //_____________________________________________
+
+    if (Nb_Balises == 3){
+      traitement_donnees(); //calcul des coordonnees si 3 balises
+      // Calcul de l'orientation du robot
+      // * Angle du vecteur Robot - Balise
+      float angle_vecteur_robot_balise;
+      Angle_B1 = Balise[0][1] * CONV;  // lorsque le robot est orienté vers la balise, angle codeur = 0
+      angle_vecteur_robot_balise = atan2f(X1 - Xr, Y1 - Yr);
+      Angle_Robot_RAD = -(angle_vecteur_robot_balise + Angle_B1 - ALIGNEMENT_RAD);
+      while(Angle_Robot_RAD > M_PI){
+        Angle_Robot_RAD -= 2 * M_PI;
+      }
+      while(Angle_Robot_RAD < -M_PI){
+        Angle_Robot_RAD += 2 * M_PI;
+      }
+      Angle_Robot_Deg_int = Angle_Robot_RAD * 180 / M_PI; 
+
+    }
+
     I2C_envoi_8bits(etat_balises,0);
-    
     // TODO: Mettre Xr et Yr dans la mémoire I2C ################################
     I2C_envoi_32bits(Xr, 1);
     I2C_envoi_32bits(Yr, 5);
     //I2C_envoi_32bits(Angle_Robot_RAD, 9);
     I2C_envoi_32bits(Angle_Robot_Deg_int, 9);
-    //_____________________________________________
 
-    if (Nb_Balises == 3) traitement_donnees(); //calcul des coordonnees si 3 balises
+
+    
     affichage_resultats();
     checkForClient();     
     Nb_Balises = 0; 
@@ -401,12 +421,12 @@ void affichage_resultats() {
   //M5.Lcd.setCursor(200,150);     
   //M5.Lcd.print(Balise[2][0]);
   //M5.Lcd.print("    ");
-  M5.Lcd.setCursor(150,60);
+  M5.Lcd.setCursor(140,60);
   M5.Lcd.print(Angle_B1);
-  M5.Lcd.setCursor(150,80);
+  M5.Lcd.setCursor(140,80);
   M5.Lcd.print(Angle_Ref_Theorique);
-  M5.Lcd.setCursor(150,100);
-  M5.Lcd.print(Angle_Robot_Deg);
+  M5.Lcd.setCursor(140,100);
+  M5.Lcd.print(Angle_Robot_Deg_int);
   M5.Lcd.print(" °      ");  
   M5.Lcd.setCursor(200,180);
   M5.Lcd.print(Xr);