From 222e49d83cb7b4048bae7e9546947ef5c0d1e43c Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sat, 30 Sep 2023 16:24:39 +0800 Subject: [PATCH] update feite servo motor --- components/mini_servo_motor/feite_servo_motor.cpp | 231 ++++++++++----------- components/mini_servo_motor/feite_servo_motor.hpp | 47 +++-- .../mini_servo_motor_ctrl_module.cpp | 4 +- 3 files changed, 134 insertions(+), 148 deletions(-) diff --git a/components/mini_servo_motor/feite_servo_motor.cpp b/components/mini_servo_motor/feite_servo_motor.cpp index 29c6d4f..0f31bca 100644 --- a/components/mini_servo_motor/feite_servo_motor.cpp +++ b/components/mini_servo_motor/feite_servo_motor.cpp @@ -1,4 +1,7 @@ #include "feite_servo_motor.hpp" + +#include +#include using namespace iflytop; using namespace std; using namespace feite; @@ -25,114 +28,6 @@ bool FeiTeServoMotor::ping(uint8_t id) { return tx_and_rx((uint8_t*)&ping_cmd, sizeof(ping_cmd_t), (uint8_t*)&ping_resp, sizeof(ping_resp_t), OVERTIME); } -bool FeiTeServoMotor::write8(uint8_t id, feite::reg_add_e add, uint8_t regval) { - write8_cmd_t write8_cmd = {0}; - write8_resp_t write8_resp = {0}; - write8_cmd.header = 0xffff; - write8_cmd.id = id; - write8_cmd.len = 3 + 1; - write8_cmd.cmd = kwrite; - write8_cmd.regadd = add; - write8_cmd.regval = regval; - write8_cmd.checksum = checksum_packet((uint8_t*)&write8_cmd, sizeof(write8_cmd_t)); - return tx_and_rx((uint8_t*)&write8_cmd, sizeof(write8_cmd_t), (uint8_t*)&write8_resp, sizeof(write8_resp_t), OVERTIME); -} -bool FeiTeServoMotor::read8(uint8_t id, feite::reg_add_e add, uint8_t& regval) { - read8_cmd_t read8_cmd = {0}; - read8_resp_t read8_resp = {0}; - read8_cmd.header = 0xffff; - read8_cmd.id = id; - read8_cmd.len = 3 + 1; - read8_cmd.cmd = kread; - read8_cmd.regadd = add; - read8_cmd.readlen = 1; - read8_cmd.checksum = checksum_packet((uint8_t*)&read8_cmd, sizeof(read8_cmd_t)); - if (!tx_and_rx((uint8_t*)&read8_cmd, sizeof(read8_cmd_t), (uint8_t*)&read8_resp, sizeof(read8_resp_t), OVERTIME)) { - return false; - } - regval = read8_resp.data; - return true; -} - -bool FeiTeServoMotor::write16(uint8_t id, feite::reg_add_e add, uint16_t regval) { - write16_cmd_t write16_cmd = {0}; - write16_resp_t write16_resp = {0}; - write16_cmd.header = 0xffff; - write16_cmd.id = id; - write16_cmd.len = 3 + 2; - write16_cmd.cmd = kwrite; - write16_cmd.regadd = add; - write16_cmd.regval = regval; - write16_cmd.checksum = checksum_packet((uint8_t*)&write16_cmd, sizeof(write16_cmd_t)); - return tx_and_rx((uint8_t*)&write16_cmd, sizeof(write16_cmd_t), (uint8_t*)&write16_resp, sizeof(write16_resp_t), OVERTIME); -} - -bool FeiTeServoMotor::read_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t& regval) { - uint16_t val = 0; - bool ret = read16(id, add, val); - if (!ret) return false; - uint8_t sign = (val >> signbitoff) & 0x01; - uint16_t realval = val & (~(1 << signbitoff)); - if (sign == 0) { - regval = realval; - } else { - regval = -realval; - } - return true; -} - -bool FeiTeServoMotor::write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t regval) { - uint16_t val = 0; - if (regval >= 0) { - val = regval; - } else { - val = -regval; - val |= (1 << signbitoff); - } - return write16(id, add, val); -} - -bool FeiTeServoMotor::read16(uint8_t id, feite::reg_add_e add, uint16_t& regval) { - read16_cmd_t read16_cmd = {0}; - read16_resp_t read16_resp = {0}; - read16_cmd.header = 0xffff; - read16_cmd.id = id; - read16_cmd.len = 3 + 1; - read16_cmd.cmd = kread; - read16_cmd.regadd = add; - read16_cmd.readlen = 2; - read16_cmd.checksum = checksum_packet((uint8_t*)&read16_cmd, sizeof(read16_cmd_t)); - if (!tx_and_rx((uint8_t*)&read16_cmd, sizeof(read16_cmd_t), (uint8_t*)&read16_resp, sizeof(read16_resp_t), OVERTIME)) { - return false; - } - regval = read16_resp.data; - return true; -} - -bool FeiTeServoMotor::async_write8(uint8_t id, feite::reg_add_e add, uint8_t regval) { - write8_cmd_t write8_cmd = {0}; - write8_resp_t write8_resp = {0}; - write8_cmd.header = 0xffff; - write8_cmd.id = id; - write8_cmd.len = 3 + 1; - write8_cmd.cmd = kasyncWrite; - write8_cmd.regadd = add; - write8_cmd.regval = regval; - write8_cmd.checksum = checksum_packet((uint8_t*)&write8_cmd, sizeof(write8_cmd_t)); - return tx_and_rx((uint8_t*)&write8_cmd, sizeof(write8_cmd_t), (uint8_t*)&write8_resp, sizeof(write8_resp_t), OVERTIME); -} -bool FeiTeServoMotor::async_write16(uint8_t id, feite::reg_add_e add, uint16_t regval) { - write16_cmd_t write16_cmd = {0}; - write16_resp_t write16_resp = {0}; - write16_cmd.header = 0xffff; - write16_cmd.id = id; - write16_cmd.len = 3 + 2; - write16_cmd.cmd = kasyncWrite; - write16_cmd.regadd = add; - write16_cmd.regval = regval; - write16_cmd.checksum = checksum_packet((uint8_t*)&write16_cmd, sizeof(write16_cmd_t)); - return tx_and_rx((uint8_t*)&write16_cmd, sizeof(write16_cmd_t), (uint8_t*)&write16_resp, sizeof(write16_resp_t), OVERTIME); -} static int16_t getcalibrate(int16_t nowpos, int16_t aftercalibratepos) { int16_t calibrate = nowpos - aftercalibratepos; @@ -148,18 +43,12 @@ static int16_t getcalibrate(int16_t nowpos, int16_t aftercalibratepos) { return calibrate; } -bool FeiTeServoMotor::write_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len) { // - - return false; -} -bool FeiTeServoMotor::read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len) { return false; } -bool FeiTeServoMotor::setmode(uint8_t id, run_mode_e runmode) { return write8(id, kRegServoRunMode, (uint8_t)runmode); } - +bool FeiTeServoMotor::setmode(uint8_t id, run_mode_e runmode) { return write_u8(id, kRegServoRunMode, (uint8_t)runmode); } bool FeiTeServoMotor::getServoCalibration(uint8_t id, int16_t& poscalibration) { return read_s16(id, kRegServoCalibration, 11, poscalibration); } run_mode_e FeiTeServoMotor::getmode(uint8_t id) { uint8_t data = 0; - bool suc = read8(id, kRegServoRunMode, data); + bool suc = read_u8(id, kRegServoRunMode, data); if (suc) { return (run_mode_e)data; } else { @@ -169,15 +58,15 @@ run_mode_e FeiTeServoMotor::getmode(uint8_t id) { bool FeiTeServoMotor::getmode(uint8_t id, run_mode_e& runmode) { uint8_t data = 0; - bool suc = read8(id, kRegServoRunMode, data); + bool suc = read_u8(id, kRegServoRunMode, data); runmode = (run_mode_e)data; return suc; } -bool FeiTeServoMotor::setTorqueSwitch(uint8_t id, bool on) { return write8(id, kRegServoTorqueSwitch, on ? 1 : 0); } +bool FeiTeServoMotor::setTorqueSwitch(uint8_t id, bool on) { return write_u8(id, kRegServoTorqueSwitch, on ? 1 : 0); } bool FeiTeServoMotor::getTorqueSwitch(uint8_t id, bool& on) { uint8_t data = 0; - bool suc = read8(id, kRegServoTorqueSwitch, data); + bool suc = read_u8(id, kRegServoTorqueSwitch, data); on = data; return suc; } @@ -210,9 +99,9 @@ bool FeiTeServoMotor::reCalibration(int id, int16_t pos) { /** * @brief 写入新的校准值 */ - DO(write8(id, kRegServoLockFlag, 0)); + DO(write_u8(id, kRegServoLockFlag, 0)); DO(write_s16(id, kRegServoCalibration, 11, newcalibrate)); - DO(write8(id, kRegServoLockFlag, 1)); + DO(write_u8(id, kRegServoLockFlag, 1)); /** * @brief 重新设置目标位置为当前这个位置 */ @@ -236,7 +125,99 @@ static void dumphex(char* tag, uint8_t* data, uint8_t len) { } printf("\n"); } +bool FeiTeServoMotor::write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval) { return write_reg(id, false, add, ®val, 1); } +bool FeiTeServoMotor::read_u8(uint8_t id, feite::reg_add_e add, uint8_t& regval) { return read_reg(id, add, ®val, 1); } +bool FeiTeServoMotor::write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval) { return write_reg(id, false, add, (uint8_t*)®val, 2); } +bool FeiTeServoMotor::read_u16(uint8_t id, feite::reg_add_e add, uint16_t& regval) { return read_reg(id, add, (uint8_t*)®val, 2); } +bool FeiTeServoMotor::async_write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval) { return write_reg(id, true, add, ®val, 1); } +bool FeiTeServoMotor::async_write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval) { return write_reg(id, true, add, (uint8_t*)®val, 2); } +bool FeiTeServoMotor::async_write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t regval) { + uint16_t val = 0; + if (regval >= 0) { + val = regval; + } else { + val = -regval; + val |= (1 << signbitoff); + } + return async_write_u16(id, add, val); +} + +bool FeiTeServoMotor::read_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t& regval) { + uint16_t val = 0; + bool ret = read_u16(id, add, val); + if (!ret) return false; + uint8_t sign = (val >> signbitoff) & 0x01; + uint16_t realval = val & (~(1 << signbitoff)); + if (sign == 0) { + regval = realval; + } else { + regval = -realval; + } + return true; +} +bool FeiTeServoMotor::write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t regval) { + uint16_t val = 0; + if (regval >= 0) { + val = regval; + } else { + val = -regval; + val |= (1 << signbitoff); + } + return write_u16(id, add, val); +} +bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* data, uint8_t len) { // + // ZLOGI(TAG, "write_reg id:%d add:%d len:%d", id, add, len); + cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf; + receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf; + cmd_header->header = 0xffff; + cmd_header->id = id; + cmd_header->len = 3 + len; // 3 == cmd + add + checksum + cmd_header->cmd = async ? kasyncWrite : kwrite; + cmd_header->data[0] = add; + memcpy(&cmd_header->data[1], data, len); + + int txpacketlen = sizeof(cmd_header_t) + 1 + len + 1; + int rxpacketlen = sizeof(receipt_header_t) + 1; + + 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, "write_reg fail,overtime"); + return false; + } + if (!(receipt_header->header == 0xffff && receipt_header->id == id)) { + ZLOGE(TAG, "write_reg fail,receipt header error"); + return false; + } + return true; +} +bool FeiTeServoMotor::read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len) { + // return false; + cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf; + receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf; + cmd_header->header = 0xffff; + cmd_header->id = id; + cmd_header->len = 3 + 1; // 4 == cmd + add + checksum + readlen + cmd_header->cmd = kread; + cmd_header->data[0] = add; + cmd_header->data[1] = len; + int txpacketlen = sizeof(cmd_header_t) + 3; + int rxpacketlen = sizeof(receipt_header_t) + 1 + len; + + 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)) { + return false; + } + if (!(receipt_header->header == 0xffff && receipt_header->id == id)) { + ZLOGE(TAG, "read_reg fail,receipt header error"); + return false; + } + memcpy(data, receipt_header->data, len); + return true; +} bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, uint16_t overtimems) { uint32_t enter_ticket = HAL_GetTick(); dumphex("tx:", tx, txdatalen); @@ -270,13 +251,13 @@ bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uin bool FeiTeServoMotor::readversion(uint8_t id, uint8_t& mainversion, uint8_t& subversion, uint8_t& miniserv_mainversion, uint8_t& miniserv_subversion) { uint8_t data = 0; - DO(read8(id, kRegFirmwareMainVersion, data)); + DO(read_u8(id, kRegFirmwareMainVersion, data)); mainversion = data; - DO(read8(id, kRegFirmwareSubVersion, data)); + DO(read_u8(id, kRegFirmwareSubVersion, data)); subversion = data; - DO(read8(id, kRegServoMainVersion, data)); + DO(read_u8(id, kRegServoMainVersion, data)); miniserv_mainversion = data; - DO(read8(id, kRegServoSubVersion, data)); + DO(read_u8(id, kRegServoSubVersion, data)); miniserv_subversion = data; return true; } diff --git a/components/mini_servo_motor/feite_servo_motor.hpp b/components/mini_servo_motor/feite_servo_motor.hpp index 7df5381..b97d18b 100644 --- a/components/mini_servo_motor/feite_servo_motor.hpp +++ b/components/mini_servo_motor/feite_servo_motor.hpp @@ -85,17 +85,19 @@ typedef enum { #pragma pack(1) ZSTRUCT(ping_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t checksum;) ZSTRUCT(ping_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t checksum;) -ZSTRUCT(read8_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint8_t readlen; uint8_t checksum;) -ZSTRUCT(read8_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t data; uint8_t checksum;) -ZSTRUCT(write8_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint8_t regval; uint8_t checksum;) -ZSTRUCT(write8_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t checksum;) -ZSTRUCT(read16_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint8_t readlen; uint8_t checksum;) -ZSTRUCT(read16_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint16_t data; uint8_t checksum;) -ZSTRUCT(write16_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint16_t regval; uint8_t checksum;) -ZSTRUCT(write16_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t checksum;) - -ZSTRUCT(receipt_header_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd;) -ZSTRUCT(cmd_header_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd;) +#if 0 +ZSTRUCT(read_u8_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint8_t readlen; uint8_t checksum;) +ZSTRUCT(read_u8_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t data; uint8_t checksum;) +ZSTRUCT(write_u8_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint8_t regval; uint8_t checksum;) +ZSTRUCT(write_u8_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t checksum;) +ZSTRUCT(read_u16_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint8_t readlen; uint8_t checksum;) +ZSTRUCT(read_u16_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint16_t data; uint8_t checksum;) +ZSTRUCT(write_u16_cmd_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t regadd; uint16_t regval; uint8_t checksum;) +ZSTRUCT(write_u16_resp_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t checksum;) +#endif + +ZSTRUCT(receipt_header_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t status; uint8_t data[];) +ZSTRUCT(cmd_header_t, /* */ uint16_t header; uint8_t id; uint8_t len; uint8_t cmd; uint8_t data[];) #pragma pack() @@ -107,6 +109,9 @@ class FeiTeServoMotor { DMA_HandleTypeDef* m_hdma_rx; DMA_HandleTypeDef* m_hdma_tx; + uint8_t m_txbuf[128] = {0}; + uint8_t m_rxbuf[128] = {0}; + public: void initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx); bool ping(uint8_t id); @@ -127,20 +132,20 @@ class FeiTeServoMotor { bool reCalibration(int id, int16_t pos); public: - bool write8(uint8_t id, feite::reg_add_e add, uint8_t regval); - bool read8(uint8_t id, feite::reg_add_e add, uint8_t& regval); - - bool write16(uint8_t id, feite::reg_add_e add, uint16_t regval); - bool read16(uint8_t id, feite::reg_add_e add, uint16_t& regval); - - bool read_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t& regval); + bool write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval); + bool write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval); bool write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t regval); + + bool async_write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval); + bool async_write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval); + bool async_write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff,int16_t regval); - bool async_write8(uint8_t id, feite::reg_add_e add, uint8_t regval); - bool async_write16(uint8_t id, feite::reg_add_e add, uint16_t regval); + bool read_u8(uint8_t id, feite::reg_add_e add, uint8_t& regval); + bool read_u16(uint8_t id, feite::reg_add_e add, uint16_t& regval); + bool read_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t& regval); public: - bool write_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len); + bool write_reg(uint8_t id, bool async, uint8_t add, uint8_t* data, uint8_t len); bool read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len); private: 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 0e0bca6..1dc362a 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -4,10 +4,10 @@ using namespace iflytop; using namespace std; -void MiniRobotCtrlModule::initialize(FeiTeServoMotor* bus, uint8_t id) { m_bus->write8(m_id, feite::kRegServoRunMode, feite::kServoMode); } +void MiniRobotCtrlModule::initialize(FeiTeServoMotor* bus, uint8_t id) { m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode); } int32_t MiniRobotCtrlModule::enable(u8 enable) { - bool suc = m_bus->write8(m_id, feite::reg_add_e::kRegServoTorqueSwitch, enable); + bool suc = m_bus->write_u8(m_id, feite::reg_add_e::kRegServoTorqueSwitch, enable); if (!suc) return err::kce_subdevice_overtime; return err::ksucc; }