Récupération des VL53L1X

Balise_VL53L1X.c

@@ -0,0 +1,66 @@
+#include "i2c_annexe.h"
+#include "Localisation.h"
+#include "math.h"
+
+#define NB_CAPTEURS 12
+#define DISTANCE_OBSTACLE_IGNORE 200
+#define DISTANCE_CAPTEUR_CENTRE_ROBOT 40
+
+uint8_t distance_capteur_cm[NB_CAPTEURS];
+
+struct capteur_VL53L1X_t{
+    uint8_t distance_cm;
+    double angle_ref_robot;
+    double angle_ref_terrain;
+}capteurs_VL53L1X[NB_CAPTEURS];
+
+void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm);
+
+void Balise_VL53L1X_gestion(){
+    i2c_annexe_get_distances(distance_capteur_cm);
+    for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
+        actualise_VL53L1X(&(capteurs_VL53L1X[capteur]), distance_capteur_cm[capteur]);
+    }
+}
+
+void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm){
+    // Actualisation de l'angle du capteur
+    capteur_VL53L1X->angle_ref_terrain = capteur_VL53L1X->angle_ref_robot + Localisation_get().angle_radian;
+    // Maintien de l'angle entre -PI et PI
+    while(capteur_VL53L1X->angle_ref_terrain > 2* M_PI){
+        capteur_VL53L1X->angle_ref_terrain -= 2* M_PI;
+    }
+    while(capteur_VL53L1X->angle_ref_terrain < 2* -M_PI){
+        capteur_VL53L1X->angle_ref_terrain += 2* M_PI;
+    }
+    capteur_VL53L1X->distance_cm = distance_capteur_cm + DISTANCE_CAPTEUR_CENTRE_ROBOT;
+}
+
+void Balise_VL53L1X_init(){
+    capteurs_VL53L1X[9].angle_ref_robot = 0;
+    capteurs_VL53L1X[10].angle_ref_robot =   M_PI/6;
+    capteurs_VL53L1X[11].angle_ref_robot = 2*M_PI/6;
+    capteurs_VL53L1X[0].angle_ref_robot =  3*M_PI/6;
+    capteurs_VL53L1X[1].angle_ref_robot =  4*M_PI/6;
+    capteurs_VL53L1X[2].angle_ref_robot =  5*M_PI/6;
+    capteurs_VL53L1X[3].angle_ref_robot =  6*M_PI/6;
+    capteurs_VL53L1X[4].angle_ref_robot =  7*M_PI/6;
+    capteurs_VL53L1X[5].angle_ref_robot =  8*M_PI/6;
+    capteurs_VL53L1X[6].angle_ref_robot =  9*M_PI/6;
+    capteurs_VL53L1X[7].angle_ref_robot =  10*M_PI/6;
+    capteurs_VL53L1X[8].angle_ref_robot =  11*M_PI/6;
+}
+
+uint8_t Balise_VL53L1X_get_min_distance(void){
+    uint8_t min_distance = DISTANCE_OBSTACLE_IGNORE;
+    for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
+        if(min_distance> capteurs_VL53L1X[capteur].distance_cm){
+            min_distance = capteurs_VL53L1X[capteur].distance_cm;
+        }
+    }
+    return min_distance;
+}
+
+uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur){
+    return capteurs_VL53L1X[capteur].distance_cm;
+}

Balise_VL53L1X.h

@@ -0,0 +1,5 @@
+
+void Balise_VL53L1X_init(void);
+void Balise_VL53L1X_gestion(void);
+uint8_t Balise_VL53L1X_get_min_distance(void);
+uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur);

CMakeLists.txt

@@ -12,6 +12,7 @@ Holonome2023.c
 APDS_9960.c
 Asser_Moteurs.c
 Asser_Position.c
+Balise_VL53L1X.c
 Commande_vitesse.c
 QEI.c
 gyro.c

Test.c

@@ -12,6 +12,7 @@
 #include "gyro.h"
 #include "Asser_Moteurs.h"
 #include "Asser_Position.h"
+#include "Balise_VL53L1X.h"
 #include "Commande_vitesse.h"
 #include "Geometrie_robot.h"
 #include "i2c_annexe.h"
@@ -57,6 +58,7 @@ int test_i2c_ecriture_pico_annex_nb();
 int test_i2c_ecriture_pico_annex_nb_2();
 int test_aller_retour();
 void test_trajectoire_teleplot();
+int test_capteurs_balise(void);
 
 
 // Mode test : renvoie 0 pour quitter le mode test
@@ -69,6 +71,7 @@ int mode_test(){
     printf("D - pour les codeurs (somme en mm)\n");
     printf("E - Commande en vitesse...\n");
     printf("F - Strategie...\n");
+    printf("G - Lecture des capteurs\n");
     printf("H - Asser Position - avance\n");
     printf("I - Asser Position - avance et tourne (gyro)\n");
     printf("J - Asser Position - avance et tourne (sans gyro)\n");
@@ -116,6 +119,11 @@ int mode_test(){
         while(test_strategie());
         break;
 
+    case 'G':
+    case 'g':
+        while(test_capteurs_balise());
+        break;
+
     case 'H':
     case 'h':
         while(test_asser_position_avance());
@@ -198,7 +206,7 @@ int mode_test(){
 
 int test_continue_test(){
     int lettre;
-    printf("q pour quitter, une autre touche pour un nouveau test.\n");
+    //printf("q pour quitter, une autre touche pour un nouveau test.\n");
     do{
         lettre = getchar_timeout_us(0);
     }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
@@ -212,6 +220,25 @@ int test_continue_test(){
 
 }
 
+int test_capteurs_balise(void){
+    printf("Test de la balise\n");
+    i2c_maitre_init();
+    
+    while(true){
+        uint8_t min_distance;
+        i2c_gestion(i2c0);
+        i2c_annexe_gestion();
+        Balise_VL53L1X_gestion();
+
+        min_distance = Balise_VL53L1X_get_min_distance();
+        printf(">min_distance:%d\n",min_distance);
+        for(uint8_t capteur=0; capteur<12; capteur++){
+            printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur));
+        }
+    }
+
+}
+
 bool reserved_addr(uint8_t addr) {
     return (addr & 0x78) == 0 || (addr & 0x78) == 0x78;
 }

i2c_annexe.c

@@ -7,8 +7,9 @@
 #define ADRESSE_DEBUT_R 0x0A
 #define ADRESSE_TURBINE_PORTE 0x0A
 #define ADRESSE_CONTACTEURS 0x0B
+#define ADRESSE_VL53L1X 0x0C
 #define TAILLE_DONNEES_EMISSION 3
-#define TAILLE_DONNEES_RECEPTION 13
+#define TAILLE_DONNEES_RECEPTION 14
 
 
 
@@ -104,3 +105,9 @@ uint8_t i2c_annexe_get_contacteur_longer_A(void){
 uint8_t i2c_annexe_get_contacteur_longer_C(void){
     return (donnees_reception[ADRESSE_CONTACTEURS - ADRESSE_DEBUT_R] >> 1) & 0x01;
 }
+
+void i2c_annexe_get_distances(uint8_t *distance_capteur_cm){
+    for (uint8_t capteur = 0; capteur < 12; capteur++){
+        distance_capteur_cm[capteur] = donnees_reception[capteur + ADRESSE_VL53L1X - ADRESSE_DEBUT_R];
+    }
+}

i2c_annexe.h

@@ -18,3 +18,5 @@ uint8_t i2c_annexe_get_contacteur_butee_C(void);
 
 uint8_t i2c_annexe_get_contacteur_longer_A(void);
 uint8_t i2c_annexe_get_contacteur_longer_C(void);
+
+void i2c_annexe_get_distances(uint8_t *distance_capteur_cm);