From 038b6538f837519f0759a668a2ba533bb7c500d3 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Tue, 24 Oct 2023 16:45:32 +0800 Subject: [PATCH] fix some bug --- .../mini_servo_motor_ctrl_module.cpp | 41 +++++++++++++--------- .../mini_servo_motor_ctrl_module.hpp | 5 +-- 2 files changed, 27 insertions(+), 19 deletions(-) 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 0dd3d01..8ecc4d5 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -46,7 +46,7 @@ int32_t MiniRobotCtrlModule::position_calibrate(s32 nowpos) { return 0; } int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, action_cb_status_t status_cb) { - ZLOGI(TAG, "rotate speed:%d torque:%d run_time:%d", speed, torque, run_time); + ZLOGI(TAG, "%d rotate speed:%d torque:%d run_time:%d", m_module_id, speed, torque, run_time); m_thread.stop(); if (!m_bus->ping(m_id)) return err::ksubdevice_overtime; @@ -68,13 +68,13 @@ int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, action_ } m_thread.sleep(10); } - ZLOGI(TAG, "rotate stop"); + ZLOGI(TAG, "%d rotate stop", m_module_id); }); return 0; } int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) { - ZLOGI(TAG, "move_to pos:%d speed:%d torque:%d", pos, speed, torque); + ZLOGI(TAG, "%d move_to pos:%d speed:%d torque:%d", m_module_id, pos, speed, torque); m_thread.stop(); if (!m_bus->ping(m_id)) return err::ksubdevice_overtime; @@ -98,14 +98,14 @@ int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, action_cb_s m_thread.sleep(10); } stop(0); - // ZLOGI(TAG, "move_to stop"); + // ZLOGI(TAG, "%d move_to stop")m_module_id,; }); return 0; } int32_t MiniRobotCtrlModule::move_forward(s32 torque) { - ZLOGI(TAG, "move_forward torque:%d", torque); + ZLOGI(TAG, "%d move_forward torque:%d", m_module_id, torque); if (!m_bus->ping(m_id)) return err::ksubdevice_overtime; limit_input(torque, 0, 1000); @@ -118,13 +118,13 @@ int32_t MiniRobotCtrlModule::move_forward(s32 torque) { m_thread.sleep(10); } stop(0); - ZLOGI(TAG, "move_forward stop"); + ZLOGI(TAG, "%d move_forward stop", m_module_id); }); return 0; } int32_t MiniRobotCtrlModule::move_backward(s32 torque) { - ZLOGI(TAG, "move_backward torque:%d", torque); + ZLOGI(TAG, "%d move_backward torque:%d", m_module_id, torque); if (!m_bus->ping(m_id)) return err::ksubdevice_overtime; limit_input(torque, 0, 1000); @@ -137,13 +137,13 @@ int32_t MiniRobotCtrlModule::move_backward(s32 torque) { m_thread.sleep(10); } stop(0); - ZLOGI(TAG, "move_forward stop"); + ZLOGI(TAG, "%d move_forward stop", m_module_id); }); return 0; } int32_t MiniRobotCtrlModule::move_by(s32 dpos, s32 speed, s32 torque, action_cb_status_t status_cb) { - ZLOGI(TAG, "move_by pos:%d speed:%d torque:%d", dpos, speed, torque); + ZLOGI(TAG, "%d move_by pos:%d speed:%d torque:%d", m_module_id, dpos, speed, torque); m_thread.stop(); if (!m_bus->ping(m_id)) return err::ksubdevice_overtime; @@ -164,7 +164,7 @@ int32_t MiniRobotCtrlModule::move_by(s32 dpos, s32 speed, s32 torque, action_cb_ int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, action_cb_status_t status_cb) { return err::koperation_not_support; #if 0 - ZLOGI(TAG, "run_with_torque torque:%d run_time:%d", torque, run_time); + ZLOGI(TAG, "%d run_with_torque torque:%d run_time:%d",m_module_id, torque, run_time); m_thread.stop(); if (!m_bus->ping(m_id)) return err::ksubdevice_overtime; @@ -248,7 +248,7 @@ int32_t MiniRobotCtrlModule::read_detailed_status(detailed_status_t &debug_info) int32_t MiniRobotCtrlModule::set_basic_param(basic_param_t ¶m) { return err::koperation_not_support; } int32_t MiniRobotCtrlModule::get_basic_param(basic_param_t ¶m) { - ZLOGI(TAG, "get_basic_param"); + // ZLOGI(TAG, "%d get_basic_param", m_module_id); if (!m_bus->ping(m_id)) return err::ksubdevice_overtime; m_bus->read_u16(m_id, feite::kRegServoCalibration, param.posCalibrate); m_bus->read_u16(m_id, feite::kRegServoMinAngle, param.minlimit); @@ -325,14 +325,23 @@ int32_t MiniRobotCtrlModule::motor_rotate_with_torque(int32_t direction, int32_t } void MiniRobotCtrlModule::set_errorcode(int32_t errorcode) { m_errorcode = errorcode; } +int32_t MiniRobotCtrlModule::getdpos(int32_t targetpos) { + int32_t nowpos = 0; + m_bus->getNowPos(m_id, nowpos); + + int32_t dpos = targetpos - nowpos; + return abs(dpos); +} + int32_t MiniRobotCtrlModule::motor_move_to_torque(int32_t targetpos, int32_t torque, int32_t overtime) { - ZLOGI(TAG, "move_backward torque:%d", torque); + ZLOGI(TAG, "%d motor_move_to_torque torque:%d", m_module_id, torque); if (!m_bus->ping(m_id)) return err::ksubdevice_overtime; limit_input(torque, 0, 1000); m_thread.stop(); if (!m_bus->moveTo(m_id, targetpos, 0, torque)) return err::ksubdevice_overtime; + m_thread.start([this, torque, overtime, targetpos]() { int32_t entertime = zos_get_tick(); @@ -340,20 +349,18 @@ int32_t MiniRobotCtrlModule::motor_move_to_torque(int32_t targetpos, int32_t tor if (m_thread.getExitFlag()) break; if (overtime != 0 && zos_haspassedms(entertime) > overtime) { - ZLOGI(TAG, "motor_move_to_torque %d overtime", m_id); + ZLOGI(TAG, "%d motor_move_to_torque %d overtime", m_module_id, m_id); stop(0); set_errorcode(err::kmotor_run_overtime); break; } - int32_t nowpos = 0; uint8_t moveFlag = 0; - m_bus->getNowPos(m_id, nowpos); m_bus->getMoveFlag(m_id, moveFlag); - if (nowpos - targetpos < 10 && moveFlag == 0) { - ZLOGI(TAG, "motor_move_to_torque %d succ", m_id); + if (getdpos(targetpos) < 10 && moveFlag == 0) { + ZLOGI(TAG, "%d motor_move_to_torque %d succ", m_module_id, m_id); stop(0); break; } diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp index e2b8965..36e14e1 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp @@ -82,7 +82,8 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI virtual int32_t motor_read_pos(int32_t *pos); private: - void call_status_cb(action_cb_status_t cb, int32_t status); - void set_errorcode(int32_t errorcode); + void call_status_cb(action_cb_status_t cb, int32_t status); + void set_errorcode(int32_t errorcode); + int32_t getdpos(int32_t targetpos); }; } // namespace iflytop \ No newline at end of file