From 575186bae4e647a2a7a08f8ef3bf0b4fbaa8d26f Mon Sep 17 00:00:00 2001 From: zhaohe Date: Tue, 24 Oct 2023 18:34:15 +0800 Subject: [PATCH] update --- README.md | 5 ++- sdk | 2 +- usrc/intelligent_winding_robot_ctrl.cpp | 59 ++++++++++++++++++++++++++++----- usrc/intelligent_winding_robot_ctrl.hpp | 9 +++-- 4 files changed, 61 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index e5ed6eb..723dcfe 100644 --- a/README.md +++ b/README.md @@ -140,7 +140,10 @@ bullet_holder ``` ``` -取弹夹 +``` + + +``` ``` \ No newline at end of file diff --git a/sdk b/sdk index 038b653..a2715e8 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit 038b6538f837519f0759a668a2ba533bb7c500d3 +Subproject commit a2715e8c3078ed9ba6ab10f1f270e6e7cd1f2a5c diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index b4de570..cf887c8 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -82,13 +82,14 @@ void IntelligentWindingRobotCtrl::regcb() { m_cmdparse->regCMD("device_reset", "()", 0, [this](PARAM) { device_reset(); }); m_cmdparse->regCMD("enable_all_module", "()", 0, [this](PARAM) { enable_all_module(); }); m_cmdparse->regCMD("disable_all_module", "()", 0, [this](PARAM) { disable_all_module(); }); - m_cmdparse->regCMD("xy_run_to_clip_pos_test", "()", 1, [this](PARAM) { return xy_run_to_clip_pos_test(atoi(paraV[0])); }); + // m_cmdparse->regCMD("xy_run_to_clip_pos_test", "()", 1, [this](PARAM) { return xy_run_to_clip_pos_test(atoi(paraV[0])); }); + m_cmdparse->regCMD("step_take_bullet", "()", 1, [this](PARAM) { return step_take_bullet(atoi(paraV[0])); }); // m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); }); m_cmdparse->regCMD("app_m11_arm_jiaxian_move_to_reset_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_reset_pos(); }); m_cmdparse->regCMD("app_m11_arm_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_clamp_pos(); }); - m_cmdparse->regCMD("app_m12_jiaxian_move_to_reset_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_reset_pos(); }); + m_cmdparse->regCMD("app_m12_jiaxian_move_to_open_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_open_pos(); }); m_cmdparse->regCMD("app_m12_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_clamp_pos(); }); m_cmdparse->regCMD("app_m13_yaxian_move_to_reset_forward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_forward(); }); m_cmdparse->regCMD("app_m13_yaxian_move_to_reset_backward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_backward(); }); @@ -200,10 +201,9 @@ int32_t IntelligentWindingRobotCtrl::device_reset() { enable_all_module(); m4_zreset(); m11_arm_jiaxian_move_to_reset_pos(); - m21_arm_hook_claws_reset(); - WAIT_MODULES_IDLE(4, 11, 21); + WAIT_MODULES_IDLE(4, 11); - m12_jiaxian_move_to_reset_pos(); + m12_jiaxian_move_to_clamp_pos(); m14_raoxiantance_move_to_reset(); m15_paifei_moveto_reset(); m16_xianlajin_move_to_reset(); @@ -220,13 +220,16 @@ int32_t IntelligentWindingRobotCtrl::device_reset() { m_dm->xymotor_move_to_zero(XYRobot_ID); wait_module_idle(XYRobot_ID); + m21_arm_hook_claws_reset(); + wait_module_idle(21); + ZLOGI(TAG, "device_reset finished...."); + return 0; } - int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_reset_pos() { return m_dm->motor_move_to_torque(11, cfg.m11_arm_jiaxian_reset_pos, 330, 0); } int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_clamp_pos() { return m_dm->motor_rotate_with_torque(11, cfg.m11_arm_jiaxian_clamp_direction, cfg.m11_arm_jiaxian_clamp_torque); } -int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_reset_pos() { return m_dm->motor_move_to_torque(12, cfg.m12_jiaxian_reset_pos, 330, 0); } +int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_open_pos() { return m_dm->motor_move_to_torque(12, cfg.m12_jiaxian_reset_pos, 330, 0); } int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_clamp_pos() { return m_dm->motor_rotate_with_torque(12, cfg.m12_jiaxian_clamp_direction, cfg.m12_jiaxian_clamp_torque); } int32_t IntelligentWindingRobotCtrl::m13_yaxian_move_to_reset_forward() { return m_dm->motor_move_to_torque(13, cfg.m13_yaxian_forward_reset_pos, 330, 0); } int32_t IntelligentWindingRobotCtrl::m13_yaxian_move_to_reset_backward() { return m_dm->motor_move_to_torque(13, cfg.m13_yaxian_backward_reset_pos, 100, 0); } @@ -262,6 +265,46 @@ int32_t IntelligentWindingRobotCtrl::m4_zmove_to(int32_t pos) { } } +int32_t IntelligentWindingRobotCtrl::step_take_bullet(int32_t bulletindex) { +#if 0 +0. XY复位 +1. Z轴复位 +2. 钩子复位 + +3. 移动到指定位置 + +3. 取子弹 +4. 夹线 +5. Z轴移动到零点 +#endif + xy_reset(); + m4_zreset(); + m21_arm_hook_claws_reset(); + m11_arm_jiaxian_move_to_reset_pos(); + + WAIT_MODULES_IDLE(3, 4, 11, 21); + + int32_t x = 0; + int32_t y = 0; + xy_get_point(bulletindex, x, y); + xymove_to(x, y); + + WAIT_MODULES_IDLE(3); + + m4_zmove_to(cfg.z_axis_take_clip_pos); + WAIT_MODULES_IDLE(4); + + m21_arm_hook_claws_move_to_full_pos(); + m11_arm_jiaxian_move_to_clamp_pos(); + WAIT_MODULES_IDLE(21, 11); + + m4_zmove_to(0); + WAIT_MODULES_IDLE(4); + + m4_zreset(); + WAIT_MODULES_IDLE(4); +} + int32_t IntelligentWindingRobotCtrl::main_shaft_run() { ZLOGI(TAG, "main_shaft_run"); DO(m_dm->motor_rotate_acctime(2, 1, 1000, 10000)); @@ -338,13 +381,11 @@ int32_t IntelligentWindingRobotCtrl::zreset() { int32_t IntelligentWindingRobotCtrl::xymove_to(int32_t x, int32_t y) { ZLOGI(TAG, "xymove_to %d %d", x, y); m_dm->xymotor_move_to(XYRobot_ID, x, y, 0); - wait_module_idle(XYRobot_ID); return 0; } int32_t IntelligentWindingRobotCtrl::xy_reset() { ZLOGI(TAG, "xy_reset"); m_dm->xymotor_move_to_zero(XYRobot_ID); - wait_module_idle(XYRobot_ID); return 0; } diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index bf3fe54..4abe3f0 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -116,11 +116,12 @@ class IntelligentWindingRobotCtrl { int32_t initialize(APPDM* dm, ICmdParser* cmdparse); public: - void wait_module_idle(int32_t moduleid); - void wait_modules_idle(void* mark, ...); + void wait_module_idle(int32_t moduleid); + void wait_modules_idle(void* mark, ...); + int32_t m11_arm_jiaxian_move_to_reset_pos(); int32_t m11_arm_jiaxian_move_to_clamp_pos(); - int32_t m12_jiaxian_move_to_reset_pos(); + int32_t m12_jiaxian_move_to_open_pos(); int32_t m12_jiaxian_move_to_clamp_pos(); int32_t m13_yaxian_move_to_reset_forward(); int32_t m13_yaxian_move_to_reset_backward(); @@ -148,6 +149,8 @@ class IntelligentWindingRobotCtrl { int32_t disable_all_module(); int32_t enable_all_module(); + int32_t step_take_bullet(int32_t bulletindex); + int32_t xy_get_point(int32_t clip_index, int32_t& x, int32_t& y); // 取弹夹 int32_t xy_run_to(int32_t x, int32_t y, int32_t zpos = 0, bool jiaxian_reset = true); // 取弹夹 int32_t xy_run_to_clip_pos_test(int32_t clip_index);