From 4216d8236d01aefdd1fd55a09696cf304f94b75d Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Sat, 15 Jun 2024 21:49:52 +0200
Subject: [PATCH] Match 5 + Match Amical - 5 PAMI

---
 VL53L8_2024.h |  1 +
 main.c        | 27 +++++++++++++++++++++------
 2 files changed, 22 insertions(+), 6 deletions(-)

diff --git a/VL53L8_2024.h b/VL53L8_2024.h
index f489b15..6bcaf0c 100644
--- a/VL53L8_2024.h
+++ b/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);
\ No newline at end of file
diff --git a/main.c b/main.c
index 7b41900..c81cae8 100644
--- a/main.c
+++ b/main.c
@@ -37,7 +37,7 @@ 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);
 
@@ -95,9 +95,12 @@ void main(void)
 
 	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
 
-	while(get_tirette());
+	while(get_tirette(identifiant_lire()));
 
-	//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();
 
@@ -211,8 +214,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){
@@ -258,9 +265,13 @@ void configure_trajet(int identifiant, int couleur){
 					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
 					Localisation_set(3000-1364, 2000-63, 0);
 					Trajectoire_bezier(&trajectoire, 3000-1363, 2000-63, 3000-550, 2000-63, 
-					3000-900, 600, 3000-0, 0, 0, 0);
+					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;
@@ -302,6 +313,10 @@ void configure_trajet(int identifiant, int couleur){
 					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;