diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c
index 267e1f2..0b6a6cc 100644
--- a/Asser_Moteurs.c
+++ b/Asser_Moteurs.c
@@ -21,6 +21,8 @@ float consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
 float commande_I[3]; // Terme integral
 
 void AsserMoteur_Init(){
+    QEI_init();
+    Moteur_Init();
     for(unsigned int i =0; i< 3; i ++){
         commande_I[i]=0;
         consigne_mm_s[i]=0;
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a977aeb..6dd238a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -40,6 +40,7 @@ Test_gyro.c
 Test_i2c.c
 Test_log.c
 Test_strategie.c
+Tests_unitaires.c
 Trajet.c
 Trajectoire.c
 Trajectoire_bezier.c
diff --git a/Demonstration.c b/Demonstration.c
index 081dec1..de36862 100644
--- a/Demonstration.c
+++ b/Demonstration.c
@@ -117,6 +117,24 @@ void Demonstration_semiauto(){
     }
 }
 
+void Demonstration_auto(){
+    Demonstration_calage();
+    Demonstration_attente();
+
+    while(true){
+        Demonstration_rectangle(550, 1000);
+        Demonstration_calage();
+
+        Demonstration_avance_puis_tourne(300, 1000, 720.);
+        Demonstration_calage();
+
+        Demonstration_bezier();
+        Demonstration_calage();
+        
+        Demonstration_attente();
+    }
+}
+
 enum etat_action_t Demonstration_attente(){
     enum {
         ATTENTE_DETECTION,
diff --git a/Demonstration.h b/Demonstration.h
index a9928e7..f015555 100644
--- a/Demonstration.h
+++ b/Demonstration.h
@@ -1,3 +1,4 @@
  int Demonstration_menu(void);
  void Demonstration_semiauto(void);
+ void Demonstration_auto(void);
  int Demonstration_init(void);
\ No newline at end of file
diff --git a/Holonome2023.c b/Holonome2023.c
index 5e793a4..ae0c234 100644
--- a/Holonome2023.c
+++ b/Holonome2023.c
@@ -40,7 +40,7 @@ int main() {
 
     stdio_init_all();
     
-    Demonstration_init();Demonstration_semiauto();
+    //Demonstration_init();Demonstration_auto();
     while(mode_test());
     Holonome2023_init();
 
diff --git a/Log.c b/Log.c
index 1197551..b8b4e2f 100644
--- a/Log.c
+++ b/Log.c
@@ -32,6 +32,7 @@ void Log_init(void){
 /// @param message : string, without '\n' at the end.
 /// @param log_level : can be in TELEPLOT, TRACE, DEBUG, INFO, WARN, ERROR, FATAL
 void Log_message(char * message, enum Log_level log_level){
+    printf("ERROR:%s\n",message);
     if(log_error != LOG_ERROR_OK){
         return;
     }
diff --git a/Monitoring.c b/Monitoring.c
index c09e53c..e37c956 100644
--- a/Monitoring.c
+++ b/Monitoring.c
@@ -92,6 +92,11 @@ void set_debug_varf(float variable){
 void Monitoring_Error(char * msg){
     gpio_put(LED_PIN_ROUGE, 1);
     Log_message(msg, ERROR);
+    while(1){
+        sleep_ms(500);
+        printf(msg);
+        printf("\n");
+    }
 
 }
 
diff --git a/QEI.c b/QEI.c
index 7756595..fb6a76c 100644
--- a/QEI.c
+++ b/QEI.c
@@ -23,6 +23,8 @@
 
 struct QEI_t QEI_A, QEI_B, QEI_C;
 
+bool QEI_est_init = false;
+
 PIO pio_QEI = pio0;
 
 const uint CODEUR_1_A = 26;
@@ -31,7 +33,7 @@ const uint CODEUR_1_B = 27;
 void QEI_init(){
     // Initialisation des 3 modules QEI
     // Chaque module QEI sera dans une machine à état du PIO 0
-    
+    if(!QEI_est_init){
 
     // Offset le début du programme
     // Si ce n'est pas 0, le programme ne marchera pas
@@ -59,6 +61,8 @@ void QEI_init(){
     QEI_A.value=0;
     QEI_B.value=0;
     QEI_C.value=0;
+    QEI_est_init=true;
+    }
 
 }
 
diff --git a/Test.c b/Test.c
index fe09aa9..e832163 100644
--- a/Test.c
+++ b/Test.c
@@ -34,17 +34,13 @@
 #include "Test_i2c.h"
 #include "Test_log.h"
 #include "Test_strategie.h"
+#include "Tests_unitaires.h"
 #include "Test.h"
 
 #define V_INIT -999.0
 #define TEST_TIMEOUT_US 10000000
 
 int test_APDS9960(void);
-int test_moteurs(void);
-int test_QIE(void);
-int test_QIE_mm(void);
-int test_vitesse_moteur(enum t_moteur moteur);
-int test_asser_moteur(void);
 int test_localisation(void);
 int test_avance(void);
 int test_cde_vitesse(void);
@@ -71,10 +67,8 @@ int continuous_printf = 1;
 int mode_test(){
     static int iteration = 2;
     printf("Appuyez sur une touche pour entrer en mode test :\n");
-    printf("A - pour asser_moteurs (rotation)\n");
+    printf("A - Tests unitaires\n");
     printf("B - pour avance (asser_moteur)\n");
-    printf("C - pour les codeurs\n");
-    printf("D - pour les codeurs (somme en mm)\n");
     printf("E - Commande en vitesse...\n");
     printf("F - Strategie...\n");
     printf("G - Lecture des capteurs\n");
@@ -83,7 +77,6 @@ int mode_test(){
     printf("J - Asser Position - avance et tourne (sans gyro)\n");
     printf("K - Trajets aller retour avec Gyro\n");
     printf("L - pour la localisation\n");
-    printf("M - pour les moteurs\n");
     printf("N - Fonctions geometrique\n");
     printf("O - Analyse obstacle\n");
     printf("P - Asser Position - perturbation\n");
@@ -102,22 +95,12 @@ int mode_test(){
     {
     case 'a':
     case 'A':
-        while(test_asser_moteur());
+        while(mode_test_unitaire());
         break;
     case 'b':
     case 'B':
         while(test_avance());
         break;
-        
-    case 'C':
-    case 'c':
-        while(test_QIE());
-        break;
-
-    case 'D':
-    case 'd':
-        while(test_QIE_mm());
-        break;
 
     case 'E':
     case 'e':
@@ -159,11 +142,6 @@ int mode_test(){
         while(test_localisation());
         break;
 
-    case 'M':
-    case 'm':
-        while(test_moteurs());
-        break;
-
     case 'N':
     case 'n':
         while(test_geometrie());
@@ -390,7 +368,7 @@ int test_aller_retour(){
             Trajectoire_rotation(&trajectoire, 0, 0, 0, 700);
             trajectoire.orientation_debut_rad = 0;
             trajectoire.orientation_fin_rad = M_PI;
-            printf("Trajectoire droite avec rotation\n");
+            printf("Trajectoire rotation pure\n");
             break;
         
         default: return 0;
@@ -398,7 +376,7 @@ int test_aller_retour(){
 
     printf("Init gyroscope\n");
     Gyro_Init();
-    //printf("C'est parti !\n");
+    printf("C'est parti !\n");
     stdio_flush();
 
     set_position_avec_gyroscope(1);
@@ -976,73 +954,6 @@ void affiche_localisation(){
     }
 }
 
-void test_asser_moteur_printf(){
-    int _step_ms = 1;
-    while(1){
-        printf("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
-            AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms));
-        //sleep_ms(5);
-    }
-}
-
-int test_asser_moteur(){
-    int lettre;
-    int _step_ms = 1;
-    printf("Asservissement des moteurs :\nAppuyez sur une touche pour quitter\n");
-    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 100);
-    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100);
-    AsserMoteur_setConsigne_mm_s(MOTEUR_C, 100);
-    multicore_launch_core1(test_asser_moteur_printf);
-    do{
-        QEI_update();
-        AsserMoteur_Gestion(_step_ms);
-        sleep_ms(_step_ms);
-        //printf("Vitesse A : %d, codeur B : %d, codeur C : %d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME), QEI_get(QEI_C_NAME));
-        //printf("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
-        //    AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms));
-        lettre = getchar_timeout_us(0);
-    }while(lettre == PICO_ERROR_TIMEOUT);
-    Moteur_SetVitesse(MOTEUR_A, 0);
-    Moteur_SetVitesse(MOTEUR_B, 0);
-    Moteur_SetVitesse(MOTEUR_C, 0);
-    multicore_reset_core1();
-    return 0;
-}
-
-int test_QIE(){
-    int lettre;
-    printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
-    do{
-        QEI_update();
-        printf("Codeur A : %d (%3.2f mm), codeur B : %d (%3.2f mm), codeur C : %d (%3.2f mm)\n", 
-            QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME),
-            QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME),
-            QEI_get(QEI_C_NAME), QEI_get_mm(QEI_C_NAME));
-        sleep_ms(100);
-
-        lettre = getchar_timeout_us(0);
-    }while(lettre == PICO_ERROR_TIMEOUT);
-    return 0;
-
-}
-
-int test_QIE_mm(){
-    int lettre;
-    printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
-    float a_mm=0, b_mm=0, c_mm=0;
-    do{
-        QEI_update();
-        a_mm += QEI_get_mm(QEI_A_NAME);
-        b_mm += QEI_get_mm(QEI_B_NAME);
-        c_mm += QEI_get_mm(QEI_C_NAME);
-        printf("Codeur A : %3.2f mm, codeur B : %3.2f mm, codeur C : %3.2f mm\n", a_mm, b_mm, c_mm);
-        sleep_ms(100);
-
-        lettre = getchar_timeout_us(0);
-    }while(lettre == PICO_ERROR_TIMEOUT);
-    return 0;
-
-}
 
 int test_localisation(){
     int lettre;
@@ -1054,6 +965,10 @@ int test_localisation(){
     int16_t vitesse;
     int propulseur=0;
 
+    Temps_init();
+    QEI_init();
+    Localisation_init();
+
     printf("A - Sans gyroscope\n");
     printf("B - Avec Gyroscope\n");
     printf("C - Avec Gyroscope + moteurs\n");
@@ -1131,95 +1046,6 @@ int test_localisation(){
     
 }
 
-int test_moteurs(){
-    int lettre_moteur;
-
-    printf("Indiquez le moteurs à tester (A, B ou C):\n");
-    do{
-         lettre_moteur = getchar_timeout_us(TEST_TIMEOUT_US);
-         stdio_flush();
-    }while(lettre_moteur == PICO_ERROR_TIMEOUT);
-    printf("Moteur choisi : %c %d %x\n", lettre_moteur, lettre_moteur, lettre_moteur);
-
-    switch (lettre_moteur)
-    {
-    case 'A':
-    case 'a':
-        while(test_vitesse_moteur(MOTEUR_A));
-        break;
-
-    case 'B':
-    case 'b':
-        while(test_vitesse_moteur(MOTEUR_B));
-        break;
-
-    case 'C':
-    case 'c':
-        while(test_vitesse_moteur(MOTEUR_C));
-        break;
-    
-    case 'Q':
-    case 'q':
-        return 0;
-        break;
-    
-    default:
-        break;
-    }
-    
-    return 1;
-}
-
-int test_vitesse_moteur(enum t_moteur moteur){
-    printf("Vitesse souhaitée :\n0 - 0%%\n1 - 10%%\n2 - 20%%\n...\n9 - 90%%\nA - 100%%\n");
-
-    int vitesse_moteur;
-    do{ 
-        vitesse_moteur = getchar_timeout_us(0);
-        stdio_flush();
-
-    }while(vitesse_moteur == PICO_ERROR_TIMEOUT);
-    
-    switch (vitesse_moteur)
-    {
-    case '0':
-    case '1':
-    case '2':
-    case '3':
-    case '4':
-    case '5':
-    case '6':
-    case '7':
-    case '8':
-    case '9':
-        printf("Vitesse choisie : %c0%%\n", vitesse_moteur);
-        Moteur_SetVitesse(moteur, (vitesse_moteur - '0') * 32767.0 / 10.);
-        break;
-
-    case 'A':
-    case 'a':
-        printf("Vitesse choisie : 100%%\n");
-        Moteur_SetVitesse(moteur, (int16_t) 32766.0);
-        break;
-
-    case 'b':
-    case 'B':
-        printf("Vitesse choisie : -50%%\n");
-        Moteur_SetVitesse(moteur, (int16_t) -32766.0/2);
-        break;
-
-    case 'q':
-    case 'Q':
-        return 0;
-        break;
-
-    default:
-        break;
-    }
-    return 1;
-}
-
-
 int test_geometrie(){
     float angle = 270, angle_min, angle_max;
     printf("Normalise %f° : %f°\n", angle, Geometrie_get_angle_normalisee(angle*DEGRE_EN_RADIAN)/DEGRE_EN_RADIAN);
diff --git a/Tests_unitaires.c b/Tests_unitaires.c
new file mode 100644
index 0000000..1de5c7b
--- /dev/null
+++ b/Tests_unitaires.c
@@ -0,0 +1,228 @@
+#include <stdio.h>
+#include "pico/multicore.h"
+
+#include "QEI.h"
+#include "Moteurs.h"
+#include "Asser_Moteurs.h"
+
+#define TEST_TIMEOUT_US 10000000
+
+int test_QIE(void);
+int test_QIE_mm(void);
+int test_asser_moteur(void);
+int test_vitesse_moteur(enum t_moteur moteur);
+int test_moteurs(void);
+
+
+// Mode test : renvoie 0 pour quitter le mode test
+int mode_test_unitaire(){
+    static int iteration = 2;
+    printf("Appuyez sur une touche pour entrer en mode test :\n");
+    printf("A - pour les codeurs\n");
+    printf("B - pour les codeurs (somme en mm)\n");
+    printf("C - pour les moteurs\n");
+    printf("D - pour asser_moteurs (rotation)\n");
+    
+    
+    stdio_flush();
+    int rep = getchar_timeout_us(TEST_TIMEOUT_US);
+    stdio_flush();
+    switch (rep)
+    {
+    case 'a':
+    case 'A':
+        while(test_QIE());
+        break;
+    case 'b':
+    case 'B':
+        while(test_QIE_mm());
+        break;
+        
+    case 'C':
+    case 'c':
+        while(test_moteurs());
+        break;
+
+    case 'D':
+    case 'd':
+        while(test_asser_moteur());
+        break;
+
+    case PICO_ERROR_TIMEOUT:
+        iteration--;        
+        if(iteration == 0){
+            //printf("Sortie du mode test\n");
+            //return 0;
+        }
+
+    default:
+        printf("Commande inconnue\n");
+        break;
+    }
+    return 1;
+    
+}
+
+
+int test_QIE(){
+    int lettre;
+    printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
+    QEI_init();
+    do{
+        QEI_update();
+        printf("Codeur A : %d (%3.2f mm), codeur B : %d (%3.2f mm), codeur C : %d (%3.2f mm)\n", 
+            QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME),
+            QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME),
+            QEI_get(QEI_C_NAME), QEI_get_mm(QEI_C_NAME));
+        printf(">CodeurA:%.3f\n>CodeurB:%.3f\n>CodeurC:%.3f\n", QEI_get_mm(QEI_A_NAME), QEI_get_mm(QEI_B_NAME), QEI_get_mm(QEI_C_NAME));
+        sleep_ms(100);
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    return 0;
+}
+
+int test_QIE_mm(){
+    int lettre;
+    printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
+    QEI_init();
+    float a_mm=0, b_mm=0, c_mm=0;
+    do{
+        QEI_update();
+        a_mm += QEI_get_mm(QEI_A_NAME);
+        b_mm += QEI_get_mm(QEI_B_NAME);
+        c_mm += QEI_get_mm(QEI_C_NAME);
+        printf("Codeur A : %3.2f mm, codeur B : %3.2f mm, codeur C : %3.2f mm\n", a_mm, b_mm, c_mm);
+        printf(">CodeurA:%.3f\n>CodeurB:%.3f\n>CodeurC:%.3f\n", a_mm, b_mm, c_mm);
+        sleep_ms(100);
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    return 0;
+
+}
+
+int test_moteurs(){
+    int lettre_moteur;
+    Moteur_Init();
+
+    printf("Indiquez le moteurs à tester (A, B ou C):\n");
+    do{
+         lettre_moteur = getchar_timeout_us(TEST_TIMEOUT_US);
+         stdio_flush();
+    }while(lettre_moteur == PICO_ERROR_TIMEOUT);
+    printf("Moteur choisi : %c %d %x\n", lettre_moteur, lettre_moteur, lettre_moteur);
+
+    switch (lettre_moteur)
+    {
+    case 'A':
+    case 'a':
+        while(test_vitesse_moteur(MOTEUR_A));
+        break;
+
+    case 'B':
+    case 'b':
+        while(test_vitesse_moteur(MOTEUR_B));
+        break;
+
+    case 'C':
+    case 'c':
+        while(test_vitesse_moteur(MOTEUR_C));
+        break;
+    
+    case 'Q':
+    case 'q':
+        return 0;
+        break;
+    
+    default:
+        break;
+    }
+    
+    return 1;
+}
+
+int test_vitesse_moteur(enum t_moteur moteur){
+    printf("Vitesse souhaitée :\n0 - 0%%\n1 - 10%%\n2 - 20%%\n...\n9 - 90%%\nA - 100%%\n");
+
+    int vitesse_moteur;
+    do{ 
+        vitesse_moteur = getchar_timeout_us(0);
+        stdio_flush();
+
+    }while(vitesse_moteur == PICO_ERROR_TIMEOUT);
+    
+    switch (vitesse_moteur)
+    {
+    case '0':
+    case '1':
+    case '2':
+    case '3':
+    case '4':
+    case '5':
+    case '6':
+    case '7':
+    case '8':
+    case '9':
+        printf("Vitesse choisie : %c0%%\n", vitesse_moteur);
+        Moteur_SetVitesse(moteur, (vitesse_moteur - '0') * 32767.0 / 10.);
+        break;
+
+    case 'A':
+    case 'a':
+        printf("Vitesse choisie : 100%%\n");
+        Moteur_SetVitesse(moteur, (int16_t) 32766.0);
+        break;
+
+    case 'b':
+    case 'B':
+        printf("Vitesse choisie : -50%%\n");
+        Moteur_SetVitesse(moteur, (int16_t) -32766.0/2);
+        break;
+
+    case 'q':
+    case 'Q':
+        return 0;
+        break;
+
+    default:
+        break;
+    }
+    return 1;
+}
+
+void test_asser_moteur_printf(){
+    int _step_ms = 1;
+    while(1){
+        printf(">Vitesse_A:%.0f\n>Vitesse_B:%.0f\n>Vitesse_C:%.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
+            AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms));
+        
+        //sleep_ms(5);
+    }
+}
+
+int test_asser_moteur(){
+    int lettre;
+    int _step_ms = 1;
+    AsserMoteur_Init();
+    printf("Asservissement des moteurs :\nAppuyez sur une touche pour quitter\n");
+    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 100);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_C, 100);
+    multicore_launch_core1(test_asser_moteur_printf);
+    do{
+        QEI_update();
+        AsserMoteur_Gestion(_step_ms);
+        sleep_ms(_step_ms);
+        //printf("Vitesse A : %d, codeur B : %d, codeur C : %d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME), QEI_get(QEI_C_NAME));
+        //printf("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
+        //    AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms));
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    Moteur_SetVitesse(MOTEUR_A, 0);
+    Moteur_SetVitesse(MOTEUR_B, 0);
+    Moteur_SetVitesse(MOTEUR_C, 0);
+    multicore_reset_core1();
+    return 0;
+}
+
diff --git a/Tests_unitaires.h b/Tests_unitaires.h
new file mode 100644
index 0000000..112df48
--- /dev/null
+++ b/Tests_unitaires.h
@@ -0,0 +1,2 @@
+
+int mode_test_unitaire(void);
\ No newline at end of file
diff --git a/Trajectoire.c b/Trajectoire.c
index 5fd8c91..2239f4b 100644
--- a/Trajectoire.c
+++ b/Trajectoire.c
@@ -108,6 +108,10 @@ float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
             case TRAJECTOIRE_BEZIER:
                 Trajectoire_bezier_get_longueur(trajectoire);
                 break;
+                
+            case TRAJECTOIRE_ROTATION:
+                Trajectoire_rotation_get_longueur(trajectoire);
+                break;
         }
     }
     return trajectoire->longueur;