diff --git a/VL53L8_2024.c b/VL53L8_2024.c index d7e836b..9308448 100644 --- a/VL53L8_2024.c +++ b/VL53L8_2024.c @@ -120,14 +120,16 @@ void VL53L8_init(VL53L8CX_Configuration * Dev){ } - -void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results){ - uint8_t error; +/// @brief Lit le capteur, filtre les données avec les status renvoyés et le masque de distance défini plus haut +/// @param Dev Capteur à lire +/// @param Results Stockage des résultats +/// @return 0 si tout s'est bien passé, une erreur sinon +uint8_t VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results){ + uint8_t error=0; error = vl53l8cx_get_ranging_data(Dev, Results); if(error){ - printf("Error VL53L8\n"); - VL53L8_init(Dev); - } + return error; + } /* As the sensor is set in 4x4 mode by default, we have a total @@ -137,12 +139,6 @@ void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results // Filtrage des données for(int row=0; row < 8; row++){ for(int col=0; col < 8; col++){ - /*if(Results.target_status[col+ 8*row] == 255 ||// No Target detected - Results.target_status[col+ 8*row] == 10 ||// Range valid, but no target detected at previous range - Results.target_status[col+ 8*row] == 4 ||// Target consistency failed - Results.target_status[col+ 8*row] == 3 ){// Sigma error too high - Results.distance_mm[col+ 8*row] = 3000; - }*/ // Seul status 100% valid if(Results->target_status[col+ 8*row] != 5){ Results->distance_mm[col+ 8*row] = 3000; @@ -155,17 +151,31 @@ void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results } } } + return error; +} - // Affichage des données - /* - printf(">distance:"); - for(int row=0; row < 8; row++){ - for(int col=0; col < 8; col++){ - printf("%d,", Results->distance_mm[col+ 8*row]); - } +/// @brief Contrôle si des données sont disponibles, si oui, lit le données +/// @param capteur capteur à lire +/// @param results stockage des résultats +/// @return 1 si des données ont été lues +uint8_t capteur_actualise(VL53L8CX_Configuration * capteur, VL53L8CX_ResultsData * results){ + uint8_t isReady, status; + + status = vl53l8cx_check_data_ready(capteur, &isReady); + if(status){ + printf("erreur:%d\n", status); + } + if(isReady) + { + //printf(">r%d:1\n",capteur->platform.address); + status = VL53L8_lecture(capteur, results); + if(status){ + printf("erreur:%d\n", status); } - printf("\n");*/ - + }else{ + //Serial.printf(">r:0\n"); + } + return isReady; } int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){ @@ -190,12 +200,14 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){ /// @param Results Valeurs brutes du capteurs /// @param pos_x position de la planche /// @return 0 sucess, 1 failed -int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y){ +int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y, float *angle){ uint16_t mesure1, mesure2; int min_distance = 2000; int col, row; int distance_ref_mm = 500; int last_col_avec_planche=0; + + *angle = NAN; // Contrôle si la planche est à droite for(col=7; col>=0; col--){ printf("%d, ", Results.distance_mm[col + 8*3]); @@ -210,7 +222,7 @@ int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float } // Si la distance est plus éloignée de 5 cm que celle de référence, nous sommes arrivé au bout de la planche if(distance_ref_mm + 50 < Results.distance_mm[col + 8*row]){ - last_col_avec_planche = col; + last_col_avec_planche = col+2; // Double break; col = 0; break; @@ -219,12 +231,21 @@ int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float } } printf("\n"); + row=3; if(last_col_avec_planche == 0){ // Echec *pos_x = 0; return 1; } + + if(last_col_avec_planche < 7){ + /// On peut déterminer un angle de la planche + float angle_planche; + angle_planche = atan2f((float) (Results.distance_mm[7 + 8*row] - Results.distance_mm[last_col_avec_planche + 8*row]), + -(Results.distance_mm[7 + 8*row] * sinf(angles_VL53L8[7]) - Results.distance_mm[last_col_avec_planche + 8*row]* sinf(angles_VL53L8[last_col_avec_planche]))); + *angle = angle_planche; + } *pos_x = (float)distance_ref_mm * sinf(angles_VL53L8[last_col_avec_planche]); *pos_y = (float)distance_ref_mm; @@ -235,12 +256,14 @@ int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float /// @param Results Valeurs brutes du capteurs /// @param pos_x position de la planche /// @return 0 sucess, 1 failed -int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y){ +int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y, float *angle){ uint16_t mesure1, mesure2; int min_distance = 2000; int col, row; int distance_ref_mm = 500; int last_col_avec_planche=0; + + *angle = NAN; // Controle si la planche est à gauche for(col=0; col<8; col++){ printf("%d,", Results.distance_mm[col + 8*3]); @@ -255,7 +278,7 @@ int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float * } // Si la distance est plus éloignée de 5 cm que celle de référence, nous sommes arrivé au bout de la planche if(distance_ref_mm + 50 < Results.distance_mm[col + 8*row]){ - last_col_avec_planche = col; + last_col_avec_planche = col-2; // Double break; col = 9; break; @@ -264,12 +287,21 @@ int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float * } } printf("\n"); + row=3; if(last_col_avec_planche == 0){ // Echec *pos_x = 0; return 1; } + if(last_col_avec_planche > 0){ + /// On peut déterminer un angle de la planche + float angle_planche; + angle_planche = atan2f(-(Results.distance_mm[0 + 8*row] - Results.distance_mm[last_col_avec_planche + 8*row]), + Results.distance_mm[0 + 8*row] * sinf(angles_VL53L8[0]) - Results.distance_mm[last_col_avec_planche + 8*row]* sinf(angles_VL53L8[last_col_avec_planche])); + *angle = angle_planche; + } + *pos_x = (float)distance_ref_mm * sinf(angles_VL53L8[last_col_avec_planche]); *pos_y = (float)distance_ref_mm; diff --git a/VL53L8_2024.h b/VL53L8_2024.h index 0ea33b8..22cc00e 100644 --- a/VL53L8_2024.h +++ b/VL53L8_2024.h @@ -1,9 +1,10 @@ #include "vl53l8cx_api.h" void VL53L8_init(VL53L8CX_Configuration * Dev); -void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results); +uint8_t VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results); +uint8_t capteur_actualise(VL53L8CX_Configuration * capteur, VL53L8CX_ResultsData * results); int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance); -int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y); -int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y); +int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y, float *angle); +int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y, float *angle); float VL53L8_get_old_min_distance(void); \ No newline at end of file diff --git a/main.c b/main.c index a53c923..efddd1e 100644 --- a/main.c +++ b/main.c @@ -23,7 +23,7 @@ #define COULEUR_JAUNE 0 #define OFFSET_CAPTEUR_GAUCHE_X_MM (-170) -#define OFFSET_CAPTEUR_DROIT_Y_MM (170) +#define OFFSET_CAPTEUR_DROIT_X_MM (170) // XIAO RP2040 #define SCK 2 @@ -179,25 +179,7 @@ uint8_t capteur_init(VL53L8CX_Configuration * capteur){ } -uint8_t capteur_actualise(VL53L8CX_Configuration * capteur, VL53L8CX_ResultsData * results){ - uint8_t isReady, status; - status = vl53l8cx_check_data_ready(capteur, &isReady); - if(status){ - printf("erreur:%d\n", status); - } - if(isReady) - { - //printf(">r%d:1\n",capteur->platform.address); - status = vl53l8cx_get_ranging_data(capteur, results); - if(status){ - printf("erreur:%d\n", status); - } - }else{ - //Serial.printf(">r:0\n"); - } - return isReady; -} void capteurs_affiche_donnees(VL53L8CX_ResultsData * results_gauche, VL53L8CX_ResultsData * results_droit){ uint8_t row, col; @@ -218,16 +200,17 @@ void capteurs_affiche_donnees(VL53L8CX_ResultsData * results_gauche, VL53L8CX_Re /// @brief Obtient la distance de l'obstacle le plus proche. /// @param -float gauche_planche_pos_x, gauche_planche_pos_y; -float droit_planche_pos_x, droit_planche_pos_y; +float gauche_planche_pos_x, gauche_planche_pos_y, gauche_planche_angle; +float droit_planche_pos_x, droit_planche_pos_y, droit_planche_angle; float planche_centre_x, planche_centre_y, planche_angle_rad; float taille_planche; +int echec; void gestion_VL53L8CX(void){ VL53L8CX_ResultsData results_gauche, results_droit; + int echec_gauche, echec_droit; float distance_obstacle; - int echec; capteur_init(&gauche); capteur_init(&droit); sleep_ms(100); @@ -239,24 +222,51 @@ void gestion_VL53L8CX(void){ isReady |= capteur_actualise( &gauche, &results_gauche); isReady |= capteur_actualise( &droit, &results_droit); if(isReady){ - //capteurs_affiche_donnees(&results_gauche, &results_droit); + capteurs_affiche_donnees(&results_gauche, &results_droit); //VL53L8_lecture( &gauche, &Results); echec = 0; - echec |= VL53L8_pos_planche_gauche(results_gauche, &gauche_planche_pos_x, &gauche_planche_pos_y); - echec |= VL53L8_pos_planche_droit(results_droit, &droit_planche_pos_x, &droit_planche_pos_y); + echec_gauche = VL53L8_pos_planche_gauche(results_gauche, &gauche_planche_pos_x, &gauche_planche_pos_y, &gauche_planche_angle); + echec_droit = VL53L8_pos_planche_droit(results_droit, &droit_planche_pos_x, &droit_planche_pos_y, &droit_planche_angle); - if(echec){ - ws2812_set(0x0F,0,0); - }else{ - ws2812_set(0,0x0F,0); - } - - droit_planche_pos_x = -droit_planche_pos_x + OFFSET_CAPTEUR_DROIT_Y_MM; + droit_planche_pos_x = -droit_planche_pos_x + OFFSET_CAPTEUR_DROIT_X_MM; gauche_planche_pos_x = -gauche_planche_pos_x + OFFSET_CAPTEUR_GAUCHE_X_MM; - planche_centre_x = (droit_planche_pos_x + gauche_planche_pos_x)/2; - planche_centre_y = (droit_planche_pos_y + gauche_planche_pos_y)/2; - planche_angle_rad = atan2f(droit_planche_pos_y - gauche_planche_pos_y, droit_planche_pos_x - gauche_planche_pos_x); + echec = echec_gauche + echec_droit; + + if(echec == 2){ + // Aucun capteur valide + ws2812_set(0x0F,0,0); + }else if(echec == 1){ + // Un seul capteur valide + if(echec_gauche == 0 && ! isnan(gauche_planche_angle)){ + // capteur gauche permet de déterminer la position de la planche + ws2812_set(0x0F,0x8,0); + planche_centre_x = gauche_planche_pos_x + 200 * cos(gauche_planche_angle); + planche_centre_y = gauche_planche_pos_y + 200 * sin(gauche_planche_angle); + planche_angle_rad = gauche_planche_angle; + + }else if(echec_droit == 0 && ! isnan(droit_planche_angle)){ + // capteur droit permet de déterminer la position de la planche + ws2812_set(0x0F,0x8,0); + planche_centre_x = droit_planche_pos_x - 200 * cos(droit_planche_angle); + planche_centre_y = droit_planche_pos_y - 200 * sin(droit_planche_angle); + planche_angle_rad = droit_planche_angle; + + }else{ + // On a un bout de la planche mais pas d'angle, c'est un echec + echec = 2; + ws2812_set(0x0F,0,0); + } + + }else{ + // 2 capteurs valides + ws2812_set(0,0x0F,0); + planche_centre_x = (droit_planche_pos_x + gauche_planche_pos_x)/2; + planche_centre_y = (droit_planche_pos_y + gauche_planche_pos_y)/2; + planche_angle_rad = atan2f(droit_planche_pos_y - gauche_planche_pos_y, droit_planche_pos_x - gauche_planche_pos_x); + } + + } affichage(); printf("\n"); @@ -274,14 +284,17 @@ void gestion_affichage(void){ } void affichage(void){ - /* + printf(">planche_g_pos_x:%f\n",gauche_planche_pos_x); printf(">planche_g_pos_y:%f\n",gauche_planche_pos_y); printf(">planche_d_pos_x:%f\n",droit_planche_pos_x); - printf(">planche_d_pos_y:%f\n",droit_planche_pos_y);*/ + printf(">planche_d_pos_y:%f\n",droit_planche_pos_y); printf(">planche_centre_x:%f\n",planche_centre_x); printf(">planche_centre_y:%f\n",planche_centre_y); printf(">planche_angle:%f\n",planche_angle_rad / M_PI * 180); + printf(">gauche_planche_angle:%f\n",gauche_planche_angle / M_PI * 180); + printf(">droit_planche_angle:%f\n",droit_planche_angle / M_PI * 180); + printf(">echec:%d\n", echec); } \ No newline at end of file