Annulation de la validité de la position de la triangulation tant que le robot est en mouvement

Cerveau/Cerveau.ino

@@ -293,31 +293,31 @@ void gestion_match(){
         index_Maitre = DEPLACEMENT_RELATIF;
       }
       if(M5.BtnA.read() == 1){
+        Serial.println("BtnB");
         // Déplacement en X
-        translation_x_mm = 2000;
+        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){
         Serial.println("BtnB");
-        // Déplacement en Y
+        // Déplacement en Y       
-        
+        //Triangulation_send_immobile(0);
-        translation_x_mm = 0;
-        translation_y_mm = 2000;
-        rotation_rad = 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 = DEPLACEMENT_RELATIF;
         
       }
     break;
@@ -345,7 +345,7 @@ void gestion_match(){
       break;
 
     case TEST_DEPLACEMENT_ABSOLU:
-      if(deplacement_absolu(1000, 1000, 0, 0) == ACTION_TERMINEE){
+      if(deplacement_absolu(800, 800, 0, 0) == ACTION_TERMINEE){
         index_Maitre = ATTENTE_ORDRE;
       }
       break;
@@ -407,7 +407,9 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int
           // 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("trans_x:%d\ntrans_y:%d\nrot:%d\n",chassis_emission.translation_x_mm, 
+          Serial.printf("pos_x:%d\tpos_y:%d\tOrientation:%d\n",triangulation_reception.pos_x_mm, triangulation_reception.pos_y_mm,
+          triangulation_reception.angle_rad); 
+          Serial.printf("trans_x:%d\ttrans_y:%d\trot:%d\n",chassis_emission.translation_x_mm, 
           chassis_emission.translation_y_mm, chassis_emission.rotation_z_rad);
           etat_deplacement = DA_MVT_EN_COUR;
         }else{
@@ -420,6 +422,7 @@ enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, int
       break;
     
     case DA_MVT_EN_COUR:
+      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){
@@ -454,6 +457,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;
 
@@ -472,6 +476,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;
       }
@@ -481,45 +486,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)
-  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.;
-    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 scan_I2C_bus(){
   char error, address;

Cerveau/Com_triangulation.ino

@@ -0,0 +1,49 @@
+/// @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.;
+    Serial.printf("Scan triangulation 0: 0x%X\n", tampon2[0]);
+    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.ino

@@ -24,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[1] = nb_pas_x >>8; 
-    Mot[2] = chassis_emission->translation_x_mm;
+    Mot[2] = nb_pas_x;
-    Mot[3] = chassis_emission->translation_y_mm >>8;
+    Mot[3] = nb_pas_y >>8;
-    Mot[4] = chassis_emission->translation_y_mm;
+    Mot[4] = nb_pas_y;
     //Serial.println(y);
-    Mot[5] = chassis_emission->rotation_z_rad >>8; 
+    Mot[5] = nb_pas_rot >>8; 
-    Mot[6] = chassis_emission->rotation_z_rad;
+    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

@@ -5,6 +5,10 @@ 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);

Triangulation/Triangulation.ino

@@ -19,6 +19,7 @@ String Lecture;
 #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
@@ -190,7 +191,13 @@ void setup() {
    //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(".");
@@ -260,6 +267,8 @@ void setup() {
   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);
@@ -268,6 +277,15 @@ void setup() {
 
 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){
@@ -316,6 +334,10 @@ void loop() {
     } 
     vitesse_moteur();
   }
+  // Si le robot n'est pas immobile, on invalide les balises.
+  
+
+
   //________________________________________
 }
 void rapport(){
@@ -360,6 +382,13 @@ void affichage_resultats() {
   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("     ");  
@@ -447,3 +476,5 @@ void checkForClient(){
   } 
 }
 
+
+