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

Repérage du centre de la plache et de son orientaiton

Browse Source
This commit is contained in:
Samuel 2025-03-11 15:05:08 +01:00
parent 6876eab946
commit 2e085ded39
6 changed files with 91 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.vscode/settings.json vendored
View File

@ -9,6 +9,7 @@
"trajet.h": "c",
"trajectoire.h": "c",
"compare": "c",
"asser_position.h": "c"
"asser_position.h": "c",
"vl53l8cx_api.h": "c"
}
}

2
CMakeLists.txt
View File

@ -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

4
VL53L8CX_ULD_API/src/vl53l8cx_api.c
View File

@ -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
{

55
VL53L8_2024.c
View File

@ -2,6 +2,10 @@
#include <stdio.h>
#include <math.h>
#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++){

3
VL53L8_2024.h
View File

@ -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);

52
main.c
View File

@ -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()));*/
}