diff --git a/components/eq_20_asb_motor/eq20_servomotor.cpp b/components/eq_20_asb_motor/eq20_servomotor.cpp index 3a09c3c..51c6e51 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/eq20_servomotor.cpp @@ -35,13 +35,13 @@ write_pn 1 617 100 #Pn617 write_pn_bit 1 1501 0 0 write_pn_bit 1 1501 0 1 #endif - DO(write_pn(m_deviceId, 610, 0x0001)); - DO(write_pn(m_deviceId, 615, rpm)); - DO(write_pn(m_deviceId, 614, topos)); - DO(write_pn(m_deviceId, 616, acctime)); - DO(write_pn(m_deviceId, 617, acctime)); - DO(write_pn_bit(m_deviceId, 1501, 0, 0)); - DO(write_pn_bit(m_deviceId, 1501, 0, 1)); + DO(write_pn(610, 0x0001)); + DO(write_pn(615, rpm)); + DO(write_pn(614, topos)); + DO(write_pn(616, acctime)); + DO(write_pn(617, acctime)); + DO(write_pn_bit(1501, 0, 0)); + DO(write_pn_bit(1501, 0, 1)); return 0; } int32_t Eq20ServoMotor::move_by(int32_t pos, int32_t rpm, int32_t acctime) { @@ -54,13 +54,13 @@ write_pn 1 617 100 #Pn617 write_pn_bit 1 1501 0 0 write_pn_bit 1 1501 0 1 #endif - DO(write_pn(m_deviceId, 610, 0x0011)); - DO(write_pn(m_deviceId, 614, pos)); - DO(write_pn(m_deviceId, 615, rpm)); - DO(write_pn(m_deviceId, 616, acctime)); - DO(write_pn(m_deviceId, 617, acctime)); - DO(write_pn_bit(m_deviceId, 1501, 0, 0)); - DO(write_pn_bit(m_deviceId, 1501, 0, 1)); + DO(write_pn(610, 0x0011)); + DO(write_pn(614, pos)); + DO(write_pn(615, rpm)); + DO(write_pn(616, acctime)); + DO(write_pn(617, acctime)); + DO(write_pn_bit(1501, 0, 0)); + DO(write_pn_bit(1501, 0, 1)); return 0; } int32_t Eq20ServoMotor::rotate(int32_t rpm, int32_t acctime) { return move_by(100000 * 10000, rpm, acctime); } @@ -77,46 +77,50 @@ write_pn 1 606 0 # write_pn_bit 1 1501 1 0 write_pn_bit 1 1501 1 1 #endif - DO(write_pn(m_deviceId, 600, 11)); - DO(write_pn(m_deviceId, 601, lookzeropoint_rpm)); - DO(write_pn(m_deviceId, 602, findzero_edge_rpm)); - DO(write_pn(m_deviceId, 603, lookzeropoint_acc_time)); - DO(write_pn(m_deviceId, 604, 0)); - DO(write_pn(m_deviceId, 605, 0)); - DO(write_pn(m_deviceId, 606, 0)); - DO(write_pn_bit(m_deviceId, 1501, 1, 0)); - DO(write_pn_bit(m_deviceId, 1501, 1, 1)); + DO(write_pn(600, 11)); + DO(write_pn(601, lookzeropoint_rpm)); + DO(write_pn(602, findzero_edge_rpm)); + DO(write_pn(603, lookzeropoint_acc_time)); + DO(write_pn(604, 0)); + DO(write_pn(605, 0)); + DO(write_pn(606, 0)); + DO(write_pn_bit(1501, 1, 0)); + DO(write_pn_bit(1501, 1, 1)); return 0; } int32_t Eq20ServoMotor::move_to_zero_backward(int32_t lookzeropoint_rpm, int32_t findzero_edge_rpm, int32_t lookzeropoint_acc_time) { - DO(write_pn(m_deviceId, 600, 8)); - DO(write_pn(m_deviceId, 601, lookzeropoint_rpm)); - DO(write_pn(m_deviceId, 602, findzero_edge_rpm)); - DO(write_pn(m_deviceId, 603, lookzeropoint_acc_time)); - DO(write_pn(m_deviceId, 604, 0)); // 零点偏移 - DO(write_pn(m_deviceId, 605, 0)); // 回零动作超时时间 - DO(write_pn(m_deviceId, 606, 0)); // 回零动作中止使能 - DO(write_pn_bit(m_deviceId, 1501, 1, 0)); - DO(write_pn_bit(m_deviceId, 1501, 1, 1)); + DO(write_pn(600, 8)); + DO(write_pn(601, lookzeropoint_rpm)); + DO(write_pn(602, findzero_edge_rpm)); + DO(write_pn(603, lookzeropoint_acc_time)); + DO(write_pn(604, 0)); // 零点偏移 + DO(write_pn(605, 0)); // 回零动作超时时间 + DO(write_pn(606, 0)); // 回零动作中止使能 + DO(write_pn_bit(1501, 1, 0)); + DO(write_pn_bit(1501, 1, 1)); return 0; } -int32_t Eq20ServoMotor::getpos(int32_t &pos) { - DO(readreg(m_deviceId, 2036, pos)); +int32_t Eq20ServoMotor::get_pos(int32_t &pos) { + DO(readreg(2036, pos)); return 0; } int32_t Eq20ServoMotor::enable(int32_t enable) { - DO(write_pn_bit(m_deviceId, 1501, 2, enable)); + DO(write_pn_bit(1501, 2, enable)); return 0; } int32_t Eq20ServoMotor::stop() { - DO(write_pn_bit(m_deviceId, 1501, 0, 0)); - DO(write_pn_bit(m_deviceId, 1501, 1, 0)); + DO(write_pn_bit(1501, 0, 0)); + DO(write_pn_bit(1501, 1, 0)); + return 0; +} + +int32_t Eq20ServoMotor::get_servo_internal_state(servo_internal_status_t &state) { + DO(read_pn(1065, state.status)); return 0; } -int32_t Eq20ServoMotor::isReachTarget(uint8_t &isReachTarget) {} #define AUTO_RESEND(exptr) \ int ret = 0; \ @@ -127,57 +131,57 @@ int32_t Eq20ServoMotor::isReachTarget(uint8_t &isReachTarget) {} } \ } -int32_t Eq20ServoMotor::writereg(int deviceid, uint32_t regadd, int32_t value) { // - AUTO_RESEND(_writereg(deviceid, regadd, value)); +int32_t Eq20ServoMotor::writereg(uint32_t regadd, int32_t value) { // + AUTO_RESEND(_writereg(regadd, value)); } -int32_t Eq20ServoMotor::readreg(int deviceid, uint32_t regadd, int32_t &value) { // - AUTO_RESEND(_readreg(deviceid, regadd, value)); +int32_t Eq20ServoMotor::readreg(uint32_t regadd, int32_t &value) { // + AUTO_RESEND(_readreg(regadd, value)); } -int32_t Eq20ServoMotor::write_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t value) { // - AUTO_RESEND(_write_reg_bit(deviceid, regadd, off, value)); +int32_t Eq20ServoMotor::write_reg_bit(uint32_t regadd, int32_t off, int32_t value) { // + AUTO_RESEND(_write_reg_bit(regadd, off, value)); } -int32_t Eq20ServoMotor::write_pn(int deviceid, uint32_t pnadd, int32_t value) { // - AUTO_RESEND(_write_pn(deviceid, pnadd, value)); +int32_t Eq20ServoMotor::write_pn(uint32_t pnadd, int32_t value) { // + AUTO_RESEND(_write_pn(pnadd, value)); } -int32_t Eq20ServoMotor::read_pn(int deviceid, uint32_t pnadd, int32_t &value) { // - AUTO_RESEND(_read_pn(deviceid, pnadd, value)); +int32_t Eq20ServoMotor::read_pn(uint32_t pnadd, int32_t &value) { // + AUTO_RESEND(_read_pn(pnadd, value)); } -int32_t Eq20ServoMotor::write_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t value) { AUTO_RESEND(_write_pn_bit(deviceid, pnadd, off, value)); } -int32_t Eq20ServoMotor::read_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t &value) { AUTO_RESEND(_read_reg_bit(deviceid, regadd, off, value)); } -int32_t Eq20ServoMotor::read_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t &value) { AUTO_RESEND(_read_pn_bit(deviceid, pnadd, off, value)); } +int32_t Eq20ServoMotor::write_pn_bit(uint32_t pnadd, int32_t off, int32_t value) { AUTO_RESEND(_write_pn_bit(pnadd, off, value)); } +int32_t Eq20ServoMotor::read_reg_bit(uint32_t regadd, int32_t off, int32_t &value) { AUTO_RESEND(_read_reg_bit(regadd, off, value)); } +int32_t Eq20ServoMotor::read_pn_bit(uint32_t pnadd, int32_t off, int32_t &value) { AUTO_RESEND(_read_pn_bit(pnadd, off, value)); } /******************************************************************************* * BASIC_API * *******************************************************************************/ -int32_t Eq20ServoMotor::_write_pn(int deviceid, uint32_t pnadd, int32_t value) { return _writereg(deviceid, pnadd * 2, value); } -int32_t Eq20ServoMotor::_read_pn(int deviceid, uint32_t pnadd, int32_t &value) { return _readreg(deviceid, pnadd * 2, value); } -int32_t Eq20ServoMotor::_write_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t value) { return _write_reg_bit(deviceid, pnadd * 2, off, value); } -int32_t Eq20ServoMotor::_read_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t &value) { return _read_reg_bit(deviceid, pnadd * 2, off, value); } +int32_t Eq20ServoMotor::_write_pn(uint32_t pnadd, int32_t value) { return _writereg(pnadd * 2, value); } +int32_t Eq20ServoMotor::_read_pn(uint32_t pnadd, int32_t &value) { return _readreg(pnadd * 2, value); } +int32_t Eq20ServoMotor::_write_pn_bit(uint32_t pnadd, int32_t off, int32_t value) { return _write_reg_bit(pnadd * 2, off, value); } +int32_t Eq20ServoMotor::_read_pn_bit(uint32_t pnadd, int32_t off, int32_t &value) { return _read_reg_bit(pnadd * 2, off, value); } /******************************************************************************* * REG * *******************************************************************************/ -int32_t Eq20ServoMotor::_writereg(int deviceid, uint32_t regadd, int32_t value) { // - DO(m_modbusBlockHost->writeReg10Muti(deviceid, regadd, (uint16_t *)&value, 2, 100)); +int32_t Eq20ServoMotor::_writereg(uint32_t regadd, int32_t value) { // + DO(m_modbusBlockHost->writeReg10Muti(m_deviceId, regadd, (uint16_t *)&value, 2, 100)); return 0; } -int32_t Eq20ServoMotor::_readreg(int deviceid, uint32_t regadd, int32_t &value) { // - DO(m_modbusBlockHost->readReg03Muti(deviceid, regadd, (uint16_t *)&value, 2, 100)); +int32_t Eq20ServoMotor::_readreg(uint32_t regadd, int32_t &value) { // + DO(m_modbusBlockHost->readReg03Muti(m_deviceId, regadd, (uint16_t *)&value, 2, 100)); return 0; } -int32_t Eq20ServoMotor::_write_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t value) { +int32_t Eq20ServoMotor::_write_reg_bit(uint32_t regadd, int32_t off, int32_t value) { int32_t regval; - DO(_readreg(deviceid, regadd, regval)); + DO(_readreg(regadd, regval)); if (value) { regval |= (1 << off); } else { regval &= ~(1 << off); } - DO(_writereg(deviceid, regadd, regval)); + DO(_writereg(regadd, regval)); return 0; } -int32_t Eq20ServoMotor::_read_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t &value) { +int32_t Eq20ServoMotor::_read_reg_bit(uint32_t regadd, int32_t off, int32_t &value) { int32_t regval; - DO(_readreg(deviceid, regadd, regval)); + DO(_readreg(regadd, regval)); value = (regval >> off) & 0x01; return 0; } \ No newline at end of file diff --git a/components/eq_20_asb_motor/eq20_servomotor.hpp b/components/eq_20_asb_motor/eq20_servomotor.hpp index a3c0166..38ac4c8 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.hpp +++ b/components/eq_20_asb_motor/eq20_servomotor.hpp @@ -9,6 +9,55 @@ using namespace std; class Eq20ServoMotor { public: + typedef struct { + int32_t status; + } servo_internal_status_t; + + // bit0:同服准备好 + // bitl:伺服报警 + // bit2:电机旋转 + // bit3:定位完成 + // bit4:速度一致 + // bit5:电机零速 + // bit6: 制动器(抱闸) + // bit7:定位接近 + // bit8:转矩受限 + // bit9:速度受限 + // bit10:转矩检测 + // bit11:伺服警告 + // bit12:伺服故障代码 1 + // bit13:伺服故障代码 2 + // bit14:伺服故障代码 3 + // bit15:回零完成 + // bit16:编码器 C 脉冲有效 + // bit17:电机励磁 + // bit18:超程状态 + // bit19 编码器电池警告 + // bit20:编码器电池报数 + typedef struct { + uint32_t servo_is_ready : 1; + uint32_t servo_warning : 1; + uint32_t motor_is_rotating : 1; + uint32_t is_reach_target : 1; + uint32_t is_speed_consistent : 1; + uint32_t motor_is_zero_speed : 1; + uint32_t brake_is_on : 1; + uint32_t is_pos_close : 1; + uint32_t torque_is_limited : 1; + uint32_t speed_is_limited : 1; + uint32_t torque_is_detected : 1; + uint32_t servo_warning2 : 1; + uint32_t servo_error_code1 : 1; + uint32_t servo_error_code2 : 1; + uint32_t servo_error_code3 : 1; + uint32_t is_zero_complete : 1; + uint32_t encoder_c_pulse_is_valid : 1; + uint32_t motor_is_excited : 1; + uint32_t is_over_travel : 1; + uint32_t encoder_battery_warning : 1; + uint32_t encoder_battery_report : 1; + } status_bit; + private: ModbusBlockHost *m_modbusBlockHost; int32_t m_deviceId = 0; @@ -30,31 +79,33 @@ class Eq20ServoMotor { int32_t move_to_zero_backward(int32_t lookzeropoint_rpm, int32_t findzero_edge_rpm, int32_t lookzeropoint_acc_time); int32_t stop(); - int32_t isReachTarget(uint8_t &isReachTarget); + // int32_t is_reach_target(int32_t &isReachTarget); - int32_t getpos(int32_t &pos); + int32_t get_servo_internal_state(servo_internal_status_t &state); + + int32_t get_pos(int32_t &pos); public: - int32_t writereg(int deviceid, uint32_t regadd, int32_t value); - int32_t readreg(int deviceid, uint32_t regadd, int32_t &value); - int32_t write_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t value); - int32_t read_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t &value); + int32_t writereg(uint32_t regadd, int32_t value); + int32_t readreg(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); - int32_t read_pn(int deviceid, uint32_t pnadd, int32_t &value); - int32_t write_pn(int deviceid, uint32_t pnadd, int32_t value); - int32_t write_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t value); - int32_t read_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t &value); + int32_t read_pn(uint32_t pnadd, int32_t &value); + int32_t write_pn(uint32_t pnadd, int32_t value); + int32_t write_pn_bit(uint32_t pnadd, int32_t off, int32_t value); + int32_t read_pn_bit(uint32_t pnadd, int32_t off, int32_t &value); private: - int32_t _writereg(int deviceid, uint32_t regadd, int32_t value); - int32_t _readreg(int deviceid, uint32_t regadd, int32_t &value); - int32_t _write_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t value); - int32_t _read_reg_bit(int deviceid, uint32_t regadd, int32_t off, int32_t &value); - - int32_t _write_pn(int deviceid, uint32_t pnadd, int32_t value); - int32_t _read_pn(int deviceid, uint32_t pnadd, int32_t &value); - int32_t _write_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t value); - int32_t _read_pn_bit(int deviceid, uint32_t pnadd, int32_t off, int32_t &value); + int32_t _writereg(uint32_t regadd, int32_t value); + int32_t _readreg(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); + + int32_t _write_pn(uint32_t pnadd, int32_t value); + int32_t _read_pn(uint32_t pnadd, int32_t &value); + int32_t _write_pn_bit(uint32_t pnadd, int32_t off, int32_t value); + int32_t _read_pn_bit(uint32_t pnadd, int32_t off, int32_t &value); private: int32_t send(uint8_t *data, size_t len, int32_t overtime); diff --git a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp new file mode 100644 index 0000000..8d39722 --- /dev/null +++ b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp @@ -0,0 +1,48 @@ + +#include "script_cmder_eq20_servomotor.hpp" + +#include + +// #include "sdk\components\zprotocols\zcancmder\zcancmder_protocol.hpp" +using namespace iflytop; +#define TAG "CMD" + +void ScriptCmderEq20Servomotor::initialize(CmdScheduler* cmdScheduler) { + m_cmdScheduler = cmdScheduler; + ZASSERT(m_cmdScheduler != nullptr); + regcmd(); +} + +int32_t ScriptCmderEq20Servomotor::findmodule(int id, Eq20ServoMotor** module) { + auto it = moduler.find(id); + if (it == moduler.end()) { + return err::kce_no_such_module; + } + *module = it->second; + return 0; +} +static void cmd_dump_ack(int32_t& ack) { // + ZLOGI(TAG, "value:%d", ack); +} +static void cmd_dump_ack(Eq20ServoMotor::servo_internal_status_t ack) { // + ZLOGI(TAG, "value:%d", ack.status); +} +static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; } + +void ScriptCmderEq20Servomotor::regmodule(int id, Eq20ServoMotor* robot) { + ZASSERT(moduler.find(id) == moduler.end()); + ZASSERT(robot != nullptr); + moduler[id] = robot; +} + +void ScriptCmderEq20Servomotor::regcmd() { + REG_CMD___NO_ACK("eq20_", enable, "(id,en)", 2, con->getBool(2)); + REG_CMD___NO_ACK("eq20_", move_to, "(id,pos,rpm,acctime)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); + REG_CMD___NO_ACK("eq20_", move_by, "(id,pos,rpm,acctime)", 4, con->getInt(2), con->getInt(3), con->getInt(4)); + REG_CMD___NO_ACK("eq20_", rotate, "(id,rpm,acctime)", 3, con->getInt(2), con->getInt(3)); + REG_CMD___NO_ACK("eq20_", move_to_zero_forward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 5, con->getInt(2), con->getInt(3), con->getInt(4)); + REG_CMD___NO_ACK("eq20_", move_to_zero_backward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 5, con->getInt(2), con->getInt(3), con->getInt(4)); + 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); +} \ No newline at end of file diff --git a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.hpp b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.hpp new file mode 100644 index 0000000..1781462 --- /dev/null +++ b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.hpp @@ -0,0 +1,24 @@ +#pragma once +#include + +#include "eq20_servomotor.hpp" +#include "sdk/os/zos.hpp" +#include "sdk\components\cmdscheduler\cmd_scheduler.hpp" +namespace iflytop { + +class ScriptCmderEq20Servomotor { + CmdScheduler* m_cmdScheduler; + map moduler; + + Eq20ServoMotor* module; + + public: + void initialize(CmdScheduler* cmdScheduler); + void regmodule(int id, Eq20ServoMotor* robot); + void regcmd(); + + private: + int32_t findmodule(int id, Eq20ServoMotor** module); +}; +} // namespace iflytop +