From c3ac5fc52a348c2bf7527cdecec8c11e717b8cfb Mon Sep 17 00:00:00 2001 From: Samuel Date: Mon, 26 May 2025 11:02:37 +0200 Subject: [PATCH] =?UTF-8?q?R=C3=A9glage=20pour=20la=20Coupe=20de=20France?= =?UTF-8?q?=202025?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- VL53L8_2024.c | 36 +++++++++++++++++++++++++++--------- main.c | 31 ++++++++++++++++++++++++++++++- 2 files changed, 57 insertions(+), 10 deletions(-) diff --git a/VL53L8_2024.c b/VL53L8_2024.c index 464eb75..5d05494 100644 --- a/VL53L8_2024.c +++ b/VL53L8_2024.c @@ -196,7 +196,7 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){ old_min_distance = *distance; } -const int selected_row = 5; +const int selected_row = 3; /// @brief Renvoie la position de l'extrémité gauche de la planche /// @param Results Valeurs brutes du capteurs @@ -214,15 +214,16 @@ int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float *angle = NAN; // Contrôle si la planche est à droite for(col=7; col>=0; col--){ - printf("%d, ", Results.distance_mm[col + 8*3]); + printf("%d, ", Results.distance_mm[col + 8*selected_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){ + // Si c'est la première mesure, on la prend comme référence distance_ref_mm = Results.distance_mm[col + 8*row]; }else{ // Au moins une mesure proche @@ -235,7 +236,7 @@ int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float if(distance_ref_mm + 50 < Results.distance_mm[col + 8*row]){ last_col_avec_planche = col+2; // Double break; - col = 0; + col = -1; break; } } @@ -243,20 +244,28 @@ int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float } printf("\n"); row=selected_row; + + if(last_col_avec_planche == 8){ + last_col_avec_planche = 7; + } - if(last_col_avec_planche == 0 || last_col_avec_planche > 6){ + if(last_col_avec_planche == 0 || last_col_avec_planche > 7){ // Echec *pos_x = 0; if (no_detection){ return 2; } if(full_detection){ + float angle_planche; + angle_planche = atan2f(-(Results.distance_mm[0 + 8*row] - Results.distance_mm[7 + 8*row]), + Results.distance_mm[0 + 8*row] * sinf(angles_VL53L8[0]) - Results.distance_mm[7 + 8*row]* sinf(angles_VL53L8[7])); + *angle = angle_planche; return 3; } return 1; } - if(last_col_avec_planche < 7){ + 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]), @@ -278,7 +287,7 @@ int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float * int min_distance = 2000; int col, row; int distance_ref_mm = 500; - int last_col_avec_planche=0; + int last_col_avec_planche=9; int full_detection = 1; int no_detection = 1; @@ -314,15 +323,24 @@ int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float * } } printf("\n"); + + printf("last_col_avec_planche:%d\n", last_col_avec_planche); + if(last_col_avec_planche == -1){ + last_col_avec_planche = 0; + } row=selected_row; - if(last_col_avec_planche == 0 || last_col_avec_planche > 7){ + if( last_col_avec_planche > 7 ){ // Echec *pos_x = 0; if (no_detection){ return 2; } if(full_detection){ + float angle_planche; + angle_planche = atan2f(-(Results.distance_mm[0 + 8*row] - Results.distance_mm[7 + 8*row]), + Results.distance_mm[0 + 8*row] * sinf(angles_VL53L8[0]) - Results.distance_mm[7 + 8*row]* sinf(angles_VL53L8[7])); + *angle = angle_planche; return 3; } return 1; diff --git a/main.c b/main.c index 2c47bfd..4c9fc8b 100644 --- a/main.c +++ b/main.c @@ -26,6 +26,9 @@ #define OFFSET_CAPTEUR_GAUCHE_X_MM (-170) #define OFFSET_CAPTEUR_DROIT_X_MM (170) +#define FULL_DETECTION 3 +#define NO_DETECTION 2 + // XIAO RP2040 #define SCK 2 #define MISO 4 @@ -315,7 +318,31 @@ void gestion_VL53L8CX(void){ } }else if(echec == 1){ // Un seul capteur valide - if(echec_gauche == 0 && ! isnan(gauche_planche_angle)){ + if(echec_gauche == 0 && echec_droit == FULL_DETECTION){ + // capteur gauche permet de déterminer la position de la planche + // capteur droit permet de déterminer l'angle de la planche + ws2812_set(0, 0, 0xF); + planche_centre_x = gauche_planche_pos_x + 200 * cos(droit_planche_angle); + planche_centre_y = gauche_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] = 2; + + }else if(echec_gauche == FULL_DETECTION && echec_droit == 0){ + // capteur droit permet de déterminer la position de la planche + // capteur gauche permet de déterminer l'angle de la planche + ws2812_set(0, 0, 0xF); + planche_centre_x = droit_planche_pos_x - 200 * cos(gauche_planche_angle); + planche_centre_y = droit_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] = 2; + + }else 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); @@ -358,9 +385,11 @@ void gestion_VL53L8CX(void){ } + /* affichage(); printf("\n"); sleep_ms(150); + */ } }