diff --git a/Moteurs.c b/Moteurs.c
index 58d53c0..95d68e9 100644
--- a/Moteurs.c
+++ b/Moteurs.c
@@ -15,7 +15,7 @@
 
 uint slice_moteur_A, slice_moteur_B, slice_moteur_C;
 
-
+/// @brief Initialisation les entrées / sorties requises pour les moteurs
 void Moteur_Init(){
     // Initialisation des broches pour les PWM
     gpio_set_function(MOTEUR_A_VITESSE, GPIO_FUNC_PWM);
@@ -26,9 +26,9 @@ void Moteur_Init(){
     gpio_init(MOTEUR_A_SENS);
     gpio_init(MOTEUR_B_SENS);
     gpio_init(MOTEUR_C_SENS);
-    gpio_set_dir(MOTEUR_A_SENS, true);
-    gpio_set_dir(MOTEUR_B_SENS, true);
-    gpio_set_dir(MOTEUR_C_SENS, true);
+    gpio_set_dir(MOTEUR_A_SENS, GPIO_OUT);
+    gpio_set_dir(MOTEUR_B_SENS, GPIO_OUT);
+    gpio_set_dir(MOTEUR_C_SENS, GPIO_OUT);
 
     // Configuration des PWM à 1 kHz
     // Clock_system 125 MHz - 16 bits PWM (65536)
@@ -44,7 +44,7 @@ void Moteur_Init(){
 
     pwm_config_set_clkdiv_int(&pwm_moteur, 2); 
     pwm_config_set_phase_correct(&pwm_moteur, false);
-    pwm_config_set_wrap(&pwm_moteur, (uint16_t)65635);
+    pwm_config_set_wrap(&pwm_moteur, (uint16_t)65535);
 
     pwm_init(slice_moteur_A, &pwm_moteur, true);
     pwm_init(slice_moteur_B, &pwm_moteur, true);
@@ -57,6 +57,9 @@ void Moteur_Init(){
 }
 
 
+/// @brief Configure le PWM et la broche de sens en fonction de la vitesse et du moteur
+/// @param moteur : Moteur (voir enum t_moteur)
+/// @param vitesse : Vitesse entre -32767 et 32767
 void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ){
     uint16_t u_vitesse;
 
@@ -70,27 +73,27 @@ void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ){
         case MOTEUR_A:
             pwm_set_chan_level(slice_moteur_A, PWM_CHAN_A, u_vitesse);
             if(vitesse < 0){
-                gpio_put(MOTEUR_A_SENS, false);
+                gpio_put(MOTEUR_A_SENS, 0);
             }else{
-                gpio_put(MOTEUR_A_SENS, true);
+                gpio_put(MOTEUR_A_SENS, 1);
             }
             break;
 
         case MOTEUR_B:
             pwm_set_chan_level(slice_moteur_B, PWM_CHAN_A, u_vitesse);
             if(vitesse < 0){
-                gpio_put(MOTEUR_B_SENS, false);
+                gpio_put(MOTEUR_B_SENS, 0);
             }else{
-                gpio_put(MOTEUR_B_SENS, true);
+                gpio_put(MOTEUR_B_SENS, 1);
             }
             break;
 
         case MOTEUR_C:
             pwm_set_chan_level(slice_moteur_C, PWM_CHAN_A, u_vitesse);
             if(vitesse < 0){
-                gpio_put(MOTEUR_B_SENS, false);
+                gpio_put(MOTEUR_C_SENS, 0);
             }else{
-                gpio_put(MOTEUR_B_SENS, true);
+                gpio_put(MOTEUR_C_SENS, 1);
             }
             break;
     }
diff --git a/test.c b/test.c
index 5c00457..769c0fd 100644
--- a/test.c
+++ b/test.c
@@ -7,14 +7,16 @@
 #include "Temps.h"
 #include "spi_nb.h"
 #include "Servomoteur.h"
+#include "QEI.h"
 
 const uint LED_PIN = 25;
 
 #define V_INIT -999.0
+#define TEST_TIMEOUT_US 10000000
 
 int mode_test();
-
 int test_moteurs();
+int test_vitesse_moteur(enum t_moteur moteur);
 
 int main() {
     bi_decl(bi_program_description("This is a test binary."));
@@ -37,10 +39,14 @@ int main() {
 
     //while(1);
     Temps_init();
-    Gyro_Init();
+    Moteur_Init();
 
     while(mode_test());
 
+    Gyro_Init();
+
+    
+
     temps_ms = Temps_get_temps_ms();
     temps_ms_old = temps_ms;
     while (1) { 
@@ -95,29 +101,113 @@ int main() {
 
 // Mode test : renvoie 0 pour quitter le mode test
 int mode_test(){
+    static int iteration = 3;
     printf("Appuyez sur une touche pour entrer en mode test :\n");
     printf("M - pour les moteurs\n");
-    int rep = getchar_timeout_us(10000000);
+    stdio_flush();
+    int rep = getchar_timeout_us(TEST_TIMEOUT_US);
+    stdio_flush();
     switch (rep)
     {
     case 'M':
+    case 'm':
         /* code */
-
+        while(test_moteurs());
         break;
     case PICO_ERROR_TIMEOUT:
-        printf("Sortie du mode test\n");
-        return 0;
+        iteration--;        
+        if(iteration == 0){
+            printf("Sortie du mode test\n");
+            return 0;
+        }
 
     default:
         printf("Commande inconnue\n");
-        return 1;
         break;
     }
+    return 1;
     
 }
 
 int test_moteurs(){
+    int lettre_moteur;
+
     printf("Indiquez le moteurs à tester (A, B ou C):\n");
-    int moteur = getchar_timeout_us(0);
-    printf("Moteur choisi : %d %x", moteur, moteur);
+    do{
+         lettre_moteur = getchar_timeout_us(TEST_TIMEOUT_US);
+    }while(lettre_moteur == PICO_ERROR_TIMEOUT);
+    printf("Moteur choisi : %c %d %x\n", lettre_moteur, lettre_moteur, lettre_moteur);
+    stdio_flush();
+
+    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(TEST_TIMEOUT_US);
+    }while(vitesse_moteur == PICO_ERROR_TIMEOUT);
+    stdio_flush();
+    
+    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 'q':
+    case 'Q':
+        return 0;
+        break;
+
+    default:
+        break;
+    }
+    return 1;
+
 }
\ No newline at end of file