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

.vscode/settings.json

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

CMakeLists.txt

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

VL53L8CX_ULD_API/src/vl53l8cx_api.c

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

VL53L8_2024.c

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

VL53L8_2024.h

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

main.c

@@ -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);
+			echec = 0;
-			VL53L8_pos_planche(results_gauche, &planche_pos_x, &planche_pos_y);
+			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(">planche_g_pos_x:%f\n",gauche_planche_pos_x);
-/*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) );
+		printf(">planche_g_pos_y:%f\n",gauche_planche_pos_y);
-	printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/
+		printf(">planche_d_pos_x:%f\n",droit_planche_pos_x);
-	/*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(">planche_d_pos_y:%f\n",droit_planche_pos_y);*/
-	printf(">distance_obstacle:%f\n",Trajet_get_obstacle_mm());
+		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()));*/
 }