From 2f5bd67b4eff3ffb2e123fe7d97e01159f7884a4 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Tue, 12 Aug 2025 20:24:12 +0200
Subject: [PATCH] Envoi de la vitesse en direct vers le registre

---
 src/main.cpp | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

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 ){