Keuronde/Gradin_2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Détection de la position de la planche aussi avec 1 seul capteur valide

Browse Source
This commit is contained in:
Samuel 2025-03-12 11:19:16 +01:00
parent 5c88d4ee81
commit 675aae3dd0
3 changed files with 111 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
VL53L8_2024.c
View File

@ -120,14 +120,16 @@ void VL53L8_init(VL53L8CX_Configuration * Dev){
} }
/// @brief Lit le capteur, filtre les données avec les status renvoyés et le masque de distance défini plus haut
void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results){ /// @param Dev Capteur à lire
uint8_t error; /// @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); error = vl53l8cx_get_ranging_data(Dev, Results);
if(error){ if(error){
printf("Error VL53L8\n"); return error;
VL53L8_init(Dev); }
}
/* As the sensor is set in 4x4 mode by default, we have a total /* 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 // Filtrage des données
for(int row=0; row < 8; row++){ for(int row=0; row < 8; row++){
for(int col=0; col < 8; col++){ 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 // Seul status 100% valid
if(Results->target_status[col+ 8*row] != 5){ if(Results->target_status[col+ 8*row] != 5){
Results->distance_mm[col+ 8*row] = 3000; 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 /// @brief Contrôle si des données sont disponibles, si oui, lit le données
/* /// @param capteur capteur à lire
printf(">distance:"); /// @param results stockage des résultats
for(int row=0; row < 8; row++){ /// @return 1 si des données ont été lues
for(int col=0; col < 8; col++){ uint8_t capteur_actualise(VL53L8CX_Configuration * capteur, VL53L8CX_ResultsData * results){
printf("%d,", Results->distance_mm[col+ 8*row]); 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){ 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 Results Valeurs brutes du capteurs
/// @param pos_x position de la planche /// @param pos_x position de la planche
/// @return 0 sucess, 1 failed /// @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; uint16_t mesure1, mesure2;
int min_distance = 2000; int min_distance = 2000;
int col, row; int col, row;
int distance_ref_mm = 500; int distance_ref_mm = 500;
int last_col_avec_planche=0; int last_col_avec_planche=0;
*angle = NAN;
// Contrôle si la planche est à droite // Contrôle si la planche est à droite
for(col=7; col>=0; col--){ for(col=7; col>=0; col--){
printf("%d, ", Results.distance_mm[col + 8*3]); 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 // 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]){ if(distance_ref_mm + 50 < Results.distance_mm[col + 8*row]){
last_col_avec_planche = col; last_col_avec_planche = col+2;
// Double break; // Double break;
col = 0; col = 0;
break; break;
@ -219,12 +231,21 @@ int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float
} }
} }
printf("\n"); printf("\n");
row=3;
if(last_col_avec_planche == 0){ if(last_col_avec_planche == 0){
// Echec // Echec
*pos_x = 0; *pos_x = 0;
return 1; 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_x = (float)distance_ref_mm * sinf(angles_VL53L8[last_col_avec_planche]);
*pos_y = (float)distance_ref_mm; *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 Results Valeurs brutes du capteurs
/// @param pos_x position de la planche /// @param pos_x position de la planche
/// @return 0 sucess, 1 failed /// @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; uint16_t mesure1, mesure2;
int min_distance = 2000; int min_distance = 2000;
int col, row; int col, row;
int distance_ref_mm = 500; int distance_ref_mm = 500;
int last_col_avec_planche=0; int last_col_avec_planche=0;
*angle = NAN;
// Controle si la planche est à gauche // Controle si la planche est à gauche
for(col=0; col<8; col++){ for(col=0; col<8; col++){
printf("%d,", Results.distance_mm[col + 8*3]); 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 // 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]){ if(distance_ref_mm + 50 < Results.distance_mm[col + 8*row]){
last_col_avec_planche = col; last_col_avec_planche = col-2;
// Double break; // Double break;
col = 9; col = 9;
break; break;
@ -264,12 +287,21 @@ int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float *
} }
} }
printf("\n"); printf("\n");
row=3;
if(last_col_avec_planche == 0){ if(last_col_avec_planche == 0){
// Echec // Echec
*pos_x = 0; *pos_x = 0;
return 1; 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_x = (float)distance_ref_mm * sinf(angles_VL53L8[last_col_avec_planche]);
*pos_y = (float)distance_ref_mm; *pos_y = (float)distance_ref_mm;

7
VL53L8_2024.h
View File

@ -1,9 +1,10 @@
#include "vl53l8cx_api.h" #include "vl53l8cx_api.h"
void VL53L8_init(VL53L8CX_Configuration * Dev); 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_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_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); int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y, float *angle);
float VL53L8_get_old_min_distance(void); float VL53L8_get_old_min_distance(void);

87
main.c
View File

@ -23,7 +23,7 @@
#define COULEUR_JAUNE 0 #define COULEUR_JAUNE 0
#define OFFSET_CAPTEUR_GAUCHE_X_MM (-170) #define OFFSET_CAPTEUR_GAUCHE_X_MM (-170)
#define OFFSET_CAPTEUR_DROIT_Y_MM (170) #define OFFSET_CAPTEUR_DROIT_X_MM (170)
// XIAO RP2040 // XIAO RP2040
#define SCK 2 #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){ void capteurs_affiche_donnees(VL53L8CX_ResultsData * results_gauche, VL53L8CX_ResultsData * results_droit){
uint8_t row, col; 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. /// @brief Obtient la distance de l'obstacle le plus proche.
/// @param /// @param
float gauche_planche_pos_x, gauche_planche_pos_y; float gauche_planche_pos_x, gauche_planche_pos_y, gauche_planche_angle;
float droit_planche_pos_x, droit_planche_pos_y; float droit_planche_pos_x, droit_planche_pos_y, droit_planche_angle;
float planche_centre_x, planche_centre_y, planche_angle_rad; float planche_centre_x, planche_centre_y, planche_angle_rad;
float taille_planche; float taille_planche;
int echec;
void gestion_VL53L8CX(void){ void gestion_VL53L8CX(void){
VL53L8CX_ResultsData results_gauche, results_droit; VL53L8CX_ResultsData results_gauche, results_droit;
int echec_gauche, echec_droit;
float distance_obstacle; float distance_obstacle;
int echec;
capteur_init(&gauche); capteur_init(&gauche);
capteur_init(&droit); capteur_init(&droit);
sleep_ms(100); sleep_ms(100);
@ -239,24 +222,51 @@ void gestion_VL53L8CX(void){
isReady |= capteur_actualise( &gauche, &results_gauche); isReady |= capteur_actualise( &gauche, &results_gauche);
isReady |= capteur_actualise( &droit, &results_droit); isReady |= capteur_actualise( &droit, &results_droit);
if(isReady){ if(isReady){
//capteurs_affiche_donnees(&results_gauche, &results_droit); capteurs_affiche_donnees(&results_gauche, &results_droit);
//VL53L8_lecture( &gauche, &Results); //VL53L8_lecture( &gauche, &Results);
echec = 0; echec = 0;
echec |= VL53L8_pos_planche_gauche(results_gauche, &gauche_planche_pos_x, &gauche_planche_pos_y); echec_gauche = VL53L8_pos_planche_gauche(results_gauche, &gauche_planche_pos_x, &gauche_planche_pos_y, &gauche_planche_angle);
echec |= VL53L8_pos_planche_droit(results_droit, &droit_planche_pos_x, &droit_planche_pos_y); echec_droit = VL53L8_pos_planche_droit(results_droit, &droit_planche_pos_x, &droit_planche_pos_y, &droit_planche_angle);
if(echec){ droit_planche_pos_x = -droit_planche_pos_x + OFFSET_CAPTEUR_DROIT_X_MM;
ws2812_set(0x0F,0,0);
}else{
ws2812_set(0,0x0F,0);
}
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; 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; echec = echec_gauche + echec_droit;
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); 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(); affichage();
printf("\n"); printf("\n");
@ -274,14 +284,17 @@ void gestion_affichage(void){
} }
void affichage(void){ void affichage(void){
/*
printf(">planche_g_pos_x:%f\n",gauche_planche_pos_x); printf(">planche_g_pos_x:%f\n",gauche_planche_pos_x);
printf(">planche_g_pos_y:%f\n",gauche_planche_pos_y); 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_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_x:%f\n",planche_centre_x);
printf(">planche_centre_y:%f\n",planche_centre_y); printf(">planche_centre_y:%f\n",planche_centre_y);
printf(">planche_angle:%f\n",planche_angle_rad / M_PI * 180); 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);
} }