Mode 3: Pas à pas

Readme.md

@@ -0,0 +1,47 @@
+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/SCSerial.cpp

@@ -81,11 +81,18 @@ int SCSerial::writeSCS(unsigned char *nDat, int nLen)
 	if(nDat==NULL){
 		return 0;
 	}
+	for(int i=0; i<nLen; i++){
+		Serial.print(nDat[i], DEC);
+		Serial.print(" ");
+	}
+	Serial.println();
+	
 	return pSerial->write(nDat, nLen);
 }
 
 int SCSerial::writeSCS(unsigned char bDat)
 {
+	Serial.print(bDat, DEC);Serial.println();
 	return pSerial->write(&bDat, 1);
 }
 

src/main.cpp

@@ -1,85 +1,23 @@
 #include <Arduino.h>
 #include "SCServo.h"
 
+#define SERVO_ID 1
+
 SMS_STS sms_sts;
 
-void test_ping();
+enum etat_t{
-void lire_tous_les_registres(int servo_id);
+  EN_COURS,
-
+  TERMINE,
-
+  ECHEC
-char registre_description[70][100]={
-  "Version majeure du firmware",
-  "Version mineure du firmware",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  "",
-  ""
 };
 
+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);
+
 // the setup routine runs once when you press reset:
 void setup() {
   // initialize the digital pin as an output.
@@ -87,18 +25,58 @@ void setup() {
   Serial.begin(115200);//sts舵机波特率1000000
   Serial1.begin(1000000);//sts舵机波特率1000000
   sms_sts.pSerial = &Serial1;
+  delay(5000);
 
 }
 
 // the loop routine runs over and over again forever:
 void loop() {
   digitalWrite(LED_BUILTIN, HIGH);   // turn the LED on (HIGH is the voltage level)
-  test_ping();
+  gestion_servo();
   digitalWrite(LED_BUILTIN, LOW);    // turn the LED off by making the voltage LOW
-  test_ping();
+  gestion_servo();
 }
 
-void test_ping()
+void gestion_servo(){
+  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;
@@ -108,6 +86,7 @@ void test_ping()
     if(found == 0){
       found = 1;
       lire_tous_les_registres(ID);
+      return TERMINE;
     }
     delay(100);
   }else{
@@ -115,6 +94,55 @@ void test_ping()
     found = 0;
     delay(2000);
   }
+  return EN_COURS;
+}
+
+enum etat_t configure_servomoteur(){
+  sms_sts.writeByte(SERVO_ID, SMS_STS_MODE, 3);		
+  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, désactivation de la butée
+  sms_sts.writeByte(SERVO_ID, 0x0C, 0); // Buté max à 0, désactivation de la butée
+
+  sms_sts.writeByte(SERVO_ID, 0x1E, 3);	
+  sms_sts.writeByte(SERVO_ID, 0x12, 0x6C);	// Registre de "Phase", valeur par défaut 108 (0x6C)
+
+
+  return TERMINE;
+}
+
+enum etat_t mouvement_servomoteur(){
+  static int position=0;
+  static int temps_pas_ms = 0;
+  static int temps_aff_ms = 0;
+  static int pas_servo = 2048;
+  int position_lue;
+  char tampon[200];
+  /// Toutes les 500 ms
+  if(millis() - temps_pas_ms > 5000 ){
+    temps_pas_ms = millis();
+    // On avance ou recule de d'un pas
+    position += pas_servo;
+    sms_sts.WritePosEx(SERVO_ID, pas_servo, 2000);
+    // si position > 5000 ou position < 0
+    if(position > 35100 || position < 0){
+      pas_servo = -pas_servo;
+    }
+    
+  }
+
+  if(millis() - temps_aff_ms > 10 ){
+    temps_aff_ms = millis();
+    sprintf(tampon, ">pos_consigne:%d\n", position);
+    Serial.print(tampon);
+    position_lue = sms_sts.ReadPos(SERVO_ID);
+    if(position_lue != 32769){
+      sprintf(tampon, ">pos_actuelle:%d\n", position_lue);
+    }
+    Serial.print(tampon);
+  }
+  
+  return EN_COURS;
 }
 
 int lire_registre(int servo_id, int registre_adresse){
@@ -124,7 +152,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\t\t%s\n", i, lire_registre(servo_id, i), registre_description[i]);
+    sprintf(message, "registre 0x%x: %d\n", i, lire_registre(servo_id, i));
     Serial.print(message);
   }