飞特电机支持上传温度错误等状态

components/mini_servo_motor/feite_servo_motor.cpp

@@ -185,6 +185,36 @@ bool FeiTeServoMotor::read_detailed_status(uint8_t id, detailed_status_t* detail
 
   return true;
 }
+
+bool FeiTeServoMotor::read_voltage(uint8_t id, int32_t* val) {
+  zlock_guard l(m_mutex);
+  uint8_t     buf[2];
+  bool        suc = read_u8(id, kRegServoCurrentVoltage, buf[0]);
+  *val            = buf[0];
+  return suc;
+}
+bool FeiTeServoMotor::read_temperature(uint8_t id, int32_t* val) {
+  zlock_guard l(m_mutex);
+  uint8_t     buf[2];
+  bool        suc = read_u8(id, kRegServoCurrentTemp, buf[0]);
+  *val            = buf[0];
+  return suc;
+}
+bool FeiTeServoMotor::read_error_state(uint8_t id, int32_t* val) {
+  zlock_guard l(m_mutex);
+  uint8_t     buf[2];
+  bool        suc = read_u8(id, kRegServoStatus, buf[0]);
+  *val            = buf[0];
+  return suc;
+}
+bool FeiTeServoMotor::read_electric_current(uint8_t id, int32_t* val) {
+  zlock_guard l(m_mutex);
+  uint16_t    buf;
+  bool        suc = read_u16(id, kRegServoCurrentCurrent, buf);
+  *val            = buf * 6.5;
+  return suc;
+}
+
 void FeiTeServoMotor::dump_status(status_t* status) {
   ZLOGI(TAG, "===========status===========");
   ZLOGI(TAG, "= status->pos        :%d", status->pos);
@@ -320,11 +350,11 @@ bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* da
   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)) {
-    ZLOGE(TAG, "%d write_reg fail,overtime",id);
+    ZLOGE(TAG, "%d write_reg fail,overtime", id);
     return false;
   }
   if (!(receipt_header->header == 0xffff && receipt_header->id == id)) {
-    ZLOGE(TAG, "%d write_reg fail,receipt header error",id);
+    ZLOGE(TAG, "%d write_reg fail,receipt header error", id);
     return false;
   }
   return true;

components/mini_servo_motor/feite_servo_motor.hpp

@@ -181,6 +181,12 @@ class FeiTeServoMotor {
   bool read_status(uint8_t id, status_t* status);
   void dump_status(status_t* status);
   bool read_detailed_status(uint8_t id, detailed_status_t* detailed_status);
+
+  bool read_voltage(uint8_t id,int32_t* val);            // 0.1V
+  bool read_temperature(uint8_t id,int32_t* val);        // 1
+  bool read_error_state(uint8_t id,int32_t* val);        // estate
+  bool read_electric_current(uint8_t id,int32_t* val);  // ma
+
   void dump_detailed_status(detailed_status_t* detailed_status);
 
   bool getMoveFlag(uint8_t id, uint8_t& moveflag);

components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -337,6 +337,12 @@ int32_t MiniRobotCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t
     PROCESS_REG(kreg_motor_default_velocity, REG_GET(m_cfg.default_velocity), REG_SET(m_cfg.default_velocity));
     PROCESS_REG(kreg_module_input_state, module_readio(&val), ACTION_NONE);
     PROCESS_REG(kreg_robot_pos, motor_read_pos(&val), ACTION_NONE);
+
+    PROCESS_REG(kreg_motor_temperature, read_voltage(&val), ACTION_NONE);
+    PROCESS_REG(kreg_motor_error_state, read_temperature(&val), ACTION_NONE);
+    PROCESS_REG(kreg_motor_current, read_error_state(&val), ACTION_NONE);
+    PROCESS_REG(kreg_motor_voltage, read_electric_current(&val), ACTION_NONE);
+
     default:
       return err::kmodule_not_find_config_index;
       break;
@@ -436,3 +442,20 @@ int32_t MiniRobotCtrlModule::motor_easy_set_current_pos(int32_t pos) {
   ZLOGI(TAG, "%d motor_easy_set_current_pos %d", m_module_id, pos);
   return position_calibrate(pos);
 }
+
+int32_t MiniRobotCtrlModule::read_voltage(int32_t *val) {
+  if (!m_bus->read_voltage(m_id, val)) return err::ksubdevice_overtime;
+  return 0;
+}
+int32_t MiniRobotCtrlModule::read_temperature(int32_t *val) {
+  if (!m_bus->read_temperature(m_id, val)) return err::ksubdevice_overtime;
+  return 0;
+}
+int32_t MiniRobotCtrlModule::read_error_state(int32_t *val) {
+  if (!m_bus->read_error_state(m_id, val)) return err::ksubdevice_overtime;
+  return 0;
+}
+int32_t MiniRobotCtrlModule::read_electric_current(int32_t *val) {
+  if (!m_bus->read_electric_current(m_id, val)) return err::ksubdevice_overtime;
+  return 0;
+}

components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp

@@ -98,6 +98,11 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI
   virtual int32_t motor_easy_move_to_zero(int32_t direction) override;
   virtual int32_t motor_easy_set_current_pos(int32_t pos) override;
 
+  virtual int32_t read_voltage(int32_t *val);
+  virtual int32_t read_temperature(int32_t *val);
+  virtual int32_t read_error_state(int32_t *val);
+  virtual int32_t read_electric_current(int32_t *val);
+
  private:
   void    call_status_cb(action_cb_status_t cb, int32_t status);
   void    set_errorcode(int32_t errorcode);

components/step_motor_ctrl_module/tmc51x0_motor.cpp

@@ -7,6 +7,7 @@
 #include "sdk\components\zprotocols\zcancmder_v2\api\errorcode.hpp"
 using namespace iflytop;
 #define TAG "SMCM"
+#ifdef HAL_SPI_MODULE_ENABLED
 
 void TMC51X0Motor::initialize(int moduleid, TMC5130* stepM, ZGPIO iotable[], int nio, const char* flashmark, flash_config_t* defaultcfg) {
   m_id      = moduleid;
@@ -914,3 +915,4 @@ int32_t TMC51X0Motor::_exec_move_to_zero_task() {
 
   return 0;
 }
+#endif

components/step_motor_ctrl_module/tmc51x0_motor.hpp

@@ -4,7 +4,7 @@
 #include "sdk\components\tmc\basic\tmc_ic_interface.hpp"
 #include "sdk\components\tmc\ic\ztmc5130.hpp"
 #include "sdk\components\zcancmder\zcanreceiver.hpp"
-
+#ifdef HAL_SPI_MODULE_ENABLED
 namespace iflytop {
 class TMC51X0Motor : public ZIModule, public ZIMotor {
   ENABLE_MODULE(TMC51X0Motor, kmotor_module, 0x0001);
@@ -213,3 +213,5 @@ class TMC51X0Motor : public ZIModule, public ZIMotor {
   int32_t _exec_move_to_io_task_fn(int32_t ioindex, int32_t direction);
 };
 }  // namespace iflytop
+
+#endif

components/zprotocols/zcancmder_v2/api/reg_index.hpp

@@ -221,6 +221,11 @@ typedef enum {
   kreg_motor_max_x_d = REG_INDEX(11, 50, 28),  // 最大限制距离
   kreg_motor_min_x_d = REG_INDEX(11, 50, 29),  // 最小限制距离
 
+  kreg_motor_temperature = REG_INDEX(11, 50, 30),  //
+  kreg_motor_error_state = REG_INDEX(11, 50, 31),  //
+  kreg_motor_current     = REG_INDEX(11, 50, 32),  //
+  kreg_motor_voltage     = REG_INDEX(11, 50, 33),  //
+
   /*******************************************************************************
    *                                   MOTOR_Y                                   *
    *******************************************************************************/

components/zprotocols/zcancmder_v2/api/zi_motor.hpp

@@ -47,6 +47,7 @@ class ZIMotor {
   virtual int32_t motor_set_subdevice_reg(int32_t reg_addr, int32_t reg_val) { return err::koperation_not_support; }
   virtual int32_t motor_get_subdevice_reg(int32_t reg_addr, int32_t* reg_val) { return err::koperation_not_support; }
 
+
   // virtual int32_t motor_set_shaft();
 
   // s32 pos, s32 speed, s32 torque,

components/zprotocols/zcancmder_v2/cmdid.hpp

@@ -100,6 +100,7 @@ typedef enum {
   kmotor_set_enc_resolution = CMDID(2, 26),  // para:{4}, ack:{}
   kmotor_get_enc_resolution = CMDID(2, 27),  // para:{}, ack:{4}
 
+
 #if 0
   virtual ~ZIXYMotor() {}
   virtual int32_t xymotor_enable(int32_t enable) { return err::koperation_not_support; }