diff --git a/components/mini_servo_motor/feite_servo_motor.cpp b/components/mini_servo_motor/feite_servo_motor.cpp index 6f1a15b..b64326f 100644 --- a/components/mini_servo_motor/feite_servo_motor.cpp +++ b/components/mini_servo_motor/feite_servo_motor.cpp @@ -536,6 +536,17 @@ bool FeiTeServoMotor::read_regs(uint8_t id, uint8_t add, uint8_t* data, uint8_t return true; } bool FeiTeServoMotor::tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems) { + for (int i = 0; i < 3; i++) { + bool suc = _tx_and_rx(tx, txdatalen, rx, expectrxsize, overtimems); + if (suc) { + return true; + } + osDelay(1); + } + return false; +} + +bool FeiTeServoMotor::_tx_and_rx(uint8_t* tx, uint8_t txdatalen, uint8_t* rx, uint8_t expectrxsize, int32_t overtimems) { uint32_t enter_ticket = HAL_GetTick(); dumphex("tx:", tx, txdatalen); #if 1 diff --git a/components/mini_servo_motor/feite_servo_motor.hpp b/components/mini_servo_motor/feite_servo_motor.hpp index b5cb731..6e521df 100644 --- a/components/mini_servo_motor/feite_servo_motor.hpp +++ b/components/mini_servo_motor/feite_servo_motor.hpp @@ -198,6 +198,7 @@ class FeiTeServoMotor { bool read_regs(uint8_t id, uint8_t add, uint8_t* data, uint8_t len); 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); uint8_t checksum(uint8_t* data, uint8_t len); uint8_t checksum_packet(uint8_t* data, uint8_t len); 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 52163af..4d4ca2b 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -106,7 +106,17 @@ int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) { { int32_t velocity = m_cfg.limit_velocity; - m_bus->runInMode0(m_idinbus, m_cfg.limit_torque, m_cfg.limit_velocity, pos3600); + bool callsuc = false; + for (size_t i = 0; i < 3; i++) { + callsuc = m_bus->runInMode0(m_idinbus, m_cfg.limit_torque, m_cfg.limit_velocity, pos3600); + if (!callsuc) continue; + break; + } + + if (!callsuc) { + creg.module_errorcode = err::ksubdevice_overtime; + return; + } int32_t moveflag = 0; int32_t nowpos = 0; @@ -125,10 +135,12 @@ int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) { } } - after_motor_move(); ZLOGI(TAG, "move to pos %d", pos3600); }, - [this]() { inter_mini_servo_stop(0); }); + [this]() { + after_motor_move(); + inter_mini_servo_stop(0); + }); return 0; } @@ -174,8 +186,8 @@ int32_t MiniServoCtrlModule::mini_servo_read_io_state(int32_t ioindex, int32_t * } void MiniServoCtrlModule::befor_motor_move() { - creg.module_status = 1; - creg.module_errorcode = 0; + creg.module_status = 1; + creg.module_errorcode = 0; } void MiniServoCtrlModule::after_motor_move() { if (creg.module_errorcode != 0) {