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

VL53L8_2024.c

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

main.c

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