update

components/mini_servo_motor/feite_servo_motor.cpp

@@ -14,7 +14,7 @@ using namespace feite;
     return false;                                  \
   }
 
-#define DEBUG_MODE 1
+#define DEBUG_MODE 0
 
 static void dumphex(const char* tag, uint8_t* data, uint8_t len) {
 #if DEBUG_MODE
@@ -171,7 +171,7 @@ bool FeiTeServoMotor::stop(uint8_t id) {
 
   } else {
     DO(write_reg(id, kRegServoTorqueSwitch, 0));
-    DO(write_reg(id, kRegServoRunMode, 0));
+    DO(write_reg(id, kRegServoRunSpeed, 0));
     DO(write_reg(id, kRegServoTorqueSwitch, 1));
     return true;
   }
@@ -219,8 +219,13 @@ bool FeiTeServoMotor::runInMode0(uint8_t id, int32_t limitTorque, int32_t speed,
     // stop(id);
     int32_t val = 0;
 
+    int32_t nowmode = 0;
+    DO(read_reg(id, kRegServoRunMode, &nowmode));
+    if (nowmode != 0) {
+      DO(write_reg(id, kRegServoRunMode, (uint8_t)0));
+    }
+
     DO(write_reg(id, kRegServoTorqueSwitch, 1));
-    DO(write_reg(id, kRegServoRunMode, (uint8_t)0));
     DO(write_reg(id, kRegServoTorqueLimit, limitTorque));
     DO(write_reg(id, kRegServoRunSpeed, speed));
     DO(write_reg(id, kRegServoTargetPos, motor_pos));
@@ -266,8 +271,11 @@ bool FeiTeServoMotor::runInMode1(uint8_t id, int32_t limitTorque, int32_t speed)
     if (limitTorque <= 0) limitTorque = 0;
     if (limitTorque > 1000) limitTorque = 1000;
     stop(id);
-
-    DO(write_reg(id, kRegServoRunMode, (uint8_t)1));
+    int32_t nowmode = 0;
+    DO(read_reg(id, kRegServoRunMode, &nowmode));
+    if (nowmode != 1) {
+      DO(write_reg(id, kRegServoRunMode, (uint8_t)1));
+    }
     DO(write_reg(id, kRegServoTorqueLimit, limitTorque));
     DO(write_reg(id, kRegServoRunSpeed, speed));
     return true;