From b0e750106d7c4084b9bce7ac1499fcf81d405222 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Sat, 22 Jun 2024 19:04:37 +0200
Subject: [PATCH] =?UTF-8?q?Ajout=20du=20contr=C3=B4le=20anti-panique=20?=
 =?UTF-8?q?=C3=A0=20la=20fin=20du=20trajet?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Asser_Position.c | 17 ++++++++++++++++-
 Asser_Position.h |  2 ++
 main.c           |  6 +++++-
 3 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/Asser_Position.c b/Asser_Position.c
index 8289949..74870ff 100644
--- a/Asser_Position.c
+++ b/Asser_Position.c
@@ -5,6 +5,8 @@
 #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
@@ -60,4 +62,17 @@ void Asser_Position_set_Pos_Maintien(struct position_t position){
 
 void Asser_Position_maintien(){
     Asser_Position(position_maintien);
-}
\ No newline at end of file
+}
+
+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;
+}
diff --git a/Asser_Position.h b/Asser_Position.h
index d728db3..aa2e802 100644
--- a/Asser_Position.h
+++ b/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();
diff --git a/main.c b/main.c
index 64f86e7..cedc1c1 100644
--- a/main.c
+++ b/main.c
@@ -56,6 +56,7 @@ void main(void)
 {
 	int ledpower = 500;
 	VL53L8CX_ResultsData Results;
+	bool fin_match = false;
 
 
 
@@ -108,7 +109,7 @@ void main(void)
 	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);
 		}
@@ -122,6 +123,9 @@ void main(void)
 					etat_trajet = Trajet_avance((float)step_ms/1000.);
 				}else{
 					Asser_Position_maintien();
+					if(Asser_Position_panic_angle()){
+						fin_match=1;
+					}
 				}
 				AsserMoteur_Gestion(step_ms);
 			}