Ajout du contrôle anti-panique à la fin du trajet

.vscode/settings.json

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

Asser_Moteurs.c

@@ -8,8 +8,8 @@
 float consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
 float commande_I[3]; // Terme integral
 
-void AsserMoteur_Init(){
-    QEI_init();
+void AsserMoteur_Init(int id){
+    QEI_init(id);
     Moteur_Init();
     for(unsigned int i =0; i< 2; i ++){
         commande_I[i]=0;

Asser_Moteurs.h

@@ -5,4 +5,4 @@ void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float consigne_mm_s);
 float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur);
 float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms);
 void AsserMoteur_Gestion(int step_ms);
-void AsserMoteur_Init();
+void AsserMoteur_Init(int);

Asser_Position.c

@@ -2,9 +2,11 @@
 #include "Commande_vitesse.h"
 #include "math.h"
 
-#define GAIN_P_POSITION 1
+#define GAIN_P_POSITION 15
 #define GAIN_P_ORIENTATION 10
 
+#define MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN)
+
 struct position_t position_maintien;
 
 /// @brief Asservissement de la position du robot. Les gains sont déterminés pour des positions très proches du robot
@@ -30,14 +32,14 @@ void Asser_Position(struct position_t position_consigne){
     delta_orientation_radian = Geometrie_get_angle_optimal(0. , delta_orientation_radian_tmp);
     
     // On asservi sur +PI/2 / -PI/2
-    /*if(delta_orientation_radian > (M_PI/2)){
+    if(delta_orientation_radian > (M_PI/2)){
         delta_orientation_radian -= M_PI;
         delta_avance_mm = -delta_avance_mm;
     }
     if(delta_orientation_radian < -(M_PI/2)){
         delta_orientation_radian += M_PI;
         delta_avance_mm = -delta_avance_mm;
-    }*/
+    }
     
 
 
@@ -60,4 +62,17 @@ void Asser_Position_set_Pos_Maintien(struct position_t position){
 
 void Asser_Position_maintien(){
     Asser_Position(position_maintien);
-}
+}
+
+float Asser_Position_get_erreur_angle(){
+    return delta_orientation_radian;
+}
+
+/// @brief Renvoi 1 si l'erreur d'angle supérieur au seuil
+/// @return 1 si panic, 0 si nominal
+int Asser_Position_panic_angle(){
+    if(delta_orientation_radian > MAX_ERREUR_ANGLE){
+        return 1;
+    }
+    return 0;
+}

Asser_Position.h

@@ -2,3 +2,5 @@
 void Asser_Position(struct position_t position_consigne);
 void Asser_Position_set_Pos_Maintien(struct position_t position);
 void Asser_Position_maintien();
+
+int Asser_Position_panic_angle();

Localisation.c

@@ -6,9 +6,9 @@
 
 struct position_t position;
 
-void Localisation_init(){
+void Localisation_init(int id){
     Temps_init();
-    QEI_init();
+    QEI_init(id);
     position.x_mm = 0;
     position.y_mm = 0;
     position.angle_radian = 0;

Localisation.h

@@ -2,7 +2,7 @@
 
 struct position_t Localisation_get(void);
 void Localisation_gestion();
-void Localisation_init();
+void Localisation_init(int);
 
 void Localisation_set(float x_mm, float y_mm, float angle_radian);
 void Localisation_set_x(float x_mm);

QEI.c

@@ -17,8 +17,10 @@
 // Nombre d'impulsions par tour de roue : 8000
 // Impulsion / mm : 42,44
 
-#define IMPULSION_PAR_MM (12.45f)
+#define IMPULSION_PAR_MM_50_1 (12.45f)
+#define IMPULSION_PAR_MM_30_1 (7.47f)
 
+float impulsion_par_mm;
 
 
 struct QEI_t QEI_A, QEI_B;
@@ -28,7 +30,7 @@ bool QEI_est_init = false;
 PIO pio_QEI = pio0;
 
 
-void QEI_init(){
+void QEI_init(int identifiant){
     // Initialisation des 3 modules QEI
     // Chaque module QEI sera dans une machine à état du PIO 0
     if(!QEI_est_init){
@@ -48,6 +50,10 @@ void QEI_init(){
         QEI_B.value=0;
         QEI_est_init=true;
     }
+    impulsion_par_mm = IMPULSION_PAR_MM_50_1;
+    if(identifiant == 0 || identifiant >= 4){
+        impulsion_par_mm = IMPULSION_PAR_MM_30_1;
+    }
 
 }
 
@@ -91,5 +97,5 @@ int QEI_get(enum QEI_name_t qei){
 /// @param  qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME)
 /// @return la distance parcourue en mm calculée lors du dernier appel de la function QEI_Update()
 float QEI_get_mm(enum QEI_name_t qei){
-    return ((float) QEI_get(qei)) / (float)IMPULSION_PAR_MM;
+    return ((float) QEI_get(qei)) / impulsion_par_mm;
 }

QEI.h

@@ -11,6 +11,6 @@ enum QEI_name_t{
 extern struct QEI_t QEI_A, QEI_B, QEI_C;
 
 void QEI_update(void);
-void QEI_init(void);
+void QEI_init(int);
 int QEI_get(enum QEI_name_t qei);
 float QEI_get_mm(enum QEI_name_t qei);

Trajet.c

@@ -25,9 +25,9 @@ const float distance_pas_obstacle = 2000;
 float vitesse_max_contrainte_obstacle;
 
 /// @brief Initialise le module Trajet. A appeler en phase d'initilisation
-void Trajet_init(){
+void Trajet_init(int id){
     Temps_init();
-    AsserMoteur_Init();
+    AsserMoteur_Init(id);
     abscisse = 0;
     vitesse_mm_s = 0;
     position_mm = 0;
@@ -141,16 +141,16 @@ float Trajet_calcul_vitesse(float pas_de_temps_s){
         vitesse_max_contrainte = 0;
     }
 
-    /*distance_contrainte_obstacle = Trajet_get_obstacle_mm();
+    distance_contrainte_obstacle = Trajet_get_obstacle_mm();
     if(distance_contrainte_obstacle != DISTANCE_INVALIDE){
         vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
         if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){
             vitesse_max_contrainte = vitesse_max_contrainte_obstacle;
         }
-    }*/
-    if(Trajet_get_obstacle_mm() < 50){
+    }/*
+    if((Trajet_get_obstacle_mm() != DISTANCE_INVALIDE) && (Trajet_get_obstacle_mm() < 50)){
         vitesse = 0;
-    }
+    }*/
     
 
     // Selection de la vitesse la plus faible

Trajet.h

@@ -11,6 +11,7 @@ enum etat_trajet_t{
 
 // Vitesse et acceleration pour translation pure (en mm/s et mm/s²)
 #define TRAJECT_CONFIG_RAPIDE 300, 1200
+#define TRAJECT_CONFIG_RAPIDE_ROUGE 500, 1200
 // Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²)
 #define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500
 // Vitesse et acceleration - standard (en mm et mm/s²)
@@ -20,7 +21,7 @@ enum etat_trajet_t{
 
 extern const float distance_pas_obstacle;
 
-void Trajet_init();
+void Trajet_init(int);
 void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss);
 void Trajet_debut_trajectoire(struct trajectoire_t trajectoire);
 enum etat_trajet_t Trajet_avance(float temps_s);

VL53L8_2024.c

@@ -14,6 +14,8 @@ int masque[64]={
 	195,200,202,206,213,200,200,202
 };
 
+float old_min_distance;
+
 void VL53L8_init(VL53L8CX_Configuration * Dev){
 	uint8_t status, isAlive, isReady, i;
 
@@ -152,7 +154,7 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){
 	int min_distance = 2000;
 	int col, row;
 	for(col=0; col<8; col++){
-		for(row=4; row<=6; row++){
+		for(row=0; row<=3; row++){
 			if(min_distance >  Results.distance_mm[col + 8*row]){
 				min_distance = Results.distance_mm[col + 8*row];
 			}
@@ -162,7 +164,12 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){
 	if(min_distance-50 > 0){
 		*distance = min_distance-50;
 	}
+	old_min_distance = *distance;
 	
 	
 }
 
+float VL53L8_get_old_min_distance(){
+	return 	old_min_distance;
+}
+

VL53L8_2024.h

@@ -4,3 +4,4 @@ void VL53L8_init(VL53L8CX_Configuration * Dev);
 void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results);
 int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance);
 
+float VL53L8_get_old_min_distance(void);

main.c

@@ -37,13 +37,13 @@ uint16_t tension_batterie_lire(void);
 void identifiant_init(void);
 uint identifiant_lire(void);
 
-int get_tirette(void);
+int get_tirette(int);
 int get_couleur(void);
 void configure_trajet(int identifiant, int couleur);
 
 void gestion_VL53L8CX(void);
 
-uint32_t step_ms=1;
+const uint32_t step_ms=1;
 float distance1_mm=0, distance2_mm=0;
 
 // DEBUG
@@ -56,6 +56,7 @@ void main(void)
 {
 	int ledpower = 500;
 	VL53L8CX_ResultsData Results;
+	bool fin_match = false;
 
 
 
@@ -63,8 +64,8 @@ void main(void)
 	Temps_init();
 	//tension_batterie_init();
 	identifiant_init();
-	Localisation_init();
-	Trajet_init();
+	Localisation_init(identifiant_lire());
+	Trajet_init(identifiant_lire());
 	i2c_maitre_init();
 
 	
@@ -82,7 +83,8 @@ void main(void)
 	
 	//multicore_launch_core1(gestion_affichage);
 	multicore_launch_core1(gestion_VL53L8CX);
-	sleep_ms(4000);
+	sleep_ms(5000);
+	printf("Demarrage...\n");
 	
 	
 
@@ -94,16 +96,20 @@ void main(void)
 
 	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
 
-	while(get_tirette());
+	while(get_tirette(identifiant_lire()));
+	gpio_put(LED1PIN, 0);
 
-	// sleep_ms(90000);
+	// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
+	if(identifiant_lire() == 3){
+		sleep_ms(90000);
+	}
 
 	temps_depart_ms = Temps_get_temps_ms();
 
 	while(1){
 		
 		// Fin du match 
-		if((Temps_get_temps_ms() -temps_depart_ms) >10000 ){
+		if((Temps_get_temps_ms() -temps_depart_ms) >10000 || (fin_match == 1)){
 			Moteur_Stop();
 			while(1);
 		}
@@ -112,16 +118,16 @@ void main(void)
 			if(temps_ms % step_ms == 0){
 				QEI_update();
 				Localisation_gestion();
-				if(Trajet_get_obstacle_mm() < 50){
-					Moteur_Stop();
+				
+				if(etat_trajet != TRAJET_TERMINE){
+					etat_trajet = Trajet_avance((float)step_ms/1000.);
 				}else{
-					if(etat_trajet != TRAJET_TERMINE){
-						etat_trajet = Trajet_avance((float)step_ms/1000.);
-					}else{
-						Asser_Position_maintien();
+					Asser_Position_maintien();
+					if(Asser_Position_panic_angle()){
+						fin_match=1;
 					}
-					AsserMoteur_Gestion(step_ms);
 				}
+				AsserMoteur_Gestion(step_ms);
 			}
 		}
 
@@ -173,7 +179,7 @@ void affichage(void){
 	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", get_couleur(), identifiant_lire());
+	printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette(identifiant_lire()));
 }
 
 void tension_batterie_init(void){
@@ -213,8 +219,12 @@ void identifiant_init(){
 	gpio_set_dir(COULEUR_PIN, GPIO_IN);
 }
 
-int get_tirette(void){
-	return !gpio_get(TIRETTE_PIN);
+int get_tirette(int id){
+	if(id == 3){
+		return !gpio_get(TIRETTE_PIN);
+	}
+	return (VL53L8_get_old_min_distance() <50);
+	
 }
 
 int get_couleur(void){
@@ -230,28 +240,43 @@ uint identifiant_lire(){
 void configure_trajet(int identifiant, int couleur){
 	
 	struct trajectoire_t trajectoire;
+	Trajet_config(TRAJECT_CONFIG_RAPIDE);
 
 	switch(couleur){
 		case COULEUR_JAUNE:
 			switch (identifiant)
 			{
 				case 0:
+					Localisation_set(3000-1249, 2000-63, 0);
+					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
+					3000-750, 1400, 3000-750, 2100, 0, 0);
 					break;
 				case 1:
-					Localisation_set(1465, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 3000-1465, 2000-63, 3000-1260, 2000-63, 
-					3000-600, 1400, 3000-0, 2000, M_PI, M_PI);
+					Localisation_set(3000-1249, 2000-63, 0);
+					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
+					3000-750, 1400, 3000-750, 2100, 0, 0);
 					break;
 				case 2:
+					Localisation_set(3000-1245, 2000-63, 0);
+					Trajectoire_bezier(&trajectoire, 3000-1244, 2000-63, 3000-950, 2000-63, 
+					3000-540, 1400, 3100, 1400, M_PI, M_PI);
 					break;
 				case 3:
-					Localisation_set(3000 - 1130, 2000-63, 0);
+					Localisation_set(3000-1130, 2000-63, 0);
 					Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63, 
-					3000-606, 2000-590, 3000-225, 2000-225, 0, 0);
+					3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI);
 					break;
 				case 4:
+					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
+					Localisation_set(3000-1364, 2000-63, 0);
+					Trajectoire_bezier(&trajectoire, 3000-1363, 2000-63, 3000-550, 2000-63, 
+					2700-900, 600, 2700-0, 0, 0, 0);
 					break;
 				case 5:
+					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
+					Localisation_set(3000-1450, 2000-63, 0);
+					Trajectoire_bezier(&trajectoire, 3000-1449, 2000-63, 3000-675, 2000-63, 
+					3000-930, 970, 0, 1200, -M_PI / 2., M_PI);
 					break;
 				case 6:
 					break;
@@ -267,22 +292,36 @@ void configure_trajet(int identifiant, int couleur){
 			switch (identifiant)
 			{
 				case 0:
+					Localisation_set(1249, 2000-63, M_PI);
+					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
+					750, 1400, 750, 2100, M_PI, M_PI);
 					break;
 				case 1:
-					Localisation_set(1465, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1465, 2000-63, 1260, 2000-63, 
-					600, 1400, 0, 2000, M_PI, M_PI);
+					Localisation_set(1249, 2000-63, M_PI);
+					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
+					750, 1400, 750, 2100, M_PI, M_PI);
 					break;
 				case 2:
+					Localisation_set(1245, 2000-63, M_PI);
+					Trajectoire_bezier(&trajectoire, 1244, 2000-63, 950, 2000-63, 
+					540, 1400, -100, 1400, M_PI, M_PI);
 					break;
 				case 3:
-					Localisation_set(1130, 2000-63, M_PI);
+					Localisation_set(1121, 2000-63, M_PI);
 					Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, 
 					606, 2000-590, 225, 2000-225, M_PI, M_PI);
 					break;
 				case 4:
+					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
+					Localisation_set(1364, 2000-63, M_PI);
+					Trajectoire_bezier(&trajectoire, 1363, 2000-63, 550, 2000-63, 
+					900, 600, 0, 0, -M_PI / 2., M_PI);
 					break;
 				case 5:
+					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
+					Localisation_set(1450, 2000-63, M_PI);
+					Trajectoire_bezier(&trajectoire, 1449, 2000-63, 675, 2000-63, 
+					930, 970, 3000, 1200, -M_PI / 2., M_PI);
 					break;
 				case 6:
 					break;
@@ -295,10 +334,6 @@ void configure_trajet(int identifiant, int couleur){
 			break;
 	}
 	
-	
-	
-
-	Trajet_config(TRAJECT_CONFIG_RAPIDE);
 	Trajet_debut_trajectoire(trajectoire);
 
 }