From 4f2d941258a3c2a76b91d5e9f10df462eec66d64 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sat, 27 Apr 2024 16:18:19 +0800 Subject: [PATCH] =?UTF-8?q?=E9=A3=9E=E7=89=B9=E7=94=B5=E6=9C=BA=E6=94=AF?= =?UTF-8?q?=E6=8C=81=E4=B8=8A=E4=BC=A0=E6=B8=A9=E5=BA=A6=E9=94=99=E8=AF=AF?= =?UTF-8?q?=E7=AD=89=E7=8A=B6=E6=80=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- components/mini_servo_motor/feite_servo_motor.cpp | 34 ++++++++++++++++++++-- components/mini_servo_motor/feite_servo_motor.hpp | 6 ++++ .../mini_servo_motor_ctrl_module.cpp | 23 +++++++++++++++ .../mini_servo_motor_ctrl_module.hpp | 5 ++++ .../step_motor_ctrl_module/tmc51x0_motor.cpp | 4 ++- .../step_motor_ctrl_module/tmc51x0_motor.hpp | 6 ++-- .../zprotocols/zcancmder_v2/api/reg_index.hpp | 5 ++++ .../zprotocols/zcancmder_v2/api/zi_motor.hpp | 1 + components/zprotocols/zcancmder_v2/cmdid.hpp | 1 + 9 files changed, 80 insertions(+), 5 deletions(-) diff --git a/components/mini_servo_motor/feite_servo_motor.cpp b/components/mini_servo_motor/feite_servo_motor.cpp index 219af11..fabe300 100644 --- a/components/mini_servo_motor/feite_servo_motor.cpp +++ b/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; diff --git a/components/mini_servo_motor/feite_servo_motor.hpp b/components/mini_servo_motor/feite_servo_motor.hpp index f88e1fe..3f72343 100644 --- a/components/mini_servo_motor/feite_servo_motor.hpp +++ b/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); 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 d363339..95ca4de 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/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; +} diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp index aa2b607..5350bd8 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/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); diff --git a/components/step_motor_ctrl_module/tmc51x0_motor.cpp b/components/step_motor_ctrl_module/tmc51x0_motor.cpp index 1f5d74c..d4f2494 100644 --- a/components/step_motor_ctrl_module/tmc51x0_motor.cpp +++ b/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; @@ -913,4 +914,5 @@ int32_t TMC51X0Motor::_exec_move_to_zero_task() { ZLOGI(TAG, "move_to_zero success"); return 0; -} \ No newline at end of file +} +#endif \ No newline at end of file diff --git a/components/step_motor_ctrl_module/tmc51x0_motor.hpp b/components/step_motor_ctrl_module/tmc51x0_motor.hpp index 1da1d93..86ad449 100644 --- a/components/step_motor_ctrl_module/tmc51x0_motor.hpp +++ b/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); @@ -212,4 +212,6 @@ class TMC51X0Motor : public ZIModule, public ZIMotor { int32_t _exec_move_to_io_task(int32_t ioindex, int32_t direction); int32_t _exec_move_to_io_task_fn(int32_t ioindex, int32_t direction); }; -} // namespace iflytop \ No newline at end of file +} // namespace iflytop + +#endif \ No newline at end of file diff --git a/components/zprotocols/zcancmder_v2/api/reg_index.hpp b/components/zprotocols/zcancmder_v2/api/reg_index.hpp index f463eab..afd2e3a 100644 --- a/components/zprotocols/zcancmder_v2/api/reg_index.hpp +++ b/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 * *******************************************************************************/ diff --git a/components/zprotocols/zcancmder_v2/api/zi_motor.hpp b/components/zprotocols/zcancmder_v2/api/zi_motor.hpp index e99ddba..9205bfe 100644 --- a/components/zprotocols/zcancmder_v2/api/zi_motor.hpp +++ b/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, diff --git a/components/zprotocols/zcancmder_v2/cmdid.hpp b/components/zprotocols/zcancmder_v2/cmdid.hpp index 59101cf..8a44377 100644 --- a/components/zprotocols/zcancmder_v2/cmdid.hpp +++ b/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; }