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

Réglage pour la Coupe de France 2025

Browse Source
This commit is contained in:
Samuel 2025-05-26 11:02:37 +02:00
parent d1a95b02a3
commit c3ac5fc52a
2 changed files with 57 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
VL53L8_2024.c
View File

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

31
main.c
View File

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