Mode 3: Pas à pas

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

@@ -49,7 +49,7 @@ void gestion_servo(){
   switch(etat_gestion_servo){
     case PING_SERVO:
       if(test_ping() == TERMINE){
-        etat_gestion_servo = CONFIG_SERVO;
+//        etat_gestion_servo = CONFIG_SERVO;
       }
       break;
 
@@ -98,14 +98,14 @@ enum etat_t test_ping()
 }
 
 enum etat_t configure_servomoteur(){
-  sms_sts.ServoMode(SERVO_ID);
+  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, pour activer le multitour
+  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, pour activer le multitour
+  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, 0x7C);	// Registre de "Phase", valeur par défaut 108 (0x6C)
+  sms_sts.writeByte(SERVO_ID, 0x12, 0x6C);	// Registre de "Phase", valeur par défaut 108 (0x6C)
 
 
   return TERMINE;
@@ -115,19 +115,20 @@ 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 = 512;
+  static int pas_servo = 2048;
   int position_lue;
   char tampon[200];
   /// Toutes les 500 ms
-  if(millis() - temps_pas_ms > 500 ){
+  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 > 20100 || position < 0){
+    if(position > 35100 || position < 0){
       pas_servo = -pas_servo;
     }
-    sms_sts.WritePosEx(SERVO_ID, position, 4800);
+    
   }
 
   if(millis() - temps_aff_ms > 10 ){