v1007 | 舵机支持读取寄存器

a8000_protocol

@@ -1 +1 @@
-Subproject commit d8fd6a265f51579472c70d8682a0e3bd57d1348a
+Subproject commit 4ed86854f7c2a6906d17b922d9af85677fb31033

sdk/components/mini_servo_motor/feite_servo_motor.cpp

@@ -14,7 +14,7 @@ using namespace feite;
     return false;                                  \
   }
 
-#define DEBUG_MODE 0
+#define DEBUG_MODE 1
 
 static void dumphex(const char* tag, uint8_t* data, uint8_t len) {
 #if DEBUG_MODE
@@ -154,7 +154,7 @@ bool FeiTeServoMotor::ping(uint8_t id) {
   ping_cmd.cmd      = kping;
   ping_cmd.checksum = checksum_packet((uint8_t*)&ping_cmd, sizeof(ping_cmd_t));
 
-  return tx_and_rx((uint8_t*)&ping_cmd, sizeof(ping_cmd_t), (uint8_t*)&ping_resp, sizeof(ping_resp_t), OVERTIME);
+  return tx_and_rx((cmd_header_t*)&ping_cmd, sizeof(ping_cmd_t), (receipt_header_t*)&ping_resp, sizeof(ping_resp_t), OVERTIME);
 }
 
 bool FeiTeServoMotor::stop(uint8_t id) {
@@ -572,6 +572,7 @@ bool FeiTeServoMotor::getPos(uint8_t id, int32_t* pos) {
 }
 bool FeiTeServoMotor::isMove(uint8_t id, int32_t* move) {
   zlock_guard l(m_mutex);
+  *move = 0;
   return read_reg(id, kRegServoMoveFlag, move);
 }
 
@@ -700,19 +701,10 @@ bool FeiTeServoMotor::write_regs(uint8_t id, bool async, uint8_t add, uint8_t* d
 
   uint8_t checksum         = checksum_packet((uint8_t*)cmd_header, txpacketlen);
   m_txbuf[txpacketlen - 1] = checksum;
-  if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) {
+  if (!tx_and_rx(cmd_header, txpacketlen, receipt_header, rxpacketlen, OVERTIME)) {
     ZLOGE(TAG, "write_reg id:%d add:%d len:%d fail,overtime", id, add, len);
     return false;
   }
-  if (!(receipt_header->header == 0xffff && receipt_header->id == id)) {
-    ZLOGE(TAG, "write_reg id:%d add:%d len:%d fail,receipt header error", id, add, len);
-    return false;
-  }
-
-  if (receipt_header->status != 0 && add != kRegServoTorqueSwitch) {
-    ZLOGE(TAG, "write_reg id:%d add:%d len:%d fail,receipt status error %d", id, add, len, receipt_header->status);
-    return false;
-  }
   return true;
 }
 bool FeiTeServoMotor::read_regs(uint8_t id, uint8_t add, uint8_t* data, uint8_t len) {
@@ -737,26 +729,27 @@ bool FeiTeServoMotor::read_regs(uint8_t id, uint8_t add, uint8_t* data, uint8_t
   ZASSERT((size_t)rxpacketlen < sizeof(m_rxbuf));
 
   m_txbuf[txpacketlen - 1] = checksum;
-  if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) {
+  if (!tx_and_rx(cmd_header, txpacketlen, receipt_header, rxpacketlen, OVERTIME)) {
     ZLOGE(TAG, "read_reg id:%d add:%d len:%d fail,overtime", id, add, len);
     return false;
   }
-  if (!(receipt_header->header == 0xffff && receipt_header->id == id)) {
-    ZLOGE(TAG, "read_reg id:%d add:%d len:%d fail,receipt header error", id, add, len);
-    ZLOGE(TAG, "read_reg fail,receipt header error");
-    return false;
-  }
   memcpy(data, receipt_header->data, len);
   osDelay(10);
   return true;
 }
-bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems) {
+bool FeiTeServoMotor::tx_and_rx(cmd_header_t* txheader, uint8_t txdatalen, receipt_header_t* rx, uint8_t rxlen, int32_t overtimems) {
   for (int i = 0; i < 5; i++) {
-    bool suc = _tx_and_rx(tx, txdatalen, rx, expectrxsize, overtimems);
-    if (suc) {
-      return true;
+    if (i != 0) osDelay(30);
+
+    bool suc = _tx_and_rx((uint8_t*)txheader, txdatalen, (uint8_t*)rx, rxlen, overtimems);
+    if (!suc) continue;
+
+    if (!(rx->header == 0xffff && rx->id == txheader->id)) {
+      ZLOGE(TAG, "receipt format error header->%04x id:%d!=%d", rx->header, rx->id, txheader->id);
+      return false;
     }
-    osDelay(100);
+    m_status[txheader->id] = rx->status;
+    return true;
   }
   return false;
 }

sdk/components/mini_servo_motor/feite_servo_motor.hpp

@@ -101,13 +101,13 @@ typedef enum {
   kRegServoLockFlag          = 55,  // 锁标志
   kRegServoCurrentPos        = 56,  // 当前位置
   kRegServoCurrentSpeed      = 58,  // 当前速度
-  kRegServoCurrentLoad       = 60,  // 当前负载 bit10为方向位
-  kRegServoCurrentVoltage    = 62,  // 当前电压
-  kRegServoCurrentTemp       = 63,  // 当前温度
+  kRegServoCurrentLoad       = 60,  // 当前负载 bit10为方向位 输出驱动电机的当前占空比电压,单位为0.1%,取值0-1000
+  kRegServoCurrentVoltage    = 62,  // 当前电压 反馈当前舵机工作电压,反馈精度为0.1V,即120*0.1=12V
+  kRegServoCurrentTemp       = 63,  // 当前温度 反馈当前舵机内部工作温度,反馈精度为1摄氏度
   kRegServoAsyncWriteFlag    = 64,  // 异步写标志
   kRegServoStatus            = 65,  // 舵机状态
   kRegServoMoveFlag          = 66,  // 移动标志
-  kRegServoCurrentCurrent    = 69,  // 当前电流
+  kRegServoCurrentCurrent    = 69,  // 当前电流 反馈当前工作电流值,单位为6.26.5mA,最大可反馈电流为500*6.5mA=3250mA
   kRegServoCheckSpeed        = 80,  // 80 移动检查速度
   kRegServoDTime             = 81,  // 81 D控制时间
   kRegServoSpeedUnit         = 82,  // 82 速度单位系数
@@ -163,6 +163,7 @@ class FeiTeServoMotor {
 
   feite_servo_info_t m_feite_servo_info[10];
   int32_t            m_num_feite_servo_info = 0;
+  uint8_t            m_status[255]          = {0};
 
  public:
   void initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx);
@@ -170,8 +171,9 @@ class FeiTeServoMotor {
   void                regServo(uint8_t id, feite_servo_model_number_t model_type, bool shaft);
   feite_servo_info_t* getServoInfo(int id);
 
-  bool ping(uint8_t id);
-  bool stop(uint8_t id);
+  bool    ping(uint8_t id);
+  bool    stop(uint8_t id);
+  uint8_t getStatus(uint8_t id) { return m_status[id]; }
 
   bool initServo(uint8_t id);
 
@@ -214,7 +216,7 @@ class FeiTeServoMotor {
 
  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);
+  bool    tx_and_rx(cmd_header_t* txheader, uint8_t txdatalen, receipt_header_t* rx, uint8_t rxlen, int32_t overtimems);
   uint8_t checksum(uint8_t* data, uint8_t len);
   uint8_t checksum_packet(uint8_t* data, uint8_t len);
 

sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -1,4 +1,6 @@
 #include "mini_servo_motor_ctrl_module.hpp"
+
+#include <string.h>
 using namespace iflytop;
 using namespace std;
 #define TAG "MiniServo"
@@ -36,6 +38,12 @@ int32_t MiniServoCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t
     PROCESS_REG(kreg_mini_servo_protective_torque, REG_GET(m_cfg.protective_torque), REG_SET(m_cfg.protective_torque));
     PROCESS_REG(kreg_mini_servo_is_move, mini_servo_is_move(&val), ACTION_NONE);
 
+    PROCESS_REG(kreg_mini_servo_status, mini_servo_read_reg(kreg_mini_servo_status, &val), ACTION_NONE);
+    PROCESS_REG(kreg_mini_servo_voltage, mini_servo_read_reg(kreg_mini_servo_voltage, &val), ACTION_NONE);
+    PROCESS_REG(kreg_mini_servo_current, mini_servo_read_reg(kreg_mini_servo_current, &val), ACTION_NONE);
+    PROCESS_REG(kreg_mini_servo_temperature, mini_servo_read_reg(kreg_mini_servo_temperature, &val), ACTION_NONE);
+    PROCESS_REG(kreg_mini_servo_loadvalue, mini_servo_read_reg(kreg_mini_servo_loadvalue, &val), ACTION_NONE);
+
     default:
       return err::kmodule_not_find_reg;
       break;
@@ -61,6 +69,40 @@ int32_t MiniServoCtrlModule::mini_servo_read_pos(int32_t *pos) {
   if (!m_bus->getPos(m_idinbus, pos)) return err::ksubdevice_overtime;
   return 0;
 }
+
+int32_t MiniServoCtrlModule::mini_servo_read_reg(int32_t regoff, int32_t *regval) {
+  if (regoff == kreg_mini_servo_status) {
+    bool suc = m_bus->read_reg(m_idinbus, kRegServoStatus, regval);
+    if (!suc) return err::ksubdevice_overtime;
+    return 0;
+  }
+
+  if (regoff == kreg_mini_servo_voltage) {
+    bool suc = m_bus->read_reg(m_idinbus, kRegServoCurrentVoltage, regval);
+    if (!suc) return err::ksubdevice_overtime;
+    return 0;
+  }
+
+  if (regoff == kreg_mini_servo_current) {
+    bool suc = m_bus->read_reg(m_idinbus, kRegServoCurrentCurrent, regval);
+    if (!suc) return err::ksubdevice_overtime;
+    *regval = 6.5 * (*regval);
+    return 0;
+  }
+
+  if (regoff == kreg_mini_servo_temperature) {
+    bool suc = m_bus->read_reg(m_idinbus, kRegServoCurrentTemp, regval);
+    if (!suc) return err::ksubdevice_overtime;
+    return 0;
+  }
+
+  if (regoff == kreg_mini_servo_loadvalue) {
+    bool suc = m_bus->read_reg(m_idinbus, kRegServoCurrentLoad, regval);
+    if (!suc) return err::ksubdevice_overtime;
+    return 0;
+  }
+}
+
 int32_t MiniServoCtrlModule::mini_servo_active_cfg() { return 0; }
 int32_t MiniServoCtrlModule::mini_servo_stop(int32_t breakstop) {
   m_thread.stop();
@@ -74,16 +116,19 @@ int32_t MiniServoCtrlModule::mini_servo_rotate(int32_t speed) {
   if (!m_state.enable) return err::kmini_servo_not_enable;
   if (!m_bus->isSupportMode1(m_idinbus)) return err::kmini_servo_mode_not_support;
   creg.module_status = 1;
+
   m_thread.start(
       [this, speed]() {
         befor_motor_move();
+        servo_status_t status = {0};
 
         {
           m_bus->runInMode1(m_idinbus, m_cfg.limit_torque, speed);
 
           while (true) {
             if (m_thread.getExitFlag()) break;
-            if (!check_when_run()) break;
+            if (!check_when_run(&status)) break;
+
             vTaskDelay(5);
           }
         }
@@ -114,7 +159,8 @@ int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) {
         befor_motor_move();
 
         {
-          int32_t velocity = m_cfg.limit_velocity;
+          int32_t        velocity = m_cfg.limit_velocity;
+          servo_status_t status   = {0};
 
           bool callsuc = false;
           for (size_t i = 0; i < 3; i++) {
@@ -128,18 +174,12 @@ int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) {
             return;
           }
 
-          int32_t moveflag = 0;
-          int32_t nowpos   = 0;
 
           while (true) {
             if (m_thread.getExitFlag()) break;
-            if (!check_when_run()) break;
+            if (!check_when_run(&status)) break;
 
-            moveflag = 0;
-            m_bus->isMove(m_idinbus, &moveflag);
-            m_bus->getPos(m_idinbus, &nowpos);
-
-            if (poseq(nowpos, pos3600, 10) && moveflag == 0) break;
+            if (poseq(status.pos, pos3600, 10) && status.isMove == 0) break;
 
             vTaskDelay(5);
           }
@@ -165,13 +205,13 @@ int32_t MiniServoCtrlModule::mini_servo_rotate_with_torque(int32_t torque) {
   m_thread.start(
       [this, torque]() {
         befor_motor_move();
-
+        servo_status_t status = {0};
         {
           m_bus->runInMode2(m_idinbus, torque);
 
           while (true) {
             if (m_thread.getExitFlag()) break;
-            if (!check_when_run()) break;
+            if (!check_when_run(&status)) break;
             vTaskDelay(5);
           }
         }
@@ -224,7 +264,32 @@ void MiniServoCtrlModule::after_motor_move() {
     creg.module_status = 0;
   }
 }
-bool MiniServoCtrlModule::check_when_run() { return true; }
+bool MiniServoCtrlModule::check_when_run(servo_status_t *status) {
+  memset(status, 0, sizeof(servo_status_t));
+
+  int32_t moveflag = 0;
+  int32_t nowpos   = 0;
+  bool    suc      = m_bus->isMove(m_idinbus, &moveflag);
+  if (!suc) {
+    return false;
+  }
+  suc = m_bus->getPos(m_idinbus, &nowpos);
+  if (!suc) {
+    return false;
+  }
+
+  if (m_bus->getStatus(m_idinbus) != 0) {
+    status->status = m_bus->getStatus(m_idinbus);
+    ZLOGE(TAG, "check fail, status = %x", status->status);
+    return false;
+  }
+
+  status->isMove = moveflag;
+  status->pos    = nowpos;
+  status->status = m_bus->getStatus(m_idinbus);
+  ZLOGI(TAG, "moduleid:%d idinbus:%d status %x isMove %d pos %d", m_module_id, m_idinbus, status->status, status->isMove, status->pos);
+  return true;
+}
 
 /***********************************************************************************************************************
  *                                                      INTERNAL                                                       *

sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp

@@ -22,6 +22,12 @@ class MiniServoCtrlModule : public ZIModule {
     int32_t enable;
   } state_t;
 
+  typedef struct {
+    uint8_t status;
+    int32_t pos;
+    int32_t isMove;
+  } servo_status_t;
+
  private:
   uint8_t m_module_id = 0;
 
@@ -62,7 +68,9 @@ class MiniServoCtrlModule : public ZIModule {
  private:
   void befor_motor_move();
   void after_motor_move();
-  bool check_when_run();
+  bool check_when_run(servo_status_t *status);
+
+  virtual int32_t mini_servo_read_reg(int32_t regoff, int32_t *regval);
 
   int32_t mini_servo_is_move(int32_t *isMove);