diff --git a/Balise_VL53L1X.c b/Balise_VL53L1X.c new file mode 100644 index 0000000..b73d56d --- /dev/null +++ b/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; capteurangle_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 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; +} \ No newline at end of file diff --git a/Balise_VL53L1X.h b/Balise_VL53L1X.h new file mode 100644 index 0000000..4e9a5b6 --- /dev/null +++ b/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); \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 873fdc9..044c30a 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/Test.c b/Test.c index a751a7d..88aa688 100644 --- a/Test.c +++ b/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; } diff --git a/i2c_annexe.c b/i2c_annexe.c index cd79057..d4e0fb3 100644 --- a/i2c_annexe.c +++ b/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 @@ -103,4 +104,10 @@ 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]; + } } \ No newline at end of file diff --git a/i2c_annexe.h b/i2c_annexe.h index 550747d..e71a42d 100644 --- a/i2c_annexe.h +++ b/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); \ No newline at end of file