Premisse de la stratégie

Cerveau/Cerveau.ino

@@ -12,6 +12,8 @@
 
 #define Rad_Deg 57.2957795130823
 
+#define MOUVEMEMENT_ROTATION 0x10
+#define MOUVEMEMENT_TRANSLATION 0x8
 #define MOUVEMENT_FINI 0x4
 #define MOUVEMENT_EN_COURS 0x2
 #define MOUVEMENT_INTERRUPTION 0x1
@@ -90,9 +92,10 @@ int index_Maitre = 0;
 bool Mvt_tolerance_OK =false;
 bool Balises_OK = 0;
 int tolerance_position =100;
+float tolerance_orientation =0.05; // 3°
 
 
-char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position"};
+char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position", "Deplacement absolu"};
 char* statu[] = {"/..","./.","../"};
 int index_statu=0;
 
@@ -152,7 +155,7 @@ void loop() {
   static int64_t time;
   struct chassis_reception_t chassis_reception;
 
-  strategie();
+  gestion_match();
   affichage_resultats();
   delay(10);
 }
@@ -256,7 +259,7 @@ void affiche_msg(char * chaine1, char * chaine2){
   M5.Lcd.print(chaine2); 
 }
 
-void strategie(){
+void gestion_match(){
   struct chassis_reception_t chassis_reception;
   struct chassis_emission_t chassis_emission;
   struct triangulation_reception_t triangulation_reception;
@@ -267,6 +270,8 @@ void strategie(){
     ATTENTE_ORDRE=0,
     LECTURE_TRIANGULATION=1,
     DEPLACEMENT_RELATIF=2,
+    MATCH_EN_COURS=3,
+    TEST_DEPLACEMENT_ABSOLU=4,
   };
 
   switch(index_Maitre){
@@ -291,29 +296,31 @@ void strategie(){
         index_Maitre = DEPLACEMENT_RELATIF;
       }
       if(M5.BtnA.read() == 1){
+        Serial.println("BtnA");
         // Déplacement en X
-        translation_x_mm = 10000;
+        translation_x_mm = 500;
         translation_y_mm = 0;
         rotation_rad = 0;
         
-        index_Maitre = DEPLACEMENT_RELATIF;
+        //index_Maitre = DEPLACEMENT_RELATIF;
+        Scan_Triangulation(&triangulation_reception);
       }
       if(M5.BtnB.read() == 1){
-        // Déplacement en Y
-        translation_x_mm = 0;
-        translation_y_mm = 2000;
-        rotation_rad = 0;
-        
-        index_Maitre = DEPLACEMENT_RELATIF;
+        Serial.println("BtnB");
+        // Déplacement en Y       
+        //Triangulation_send_immobile(0);
+        index_Maitre = TEST_DEPLACEMENT_ABSOLU;
         
       }
       if(M5.BtnC.read() == 1){
         // Rotation
+        Serial.println("BtnC");
         translation_x_mm = 0;
         translation_y_mm = 0;
         rotation_rad = 100;
+        Triangulation_send_immobile(1);
 
-        index_Maitre = DEPLACEMENT_RELATIF;
+        index_Maitre = MATCH_EN_COURS;
         
       }
     break;
@@ -336,54 +343,168 @@ void strategie(){
         index_Maitre = ATTENTE_ORDRE;
       }
       break;
+
+    case MATCH_EN_COURS:
+      if(Strategie() == ACTION_TERMINEE){
+        index_Maitre = ATTENTE_ORDRE;
+      }
+      break;
+
+    case TEST_DEPLACEMENT_ABSOLU:
+      if(deplacement_absolu(800, 800, 0, 0) == ACTION_TERMINEE){
+        index_Maitre = ATTENTE_ORDRE;
+      }
+      break;
   }
 }
 
-void compar_cinematique(){  
-  
-  // Consigne de position à atteindre en X, Y et angle par rapport à la position actuel
-  // dans le repère du terrain
-  //Position à atteindre théorique
-  struct triangulation_reception_t triangulation_reception;
+enum etat_action_t Strategie(void){
+  static enum {
+    STRAT_RECULE_BANDEROLE, // Deplacement relatif
+    STRAT_ALLER_GRADINS_1,  // Déplacement absolu
+    STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement absolu
+    STRAT_ALLER_BACKSTAGE // Déplacement relatif
 
-  X_futur = MemCmd_X;     
-  Y_futur = MemCmd_Y;
+  }etat_strategie = STRAT_RECULE_BANDEROLE;
+  enum etat_action_t etat_action;
+  int translation_x_mm, translation_y_mm;
+  float rotation_rad;
 
-  Scan_Triangulation(&triangulation_reception);   //Prise de la position actuel
-  if(Balises_OK == true && error == 0){   //triangulation calcul valide ************** a prendre sur I2c
-    compar_X =  X_futur - Position_actuelle_X;     //compar de la position théoriquement atteinte avec la position actuel
-    compar_Y =  Y_futur - Position_actuelle_Y;    //YR : position actuel    Y_futur : Position de départ + mouvement demander (donc point d'arrivé théorique)
-
-    if(abs(compar_X) < tolerance_position && abs(compar_Y) < tolerance_position){
-        Mvt_tolerance_OK = true;
-    }else{
-      // print de la difference ; determiné cmd il nous faudrait faire à nouveau pour atteindre la position voulue
-
-      // Distance à parcourir
-      float distance_calculee = sqrt(sq(compar_X) + sq(compar_Y));
-      float angle_robot_vers_destination =  M_PI_2 - atan2(compar_Y, compar_X);
-
-      float distance_Y_ref_robot = cos(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee;
-      float distance_X_ref_robot = sin(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee;
-
-      if(corrige == false){
-        mem_x = distance_X_ref_robot;   // faire une memoire et travailler avec
-        mem_y = distance_Y_ref_robot;
-        corrige = true;
+  switch(etat_strategie){
+    case STRAT_RECULE_BANDEROLE:
+      // Déplacement en X
+      translation_x_mm = -450;
+      translation_y_mm = 0;
+      rotation_rad = 0;
+      etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 0);
+      if(etat_action == ACTION_TERMINEE){
+        etat_strategie = STRAT_ALLER_GRADINS_1;
       }
-      Mvt_tolerance_OK = false;
-    }
-  }else{
-    compar_X =0 ;     //compar de la position théoriquement atteinte avec la position actuel
-    compar_Y =0 ;
-    mem_x =0 ;     //compar de la position théoriquement atteinte avec la position actuel
-    mem_y =0 ;
+      break;
+
+    case STRAT_ALLER_GRADINS_1:
+      etat_action = deplacement_absolu(730, 550, M_PI/2., 0);
+      if(etat_action == ACTION_TERMINEE){
+        etat_strategie = STRAT_RECULE_BANDEROLE;
+        return ACTION_TERMINEE;
+      }
+
+
   }
+
+  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.
+void compar_cinematique(int consigne_x_mm, int consigne_y_mm, float consigne_orientation_rad, 
+    struct triangulation_reception_t triangulation_reception, struct chassis_emission_t * chassis_emission){ 
+
+  float compar_rotation;
+  
+  compar_X =  consigne_x_mm - triangulation_reception.pos_x_mm;     //compar de la position théoriquement atteinte avec la position actuel
+  compar_Y =  consigne_y_mm - triangulation_reception.pos_y_mm;    //YR : position actuel    Y_futur : Position de départ + mouvement demander (donc point d'arrivé théorique)
+  compar_rotation = consigne_orientation_rad - triangulation_reception.angle_rad;
+  while(compar_rotation < -M_PI){
+    compar_rotation += 2* M_PI;
+  }
+  while(compar_rotation > M_PI){
+    compar_rotation -= 2* M_PI;
+  }
+  printf("compar_X:%d\tcompar_y:%d\tcompar_rot:%f\n", compar_X, compar_Y, compar_rotation);
+
+  if(abs(compar_X) < tolerance_position && abs(compar_Y) < tolerance_position){
+      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->status = MOUVEMENT_EN_COURS;
+      }else{
+        chassis_emission->status = MOUVEMENT_FINI;
+      }
+  }else{
+    // print de la difference ; determiné cmd il nous faudrait faire à nouveau pour atteindre la position voulue
+
+    // Distance à parcourir
+    float distance_calculee = sqrt(sq(compar_X) + sq(compar_Y));
+    float angle_robot_vers_destination =  M_PI_2 - atan2(compar_Y, compar_X);
+
+    chassis_emission->translation_x_mm = sin(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee;
+    chassis_emission->translation_y_mm = cos(angle_robot_vers_destination - Angle_Robot_RAD) * distance_calculee;
+    chassis_emission->rotation_z_rad = 0;
+    chassis_emission->status = MOUVEMENT_EN_COURS;
+  }
+}
+
+enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int consigne_orientation_rad, int evitement){
+  static enum{
+    DA_INIT,
+    DA_COMPARE_POSITIONS,
+    DA_MVT_EN_COURS,
+    DA_ATTENTE,
+  } etat_deplacement = DA_INIT;
+  static int mem_consigne_x_mm, mem_consigne_y_mm, mem_consigne_orientation_rad;
+  static struct chassis_emission_t chassis_emission;
+  struct triangulation_reception_t triangulation_reception;
+  enum etat_action_t etat_deplacement_relatif;
+
+  switch(etat_deplacement){
+    case DA_INIT:
+      mem_consigne_x_mm = consigne_x_mm;
+      mem_consigne_y_mm = consigne_y_mm;
+      mem_consigne_orientation_rad = consigne_orientation_rad;
+      etat_deplacement = DA_COMPARE_POSITIONS;
+      Serial.printf("DA_INIT\n");
+      break;
+
+    case DA_COMPARE_POSITIONS:
+      Scan_Triangulation(&triangulation_reception);   //Prise de la position actuel
+      if(triangulation_reception.validite == true){
+        Serial.printf("Compare cinematique\n");
+        compar_cinematique(mem_consigne_x_mm, mem_consigne_y_mm, mem_consigne_orientation_rad,
+          triangulation_reception, &chassis_emission);
+        if(chassis_emission.status == MOUVEMENT_EN_COURS){
+          // C'est que la fonction compar_cinematique indique qu'on doit se déplacer
+          // Les valeurs du déplacement sont renseignées dans "chassis_emission".
+          Serial.printf("DA_MVT_EN_COUR\n");
+          Serial.printf("pos_x:%d\tpos_y:%d\tOrientation:%f\n",triangulation_reception.pos_x_mm, triangulation_reception.pos_y_mm,
+          triangulation_reception.angle_rad); 
+          Serial.printf("trans_x:%d\ttrans_y:%d\trot:%f\n",chassis_emission.translation_x_mm, 
+          chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad);
+          etat_deplacement = DA_MVT_EN_COURS;
+        }else{
+          // Alors nous sommes arrivés
+          // On réinitialise la mahcine à état
+          etat_deplacement = DA_INIT;
+          return ACTION_TERMINEE;
+        }
+      }
+      break;
+    
+    case DA_MVT_EN_COURS:
+      Scan_Triangulation(&triangulation_reception);   //Prise de la position actuel
+      etat_deplacement_relatif = deplacement_relatif(- chassis_emission.translation_x_mm, 
+          -chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad, evitement);
+      if(etat_deplacement_relatif == ACTION_TERMINEE){
+        Serial.printf("DA_COMPARE_POSITIONS\n");
+        etat_deplacement = DA_ATTENTE;
+      }
+      break;
+
+    case DA_ATTENTE:
+      delay(3000);
+      etat_deplacement = DA_COMPARE_POSITIONS;
+      break;
+  }
+
+  return ACTION_EN_COURS;
 }
 
 /// @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, int rotation_rad, int evitement){
+enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, float rotation_rad, int evitement){
   static enum{
     DR_INIT,
     DR_MVT_EN_COUR,
@@ -403,6 +524,7 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, int
       chassis_emission.acceleration = ACCELERATION_STANDARD;
 
       send_Chassis(&chassis_emission);
+      Triangulation_send_immobile(0);
       etat_deplacement = DR_MVT_EN_COUR;
       break;
 
@@ -421,6 +543,7 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, int
       Scan_chassis(&chassis_reception);
 
       if(chassis_reception.status == MOUVEMENT_FINI){
+        Triangulation_send_immobile(1);
         etat_deplacement = DR_INIT;
         return ACTION_TERMINEE;
       }
@@ -430,46 +553,6 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, int
   return ACTION_EN_COURS;
 }
 
-/// @brief 
-
-/// @brief Récupère position (X, Y) et l'orientation du robot
-void Scan_Triangulation(struct triangulation_reception_t * triangulation_reception){
-  unsigned char tampon2[14];
-  lec_Balise_1, lec_Balise_2, lec_Balise_3 = 0, 0, 0;
-  //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
-  error = I2C_lire_registre(I2C_SLAVE_trian, 0, tampon2, 13);    // si errror != de 0 alors erreur de communication
-  if (error !=0){
-    Err_Tri_com =1;IndexErr = 1;lec_Balise_1=0;lec_Balise_2=0;lec_Balise_3=0;
-    affiche_erreur("Scan_Triangulation", "Erreur I2C");
-    while(1);
-  }
-  else{Err_Tri_com =0;IndexErr = 0;}
-  if (error ==0){
-    triangulation_reception->pos_x_mm = tampon2[1]<< 24 | tampon2[2]<< 16 | tampon2[3]<< 8 | tampon2[4] ;
-    triangulation_reception->pos_y_mm = tampon2[5]<< 24 | tampon2[6]<< 16 | tampon2[7]<< 8 | tampon2[8] ;
-    Angle_Robot_DEG_int = tampon2[9]<< 24 | tampon2[10]<< 16 | tampon2[11]<< 8 | tampon2[12] ;
-    // Serial.print(tampon2[9], BIN);Serial.print(" ");Serial.print(tampon2[10], BIN);Serial.print(" ");Serial.print(tampon2[11], BIN);Serial.print(" ");Serial.println(tampon2[12], BIN);
-    Angle_Robot_RAD = Angle_Robot_DEG_int * M_PI / 180.;
-    lec_Balise_1 = tampon2[0] & 0x01;
-    lec_Balise_2 = (tampon2[0] >>1) & 0x01;
-    lec_Balise_3 = (tampon2[0] >>2 )& 0x01;
-    lec_Calcul_ok = (tampon2[0] >>3 )& 0x01;
-
-    if(Position_actuelle_X < PosLimNeg | Position_actuelle_X > PosLimPos){
-      triangulation_reception->pos_x_mm  = 9999;
-    }
-    if(Position_actuelle_Y < PosLimNeg | Position_actuelle_Y > PosLimPos){
-      triangulation_reception->pos_y_mm = 9999;
-    }
-  }
-  if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){
-    triangulation_reception->validite = true;
-  }else{
-    triangulation_reception->validite = false;
-  }
-}
-
-
 
 void scan_I2C_bus(){
   char error, address;

Cerveau/Com_triangulation.ino

@@ -0,0 +1,50 @@
+/// @brief Récupère position (X, Y) et l'orientation du robot
+void Scan_Triangulation(struct triangulation_reception_t * triangulation_reception){
+  unsigned char tampon2[14];
+  lec_Balise_1, lec_Balise_2, lec_Balise_3 = 0, 0, 0;
+  //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
+  triangulation_reception->validite = false;
+  error = I2C_lire_registre(I2C_SLAVE_trian, 0, tampon2, 13);    // si errror != de 0 alors erreur de communication
+  if (error !=0){
+    Err_Tri_com =1;IndexErr = 1;lec_Balise_1=0;lec_Balise_2=0;lec_Balise_3=0;
+    affiche_erreur("Scan_Triangulation", "Erreur I2C");
+    while(1);
+  }
+  else{Err_Tri_com =0;IndexErr = 0;}
+  if (error ==0){
+    triangulation_reception->pos_x_mm = tampon2[1]<< 24 | tampon2[2]<< 16 | tampon2[3]<< 8 | tampon2[4] ;
+    triangulation_reception->pos_y_mm = tampon2[5]<< 24 | tampon2[6]<< 16 | tampon2[7]<< 8 | tampon2[8] ;
+
+    Angle_Robot_DEG_int = tampon2[9]<< 24 | tampon2[10]<< 16 | tampon2[11]<< 8 | tampon2[12] ;
+    // Serial.print(tampon2[9], BIN);Serial.print(" ");Serial.print(tampon2[10], BIN);Serial.print(" ");Serial.print(tampon2[11], BIN);Serial.print(" ");Serial.println(tampon2[12], BIN);
+    Angle_Robot_RAD = Angle_Robot_DEG_int * M_PI / 180.;
+    triangulation_reception->angle_rad = Angle_Robot_RAD;
+
+    lec_Balise_1 = tampon2[0] & 0x01;
+    lec_Balise_2 = (tampon2[0] >>1) & 0x01;
+    lec_Balise_3 = (tampon2[0] >>2 )& 0x01;
+    lec_Calcul_ok = (tampon2[0] >>3 )& 0x01;
+
+    if(Position_actuelle_X < PosLimNeg | Position_actuelle_X > PosLimPos){
+      triangulation_reception->pos_x_mm  = 9999;
+    }
+    if(Position_actuelle_Y < PosLimNeg | Position_actuelle_Y > PosLimPos){
+      triangulation_reception->pos_y_mm = 9999;
+    }
+  }
+  if(lec_Balise_1 == 1 && lec_Balise_2 == 1 && lec_Balise_3 == 1 && lec_Calcul_ok == 1 && error ==0){
+    triangulation_reception->validite = true;
+  }
+}
+
+void Triangulation_send_immobile(int immobile){
+  unsigned char donnee=0;
+  if(immobile){
+    donnee = 1;
+  }
+  error = I2C_ecrire_registre(I2C_SLAVE_trian, 13, &donnee, 1);    // si errror != de 0 alors erreur de communication
+  if (error !=0){
+    affiche_erreur("Send_Triangulation", "Erreur I2C");
+    while(1);
+  }
+}

Cerveau/Communication_chassis.h

@@ -9,7 +9,8 @@ struct chassis_reception_t {
 
 struct chassis_emission_t {
   unsigned char status;
-  int translation_x_mm, translation_y_mm, rotation_z_rad;
+  int translation_x_mm, translation_y_mm;
+  float rotation_z_rad;
   int vitesse, acceleration;
 };
 

Cerveau/Communication_chassis.ino

@@ -13,7 +13,6 @@ void Scan_chassis(struct chassis_reception_t * chassis_reception){
     affiche_erreur("Scan_Chassi", "Erreur I2C");
     while(1);
   }else{
-    Serial.println("I2C OK");
     Err_Chassi_com =0;
     IndexErr = 0;
 
@@ -25,16 +24,24 @@ void Scan_chassis(struct chassis_reception_t * chassis_reception){
 void send_Chassis(struct chassis_emission_t * chassis_emission){
   //if(nbr_essai<=10){
     // Prévient le chassis d'un nouveau mouvement avec le 2eme bit du premier Octet
+    int nb_pas_x, nb_pas_y, nb_pas_rot;
+
+    // Conversion des mm ou radian en pas
+
+    nb_pas_x = chassis_emission->translation_x_mm * 4.049;
+    nb_pas_y = chassis_emission->translation_y_mm * 4.953;
+    nb_pas_rot = chassis_emission->rotation_z_rad * 791.;
+
     Mot[0] = chassis_emission->status;
     //y*=-1;
     //y = y*direction;
-    Mot[1] = chassis_emission->translation_x_mm >>8; 
-    Mot[2] = chassis_emission->translation_x_mm;
-    Mot[3] = chassis_emission->translation_y_mm >>8;
-    Mot[4] = chassis_emission->translation_y_mm;
+    Mot[1] = nb_pas_x >>8; 
+    Mot[2] = nb_pas_x;
+    Mot[3] = nb_pas_y >>8;
+    Mot[4] = nb_pas_y;
     //Serial.println(y);
-    Mot[5] = chassis_emission->rotation_z_rad >>8; 
-    Mot[6] = chassis_emission->rotation_z_rad;
+    Mot[5] = nb_pas_rot >>8; 
+    Mot[6] = nb_pas_rot;
     Mot[7] = chassis_emission->vitesse >>8;
     Mot[8] = chassis_emission->vitesse;
     Mot[9] = chassis_emission->acceleration >>8; 

Cerveau/ServerWeb.ino

@@ -91,7 +91,7 @@ void handleForm() {
   // y= myString1.toInt() * coef_mvt/10;
   chassis_emission_web.translation_y_mm = myString1.toInt();
   String myString2 = server.arg("R"); //positon de cmd en Rotation Deg °
-  chassis_emission_web.rotation_z_rad = myString2.toInt() * 13.88888;
+  chassis_emission_web.rotation_z_rad = myString2.toInt() / 180 * M_PI;
   String myString3 = server.arg("V"); // Vitesse de cmd en 
   chassis_emission_web.vitesse = myString3.toInt();
   String myString4 = server.arg("A"); // Acceleration de cmd

Triangulation/I2C_Slave_lib.ino

@@ -0,0 +1,60 @@
+#define TAILLE_MEMOIRE_I2C 256
+#define TAILLE_MESSAGE_ENVOI_MAX 32
+byte memoire_I2C[TAILLE_MEMOIRE_I2C];
+byte memoire_I2C_index=0;
+
+bool nouveau_message=false;
+
+uint8_t * get_i2c_data(){
+  return memoire_I2C;
+}
+
+void onRequest(){
+  uint32_t taille_envoi;
+  taille_envoi = min (TAILLE_MEMOIRE_I2C-memoire_I2C_index, TAILLE_MESSAGE_ENVOI_MAX);
+  
+  Wire.write(&memoire_I2C[memoire_I2C_index], taille_envoi);
+  memoire_I2C_index++;
+  if(memoire_I2C_index>=TAILLE_MEMOIRE_I2C){
+    Serial.printf("memoire_I2C_index>=TAILLE_MEMOIRE_I2C\n");
+  }
+}
+
+void onReceive(int len){
+  memoire_I2C_index = Wire.read();
+  while(Wire.available()){
+    nouveau_message=true;
+    memoire_I2C[memoire_I2C_index] = Wire.read();
+    memoire_I2C_index++;
+  }
+}
+
+void I2C_Slave_init(int addr){
+  Wire.onReceive(onReceive);
+  Wire.onRequest(onRequest);
+  Wire.begin(addr);
+}
+
+bool I2C_Slave_nouveau_message(){
+  if(nouveau_message){
+    nouveau_message=false;
+    return true;
+  }
+  return false;
+}
+
+void I2C_envoi_8bits(byte value, char adresse){
+  memoire_I2C[adresse] = value;
+}
+
+void I2C_envoi_16bits(int16_t value, char adresse){
+  memoire_I2C[adresse] = value;
+}
+
+void I2C_envoi_32bits(int32_t value, char adresse){
+  memoire_I2C[adresse] = value >> 24;
+  memoire_I2C[adresse+1] = (value >> 16) & 0xFF;
+  memoire_I2C[adresse+2] = (value >> 8) & 0xFF;
+  memoire_I2C[adresse+3] = value & 0xFF;
+}
+

Triangulation/Triangulation.ino

@@ -0,0 +1,480 @@
+/*
+Test de triangulation
+*/
+#include <M5Core2.h>
+#include <math.h>
+#include <WiFi.h>
+
+const char* ssid = "riombotique";
+const char* password = "password";
+IPAddress local_IP(192, 168, 99, 102);
+IPAddress gateway(192, 168, 99, 1);
+IPAddress subnet(255, 255, 255, 0);
+WiFiServer server(80);
+boolean reading = false;
+String Lecture;
+
+#define CONV 0.000628318530717959
+#define CONV2 1.570796326794896619
+#define pi  3.141592653589793
+#define pi2 6.283185307179586
+
+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
+                                // Balise[i][1] est l'angle de la balise
+
+bool Balise_Valide;
+int A_Depart_Balise;
+int A_Fin_Balise;
+long Temps_Tour = millis();
+long T_Depart_Balise = 0;
+long T_Fin_Balise = 0;
+long Frequence_Balise = 0;
+long Nb_Pulses = 0;
+long Chrono = 0;
+int Angle = 0;
+int Angle_Balise = 0;
+int Nb_Balises = 0;
+int Capteur = 2;
+bool Trigger_Balises = true;  
+bool Calcul_Valide = false;
+bool Balise_0 = false;
+bool Balise_1 = false;
+bool Balise_2 = false;
+int Nb_tours = 0;
+int Old_Nb_tours = 0;
+int Pulse_Moteur = 190;
+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
+int X1 = -90;             // abcisse balise 0
+int X2 = -90;           // abcisse balise 1
+int X3 = 3090;           // abcisse balise 2
+int Y1 = 1950;             // ordonnee balise 0
+int Y2 = 50;             // ordonnee balise 1
+int Y3 = 1000;            // ordonnee balise 2
+int Xp1 = X1 - X2;
+int Yp1 = Y1 - Y2;
+int Xp3 = X3 - X2;
+int Yp3 = Y3 - Y2;
+int Xr, Yr, Angle_Robot_Deg_int;
+int alignement = 2200;
+float Angle_Robot_RAD, Angle_Robot_Deg, Angle_B1, angle_B1_calc, Angle_Ref_Theorique, calc1, calc2, offset;
+
+void IRAM_ATTR fonction_AB(){     // fonction de comptage des pas du codeur angulaire
+  Angle ++;
+  if (Trigger_Balises){
+    Capteur = 2;
+    if (Angle > 1316) Capteur = 1;
+    if (Angle > 2633) Capteur = 2;
+    if (Angle > 4300) Capteur = 1;
+    if (Angle > 5966) Capteur = 2;
+    if (Angle > 7933) Capteur = 1;
+    if (Angle > 9299) Capteur = 2;
+  }
+  if (((Angle - A_Fin_Balise) > 100) && (!Trigger_Balises)){
+    if (Nb_Pulses > 20){ // analyse de la balise si plus de 10 pulses uniquement
+      // calcul de la frequence moyenne et validation de la balise
+      int Delta_T = (T_Fin_Balise - T_Depart_Balise) / 10;
+      Frequence_Balise = 100000 * (Nb_Pulses);
+      if (Delta_T > 0) Frequence_Balise = Frequence_Balise / Delta_T;
+      if (A_Fin_Balise < A_Depart_Balise) A_Fin_Balise = A_Fin_Balise + 10000;
+      Angle_Balise = (A_Fin_Balise + A_Depart_Balise) / 2;
+      if (Angle_Balise > 10000) Angle_Balise = Angle_Balise - 10000;
+
+      if ((Frequence_Balise > (Frequence_0 - Bande_P)) && (Frequence_Balise < (Frequence_0 + Bande_P)) && !Balise_0) {
+         Balise[0][0] = Frequence_Balise;
+         Balise[0][1] = Angle_Balise;
+         Nb_Balises ++;
+         Balise_0 = true; // la balise a ete vue, plus possible de la voir a nouveau
+      }
+      if ((Frequence_Balise > (Frequence_1 - Bande_P)) && (Frequence_Balise < (Frequence_1 + Bande_P)) && !Balise_1) {
+         Balise[1][0] = Frequence_Balise;
+         Balise[1][1] = Angle_Balise;
+         Nb_Balises ++;
+         Balise_1 = true; // la balise a ete vue, plus possible de la voir a nouveau
+      }
+      if ((Frequence_Balise > (Frequence_2 - Bande_P)) && (Frequence_Balise < (Frequence_2 + Bande_P)) && !Balise_2) {
+         Balise[2][0] = Frequence_Balise;
+         Balise[2][1] = Angle_Balise;
+         Nb_Balises ++;
+         Balise_2 = true; // la balise a ete vue, plus possible de la voir a nouveau          
+      }
+
+    }
+    Trigger_Balises = true;
+  }
+}
+
+void IRAM_ATTR fonction_Z(){     // appelée à chaque tour
+  Nb_tours ++;
+  Angle = 0;
+}
+
+void IRAM_ATTR fonction_Balise_capteur_1(){      // appelée à chaque detection de baslise capteur 1
+  if (Capteur == 1) {
+    A_Fin_Balise = Angle;
+    // effectue a partir du 2eme pulse
+    Chrono = micros();
+    Nb_Pulses++;
+    T_Fin_Balise = Chrono;
+    // effectue au 1er pulse
+    if (Trigger_Balises){
+      A_Depart_Balise = Angle;
+      T_Depart_Balise = Chrono;
+      Nb_Pulses = 0;
+      Trigger_Balises = false;
+      //est-ce bien une balise modulee ? -> mesure du temps avant apparition de la prochaine impulsion impusion 
+    }
+  }
+}
+
+void IRAM_ATTR fonction_Balise_capteur_2(){      // appelée à chaque detection de baslise capteur 2
+  if (Capteur == 2) {
+    A_Fin_Balise = Angle;
+    // effectue a partir du 2eme pulse
+    Chrono = micros();
+    Nb_Pulses++;
+    T_Fin_Balise = Chrono;
+    // effectue au 1er pulse
+    if (Trigger_Balises){
+      A_Depart_Balise = Angle;
+      T_Depart_Balise = Chrono;
+      Nb_Pulses = 0;
+      Trigger_Balises = false;
+      //est-ce bien une balise modulee ? -> mesure du temps avant apparition de la prochaine impulsion impusion 
+    }
+  }
+}
+
+void IRAM_ATTR traitement_donnees() {
+  // calcul des coordonnees ......................... 
+  Calcul_Valide = false;
+  double A1 = (Balise[1][1] - Balise[0][1]) * CONV;
+  double A2 = (Balise[2][1] - Balise[1][1]) * CONV;  
+  if(sin(A1)==0 || sin(A2)==0) return;
+  double T12 = cos(A1) / sin(A1);
+  double T23 = cos(A2) / sin(A2);
+  if ((T12 + T23)==0) return;
+  double T31 = (1 - (T12 * T23)) / (T12 + T23);
+  double Xp12 = Xp1 + (T12 * Yp1);
+  double Yp12 = Yp1 - (T12 * Xp1);
+  double Xp23 = Xp3 - (T23 * Yp3);
+  double Yp23 = Yp3 + (T23 * Xp3);
+  double Xp31 = (Xp3 + Xp1) + (T31 * (Yp3 - Yp1));
+  double Yp31 = (Yp3 + Yp1) - (T31 * (Xp3 - Xp1));
+  double Kp31 = (Xp1 * Xp3) + (Yp1 * Yp3) + (T31 * ((Xp1 * Yp3) - (Xp3 * Yp1)));
+  double D = ((Xp12 - Xp23) * (Yp23 - Yp31)) - ((Yp12 - Yp23) * (Xp23 - Xp31));
+  if (D==0) return;
+  Xr = X2 + ((Kp31 * (Yp12 - Yp23)) / D);
+  Yr = Y2 + ((Kp31 * (Xp23 - Xp12)) / D);
+  Calcul_Valide = true;
+}
+
+void setup() {
+  Serial.begin(115200);
+  M5.begin();
+  M5.Axp.SetSpkEnable(0); 
+  // moteur
+  ledcSetup(0, 1000, 8);
+  ledcAttachPin(2, 0);
+  ledcWrite(0, Pulse_Moteur); 
+  // I2C
+  Wire.setPins(32, 33);
+  I2C_Slave_init(0x30);
+  M5.lcd.setTextSize(2); 
+    //Initialisation wifi
+   //------------WIFI------------
+   //Initialisation wifi
+  WiFi.config(local_IP, gateway, subnet);
+  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.
+
+  int test_wifi = 0;
+  
+  while (WiFi.status() != WL_CONNECTED){
+    delay(300);
+    M5.Lcd.print(".");
+    test_wifi ++;
+    if (test_wifi > 10) break;
+  }
+ if (WiFi.status() == WL_CONNECTED) {
+    server.begin();
+    delay(500);
+    M5.Lcd.clear();
+    M5.Lcd.setCursor(10,10);
+    M5.Lcd.print("Connecte au reseau    ;-)");
+    M5.Lcd.setCursor(10,30);
+    M5.Lcd.print(ssid); 
+  }
+  else {
+    // le routeur riombotique n'a pas été trouvé - création d'un point d'accès
+    WiFi.mode(WIFI_OFF);
+    if (!WiFi.softAP("triangulation", "ilestsecret")) {
+      log_e("Soft AP creation failed.");
+      while(1);
+    }
+    IPAddress myIP = WiFi.softAPIP();
+    server.begin();
+    M5.Lcd.clear();
+    M5.Lcd.setCursor(10,10);
+    M5.Lcd.print("Creation Hotspot    ;-)");
+    M5.Lcd.setCursor(10,30);
+    M5.Lcd.print("triangulation 192.168.4.1"); 
+    M5.Lcd.setCursor(10,50);
+    M5.Lcd.print("mdp : ilestsecret"); 
+  }
+  delay(1000);
+  M5.Lcd.clear();
+  // --------------------
+  pinMode(19, INPUT_PULLDOWN);  // entree pulse tour codeur
+  pinMode(27, INPUT_PULLDOWN);  // entree pulse angle codeur
+  pinMode(25, INPUT_PULLDOWN);  // entree phototransistor capteur 1
+  pinMode(26, INPUT_PULLDOWN);  // entree phototransistor capteur 2
+  attachInterrupt(27, fonction_AB, RISING);       // appel fonction AB a chaque pulse codeur
+  attachInterrupt(19, fonction_Z, RISING);        // appel fonction Z a chaque tour codeur
+  attachInterrupt(25, fonction_Balise_capteur_1, FALLING);  // appel fonction Balise capteur 1 a chaque detection phototransistor
+  attachInterrupt(26, fonction_Balise_capteur_2, FALLING);  // appel fonction Balise capteur 2 a chaque detection phototransistor
+  M5.Lcd.setCursor(10,10);
+  M5.lcd.print(" Salut Riombotique");
+  delay(500);
+  M5.Lcd.clear();
+  //M5.Lcd.setCursor(10,10);
+  //M5.Lcd.print("Nb tours : ");
+  //M5.Lcd.setCursor(10,30);
+  //M5.Lcd.print("Balise   : "); 
+  M5.Lcd.setCursor(10,0);
+  M5.Lcd.print("Angle 1  : ");
+  M5.Lcd.setCursor(10,20);
+  M5.Lcd.print("Angle 2  : ");
+  M5.Lcd.setCursor(10,40);
+  M5.Lcd.print("Angle 3  : ");
+  //M5.Lcd.setCursor(10,110);
+  //M5.Lcd.print("Frequence 1 : ");
+  //M5.Lcd.setCursor(10,130);
+  //M5.Lcd.print("Frequence 2 : ");
+  //M5.Lcd.setCursor(10,150);
+  //M5.Lcd.print("Frequence 3 : ");
+  M5.Lcd.setCursor(0,60);
+  M5.Lcd.print("A float = ");
+  M5.Lcd.setCursor(0,80);
+  M5.Lcd.print("a RAD = ");
+  M5.Lcd.setCursor(0,100);
+  M5.Lcd.print("angle = ");
+  M5.Lcd.setCursor(10,120);
+  M5.Lcd.print("Status  : ");
+  M5.Lcd.setCursor(160,180);
+  M5.Lcd.print("X = ");
+  M5.Lcd.setCursor(160,210);
+  M5.Lcd.print("Y = ");
+}
+
+void loop() {
+  //Effectue a chaque tour ........................
+  if(data_i2C[13] != 1){
+    Nb_Balises = 0; 
+    Balise_0 = false;
+    Balise_1 = false;
+    Balise_2 = false;
+    Calcul_Valide = false;
+    uint8_t etat_balises = (Balise_0 | Balise_1 <<1 | Balise_2 <<2 | Calcul_Valide <<3);
+    I2C_envoi_8bits(etat_balises,0);
+  }
+  if ((Old_Nb_tours != Nb_tours) && (Trigger_Balises)){
+    Old_Nb_tours = Nb_tours;
+    if (!Balise_Valide){
+      Balise_Valide = true;
+      rapport();
+    }
+
+    //_____________________________________________ gestion I2C et Data sent
+    if(Xr>0){
+  
+      Angle_B1 = ((Balise[0][1] + 10000 - alignement) % 10000) * CONV;  // lorsque le robot est orienté vers la balise, angle codeur = 0
+
+      //calc1 = (Y1-Yr)*1000/Xr;
+      //Angle_Ref_Theorique = atan(calc1/1000);     // angle théorique entre axe X et alignement vers balise depuis la position X, Y
+      calc1 = Xr*1000/(Y1-Yr);
+      Angle_Ref_Theorique = atan(calc1/1000);     // angle théorique entre axe Y et alignement vers balise depuis la position X, Y
+
+      //calc2 = fmod(((Angle_B1/pi2*360)+360 - ((CONV2-Angle_Ref_Theorique)/pi2*360)),360); // angle codeur - (90°-angle theorique mais sur Y) modulo 360° et oui le modulo ne prend que de l'entier...
+      calc2 = fmod(((Angle_B1/pi2*360)+360 - (Angle_Ref_Theorique/pi2*360)),360); // angle codeur - angle theorique sur Y) modulo 360° et oui le modulo ne prend que de l'entier...
+      
+      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);
+    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; 
+    Balise_0 = false;
+    Balise_1 = false;
+    Balise_2 = false; 
+    for (int i = 0; i <= (2); i++) {
+      //Remise à zero des balises
+      Balise[i][0] = 0;
+      Balise[i][1] = 0;
+    } 
+    vitesse_moteur();
+  }
+  // Si le robot n'est pas immobile, on invalide les balises.
+  
+
+
+  //________________________________________
+}
+void rapport(){
+  Serial.print("Frequence ");
+  Serial.print(Frequence_Balise);
+  Serial.print(" / Pulses ");
+  Serial.print(Nb_Pulses);
+  Serial.print("/ Angle 1 ");
+  Serial.print(A_Depart_Balise);
+  Serial.print("/ Angle 2 ");
+  Serial.println(A_Fin_Balise);
+}
+
+void vitesse_moteur() {
+  if ((millis() - Temps_Tour) < 500){ //250 millisecondes par tour correspond à la vitesse de rotation desiree
+    Pulse_Moteur ++;
+  }
+  else {
+   Pulse_Moteur --;
+  }
+  ledcWrite(0, Pulse_Moteur); // Ajustement de la vitesse
+  Temps_Tour = millis();
+}
+
+void affichage_resultats() {
+  // affichage des resultats  .........................
+  //M5.Lcd.setCursor(140,10);
+  //M5.Lcd.print(Nb_tours); 
+  //M5.Lcd.print("  ");  
+  //M5.Lcd.setCursor(140,30);
+  //M5.Lcd.print(Nb_Balises); 
+  //M5.Lcd.print("     ");  
+  M5.Lcd.setCursor(140,0);
+  M5.Lcd.print(Balise[0][1]);
+  //M5.Lcd.print(" / ");  
+  //M5.Lcd.print(Angle_B[0][3]);
+  M5.Lcd.print("     ");  
+  M5.Lcd.setCursor(140,20);
+  M5.Lcd.print(Balise[1][1]);
+  //M5.Lcd.print(" / ");  
+ // M5.Lcd.print(Angle_B[1][3]);
+  M5.Lcd.print("     ");  
+  M5.Lcd.setCursor(140,40);     
+  M5.Lcd.print(Balise[2][1]);
+  M5.Lcd.print("     ");  
+  M5.Lcd.setCursor(140,120);
+  M5.Lcd.print(Balise_0);
+  M5.Lcd.setCursor(150,120);
+  M5.Lcd.print(Balise_1);
+  M5.Lcd.setCursor(160,120);
+  M5.Lcd.print(Balise_2);
+ // M5.Lcd.print(" / ");  
+ // M5.Lcd.print(Angle_B[2][3]);
+  M5.Lcd.print("     ");  
+  //M5.Lcd.setCursor(200,110);     
+  //M5.Lcd.print(Balise[0][0]);
+  //M5.Lcd.print("    ");
+  //M5.Lcd.setCursor(200,130);     
+  //M5.Lcd.print(Balise[1][0]);
+  //M5.Lcd.print("    ");
+  //M5.Lcd.setCursor(200,150);     
+  //M5.Lcd.print(Balise[2][0]);
+  //M5.Lcd.print("    ");
+  M5.Lcd.setCursor(150,60);
+  M5.Lcd.print(Angle_B1);
+  M5.Lcd.setCursor(150,80);
+  M5.Lcd.print(Angle_Ref_Theorique);
+  M5.Lcd.setCursor(150,100);
+  M5.Lcd.print(Angle_Robot_Deg);
+  M5.Lcd.print(" °      ");  
+  M5.Lcd.setCursor(200,180);
+  M5.Lcd.print(Xr);
+  M5.Lcd.print(" mm      ");  
+  M5.Lcd.setCursor(200,210);
+  M5.Lcd.print(Yr);
+  M5.Lcd.print(" mm      "); 
+}
+
+void checkForClient(){
+  WiFiClient client = server.available();
+  if (client) {
+    while (client.connected()) {
+      if (client.available()) {
+        char c = client.read();
+        if (c == '\n') {
+          // fin du message
+          client.println("HTTP/1.1 200 OK");
+          client.println("Content-Type: text/html");
+          client.println("Connection: close");
+          client.println();
+          client.println("<!DOCTYPE HTML>");
+          client.println("<meta http-equiv='Refresh' content='2'>");
+          client.print("<html>");
+          //client.print("Nb tours : ");
+          //client.print(Nb_tours);
+          //client.print("<BR>");
+          //client.print("Balise   : ");
+          //client.print(Nb_Balises); 
+          //client.print("<BR>");
+          client.print("Angle 1  : ");
+          client.print(Balise[0][1]);
+          client.print("<BR>");
+          client.print("Angle 2  : ");
+          client.print(Balise[1][1]);
+          client.print("<BR>");     
+          client.print("Angle 3  : ");
+          client.print(Balise[2][1]);
+          client.print("<BR>");     
+          //client.print("Frequence 1 : ");
+          //client.print(Balise[0][0]);
+          //client.print("<BR>");     
+          //client.print("Frequence 2 : ");
+          //client.print(Balise[1][0]);
+          //client.print("<BR>");     
+          //client.print("Frequence 3 : ");
+          //client.print(Balise[2][0]);
+          //client.print("<BR>"); 
+          client.print("X = ");
+          if (Xr < 3000) client.print(Xr);
+          client.println(" mm");
+          client.print("<BR>");
+          client.print("Y = ");
+          if (Yr < 3000) client.print(Yr);
+          client.println(" mm");
+          client.println("</html>");
+          break;
+        }
+      }
+      else {
+        break;
+      }
+    }
+    delay(10); // give the web browser time to receive the data
+    client.stop(); // clos la connection
+    delay(10);
+  } 
+}
+
+
+