From d1a95b02a32a877025fccf1f71b4c46408203cff Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 27 Apr 2025 16:09:53 +0200 Subject: [PATCH] =?UTF-8?q?Tiens=20compte=20du=20cas=20o=C3=B9=20le=20capt?= =?UTF-8?q?eur=20voit=20le=20gradin=20sans=20voir=20le=20bord?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- VL53L8_2024.c | 48 +++++++++++++++++++++++++++++++++++++++++------- main.c | 27 +++++++++++++++++++++------ 2 files changed, 62 insertions(+), 13 deletions(-) diff --git a/VL53L8_2024.c b/VL53L8_2024.c index 9308448..464eb75 100644 --- a/VL53L8_2024.c +++ b/VL53L8_2024.c @@ -196,26 +196,37 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){ old_min_distance = *distance; } +const int selected_row = 5; + /// @brief Renvoie la position de l'extrémité gauche de la planche /// @param Results Valeurs brutes du capteurs /// @param pos_x position de la planche -/// @return 0 sucess, 1 failed +/// @return 0 sucess, 1 failed, 2 failed: no detection, 3 failed: full detection 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; + int full_detection = 1; + int no_detection = 1; *angle = NAN; // Contrôle si la planche est à droite for(col=7; col>=0; col--){ printf("%d, ", Results.distance_mm[col + 8*3]); - for(row=3; row<=3; row++){ + for(row=selected_row; row<=selected_row; row++){ // SI c'est la première mesure, on la prend comme référence + if(Results.distance_mm[col + 8*row] > 500){ + // Au moins une mesure loin + full_detection = 0; + } + if(distance_ref_mm >= 500){ distance_ref_mm = Results.distance_mm[col + 8*row]; }else{ + // Au moins une mesure proche + no_detection = 0; // Si on est plus proche que la distance de référence, on la prend comme référence. if(distance_ref_mm > Results.distance_mm[col + 8*row]){ distance_ref_mm = Results.distance_mm[col + 8*row]; @@ -231,11 +242,17 @@ int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float } } printf("\n"); - row=3; + row=selected_row; - if(last_col_avec_planche == 0){ + if(last_col_avec_planche == 0 || last_col_avec_planche > 6){ // Echec *pos_x = 0; + if (no_detection){ + return 2; + } + if(full_detection){ + return 3; + } return 1; } @@ -262,16 +279,26 @@ int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float * int col, row; int distance_ref_mm = 500; int last_col_avec_planche=0; + int full_detection = 1; + int no_detection = 1; *angle = NAN; // Controle si la planche est à gauche for(col=0; col<8; col++){ printf("%d,", Results.distance_mm[col + 8*3]); - for(row=3; row<=3; row++){ + for(row=selected_row; row<=selected_row; row++){ + + if(Results.distance_mm[col + 8*row] > 500){ + // Au moins une mesure loin + full_detection = 0; + } + // SI c'est la première mesure, on la prend comme référence if(distance_ref_mm >= 500){ distance_ref_mm = Results.distance_mm[col + 8*row]; }else{ + // Au moins une mesure proche + no_detection = 0; // Si on est plus proche que la distance de référence, on la prend comme référence. if(distance_ref_mm > Results.distance_mm[col + 8*row]){ distance_ref_mm = Results.distance_mm[col + 8*row]; @@ -287,13 +314,20 @@ int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float * } } printf("\n"); - row=3; + row=selected_row; - if(last_col_avec_planche == 0){ + if(last_col_avec_planche == 0 || last_col_avec_planche > 7){ // Echec *pos_x = 0; + if (no_detection){ + return 2; + } + if(full_detection){ + return 3; + } return 1; } + if(last_col_avec_planche > 0){ /// On peut déterminer un angle de la planche float angle_planche; diff --git a/main.c b/main.c index c4c136a..2c47bfd 100644 --- a/main.c +++ b/main.c @@ -294,33 +294,48 @@ void gestion_VL53L8CX(void){ echec = 0; 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); + printf("g:%d,d:%d\n", echec_gauche, echec_droit ); 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; - echec = echec_gauche + echec_droit; + echec = (echec_gauche > 0) + (echec_droit > 0); if(echec == 2){ - // Aucun capteur valide - ws2812_set(0x0F,0,0); - context.mem[0] = 0; + if(echec_gauche == 2 && echec_droit == 3){ + ws2812_set(0, 0, 0xF); + context.mem[0] = 3; + }else if(echec_gauche == 3 && echec_droit == 2){ + ws2812_set(0, 0, 0xF); + context.mem[0] = 4; + }else{ + // Aucun capteur valide + ws2812_set(0x0F,0,0); + context.mem[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); - context.mem[0] = 1; 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; + i2c_envoi_32bits(planche_centre_x, 1); + i2c_envoi_32bits(planche_centre_y, 5); + i2c_envoi_32bits((int)(planche_angle_rad * 1000), 9); + context.mem[0] = 1; }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); - context.mem[0] = 1; 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; + i2c_envoi_32bits(planche_centre_x, 1); + i2c_envoi_32bits(planche_centre_y, 5); + i2c_envoi_32bits((int)(planche_angle_rad * 1000), 9); + context.mem[0] = 1; }else{ // On a un bout de la planche mais pas d'angle, c'est un echec