diff --git a/src/main.cpp b/src/main.cpp
index 1156997..af043d5 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -97,7 +97,7 @@ enum etat_t test_ping()
 }
 
 enum etat_t configure_servomoteur(){
-  sms_sts.writeByte(SERVO_ID, SMS_STS_MODE, 1); // Choix du mode
+  sms_sts.writeByte(SERVO_ID, SMS_STS_MODE, 2); // Choix du mode
 
   // On remet par défaut les butées
   sms_sts.writeByte(SERVO_ID, 0x09, 0); // Butée min à 0
@@ -117,52 +117,56 @@ enum etat_t configure_servomoteur(){
 }
 
 enum etat_t mouvement_servomoteur(){
-  static int vitesse=500;
+  static int commande_moteur=800;
   static int temps_pas_ms = 0;
   static int temps_aff_ms = 0;
-  int vitesse_lue;
-  int commande_moteur;
+  int commande_moteur_lue, neg;
   char tampon[200];
   /// Toutes les 500 ms
   if(millis() - temps_pas_ms > 1000 ){
+    int commande_moteur_tmp;
     temps_pas_ms = millis();
     // On avance ou recule de d'un pas
-    vitesse = -vitesse;
+    commande_moteur = -commande_moteur;
 
-    //sms_sts.writeByte(SERVO_ID, position, 4800);
-    sms_sts.WriteSpe(SERVO_ID, vitesse);
+    //sms_sts.writeByte(SERVO_ID, 0x2C, 0xFF);
+    //sms_sts.writeByte(SERVO_ID, 0x2D, 0x05);
+    commande_moteur_tmp = commande_moteur;
+    neg = 0;
+    if(commande_moteur_tmp < 0){
+      neg = 1;
+      commande_moteur_tmp = -commande_moteur_tmp;
+    }
+
+    //sms_sts.writeByte(SERVO_ID, 0x2C, commande_moteur_tmp & 0xFF);
+    //sms_sts.writeByte(SERVO_ID, 0x2D, ((commande_moteur_tmp>>8) & 0xFF)  );
+
+    sms_sts.writeWord(SERVO_ID, 0x2C, commande_moteur_tmp | neg << 10);
+  
+    
   }
 
   if(millis() - temps_aff_ms > 10 ){
     temps_aff_ms = millis();
 
-    commande_moteur = -1;
-    commande_moteur = sms_sts.readWord(SERVO_ID, SMS_STS_PRESENT_LOAD_L);
-    if(commande_moteur != -1){
-      sprintf(tampon, ">cde_moteur_brute:%d\n", commande_moteur);
+    sprintf(tampon, ">cde_moteur:%d\n", commande_moteur);
+    Serial.print(tampon);
+
+    commande_moteur_lue = -1;
+    commande_moteur_lue = sms_sts.readWord(SERVO_ID, SMS_STS_PRESENT_LOAD_L);
+    if(commande_moteur_lue != -1){
       Serial.print(tampon);
-      if(commande_moteur&(1<<10)){
-        commande_moteur &= 0x3FF;
-        commande_moteur = - commande_moteur;
+      if(commande_moteur_lue&(1<<10)){
+        commande_moteur_lue &= 0x3FF;
+        commande_moteur_lue = - commande_moteur_lue;
       }
 
-      if(commande_moteur != 32769){
-        sprintf(tampon, ">cde_moteur:%d\n", commande_moteur);
+      if(commande_moteur_lue != 32769){
+        sprintf(tampon, ">cde_moteur_lue:%d\n", commande_moteur_lue);
       }
       Serial.print(tampon);
     }
 
-    sprintf(tampon, ">cde_moteur_lib:%d\n", sms_sts.ReadLoad(SERVO_ID));
-    Serial.print(tampon);
-
-    sprintf(tampon, ">vit_consigne:%d\n", vitesse);
-    Serial.print(tampon);
-
-    vitesse_lue = sms_sts.ReadSpeed(SERVO_ID);
-    if(vitesse_lue != 32769){
-      sprintf(tampon, ">vit_actuelle:%d\n", vitesse_lue);
-    }
-    Serial.print(tampon);
   }
   
   return EN_COURS;