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

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);
-}
+}
+
+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();

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