From 2e085ded390a3ac3ccaabda96faf16d1e9f7bf6f Mon Sep 17 00:00:00 2001 From: Samuel Date: Tue, 11 Mar 2025 15:05:08 +0100 Subject: [PATCH] =?UTF-8?q?Rep=C3=A9rage=20du=20centre=20de=20la=20plache?= =?UTF-8?q?=20et=20de=20son=20orientaiton?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 3 +- CMakeLists.txt | 2 -- VL53L8CX_ULD_API/src/vl53l8cx_api.c | 4 +-- VL53L8_2024.c | 55 +++++++++++++++++++++++++++-- VL53L8_2024.h | 3 +- main.c | 54 +++++++++++++++++----------- 6 files changed, 91 insertions(+), 30 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index e3b121c..353278f 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -9,6 +9,7 @@ "trajet.h": "c", "trajectoire.h": "c", "compare": "c", - "asser_position.h": "c" + "asser_position.h": "c", + "vl53l8cx_api.h": "c" } } \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index cb6362c..4f47cd6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,10 +10,8 @@ set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR}) set(PICO_BOARD seeed_xiao_rp2040) - pico_sdk_init() - add_executable(VL53L8X_Gradin Geometrie.c i2c_maitre.c diff --git a/VL53L8CX_ULD_API/src/vl53l8cx_api.c b/VL53L8CX_ULD_API/src/vl53l8cx_api.c index eda29fb..f0303be 100644 --- a/VL53L8CX_ULD_API/src/vl53l8cx_api.c +++ b/VL53L8CX_ULD_API/src/vl53l8cx_api.c @@ -51,7 +51,7 @@ static uint8_t _vl53l8cx_poll_for_answer( { timeout++; } - printf("expected %d / %d\n", expected_value, (p_dev->temp_buffer[pos] & mask )); + //printf("expected %d / %d\n", expected_value, (p_dev->temp_buffer[pos] & mask )); }while ((p_dev->temp_buffer[pos] & mask) != expected_value); return status; @@ -238,7 +238,7 @@ uint8_t vl53l8cx_is_alive( if((device_id == (uint8_t)0xF0) && (revision_id == (uint8_t)0x0C)) { *p_is_alive = 1; - printf("device_id:%d,revision_id:%d\n",device_id, revision_id); + //printf("device_id:%d,revision_id:%d\n",device_id, revision_id); } else { diff --git a/VL53L8_2024.c b/VL53L8_2024.c index fb5c593..d7e836b 100644 --- a/VL53L8_2024.c +++ b/VL53L8_2024.c @@ -2,6 +2,10 @@ #include #include +#ifndef M_PI +#define M_PI 3.314159 +#endif + #define DEGREE_EN_RADIAN (M_PI / 180.) #define PLAGE_ANGLE_DEGREE (40*DEGREE_EN_RADIAN) @@ -182,15 +186,60 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){ old_min_distance = *distance; } -/// @brief Renvoie la position de la planche +/// @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 -int VL53L8_pos_planche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y){ +int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y){ uint16_t mesure1, mesure2; int min_distance = 2000; int col, row; - int distance_ref_mm = 200; + int distance_ref_mm = 500; + int last_col_avec_planche=0; + // 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++){ + // 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{ + // 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]; + } + // 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; + // Double break; + col = 0; + break; + } + } + } + } + printf("\n"); + + if(last_col_avec_planche == 0){ + // Echec + *pos_x = 0; + return 1; + } + *pos_x = (float)distance_ref_mm * sinf(angles_VL53L8[last_col_avec_planche]); + *pos_y = (float)distance_ref_mm; + + return 0; +} + +/// @brief Renvoie la position de l'extrémité droite de la planche +/// @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){ + uint16_t mesure1, mesure2; + int min_distance = 2000; + int col, row; + int distance_ref_mm = 500; int last_col_avec_planche=0; // Controle si la planche est à gauche for(col=0; col<8; col++){ diff --git a/VL53L8_2024.h b/VL53L8_2024.h index 4f6a7df..0ea33b8 100644 --- a/VL53L8_2024.h +++ b/VL53L8_2024.h @@ -3,6 +3,7 @@ void VL53L8_init(VL53L8CX_Configuration * Dev); void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results); int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance); -int VL53L8_pos_planche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y); +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); float VL53L8_get_old_min_distance(void); \ No newline at end of file diff --git a/main.c b/main.c index e39d57b..4255bf9 100644 --- a/main.c +++ b/main.c @@ -21,6 +21,9 @@ #define COULEUR_BLEU 1 #define COULEUR_JAUNE 0 +#define OFFSET_CAPTEUR_GAUCHE_X_MM (-170) +#define OFFSET_CAPTEUR_DROIT_Y_MM (170) + // XIAO RP2040 #define SCK 2 #define MISO 4 @@ -182,7 +185,7 @@ uint8_t capteur_actualise(VL53L8CX_Configuration * capteur, VL53L8CX_ResultsData } if(isReady) { - printf(">r%d:1\n",capteur->platform.address); + //printf(">r%d:1\n",capteur->platform.address); status = vl53l8cx_get_ranging_data(capteur, results); if(status){ printf("erreur:%d\n", status); @@ -212,10 +215,16 @@ void capteurs_affiche_donnees(VL53L8CX_ResultsData * results_gauche, VL53L8CX_Re /// @brief Obtient la distance de l'obstacle le plus proche. /// @param -float planche_pos_x, planche_pos_y; +float gauche_planche_pos_x, gauche_planche_pos_y; +float droit_planche_pos_x, droit_planche_pos_y; + +float planche_centre_x, planche_centre_y, planche_angle_rad; +float taille_planche; + void gestion_VL53L8CX(void){ VL53L8CX_ResultsData results_gauche, results_droit; float distance_obstacle; + int echec; capteur_init(&gauche); capteur_init(&droit); sleep_ms(100); @@ -227,12 +236,21 @@ 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); - VL53L8_min_distance(results_gauche, &distance_obstacle); - VL53L8_pos_planche(results_gauche, &planche_pos_x, &planche_pos_y); + 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); + + droit_planche_pos_x = -droit_planche_pos_x + OFFSET_CAPTEUR_DROIT_Y_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); } - //affichage(); + affichage(); + printf("\n"); sleep_ms(150); } } @@ -247,20 +265,14 @@ void gestion_affichage(void){ } void affichage(void){ - printf(">planche_pox_x:%f\n",planche_pos_x); - printf(">planche_pox_y:%f\n",planche_pos_y); -/*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) ); - printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/ - /*printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian); - printf(">distance_obstacle:%f\n",Trajet_get_obstacle_mm()); + /* + 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_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(">abscisse:%f\n",abscisse); - - struct position_t position_actuelle; - position_actuelle = Localisation_get(); - printf(">delta_orientation_radian:%.2f\n>angle_delta:%.2f\n",delta_orientation_radian, atan2f(delta_y_mm, delta_x_mm)); - printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm); - printf(">con_x:%.2f\n>con_y:%.2f\n", point.point_xy.x, point.point_xy.y); - - printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette(identifiant_lire()));*/ } \ No newline at end of file