diff --git a/a8000_protocol b/a8000_protocol index d8fd6a2..4ed8685 160000 --- a/a8000_protocol +++ b/a8000_protocol @@ -1 +1 @@ -Subproject commit d8fd6a265f51579472c70d8682a0e3bd57d1348a +Subproject commit 4ed86854f7c2a6906d17b922d9af85677fb31033 diff --git a/sdk/components/mini_servo_motor/feite_servo_motor.cpp b/sdk/components/mini_servo_motor/feite_servo_motor.cpp index cfb0beb..2ebeed1 100644 --- a/sdk/components/mini_servo_motor/feite_servo_motor.cpp +++ b/sdk/components/mini_servo_motor/feite_servo_motor.cpp @@ -14,7 +14,7 @@ using namespace feite; return false; \ } -#define DEBUG_MODE 0 +#define DEBUG_MODE 1 static void dumphex(const char* tag, uint8_t* data, uint8_t len) { #if DEBUG_MODE @@ -154,7 +154,7 @@ bool FeiTeServoMotor::ping(uint8_t id) { ping_cmd.cmd = kping; ping_cmd.checksum = checksum_packet((uint8_t*)&ping_cmd, sizeof(ping_cmd_t)); - return tx_and_rx((uint8_t*)&ping_cmd, sizeof(ping_cmd_t), (uint8_t*)&ping_resp, sizeof(ping_resp_t), OVERTIME); + return tx_and_rx((cmd_header_t*)&ping_cmd, sizeof(ping_cmd_t), (receipt_header_t*)&ping_resp, sizeof(ping_resp_t), OVERTIME); } bool FeiTeServoMotor::stop(uint8_t id) { @@ -572,6 +572,7 @@ bool FeiTeServoMotor::getPos(uint8_t id, int32_t* pos) { } bool FeiTeServoMotor::isMove(uint8_t id, int32_t* move) { zlock_guard l(m_mutex); + *move = 0; return read_reg(id, kRegServoMoveFlag, move); } @@ -700,19 +701,10 @@ bool FeiTeServoMotor::write_regs(uint8_t id, bool async, uint8_t add, uint8_t* d 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)) { + if (!tx_and_rx(cmd_header, txpacketlen, receipt_header, rxpacketlen, OVERTIME)) { ZLOGE(TAG, "write_reg id:%d add:%d len:%d fail,overtime", id, add, len); return false; } - if (!(receipt_header->header == 0xffff && receipt_header->id == id)) { - ZLOGE(TAG, "write_reg id:%d add:%d len:%d fail,receipt header error", id, add, len); - return false; - } - - if (receipt_header->status != 0 && add != kRegServoTorqueSwitch) { - ZLOGE(TAG, "write_reg id:%d add:%d len:%d fail,receipt status error %d", id, add, len, receipt_header->status); - return false; - } return true; } bool FeiTeServoMotor::read_regs(uint8_t id, uint8_t add, uint8_t* data, uint8_t len) { @@ -737,26 +729,27 @@ bool FeiTeServoMotor::read_regs(uint8_t id, uint8_t add, uint8_t* data, uint8_t ZASSERT((size_t)rxpacketlen < sizeof(m_rxbuf)); m_txbuf[txpacketlen - 1] = checksum; - if (!tx_and_rx(m_txbuf, txpacketlen, m_rxbuf, rxpacketlen, OVERTIME)) { + if (!tx_and_rx(cmd_header, txpacketlen, receipt_header, rxpacketlen, OVERTIME)) { ZLOGE(TAG, "read_reg id:%d add:%d len:%d fail,overtime", id, add, len); return false; } - if (!(receipt_header->header == 0xffff && receipt_header->id == id)) { - ZLOGE(TAG, "read_reg id:%d add:%d len:%d fail,receipt header error", id, add, len); - ZLOGE(TAG, "read_reg fail,receipt header error"); - return false; - } memcpy(data, receipt_header->data, len); osDelay(10); return true; } -bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems) { +bool FeiTeServoMotor::tx_and_rx(cmd_header_t* txheader, uint8_t txdatalen, receipt_header_t* rx, uint8_t rxlen, int32_t overtimems) { for (int i = 0; i < 5; i++) { - bool suc = _tx_and_rx(tx, txdatalen, rx, expectrxsize, overtimems); - if (suc) { - return true; + if (i != 0) osDelay(30); + + bool suc = _tx_and_rx((uint8_t*)txheader, txdatalen, (uint8_t*)rx, rxlen, overtimems); + if (!suc) continue; + + if (!(rx->header == 0xffff && rx->id == txheader->id)) { + ZLOGE(TAG, "receipt format error header->%04x id:%d!=%d", rx->header, rx->id, txheader->id); + return false; } - osDelay(100); + m_status[txheader->id] = rx->status; + return true; } return false; } diff --git a/sdk/components/mini_servo_motor/feite_servo_motor.hpp b/sdk/components/mini_servo_motor/feite_servo_motor.hpp index 0e01a53..c5ffc69 100644 --- a/sdk/components/mini_servo_motor/feite_servo_motor.hpp +++ b/sdk/components/mini_servo_motor/feite_servo_motor.hpp @@ -101,13 +101,13 @@ typedef enum { kRegServoLockFlag = 55, // 锁标志 kRegServoCurrentPos = 56, // 当前位置 kRegServoCurrentSpeed = 58, // 当前速度 - kRegServoCurrentLoad = 60, // 当前负载 bit10为方向位 - kRegServoCurrentVoltage = 62, // 当前电压 - kRegServoCurrentTemp = 63, // 当前温度 + kRegServoCurrentLoad = 60, // 当前负载 bit10为方向位 输出驱动电机的当前占空比电压,单位为0.1%,取值0-1000 + kRegServoCurrentVoltage = 62, // 当前电压 反馈当前舵机工作电压,反馈精度为0.1V,即120*0.1=12V + kRegServoCurrentTemp = 63, // 当前温度 反馈当前舵机内部工作温度,反馈精度为1摄氏度 kRegServoAsyncWriteFlag = 64, // 异步写标志 kRegServoStatus = 65, // 舵机状态 kRegServoMoveFlag = 66, // 移动标志 - kRegServoCurrentCurrent = 69, // 当前电流 + kRegServoCurrentCurrent = 69, // 当前电流 反馈当前工作电流值,单位为6.26.5mA,最大可反馈电流为500*6.5mA=3250mA kRegServoCheckSpeed = 80, // 80 移动检查速度 kRegServoDTime = 81, // 81 D控制时间 kRegServoSpeedUnit = 82, // 82 速度单位系数 @@ -163,6 +163,7 @@ class FeiTeServoMotor { feite_servo_info_t m_feite_servo_info[10]; int32_t m_num_feite_servo_info = 0; + uint8_t m_status[255] = {0}; public: void initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx); @@ -170,8 +171,9 @@ class FeiTeServoMotor { void regServo(uint8_t id, feite_servo_model_number_t model_type, bool shaft); feite_servo_info_t* getServoInfo(int id); - bool ping(uint8_t id); - bool stop(uint8_t id); + bool ping(uint8_t id); + bool stop(uint8_t id); + uint8_t getStatus(uint8_t id) { return m_status[id]; } bool initServo(uint8_t id); @@ -214,7 +216,7 @@ class FeiTeServoMotor { private: bool _tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems); - bool tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems); + bool tx_and_rx(cmd_header_t* txheader, uint8_t txdatalen, receipt_header_t* rx, uint8_t rxlen, int32_t overtimems); uint8_t checksum(uint8_t* data, uint8_t len); uint8_t checksum_packet(uint8_t* data, uint8_t len); diff --git a/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp b/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp index 7ac86fe..5f3da7a 100644 --- a/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -1,4 +1,6 @@ #include "mini_servo_motor_ctrl_module.hpp" + +#include using namespace iflytop; using namespace std; #define TAG "MiniServo" @@ -36,6 +38,12 @@ int32_t MiniServoCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t PROCESS_REG(kreg_mini_servo_protective_torque, REG_GET(m_cfg.protective_torque), REG_SET(m_cfg.protective_torque)); PROCESS_REG(kreg_mini_servo_is_move, mini_servo_is_move(&val), ACTION_NONE); + PROCESS_REG(kreg_mini_servo_status, mini_servo_read_reg(kreg_mini_servo_status, &val), ACTION_NONE); + PROCESS_REG(kreg_mini_servo_voltage, mini_servo_read_reg(kreg_mini_servo_voltage, &val), ACTION_NONE); + PROCESS_REG(kreg_mini_servo_current, mini_servo_read_reg(kreg_mini_servo_current, &val), ACTION_NONE); + PROCESS_REG(kreg_mini_servo_temperature, mini_servo_read_reg(kreg_mini_servo_temperature, &val), ACTION_NONE); + PROCESS_REG(kreg_mini_servo_loadvalue, mini_servo_read_reg(kreg_mini_servo_loadvalue, &val), ACTION_NONE); + default: return err::kmodule_not_find_reg; break; @@ -61,6 +69,40 @@ int32_t MiniServoCtrlModule::mini_servo_read_pos(int32_t *pos) { if (!m_bus->getPos(m_idinbus, pos)) return err::ksubdevice_overtime; return 0; } + +int32_t MiniServoCtrlModule::mini_servo_read_reg(int32_t regoff, int32_t *regval) { + if (regoff == kreg_mini_servo_status) { + bool suc = m_bus->read_reg(m_idinbus, kRegServoStatus, regval); + if (!suc) return err::ksubdevice_overtime; + return 0; + } + + if (regoff == kreg_mini_servo_voltage) { + bool suc = m_bus->read_reg(m_idinbus, kRegServoCurrentVoltage, regval); + if (!suc) return err::ksubdevice_overtime; + return 0; + } + + if (regoff == kreg_mini_servo_current) { + bool suc = m_bus->read_reg(m_idinbus, kRegServoCurrentCurrent, regval); + if (!suc) return err::ksubdevice_overtime; + *regval = 6.5 * (*regval); + return 0; + } + + if (regoff == kreg_mini_servo_temperature) { + bool suc = m_bus->read_reg(m_idinbus, kRegServoCurrentTemp, regval); + if (!suc) return err::ksubdevice_overtime; + return 0; + } + + if (regoff == kreg_mini_servo_loadvalue) { + bool suc = m_bus->read_reg(m_idinbus, kRegServoCurrentLoad, regval); + if (!suc) return err::ksubdevice_overtime; + return 0; + } +} + int32_t MiniServoCtrlModule::mini_servo_active_cfg() { return 0; } int32_t MiniServoCtrlModule::mini_servo_stop(int32_t breakstop) { m_thread.stop(); @@ -74,16 +116,19 @@ int32_t MiniServoCtrlModule::mini_servo_rotate(int32_t speed) { if (!m_state.enable) return err::kmini_servo_not_enable; if (!m_bus->isSupportMode1(m_idinbus)) return err::kmini_servo_mode_not_support; creg.module_status = 1; + m_thread.start( [this, speed]() { befor_motor_move(); + servo_status_t status = {0}; { m_bus->runInMode1(m_idinbus, m_cfg.limit_torque, speed); while (true) { if (m_thread.getExitFlag()) break; - if (!check_when_run()) break; + if (!check_when_run(&status)) break; + vTaskDelay(5); } } @@ -114,7 +159,8 @@ int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) { befor_motor_move(); { - int32_t velocity = m_cfg.limit_velocity; + int32_t velocity = m_cfg.limit_velocity; + servo_status_t status = {0}; bool callsuc = false; for (size_t i = 0; i < 3; i++) { @@ -128,18 +174,12 @@ int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) { return; } - int32_t moveflag = 0; - int32_t nowpos = 0; while (true) { if (m_thread.getExitFlag()) break; - if (!check_when_run()) break; + if (!check_when_run(&status)) break; - moveflag = 0; - m_bus->isMove(m_idinbus, &moveflag); - m_bus->getPos(m_idinbus, &nowpos); - - if (poseq(nowpos, pos3600, 10) && moveflag == 0) break; + if (poseq(status.pos, pos3600, 10) && status.isMove == 0) break; vTaskDelay(5); } @@ -165,13 +205,13 @@ int32_t MiniServoCtrlModule::mini_servo_rotate_with_torque(int32_t torque) { m_thread.start( [this, torque]() { befor_motor_move(); - + servo_status_t status = {0}; { m_bus->runInMode2(m_idinbus, torque); while (true) { if (m_thread.getExitFlag()) break; - if (!check_when_run()) break; + if (!check_when_run(&status)) break; vTaskDelay(5); } } @@ -224,7 +264,32 @@ void MiniServoCtrlModule::after_motor_move() { creg.module_status = 0; } } -bool MiniServoCtrlModule::check_when_run() { return true; } +bool MiniServoCtrlModule::check_when_run(servo_status_t *status) { + memset(status, 0, sizeof(servo_status_t)); + + int32_t moveflag = 0; + int32_t nowpos = 0; + bool suc = m_bus->isMove(m_idinbus, &moveflag); + if (!suc) { + return false; + } + suc = m_bus->getPos(m_idinbus, &nowpos); + if (!suc) { + return false; + } + + if (m_bus->getStatus(m_idinbus) != 0) { + status->status = m_bus->getStatus(m_idinbus); + ZLOGE(TAG, "check fail, status = %x", status->status); + return false; + } + + status->isMove = moveflag; + status->pos = nowpos; + status->status = m_bus->getStatus(m_idinbus); + ZLOGI(TAG, "moduleid:%d idinbus:%d status %x isMove %d pos %d", m_module_id, m_idinbus, status->status, status->isMove, status->pos); + return true; +} /*********************************************************************************************************************** * INTERNAL * diff --git a/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp b/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp index d8c55b7..883db16 100644 --- a/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp @@ -22,6 +22,12 @@ class MiniServoCtrlModule : public ZIModule { int32_t enable; } state_t; + typedef struct { + uint8_t status; + int32_t pos; + int32_t isMove; + } servo_status_t; + private: uint8_t m_module_id = 0; @@ -62,7 +68,9 @@ class MiniServoCtrlModule : public ZIModule { private: void befor_motor_move(); void after_motor_move(); - bool check_when_run(); + bool check_when_run(servo_status_t *status); + + virtual int32_t mini_servo_read_reg(int32_t regoff, int32_t *regval); int32_t mini_servo_is_move(int32_t *isMove);