Vidéo de la pseudo-homologation avant la CARAR

Cerveau/Cerveau.ino

@@ -92,7 +92,7 @@ int index_Maitre = 0;
 bool Mvt_tolerance_OK =false;
 bool Balises_OK = 0;
 int tolerance_position =100;
-float tolerance_orientation =0.05; // 3°
+float tolerance_orientation =0.03; // 2°
 
 
 char* tableau[] = {"Lecture serveur", "Prise position", "Verif mvmt end ou cmd", "Compar position", "Deplacement absolu"};
@@ -361,7 +361,9 @@ void gestion_match(){
 enum etat_action_t Strategie(void){
   static enum {
     STRAT_RECULE_BANDEROLE, // Deplacement relatif
-    STRAT_ALLER_GRADINS_1,  // Déplacement absolu
+    STRAT_ALLER_GRADINS_1_A,  // Déplacement absolu
+    STRAT_ALLER_GRADINS_1_B,  // Déplacement relatif
+    STRAT_ALLER_GRADINS_1_C,  // Déplacement relatif
     STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement absolu
     STRAT_ALLER_BACKSTAGE // Déplacement relatif
 
@@ -378,18 +380,45 @@ enum etat_action_t Strategie(void){
       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;
+        etat_strategie = STRAT_ALLER_GRADINS_1_A;
       }
       break;
 
-    case STRAT_ALLER_GRADINS_1:
-      etat_action = deplacement_absolu(730, 550, M_PI/2., 0);
+    case STRAT_ALLER_GRADINS_1_A:
+      etat_action = deplacement_absolu(800, 800, -M_PI/2., 0);
+      if(etat_action == ACTION_TERMINEE){
+        etat_strategie = STRAT_ALLER_GRADINS_1_B;
+      }
+      break;
+
+    case STRAT_ALLER_GRADINS_1_B:
+      etat_action = deplacement_relatif(300, 0, 0, 0);
+      if(etat_action == ACTION_TERMINEE){
+        etat_strategie = STRAT_ALLER_GRADINS_1_C;
+      }
+      break;
+
+    case STRAT_ALLER_GRADINS_1_C:
+      etat_action = deplacement_relatif(-250, 0, 0, 0);
+      if(etat_action == ACTION_TERMINEE){
+        etat_strategie = STRAT_ALLER_PREPA_BACKSTAGE;
+      }
+      break;
+
+    case STRAT_ALLER_PREPA_BACKSTAGE:
+      etat_action = deplacement_absolu(550, 1150, M_PI/2., 0);
+      if(etat_action == ACTION_TERMINEE){
+        etat_strategie = STRAT_ALLER_BACKSTAGE;
+      }
+      break;
+
+    case STRAT_ALLER_BACKSTAGE:
+      etat_action = deplacement_relatif(500, 0, 0, 0);
       if(etat_action == ACTION_TERMINEE){
         etat_strategie = STRAT_RECULE_BANDEROLE;
         return ACTION_TERMINEE;
       }
-
-
+      break;
   }
 
   return ACTION_EN_COURS;
@@ -419,7 +448,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;
@@ -429,23 +458,24 @@ void compar_cinematique(int consigne_x_mm, int consigne_y_mm, float consigne_ori
 
     // 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 angle_robot_vers_destination = M_PI + atan2f(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->translation_x_mm = cos(angle_robot_vers_destination - triangulation_reception.angle_rad) * distance_calculee;
+    chassis_emission->translation_y_mm = sin(angle_robot_vers_destination - triangulation_reception.angle_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){
+enum etat_action_t deplacement_absolu(int consigne_x_mm, int consigne_y_mm, float 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 int mem_consigne_x_mm, mem_consigne_y_mm;
+  static float mem_consigne_orientation_rad;
   static struct chassis_emission_t chassis_emission;
   struct triangulation_reception_t triangulation_reception;
   enum etat_action_t etat_deplacement_relatif;

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