Début de l'approche des gradins

Cerveau/Cerveau.ino

@@ -3,8 +3,9 @@
 #include <WiFi.h>
 #include <math.h>
 
-#include "Communication_chassis.h"
+#include "Com_chassis.h"
-#include "Communication_detection_adversaire.h"
+#include "Com_detection_adversaire.h"
+#include "Com_gradins.h"
 #include "ServerWeb.h"
 
 
@@ -99,7 +100,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;
 
@@ -267,6 +268,8 @@ 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;
@@ -277,6 +280,7 @@ void gestion_match(){
     DEPLACEMENT_RELATIF=2,
     MATCH_EN_COURS=3,
     TEST_DEPLACEMENT_ABSOLU=4,
+    TEST_APPROCHE_GRADIN=5
   };
 
   switch(index_Maitre){
@@ -307,8 +311,7 @@ void gestion_match(){
         translation_y_mm = 0;
         rotation_rad = 0;
         
-        index_Maitre = DEPLACEMENT_RELATIF;
+        index_Maitre = TEST_APPROCHE_GRADIN;      
-        Scan_Triangulation(&triangulation_reception);
       }
       if(M5.BtnB.read() == 1){
         Serial.println("BtnB");
@@ -365,6 +368,28 @@ 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;
+    */
   }
 }
 
@@ -466,6 +491,83 @@ enum etat_action_t Strategie(int couleur){
 
 }
 
+enum etat_action_t gradin_approche(void){
+  static enum{
+    GA_INIT,
+    GA_CHERCHE_GAUCHE,
+    GA_CHERCHE_DROIT,
+    GA_GOTO_LARGE,
+    GA_GOTO_PROCHE,
+    GA_GOTO_PREND
+  } statu_approche_gradin = GA_INIT;
+  static float angle_parcouru;
+  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;
+        nb_erreur = 0;
+      }else if(detect_gradin.status == 0){
+        // On a perdu la détection
+        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;
+        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 - 400 * cos(detect_gradin.angle_rad);
+      translation_y = -400 * sin(detect_gradin.angle_rad);
+      if(deplacement_relatif(translation_x, translation_y, 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.
@@ -578,6 +680,7 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, floa
 /// @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;
@@ -595,6 +698,8 @@ int detection_adversaire(float angle_deplacement){
   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 ){

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