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

Tiens compte du cas où le capteur voit le gradin sans voir le bord

Browse Source
This commit is contained in:
Samuel 2025-04-27 16:09:53 +02:00
parent 5da5b33650
commit d1a95b02a3
2 changed files with 62 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
VL53L8_2024.c
View File

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

27
main.c
View File

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