Readme.md

@@ -1,47 +0,0 @@
-Fonctionnement en mode Servo "Classique"
-========================================
-
-En faisant varier la consigne de position de -512 à 5120, nous observons des butées logicielles à 0 et à 4093. Notons aussi que la position renvoyée peut reboucler à 0 comme indiqué sur le graphique. Nous avons globalement le fonctionnement d'un servomoteur classique, sauf que nous avons le retour de position et que nous pouvons régler également la vitesse.
-
-
-Voici le graphique obtenu à partir du comportement du servomoteur.
-
-![](/doc/ModeServo_normal.png)
-
-
-Suppression des butées
------------------------
-
-En faisant varier la consigne de position de -512 à 5120, nous observons l'absence de butées logicielles.
-
-Voici le graphique obtenu à partir du comportement du servomoteur.
-
-![](/doc/ModeServo_SansButée.png)
-
-Seulement, en poussant le servomoteur un peu plus loin, nous observons que le multitour est limité. Lorsqu'il doit faire son 8e tour (ou presque), le servomoteur part dans l'autre sens à toute vitesse. Un tour correspond à 4096 points de codeurs et la position est enregistrée sur 16 bits, donc avec une limite vers 32700 (en positif ou en négatif). 
-
-Nous remarquons que la valeur de la position du servo est toujours comprise entre 0 et 4095. Pas moyen de savoir combien de tours le servomoteur a fait.
-
-Sur le graphique ci-dessous, nous voyons que le servomoteur (position en bleu) s'affole quand la consigne passe au dessus de 32000.
-
-![](doc/ModeServo_rebouclage.png)
-
-Note : Pour la butée logicielle haute, contrairement à ce qui est indiqué dans la documentation, des valeurs plus grandes que 4095 sont acceptées.
-
-Retour de position qui correspond à la consigne
------------------------------------------------
-
-En passant le bit4 du registre 0x12 à 1, vous obtenez un retour de position du servomoteur qui n'est plus bloqué entre 0 et 4095. Mais attention, cette valeur n'est pas celle qui persistera à une coupure de courant. Si votre servo s'arrête à la position 6000, après une extinction, vous lire une position à 1904.
-
-C'est donc une solution intéressante tant que vous avez un moyen de refaire le zéro à la mise sous tension.
-
-Fonctionnement en mode Servo "Classique" - basse résolution
-=========================================================
-
-Un paramètre permet de baisser artificiellement la résolution du codeur. Ceci permet, en interrogeant le servo de savoir où il se trouve sur une position de 0 à 4095, d'avoir une réponse sur 2 ou 3 tour, en fonction de la valeur de 0x1E : 
-- 2 : ce sera sur 2 tours
-- 3 : ce sera sur 3 tours
-
-En théorie, ceci va de pair avec une perte en précision, qui est probablement négligeable.
-
-Je ne comprends pas trop l'intérêt de ce mode. La valeur de position n'est pas persistante après une extinction...

src/main.cpp

@@ -1,29 +1,91 @@
 #include <Arduino.h>
 #include "SCServo.h"
 
-#define SERVO_ID 1
-
 SMS_STS sms_sts;
 
-enum etat_t{
+void test_ping();
-  EN_COURS,
-  TERMINE,
-  ECHEC
-};
-
-void gestion_servo(void);
-enum etat_t test_ping(void);
-enum etat_t configure_servomoteur(void);
-enum etat_t mouvement_servomoteur(void);
-
 void lire_tous_les_registres(int servo_id);
 
+
+char registre_description[70][100]={
+  "Version majeure du firmware",
+  "Version mineure du firmware",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  "",
+  ""
+};
+
 // the setup routine runs once when you press reset:
 void setup() {
   // initialize the digital pin as an output.
   pinMode(LED_BUILTIN, OUTPUT);
-  Serial.begin(115200);         // PC en USB
+  Serial.begin(115200);//sts舵机波特率1000000
-  Serial1.begin(1000000);       // Servo Feetech
+  Serial1.begin(1000000);//sts舵机波特率1000000
   sms_sts.pSerial = &Serial1;
 
 }
@@ -31,51 +93,12 @@ void setup() {
 // the loop routine runs over and over again forever:
 void loop() {
   digitalWrite(LED_BUILTIN, HIGH);   // turn the LED on (HIGH is the voltage level)
-  gestion_servo();
+  test_ping();
   digitalWrite(LED_BUILTIN, LOW);    // turn the LED off by making the voltage LOW
-  gestion_servo();
+  test_ping();
 }
 
-void gestion_servo(){
+void test_ping()
-  static enum {
-    PING_SERVO,
-    CONFIG_SERVO,
-    MOUVEMENT_SERVO
-  } etat_gestion_servo = PING_SERVO;
-
-  enum etat_t etat_action;
-
-  switch(etat_gestion_servo){
-    case PING_SERVO:
-      if(test_ping() == TERMINE){
-        etat_gestion_servo = CONFIG_SERVO;
-      }
-      break;
-
-    case CONFIG_SERVO:
-      etat_action = configure_servomoteur();
-      if(etat_action == TERMINE){
-        etat_gestion_servo = MOUVEMENT_SERVO;
-      }
-      if(etat_action == ECHEC){
-        etat_gestion_servo = PING_SERVO;
-      }
-      break;
-
-    case MOUVEMENT_SERVO:
-      etat_action = mouvement_servomoteur();
-
-      if(etat_action == ECHEC){
-        etat_gestion_servo = PING_SERVO;
-      }
-      break;
-      
-      
-
-  }
-}
-
-enum etat_t test_ping()
 {
   int ID = sms_sts.Ping(1);
   static int found = 0;
@@ -85,7 +108,6 @@ enum etat_t test_ping()
     if(found == 0){
       found = 1;
       lire_tous_les_registres(ID);
-      return TERMINE;
     }
     delay(100);
   }else{
@@ -93,87 +115,6 @@ enum etat_t test_ping()
     found = 0;
     delay(2000);
   }
-  return EN_COURS;
-}
-
-enum etat_t configure_servomoteur(){
-  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, 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
-
-  // 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 vitesse=500;
-  static int temps_pas_ms = 0;
-  static int temps_aff_ms = 0;
-  int vitesse_lue, vitesse_tmp, neg;
-  int commande_moteur;
-  char tampon[200];
-  /// Toutes les 500 ms
-  if(millis() - temps_pas_ms > 1000 ){
-    temps_pas_ms = millis();
-    // 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);
-    
-  }
-
-  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);
-      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);
-
-    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;
 }
 
 int lire_registre(int servo_id, int registre_adresse){
@@ -183,7 +124,7 @@ int lire_registre(int servo_id, int registre_adresse){
 void lire_tous_les_registres(int servo_id){
   char message[200]="";
   for (int i=0; i<0x46; i++){
-    sprintf(message, "registre 0x%x: %d\n", i, lire_registre(servo_id, i));
+    sprintf(message, "registre 0x%x: %d\t\t%s\n", i, lire_registre(servo_id, i), registre_description[i]);
     Serial.print(message);
   }