From d68fa9072441e785fa6d1a3793763857105bf0d9 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Thu, 9 May 2024 08:36:35 +0200
Subject: [PATCH] Ajustement de l'asservissement pour un truc plus doux

---
 Asser_Position.c |  8 ++++----
 Trajet.c         |  2 +-
 main.c           | 10 +++++++---
 3 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/Asser_Position.c b/Asser_Position.c
index aacb514..44018a1 100644
--- a/Asser_Position.c
+++ b/Asser_Position.c
@@ -2,8 +2,8 @@
 #include "Commande_vitesse.h"
 #include "math.h"
 
-#define GAIN_P_POSITION 1
-#define GAIN_P_ORIENTATION 10
+#define GAIN_P_POSITION 15
+#define GAIN_P_ORIENTATION 2
 
 struct position_t position_maintien;
 
@@ -30,14 +30,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;
-    }*/
+    }
     
 
 
diff --git a/Trajet.c b/Trajet.c
index 3f02ebb..2a2e8c6 100644
--- a/Trajet.c
+++ b/Trajet.c
@@ -148,7 +148,7 @@ float Trajet_calcul_vitesse(float pas_de_temps_s){
             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;
     }
     
diff --git a/main.c b/main.c
index fe97f9f..d108b21 100644
--- a/main.c
+++ b/main.c
@@ -43,7 +43,7 @@ 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
@@ -82,7 +82,8 @@ void main(void)
 	
 	//multicore_launch_core1(gestion_affichage);
 	multicore_launch_core1(gestion_VL53L8CX);
-	sleep_ms(4000);
+	sleep_ms(3000);
+	printf("Demarrage...\n");
 	
 	
 
@@ -96,7 +97,7 @@ void main(void)
 
 	while(get_tirette());
 
-	// sleep_ms(90000);
+	//sleep_ms(90000);
 
 	temps_depart_ms = Temps_get_temps_ms();
 
@@ -274,6 +275,9 @@ void configure_trajet(int identifiant, int couleur){
 					600, 1400, 0, 2000, M_PI, M_PI);
 					break;
 				case 2:
+					Localisation_set(1117, 2000-63, M_PI);
+					Trajectoire_bezier(&trajectoire, 1116, 2000-63, 975, 2000-63, 
+					540, 1400, -60, 1400, M_PI, M_PI);
 					break;
 				case 3:
 					Localisation_set(1130, 2000-63, M_PI);