Réglage pour la Coupe de France 2025
This commit is contained in:
parent
d1a95b02a3
commit
c3ac5fc52a
@ -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
31
main.c
@ -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);
|
||||
*/
|
||||
}
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user