From 6ecb1672839f07ba9f1cc8099139fecd3b5899d1 Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Fri, 28 Jun 2024 20:27:24 +0800
Subject: [PATCH] =?UTF-8?q?=E9=A3=9E=E7=89=B9=E7=94=B5=E6=9C=BA=E5=A2=9E?=
 =?UTF-8?q?=E5=8A=A0=E8=87=AA=E5=8A=A8=E9=87=8D=E5=8F=91=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 components/mini_servo_motor/feite_servo_motor.cpp  | 11 +++++++++++
 components/mini_servo_motor/feite_servo_motor.hpp  |  1 +
 .../mini_servo_motor_ctrl_module.cpp               | 22 +++++++++++++++++-----
 3 files changed, 29 insertions(+), 5 deletions(-)

diff --git a/components/mini_servo_motor/feite_servo_motor.cpp b/components/mini_servo_motor/feite_servo_motor.cpp
index 6f1a15b..b64326f 100644
--- a/components/mini_servo_motor/feite_servo_motor.cpp
+++ b/components/mini_servo_motor/feite_servo_motor.cpp
@@ -536,6 +536,17 @@ bool FeiTeServoMotor::read_regs(uint8_t id, uint8_t add, uint8_t* data, uint8_t
   return true;
 }
 bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems) {
+  for (int i = 0; i < 3; i++) {
+    bool suc = _tx_and_rx(tx, txdatalen, rx, expectrxsize, overtimems);
+    if (suc) {
+      return true;
+    }
+    osDelay(1);
+  }
+  return false;
+}
+
+bool FeiTeServoMotor::_tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems) {
   uint32_t enter_ticket = HAL_GetTick();
   dumphex("tx:", tx, txdatalen);
 #if 1
diff --git a/components/mini_servo_motor/feite_servo_motor.hpp b/components/mini_servo_motor/feite_servo_motor.hpp
index b5cb731..6e521df 100644
--- a/components/mini_servo_motor/feite_servo_motor.hpp
+++ b/components/mini_servo_motor/feite_servo_motor.hpp
@@ -198,6 +198,7 @@ class FeiTeServoMotor {
   bool read_regs(uint8_t id, uint8_t add, uint8_t* data, uint8_t len);
 
  private:
+  bool    _tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems);
   bool    tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems);
   uint8_t checksum(uint8_t* data, uint8_t len);
   uint8_t checksum_packet(uint8_t* data, uint8_t len);
diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
index 52163af..4d4ca2b 100644
--- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
+++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp
@@ -106,7 +106,17 @@ int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) {
         {
           int32_t velocity = m_cfg.limit_velocity;
 
-          m_bus->runInMode0(m_idinbus, m_cfg.limit_torque, m_cfg.limit_velocity, pos3600);
+          bool callsuc = false;
+          for (size_t i = 0; i < 3; i++) {
+            callsuc = m_bus->runInMode0(m_idinbus, m_cfg.limit_torque, m_cfg.limit_velocity, pos3600);
+            if (!callsuc) continue;
+            break;
+          }
+
+          if (!callsuc) {
+            creg.module_errorcode = err::ksubdevice_overtime;
+            return;
+          }
 
           int32_t moveflag = 0;
           int32_t nowpos   = 0;
@@ -125,10 +135,12 @@ int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) {
           }
         }
 
-        after_motor_move();
         ZLOGI(TAG, "move to pos %d", pos3600);
       },
-      [this]() { inter_mini_servo_stop(0); });
+      [this]() {
+        after_motor_move();
+        inter_mini_servo_stop(0);
+      });
 
   return 0;
 }
@@ -174,8 +186,8 @@ int32_t MiniServoCtrlModule::mini_servo_read_io_state(int32_t ioindex, int32_t *
 }
 
 void MiniServoCtrlModule::befor_motor_move() {
-  creg.module_status      = 1;
-  creg.module_errorcode     = 0;
+  creg.module_status    = 1;
+  creg.module_errorcode = 0;
 }
 void MiniServoCtrlModule::after_motor_move() {
   if (creg.module_errorcode != 0) {