From c13f0f786605b749458956e3b77c66b065315c7a Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 15 Oct 2023 10:55:42 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E6=88=90eq20=E4=B8=8A=E4=BD=8D?= =?UTF-8?q?=E6=9C=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- components/eq_20_asb_motor/eq20_servomotor.cpp | 13 +++++++------ components/eq_20_asb_motor/eq20_servomotor.hpp | 4 ++-- components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp | 12 +++++++++++- 3 files changed, 20 insertions(+), 9 deletions(-) diff --git a/components/eq_20_asb_motor/eq20_servomotor.cpp b/components/eq_20_asb_motor/eq20_servomotor.cpp index e7c0504..2c7cc7d 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/eq20_servomotor.cpp @@ -118,7 +118,7 @@ int32_t Eq20ServoMotor::move_to_zero_backward(int32_t lookzeropoint_rpm, int32_t } int32_t Eq20ServoMotor::get_pos(int32_t &pos) { - DO(readreg(2036, pos)); + DO(read_reg(2036, pos)); return 0; } @@ -132,9 +132,10 @@ int32_t Eq20ServoMotor::stop() { DO(write_pn_bit(1501, 1, 0)); return 0; } +// 2110 int32_t Eq20ServoMotor::get_servo_internal_state(servo_internal_status_t &state) { - DO(read_pn(1065, (int32_t &)state.data.status)); + DO(read_pn(1055, (int32_t &)state.data.status)); return 0; } @@ -148,10 +149,10 @@ int32_t Eq20ServoMotor::get_servo_internal_state(servo_internal_status_t &state) } \ return ret; -int32_t Eq20ServoMotor::writereg(uint32_t regadd, int32_t value) { // +int32_t Eq20ServoMotor::write_reg(uint32_t regadd, int32_t value) { // AUTO_RESEND(_writereg(regadd, value)); } -int32_t Eq20ServoMotor::readreg(uint32_t regadd, int32_t &value) { // +int32_t Eq20ServoMotor::read_reg(uint32_t regadd, int32_t &value) { // AUTO_RESEND(_readreg(regadd, value)); } int32_t Eq20ServoMotor::write_reg_bit(uint32_t regadd, int32_t off, int32_t value) { // @@ -178,10 +179,10 @@ int32_t Eq20ServoMotor::_read_pn_bit(uint32_t pnadd, int32_t off, int32_t &value * REG * *******************************************************************************/ int32_t Eq20ServoMotor::_writereg(uint32_t regadd, int32_t value) { // - return m_modbusBlockHost->writeReg10Muti(m_deviceId, regadd, (uint16_t *)&value, 2, 100); + return m_modbusBlockHost->writeReg10Muti(m_deviceId, regadd, (uint16_t *)&value, 2, 300); } int32_t Eq20ServoMotor::_readreg(uint32_t regadd, int32_t &value) { // - return m_modbusBlockHost->readReg03Muti(m_deviceId, regadd, (uint16_t *)&value, 2, 100); + return m_modbusBlockHost->readReg03Muti(m_deviceId, regadd, (uint16_t *)&value, 2, 300); } int32_t Eq20ServoMotor::_write_reg_bit(uint32_t regadd, int32_t off, int32_t value) { int32_t regval; diff --git a/components/eq_20_asb_motor/eq20_servomotor.hpp b/components/eq_20_asb_motor/eq20_servomotor.hpp index 425af18..c8e3e2a 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.hpp +++ b/components/eq_20_asb_motor/eq20_servomotor.hpp @@ -88,8 +88,8 @@ class Eq20ServoMotor { int32_t get_pos(int32_t &pos); public: - int32_t writereg(uint32_t regadd, int32_t value); - int32_t readreg(uint32_t regadd, int32_t &value); + int32_t write_reg(uint32_t regadd, int32_t value); + int32_t read_reg(uint32_t regadd, int32_t &value); int32_t write_reg_bit(uint32_t regadd, int32_t off, int32_t value); int32_t read_reg_bit(uint32_t regadd, int32_t off, int32_t &value); diff --git a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp index d9d3cc7..dde3e4d 100644 --- a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp @@ -47,7 +47,6 @@ static void cmd_dump_ack(Eq20ServoMotor::servo_internal_status_t ack) { // ZLOGI(TAG, "is_over_travel :%d", ack.data.sbit.is_over_travel); ZLOGI(TAG, "encoder_battery_warning:%d", ack.data.sbit.encoder_battery_warning); ZLOGI(TAG, "encoder_battery_report :%d", ack.data.sbit.encoder_battery_report); - } static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; } @@ -67,4 +66,15 @@ void ScriptCmderEq20Servomotor::regcmd() { REG_CMD___NO_ACK("eq20_", stop, "(id)", 1); REG_CMD_WITH_ACK("eq20_", get_pos, "(id)", 1, int32_t, ack); REG_CMD_WITH_ACK("eq20_", get_servo_internal_state, "(id)", 1, Eq20ServoMotor::servo_internal_status_t, ack); + + + REG_CMD_WITH_ACK("eq20_", read_pn, "(id,pnadd)", 2, int32_t, con->getInt(2), ack); + REG_CMD___NO_ACK("eq20_", write_pn, "(id,pnadd,value)", 3, con->getInt(2), con->getInt(3)); + REG_CMD___NO_ACK("eq20_", write_pn_bit, "(id,pnadd,off,value)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); + REG_CMD_WITH_ACK("eq20_", read_pn_bit, "(id,pnadd,off)", 3, int32_t, con->getInt(2), con->getInt(3), ack); + + REG_CMD___NO_ACK("eq20_", write_reg, "(id,regadd,value)", 3, con->getInt(2), con->getInt(3)); + REG_CMD_WITH_ACK("eq20_", read_reg, "(id,regadd)", 2, int32_t, con->getInt(2), ack); + REG_CMD___NO_ACK("eq20_", write_reg_bit, "(id,regadd,off,value)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); + REG_CMD_WITH_ACK("eq20_", read_reg_bit, "(id,regadd,off)", 3, int32_t, con->getInt(2), con->getInt(3), ack); } \ No newline at end of file