修复舵机上电位置错误的BUG

sdk/components/mini_servo_motor/feite_servo_motor.cpp

@@ -162,7 +162,7 @@ bool FeiTeServoMotor::stop(uint8_t id) {
   feite_servo_info_t* info = getServoInfo(id);
 
   if (!info) {
-    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    ZLOGE(TAG, "servo info id:%d not find", id);
     return false;
   }
   if (info->isSCS) {
@@ -184,7 +184,7 @@ bool FeiTeServoMotor::runInMode0(uint8_t id, int32_t limitTorque, int32_t speed,
   feite_servo_info_t* info = getServoInfo(id);
 
   if (!info) {
-    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    ZLOGE(TAG, "servo info id:%d not find", id);
     return false;
   }
 
@@ -252,7 +252,7 @@ bool FeiTeServoMotor::setInMode0(uint8_t id) {
   feite_servo_info_t* info = getServoInfo(id);
 
   if (!info) {
-    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    ZLOGE(TAG, "servo info id:%d not find", id);
     return false;
   }
 
@@ -266,21 +266,59 @@ bool FeiTeServoMotor::setInMode0(uint8_t id) {
   } else {
     int32_t val     = 0;
     int32_t nowmode = 0;
-    DO(read_reg(id, kRegServoRunMode, &nowmode));
-    if (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));
+
+    return true;
+  }
 }
+bool FeiTeServoMotor::initServo(uint8_t id) {
+  zlock_guard         l(m_mutex);
+  feite_servo_info_t* info = getServoInfo(id);
+
+  if (!info) {
+    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    return false;
+  }
+
+  if (info->isSCS) {
+    // 由于SCS型号的舵机如果当前工作在模式2下，
+    //      则 kRegServoRunTime 控制运行方向和扭矩，这里需要将其修改为0，让其停下来
+    stop(id);
+    DO(write_reg(id, kRegServoMinAngle, info->regDefaultVal_servoMinAngle));
+    DO(write_reg(id, kRegServoMaxAngle, info->regDefaultVal_servoMaxAngle));
+    return true;
+  } else {
+    ZLOGI(TAG, "initServo id:%d", id);
+    DO(write_reg(id, kRegServoRunMode, (uint8_t)0));
     DO(write_reg(id, kRegServoTorqueSwitch, 1));
 
+    int32_t servoCalibration = 0;
+    int32_t servoCurrentPos  = 0;
+    DO(read_reg(id, kRegServoCalibration, &servoCalibration));
+    DO(read_reg(id, kRegServoCurrentPos, &servoCurrentPos));
+    ZLOGI(TAG, "servoCalibration:%d  servoCurrentPos:%d", servoCalibration, servoCurrentPos);
+
+    // DO(write_reg(id, kRegServoTorqueLimit, 10));
+    // DO(write_reg(id, kRegServoRunSpeed, 0));
+    // DO(write_reg(id, kRegServoTargetPos, info->range / 2));
+
+    osDelay(100);
+    stop(id);
+
     return true;
   }
 }
+
 bool FeiTeServoMotor::setInMode1(uint8_t id) {
   zlock_guard         l(m_mutex);
   feite_servo_info_t* info = getServoInfo(id);
 
   if (!info) {
-    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    ZLOGE(TAG, "servo info id:%d not find", id);
     return false;
   }
   if (!info->support_mode1) {
@@ -340,7 +378,7 @@ bool FeiTeServoMotor::runInMode1(uint8_t id, int32_t limitTorque, int32_t speed)
   feite_servo_info_t* info = getServoInfo(id);
 
   if (!info) {
-    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    ZLOGE(TAG, "servo info id:%d not find", id);
     return false;
   }
   if (!info->support_mode1) {
@@ -388,7 +426,7 @@ bool FeiTeServoMotor::setCurPos(uint8_t id, int32_t pos3600) {
   feite_servo_info_t* info = getServoInfo(id);
 
   if (!info) {
-    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    ZLOGE(TAG, "servo info id:%d not find", id);
     return false;
   }
   if (info->isSCS) {
@@ -411,7 +449,7 @@ bool FeiTeServoMotor::isSupportSetCurPos(uint8_t id) {
   feite_servo_info_t* info = getServoInfo(id);
 
   if (!info) {
-    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    ZLOGE(TAG, "servo info id:%d not find", id);
     return false;
   }
   if (info->isSCS) {
@@ -450,7 +488,7 @@ bool FeiTeServoMotor::runInMode2(uint8_t id, int32_t torque) {
   feite_servo_info_t* info = getServoInfo(id);
 
   if (!info) {
-    ZLOGE(TAG, "runInMode0 id:%d not find", id);
+    ZLOGE(TAG, "servo info id:%d not find", id);
     return false;
   }
   if (!info->support_mode2) {

sdk/components/mini_servo_motor/feite_servo_motor.hpp

@@ -173,6 +173,8 @@ class FeiTeServoMotor {
   bool ping(uint8_t id);
   bool stop(uint8_t id);
 
+  bool initServo(uint8_t id);
+
   bool setInMode0(uint8_t id);
   bool setInMode1(uint8_t id);
   bool setInMode2(uint8_t id);

sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -13,7 +13,7 @@ void MiniServoCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, u
   m_cfg = *cfg;
 
   m_thread.init("MiniRobotCtrlModule", 1024, osPriorityNormal);
-  m_bus->setInMode0(idinbus);  // 设置成模式0，这样能保证回读的位置是正确的。
+  m_bus->initServo(idinbus);  // 设置成模式0，这样能保证回读的位置是正确的。
   mini_servo_enable(1);
 }
 void MiniServoCtrlModule::create_default_config(config_t &cfg) {

usrc/subboards/subboard30_shake_module/subboard30_shake_module.cpp

@@ -17,7 +17,7 @@ using namespace iflytop;
 void Subboard30ShakeModule::initialize() {
   IO_INIT();
   GService::inst()->getZCanProtocolParser()->registerModule(this);
-
+  osDelay(1000);  // 等待舵机上电
 #if 1
 
   TMC5130_MOTOR_INITER(/*motorid:*/ 1, /*moduleid:*/ 1);