From 3103c237df3e7feb51fe94cea347c80edcfdc3cf Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Tue, 12 Aug 2025 14:49:11 +0200
Subject: [PATCH] =?UTF-8?q?Commande=20en=20vitesse=20-=20param=C3=A8tre=20?=
 =?UTF-8?q?du=20PI=20en=20vitesse=20aux=20valeur=20par=20defaut.?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/main.cpp | 68 +++++++++++++++++++++++++++++++++++-----------------
 1 file changed, 46 insertions(+), 22 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 23ecef5..1156997 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -22,10 +22,9 @@ void lire_tous_les_registres(int servo_id);
 void setup() {
   // initialize the digital pin as an output.
   pinMode(LED_BUILTIN, OUTPUT);
-  Serial.begin(115200);//sts舵机波特率1000000
-  Serial1.begin(1000000);//sts舵机波特率1000000
+  Serial.begin(115200);         // PC en USB
+  Serial1.begin(1000000);       // Servo Feetech
   sms_sts.pSerial = &Serial1;
-  delay(5000);
 
 }
 
@@ -98,45 +97,70 @@ enum etat_t test_ping()
 }
 
 enum etat_t configure_servomoteur(){
-  sms_sts.ServoMode(SERVO_ID);
+  sms_sts.writeByte(SERVO_ID, SMS_STS_MODE, 1); // Choix du mode
+
+  // On remet par défaut les butées
   sms_sts.writeByte(SERVO_ID, 0x09, 0); // Butée min à 0
   sms_sts.writeByte(SERVO_ID, 0x0A, 0); // Butée min à 0
-  sms_sts.writeByte(SERVO_ID, 0x0B, 0); // Buté max à 0, pour activer le multitour
-  sms_sts.writeByte(SERVO_ID, 0x0C, 0); // Buté max à 0, pour activer le multitour
+  sms_sts.writeByte(SERVO_ID, 0x0B, 4095 & 0xFF ); // Buté max à 0, pour activer le multitour
+  sms_sts.writeByte(SERVO_ID, 0x0C, (4095>>8) & 0xFF); // Buté max à 0, pour activer le multitour
 
-  sms_sts.writeByte(SERVO_ID, 0x1E, 3);	
-  sms_sts.writeByte(SERVO_ID, 0x12, 0x7C);	// Registre de "Phase", valeur par défaut 108 (0x6C)
+  // Et les essais sur les basses résolutions
+  sms_sts.writeByte(SERVO_ID, 0x1E, 1);	
+  sms_sts.writeByte(SERVO_ID, 0x12, 0x6C);	// Registre de "Phase", valeur par défaut 108 (0x6C)
+
+  sms_sts.writeByte(SERVO_ID, 0x25, 10);	// Coef P du correcteur en vitesse
+  sms_sts.writeByte(SERVO_ID, 0x27, 200);	// Coef I du correcteur en vitesse
 
 
   return TERMINE;
 }
 
 enum etat_t mouvement_servomoteur(){
-  static int position=0;
+  static int vitesse=500;
   static int temps_pas_ms = 0;
   static int temps_aff_ms = 0;
-  static int pas_servo = 512;
-  int position_lue;
+  int vitesse_lue;
+  int commande_moteur;
   char tampon[200];
   /// Toutes les 500 ms
-  if(millis() - temps_pas_ms > 500 ){
+  if(millis() - temps_pas_ms > 1000 ){
     temps_pas_ms = millis();
     // On avance ou recule de d'un pas
-    position += pas_servo;
-    // si position > 5000 ou position < 0
-    if(position > 20100 || position < 0){
-      pas_servo = -pas_servo;
-    }
-    sms_sts.WritePosEx(SERVO_ID, position, 4800);
+    vitesse = -vitesse;
+
+    //sms_sts.writeByte(SERVO_ID, position, 4800);
+    sms_sts.WriteSpe(SERVO_ID, vitesse);
   }
 
   if(millis() - temps_aff_ms > 10 ){
     temps_aff_ms = millis();
-    sprintf(tampon, ">pos_consigne:%d\n", position);
+
+    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);
+      Serial.print(tampon);
+      if(commande_moteur&(1<<10)){
+        commande_moteur &= 0x3FF;
+        commande_moteur = - commande_moteur;
+      }
+
+      if(commande_moteur != 32769){
+        sprintf(tampon, ">cde_moteur:%d\n", commande_moteur);
+      }
+      Serial.print(tampon);
+    }
+
+    sprintf(tampon, ">cde_moteur_lib:%d\n", sms_sts.ReadLoad(SERVO_ID));
     Serial.print(tampon);
-    position_lue = sms_sts.ReadPos(SERVO_ID);
-    if(position_lue != 32769){
-      sprintf(tampon, ">pos_actuelle:%d\n", position_lue);
+
+    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);
   }