diff --git a/Asser_Position.c b/Asser_Position.c
index df20315..aca2a44 100644
--- a/Asser_Position.c
+++ b/Asser_Position.c
@@ -2,8 +2,8 @@
 #include "Commande_vitesse.h"
 #include "math.h"
 
-#define GAIN_P_POSITION 10
-#define GAIN_P_ORIENTATION 100
+#define GAIN_P_POSITION 1
+#define GAIN_P_ORIENTATION 10
 
 struct position_t position_maintien;
 
@@ -24,6 +24,12 @@ void Asser_Position(struct position_t position_consigne){
     delta_y_mm = position_consigne.y_mm - position_actuelle.y_mm;
     delta_avance_mm = sqrtf(delta_x_mm * delta_x_mm + delta_y_mm * delta_y_mm);
     delta_orientation_radian = atan2f(delta_y_mm, delta_x_mm) - position_actuelle.angle_radian;
+    delta_orientation_radian = Geometrie_get_angle_optimal(0. , delta_orientation_radian);
+    /*printf(">delta_orientation_radian:%.2f\n>angle_opti:%.2f\n>actuel:%.2f\n",delta_orientation_radian,
+        Geometrie_get_angle_optimal(atan2f(delta_y_mm, delta_x_mm), position_actuelle.angle_radian),
+        position_actuelle.angle_radian);
+    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", position_consigne.x_mm, position_consigne.y_mm);*/
 
     // Asservissement
     avance_mm_s = delta_avance_mm * GAIN_P_POSITION;
diff --git a/QEI.c b/QEI.c
index 149d115..4d8881f 100644
--- a/QEI.c
+++ b/QEI.c
@@ -17,7 +17,8 @@
 // Nombre d'impulsions par tour de roue : 8000
 // Impulsion / mm : 42,44
 
-#define IMPULSION_PAR_MM (12.45f)
+//#define IMPULSION_PAR_MM (12.45f)
+#define IMPULSION_PAR_MM (16.45f)
 
 
 struct QEI_t QEI_A, QEI_B;
diff --git a/main.c b/main.c
index 5bd8adc..3bd2b0b 100644
--- a/main.c
+++ b/main.c
@@ -41,7 +41,7 @@ void main(void)
 
     stdio_init_all();
 	Temps_init();
-	tension_batterie_init();
+	//tension_batterie_init();
 	identifiant_init();
 	Localisation_init();
 	Trajet_init();
@@ -57,14 +57,19 @@ void main(void)
 
 	
 	multicore_launch_core1(affichage);
+	Localisation_set(1122, 2000-63, M_PI);
 
 	struct trajectoire_t trajectoire;
-	Trajectoire_droite(&trajectoire, 0, 0, 500, 0, 0, 0);
+	Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, 
+		606, 2000-590, 225, 2000-225, M_PI, M_PI);
 	Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
 	Trajet_debut_trajectoire(trajectoire);
+
+	
+
 	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
 
-	sleep_ms(3000);
+	sleep_ms(8000);
 
 	while(1){
 		
@@ -96,6 +101,7 @@ void affichage(void){
 		printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/
 		printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian);
 		printf(">abscisse:%f\n",abscisse);
+		printf(">id:%d\n", identifiant_lire());
 		sleep_ms(100);
 	}
 }