From 3b6bcbe155a48abf6a7859d19f47fbe9c772fb7f Mon Sep 17 00:00:00 2001 From: zhaohe Date: Tue, 24 Oct 2023 16:12:27 +0800 Subject: [PATCH] update --- components/eq_20_asb_motor/eq20_servomotor.cpp | 2 +- components/eq_20_asb_motor/eq20_servomotor.hpp | 2 +- .../mini_servo_motor_ctrl_module.cpp | 50 ++++++++++++++++++++-- .../mini_servo_motor_ctrl_module.hpp | 6 ++- .../step_motor_ctrl_module.cpp | 2 +- .../step_motor_ctrl_module.hpp | 2 +- components/zprotocols/zcancmder_v2 | 2 +- 7 files changed, 57 insertions(+), 9 deletions(-) diff --git a/components/eq_20_asb_motor/eq20_servomotor.cpp b/components/eq_20_asb_motor/eq20_servomotor.cpp index f2e8c15..b7db3fb 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/eq20_servomotor.cpp @@ -269,7 +269,7 @@ int32_t Eq20ServoMotor::motor_enable(int32_t varenable) { int32_t Eq20ServoMotor::motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } int32_t Eq20ServoMotor::motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } int32_t Eq20ServoMotor::motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) { return err::koperation_not_support; } -int32_t Eq20ServoMotor::motor_move_to_with_torque(int32_t direction, int32_t torque) { return err::koperation_not_support; } +int32_t Eq20ServoMotor::motor_rotate_with_torque(int32_t direction, int32_t torque) { return err::koperation_not_support; } int32_t Eq20ServoMotor::motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { return rotate(direction * motor_velocity, acctime); }; int32_t Eq20ServoMotor::motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { return move_by(distance, motor_velocity, acctime); }; diff --git a/components/eq_20_asb_motor/eq20_servomotor.hpp b/components/eq_20_asb_motor/eq20_servomotor.hpp index cf78fac..d74a34a 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.hpp +++ b/components/eq_20_asb_motor/eq20_servomotor.hpp @@ -118,7 +118,7 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor { virtual int32_t motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) override; virtual int32_t motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) override; virtual int32_t motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) override; - virtual int32_t motor_move_to_with_torque(int32_t direction, int32_t torque) override; + virtual int32_t motor_rotate_with_torque(int32_t direction, int32_t torque) override; virtual int32_t motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) override; virtual int32_t motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) override; 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 3ce6425..0dd3d01 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -289,10 +289,13 @@ int32_t MiniRobotCtrlModule::module_get_error(int32_t *iserror) { * @brief TODO: * Ìí¼Ó¹ýÔؼì²â */ - *iserror = 0; + *iserror = m_errorcode; + return 0; +} +int32_t MiniRobotCtrlModule::module_clear_error() { + m_errorcode = 0; return 0; } -int32_t MiniRobotCtrlModule::module_clear_error() { return 0; } int32_t MiniRobotCtrlModule::module_set_param(int32_t param_id, int32_t param_value) { return err::kmodule_not_find_config_index; } int32_t MiniRobotCtrlModule::module_get_param(int32_t param_id, int32_t *param_value) { return err::kmodule_not_find_config_index; } @@ -313,13 +316,54 @@ int32_t MiniRobotCtrlModule::motor_enable(int32_t varenable) { return enable(var int32_t MiniRobotCtrlModule::motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { return rotate(direction, motor_velocity, acc, nullptr); } int32_t MiniRobotCtrlModule::motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) { return move_by(distance, motor_velocity, acc, nullptr); } int32_t MiniRobotCtrlModule::motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) { return move_to(position, motor_velocity, acc, nullptr); } -int32_t MiniRobotCtrlModule::motor_move_to_with_torque(int32_t direction, int32_t torque) { +int32_t MiniRobotCtrlModule::motor_rotate_with_torque(int32_t direction, int32_t torque) { if (direction >= 0) { return move_forward(torque); } else { return move_backward(torque); } } +void MiniRobotCtrlModule::set_errorcode(int32_t errorcode) { m_errorcode = errorcode; } + +int32_t MiniRobotCtrlModule::motor_move_to_torque(int32_t targetpos, int32_t torque, int32_t overtime) { + ZLOGI(TAG, "move_backward torque:%d", 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(); + + while (true) { + if (m_thread.getExitFlag()) break; + + if (overtime != 0 && zos_haspassedms(entertime) > overtime) { + ZLOGI(TAG, "motor_move_to_torque %d overtime", 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); + stop(0); + break; + } + + m_thread.sleep(10); + } + stop(0); + }); + return 0; +} int32_t MiniRobotCtrlModule::motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } int32_t MiniRobotCtrlModule::motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } 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 74cf44a..e2b8965 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp @@ -16,6 +16,8 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI s32 m_last_exec_status = 0; + int32_t m_errorcode = 0; + public: void initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus); @@ -67,7 +69,8 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI virtual int32_t motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) override; virtual int32_t motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) override; virtual int32_t motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) override; - virtual int32_t motor_move_to_with_torque(int32_t direction, int32_t torque) override; + virtual int32_t motor_rotate_with_torque(int32_t direction, int32_t torque) override; + virtual int32_t motor_move_to_torque(int32_t pos, int32_t torque, int32_t overtime) override; virtual int32_t motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) override; virtual int32_t motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) override; @@ -80,5 +83,6 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI private: void call_status_cb(action_cb_status_t cb, int32_t status); + void set_errorcode(int32_t errorcode); }; } // namespace iflytop \ No newline at end of file diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp index 158bc9f..b2b3534 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -774,7 +774,7 @@ int32_t StepMotorCtrlModule::motor_move_to(int32_t tox, int32_t motor_velocity, [this, tox]() { _motor_stop(); }); return 0; } -int32_t StepMotorCtrlModule::motor_move_to_with_torque(int32_t direction, int32_t torque) { return err::koperation_not_support; } +int32_t StepMotorCtrlModule::motor_rotate_with_torque(int32_t direction, int32_t torque) { return err::koperation_not_support; } int32_t StepMotorCtrlModule::motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } int32_t StepMotorCtrlModule::motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index fe9a808..d2f8deb 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -131,7 +131,7 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule, public ZIModule, publi virtual int32_t motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) override; virtual int32_t motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) override; virtual int32_t motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) override; - virtual int32_t motor_move_to_with_torque(int32_t direction, int32_t torque) override; + virtual int32_t motor_rotate_with_torque(int32_t direction, int32_t torque) override; virtual int32_t motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) override; virtual int32_t motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) override; diff --git a/components/zprotocols/zcancmder_v2 b/components/zprotocols/zcancmder_v2 index 84c9be4..481aede 160000 --- a/components/zprotocols/zcancmder_v2 +++ b/components/zprotocols/zcancmder_v2 @@ -1 +1 @@ -Subproject commit 84c9be43174493f0de079de781ff35fcd954a0cd +Subproject commit 481aede1a713fc1e3a12d91970ee9aea6d722d7d