diff --git a/src/main.cpp b/src/main.cpp
index 1156997..e1d022a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -120,7 +120,7 @@ enum etat_t mouvement_servomoteur(){
   static int vitesse=500;
   static int temps_pas_ms = 0;
   static int temps_aff_ms = 0;
-  int vitesse_lue;
+  int vitesse_lue, vitesse_tmp, neg;
   int commande_moteur;
   char tampon[200];
   /// Toutes les 500 ms
@@ -129,8 +129,16 @@ enum etat_t mouvement_servomoteur(){
     // On avance ou recule de d'un pas
     vitesse = -vitesse;
 
+    vitesse_tmp = vitesse;
+    neg = 0;
+    if(vitesse_tmp < 0){
+      vitesse_tmp = -vitesse_tmp;
+      neg = 1;
+    }
+    sms_sts.writeWord(SERVO_ID, 0x2E, vitesse_tmp | neg << 15);
+
     //sms_sts.writeByte(SERVO_ID, position, 4800);
-    sms_sts.WriteSpe(SERVO_ID, vitesse);
+    
   }
 
   if(millis() - temps_aff_ms > 10 ){