Gestion des configurations des balises + début d'un traitement des données de la detections de l'adversaire

Cerveau/Cerveau.ino

@@ -21,6 +21,9 @@
 #define VITESSE_STANDARD 1000
 #define ACCELERATION_STANDARD 500
 
+#define COULEUR_BLEU 0
+#define COULEUR_JAUNE 1
+
 #define gst_server;
 
 struct triangulation_reception_t {
@@ -41,7 +44,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;
@@ -265,6 +269,7 @@ void gestion_match(){
   struct triangulation_reception_t triangulation_reception;
   static int translation_x_mm, translation_y_mm;
   static float rotation_rad;
+  static int couleur;
 
   enum etat_strategie{
     ATTENTE_ORDRE=0,
@@ -302,7 +307,7 @@ void gestion_match(){
         translation_y_mm = 0;
         rotation_rad = 0;
         
-        //index_Maitre = DEPLACEMENT_RELATIF;
+        index_Maitre = DEPLACEMENT_RELATIF;
         Scan_Triangulation(&triangulation_reception);
       }
       if(M5.BtnB.read() == 1){
@@ -319,6 +324,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 +355,7 @@ void gestion_match(){
       break;
 
     case MATCH_EN_COURS:
-      if(Strategie() == ACTION_TERMINEE){
+      if(Strategie(couleur) == ACTION_TERMINEE){
         index_Maitre = ATTENTE_ORDRE;
       }
       break;
@@ -358,7 +368,30 @@ void gestion_match(){
   }
 }
 
-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 +411,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 +443,11 @@ enum etat_action_t Strategie(void){
       break;
 
     case STRAT_ALLER_PREPA_BACKSTAGE:
-      etat_action = deplacement_absolu(550, 1150, M_PI/2., 0);
+      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;
       }
@@ -532,6 +573,36 @@ 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;
+  // 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;
+  }
+  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,10 +634,18 @@ 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(detect_adv_reception.distance_cm[0] < 50){
+        if(distance_x_mm > 0){
-          // Arrêt du mouvement
+          if(detect_adv_reception.distance_cm[0] < 50){
-          chassis_emission.status = MOUVEMENT_INTERRUPTION;
+            // Arrêt du mouvement
-          send_Chassis(&chassis_emission);
+            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);
+          }
         }
       }
 

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);
+  }
+}

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 = get_i2c_data();
-  data_i2C[13] = 1; // On dit que le robot est immobile.
+  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){