From 1e5ea0166a5dea044ece6b6e19c8436dd6573870 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Tue, 24 Oct 2023 22:15:25 +0800 Subject: [PATCH] update --- sdk | 2 +- usrc/intelligent_winding_robot_ctrl.cpp | 36 ++++++++++++++++++++++++--------- usrc/intelligent_winding_robot_ctrl.hpp | 1 + 3 files changed, 29 insertions(+), 10 deletions(-) diff --git a/sdk b/sdk index a2715e8..a3908e8 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit a2715e8c3078ed9ba6ab10f1f270e6e7cd1f2a5c +Subproject commit a3908e8598d72d2a334ddef95a5578b7d8ba8770 diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index aa263dd..ab0110e 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -65,9 +65,9 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() { cfg.m16_xianlajin_reset_pos = 2047; cfg.m16_xianlajin_tight_line_pos = 1966; - cfg.m16_xianlajin_winding_low_pos = 1900; - cfg.m16_xianlajin_winding_up_pos = 1865; - cfg.m16_xianlajin_line_entry_pos = 1833; + cfg.m16_xianlajin_winding_low_pos = 1885; + cfg.m16_xianlajin_winding_up_pos = 1835; + cfg.m16_xianlajin_line_entry_pos = 1800; cfg.m21_arm_hook_claws_full_pos = 2558; cfg.m21_arm_hook_claws_half_pos = 2294; @@ -101,7 +101,8 @@ void IntelligentWindingRobotCtrl::regcb() { m_cmdparse->regCMD("step_take_bullet", "()", 1, [this](PARAM) { return step_take_bullet(atoi(paraV[0])); }); m_cmdparse->regCMD("step_take_back_bullet", "()", 1, [this](PARAM) { return step_take_back_bullet(atoi(paraV[0])); }); m_cmdparse->regCMD("step_prepare_remove_line", "()", 1, [this](PARAM) { return step_prepare_remove_line(atoi(paraV[0])); }); - m_cmdparse->regCMD("step_winding_prepare", "()", 1, [this](PARAM) { return step_winding_prepare(); }); + m_cmdparse->regCMD("step_winding_prepare", "()", 0, [this](PARAM) { return step_winding_prepare(); }); + m_cmdparse->regCMD("step_winding", "()", 0, [this](PARAM) { return step_winding(); }); // m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); }); @@ -269,8 +270,8 @@ int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_press() { return m_dm->mo int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_reset() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_reset_pos, 300, 0); } int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_tight_line_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_tight_line_pos, 100, 0); } int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_winding_low_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_winding_low_pos, 100, 0); } -int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_winding_up_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_winding_up_pos, 100, 0); } -int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_line_entry_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_line_entry_pos, 100, 0); } +int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_winding_up_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_winding_up_pos, 20, 0); } +int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_line_entry_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_line_entry_pos, 20, 0); } int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_reset() { return m_dm->motor_move_to_zero_backward(21, 0, 0, 0, 0); } int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_half_pos() { return m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_half_pos, 0, 0); } @@ -433,6 +434,7 @@ int32_t IntelligentWindingRobotCtrl::step_winding_prepare() { WAIT_MODULES_IDLE(4); xymove_to(cfg.xy_platform_takeline_pos_x, cfg.xy_platform_takeline_pos_y); + m16_xianlajin_move_to_line_entry_pos(); WAIT_MODULES_IDLE(3); // 线夹张开 @@ -451,9 +453,6 @@ int32_t IntelligentWindingRobotCtrl::step_winding_prepare() { m12_jiaxian_move_to_open_pos(); WAIT_MODULES_IDLE(12); - // 移动到转移线高度 - m16_xianlajin_move_to_line_entry_pos(); - WAIT_MODULES_IDLE(16); m4_zmove_to(cfg.z_axis_transfer_line_high); WAIT_MODULES_IDLE(4); @@ -468,14 +467,33 @@ int32_t IntelligentWindingRobotCtrl::step_winding_prepare() { WAIT_MODULES_IDLE(13); m11_arm_jiaxian_move_to_reset_pos(); WAIT_MODULES_IDLE(11); + m16_xianlajin_move_to_tight_line_pos(); m4_zmove_to(0); osDelay(500); WAIT_MODULES_IDLE(4); xymove_to(0, 0); + WAIT_MODULES_IDLE(16); // WAIT_MODULES_IDLE(13); } +int32_t IntelligentWindingRobotCtrl::step_winding() { + m_dm->motor_rotate_acctime(2, 1, 1000, 10000); + osDelay(1000); + for (size_t i = 0; i < 60; i++) { + m16_xianlajin_move_to_winding_low_pos(); + WAIT_MODULES_IDLE(16); + m16_xianlajin_move_to_winding_up_pos(); + WAIT_MODULES_IDLE(16); + } + m_dm->module_stop(2); + + m_dm->motor_move_to_zero_forward(2, 2, 2, 0, 0); + wait_module_idle(2); + ZLOGI(TAG, "step_winding end...."); + return 0; +} + int32_t IntelligentWindingRobotCtrl::main_shaft_run() { ZLOGI(TAG, "main_shaft_run"); DO(m_dm->motor_rotate_acctime(2, 1, 1000, 10000)); diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index 3e5c574..c5f1c85 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -174,6 +174,7 @@ class IntelligentWindingRobotCtrl { int32_t step_prepare_remove_line(int32_t bulletindex); int32_t step_remove_line(); int32_t step_winding_prepare(); + int32_t step_winding(); 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); // 取弹夹