From 1996dd30d146d2049c928261c60b920ee7f53ca1 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Tue, 24 Oct 2023 16:12:34 +0800 Subject: [PATCH] update --- README.md | 19 +++++ sdk | 2 +- usrc/app_zmodule_device_manager.cpp | 6 +- usrc/app_zmodule_device_manager.hpp | 2 +- usrc/intelligent_winding_robot_ctrl.cpp | 133 ++++++++++++++++++++++++-------- usrc/intelligent_winding_robot_ctrl.hpp | 11 ++- usrc/main.cpp | 6 ++ 7 files changed, 138 insertions(+), 41 deletions(-) diff --git a/README.md b/README.md index 3f9c1d3..e5ed6eb 100644 --- a/README.md +++ b/README.md @@ -124,4 +124,23 @@ xy_robot_ctrl_read_status (id) xy_robot_ctrl_read_version (id) xy_robot_ctrl_set_base_param (id,paramName,val) xy_robot_ctrl_stop (id,stop_type) +``` + +``` +子弹: +bullet + +弹壳 +Bullet_case +弹芯 +Bullet_core +弹夹 +bullet_holder + +``` + +``` +取弹夹 + + ``` \ No newline at end of file diff --git a/sdk b/sdk index d4c0048..3b6bcbe 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit d4c0048415ea76a2baf4cf731b7ae7d97298b22d +Subproject commit 3b6bcbe155a48abf6a7859d19f47fbe9c772fb7f diff --git a/usrc/app_zmodule_device_manager.cpp b/usrc/app_zmodule_device_manager.cpp index 633d760..58b901e 100644 --- a/usrc/app_zmodule_device_manager.cpp +++ b/usrc/app_zmodule_device_manager.cpp @@ -140,9 +140,9 @@ int32_t APPDM::motor_move_to_zero_backward(uint16_t id, int32_t findzerospeed, i ZLOGI(TAG, "motor_move_to_zero_backward %d %d %d %d %d", id, findzerospeed, findzeroedge_speed, acc, overtime); DOCMD(DM::motor_move_to_zero_backward(id, findzerospeed, findzeroedge_speed, acc, overtime)); } -int32_t APPDM::motor_move_to_with_torque(uint16_t id, int32_t direction, int32_t torque) { // - ZLOGI(TAG, "motor_move_to_with_torque %d %d %d", id, direction, torque); - DOCMD(DM::motor_move_to_with_torque(id, direction, torque)); +int32_t APPDM::motor_rotate_with_torque(uint16_t id, int32_t direction, int32_t torque) { // + ZLOGI(TAG, "motor_rotate_with_torque %d %d %d", id, direction, torque); + DOCMD(DM::motor_rotate_with_torque(id, direction, torque)); } int32_t APPDM::motor_read_pos(uint16_t id, int32_t *pos) { // DOCMD(DM::motor_read_pos(id, pos)); diff --git a/usrc/app_zmodule_device_manager.hpp b/usrc/app_zmodule_device_manager.hpp index 2286651..a160d6e 100644 --- a/usrc/app_zmodule_device_manager.hpp +++ b/usrc/app_zmodule_device_manager.hpp @@ -54,7 +54,7 @@ class APPDM : public ZModuleDeviceManager { virtual int32_t motor_move_to_acctime(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acctime) override; virtual int32_t motor_move_to_zero_forward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; virtual int32_t motor_move_to_zero_backward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; - virtual int32_t motor_move_to_with_torque(uint16_t id, int32_t direction, int32_t torque) override; + virtual int32_t motor_rotate_with_torque(uint16_t id, int32_t direction, int32_t torque) override; virtual int32_t motor_read_pos(uint16_t id, int32_t *pos) override; virtual int32_t motor_set_current_pos_by_change_shift(uint16_t id, int32_t pos) override; virtual int32_t motor_move_to_zero_forward_and_calculated_shift(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index 4a8e2ed..518c118 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -80,8 +80,13 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() { void IntelligentWindingRobotCtrl::regcb() { // device_reset 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("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); }); + + + + // 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(); }); @@ -136,36 +141,101 @@ void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) { } } -void IntelligentWindingRobotCtrl::wait_modules_idle(...) { +void IntelligentWindingRobotCtrl::wait_modules_idle(void* mark, ...) { int32_t moduleid = 0; va_list args; - va_start(args, moduleid); - while (moduleid != -1) { + va_start(args, mark); + moduleid = va_arg(args, int32_t); + while (moduleid > 0) { wait_module_idle(moduleid); moduleid = va_arg(args, int32_t); } va_end(args); } +int32_t IntelligentWindingRobotCtrl::disable_all_module() { + m_dm->motor_enable(2, 0); + m_dm->xymotor_enable(3, 0); + m_dm->motor_enable(4, 0); + m_dm->motor_enable(11, 0); + m_dm->motor_enable(12, 0); + m_dm->motor_enable(13, 0); + m_dm->motor_enable(14, 0); + m_dm->motor_enable(15, 0); + m_dm->motor_enable(16, 0); + + m_dm->motor_enable(21, 0); + m_dm->motor_enable(22, 0); + m_dm->motor_enable(23, 0); + return 0; +} +int32_t IntelligentWindingRobotCtrl::enable_all_module() { + m_dm->motor_enable(2, 1); + m_dm->xymotor_enable(3, 1); + m_dm->motor_enable(4, 1); + m_dm->motor_enable(11, 1); + m_dm->motor_enable(12, 1); + m_dm->motor_enable(13, 1); + m_dm->motor_enable(14, 1); + m_dm->motor_enable(15, 1); + m_dm->motor_enable(16, 1); + + m_dm->motor_enable(21, 1); + m_dm->motor_enable(22, 1); + m_dm->motor_enable(23, 1); + return 0; +} + +#define WAIT_MODULES_IDLE(...) \ + { wait_modules_idle(NULL, __VA_ARGS__, NULL); } + int32_t IntelligentWindingRobotCtrl::device_reset() { // Z轴复位 - zreset(); - m21_arm_hook_claws_reset(); + /** + * @ + * + * 1. Z轴及其组件复位 + * 2. 除了M13外，其他电机复位 + * 3. M13复位,等待一定时间，查看M13是否复位成功 + * 4. 机械臂复位 + */ + + enable_all_module(); + m4_zreset(); m11_arm_jiaxian_move_to_reset_pos(); - xy_reset(); + m21_arm_hook_claws_reset(); + WAIT_MODULES_IDLE(4, 11, 21); + + m12_jiaxian_move_to_reset_pos(); + m14_raoxiantance_move_to_reset(); + m15_paifei_moveto_reset(); + m16_xianlajin_move_to_reset(); + m22_scissors_move_reset_pos(); + m23_laxian_motor_move_to_reset_pos(); + WAIT_MODULES_IDLE(12, 14, 15, 16, 22, 23); + + /** + * @brief 优化复位逻辑，超时复位失败，报警 + */ + m13_yaxian_move_to_reset_backward(); + wait_module_idle(13); + + m_dm->xymotor_move_to_zero(XYRobot_ID); + wait_module_idle(XYRobot_ID); + ZLOGI(TAG, "device_reset finished...."); } int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_reset_pos() { return m_dm->motor_move_to(11, cfg.m11_arm_jiaxian_reset_pos, 2000, 0); } -int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_clamp_pos() { return m_dm->motor_move_to_with_torque(11, cfg.m11_arm_jiaxian_clamp_direction, cfg.m11_arm_jiaxian_clamp_torque); } +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(12, cfg.m12_jiaxian_reset_pos, 2000, 0); } -int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_clamp_pos() { return m_dm->motor_move_to_with_torque(12, cfg.m12_jiaxian_clamp_direction, cfg.m12_jiaxian_clamp_torque); } +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(13, cfg.m13_yaxian_forward_reset_pos, 2000, 0); } int32_t IntelligentWindingRobotCtrl::m13_yaxian_move_to_reset_backward() { return m_dm->motor_move_to(13, cfg.m13_yaxian_backward_reset_pos, 2000, 0); } -int32_t IntelligentWindingRobotCtrl::m13_yaxian_press_clip() { return m_dm->motor_move_to_with_torque(13, cfg.m13_jiaxian_clamp_direction, cfg.m13_jiaxian_clamp_torque); } +int32_t IntelligentWindingRobotCtrl::m13_yaxian_press_clip() { return m_dm->motor_rotate_with_torque(13, cfg.m13_jiaxian_clamp_direction, cfg.m13_jiaxian_clamp_torque); } int32_t IntelligentWindingRobotCtrl::m14_raoxiantance_move_to_reset() { return m_dm->motor_move_to(14, cfg.m14_raoxiantance_reset_pos, 2000, 0); } int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_reset() { return m_dm->motor_move_to(15, cfg.m15_paifei_reset_pos, 2000, 0); } -int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_press() { return m_dm->motor_move_to_with_torque(15, cfg.m15_paifei_press_direction, cfg.m15_paifei_press_torque); } +int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_press() { return m_dm->motor_rotate_with_torque(15, cfg.m15_paifei_press_direction, cfg.m15_paifei_press_torque); } 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); } @@ -175,9 +245,23 @@ int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_reset() { return m_dm->m 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); } int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_full_pos() { return m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_full_pos, 0, 0); } int32_t IntelligentWindingRobotCtrl::m22_scissors_move_reset_pos() { return 0; } -int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() { return m_dm->motor_move_to_with_torque(22, 1, 4095); } +int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() { return m_dm->motor_rotate_with_torque(22, 1, 4095); } int32_t IntelligentWindingRobotCtrl::m23_laxian_motor_move_to_reset_pos() { return m_dm->motor_move_to_zero_backward(23, 0, 0, 0, 0); } int32_t IntelligentWindingRobotCtrl::m23_laxian_motor_move_to_tight_line_pos() { return m_dm->motor_move_to(23, 500, 0, 0); } +int32_t IntelligentWindingRobotCtrl::m4_zreset() { m_dm->motor_move_to_zero_backward(4, 450, 300, 2000, 0); } +int32_t IntelligentWindingRobotCtrl::m4_zmove_to(int32_t pos) { + ZLOGI(TAG, "zmove_to %d", pos); + int32_t nowpos = 0; + m_dm->motor_read_pos(ZMOTOR_ID, &nowpos); + if (pos >= nowpos) { + m_dm->motor_move_to(ZMOTOR_ID, pos, 450, 600); + } else { + m_dm->motor_move_to(ZMOTOR_ID, pos, 1000, 600); + } + if (pos == 0) { + m4_zreset(); + } +} int32_t IntelligentWindingRobotCtrl::main_shaft_run() { ZLOGI(TAG, "main_shaft_run"); @@ -214,26 +298,7 @@ int32_t IntelligentWindingRobotCtrl::start_winding() { return 0; } int32_t IntelligentWindingRobotCtrl::stop_winding() { return 0; } int32_t IntelligentWindingRobotCtrl::reset_and_check_device() { return 0; } -int32_t IntelligentWindingRobotCtrl::disable_all_motor() { - m_dm->motor_enable(2, 0); - m_dm->xymotor_enable(XYRobot_ID, 0); - m_dm->motor_enable(11, 0); - m_dm->motor_enable(12, 0); - m_dm->motor_enable(13, 0); - m_dm->motor_enable(14, 0); - m_dm->motor_enable(15, 0); - m_dm->motor_enable(16, 0); - - m_dm->motor_enable(21, 0); - m_dm->motor_enable(22, 0); - m_dm->motor_enable(23, 0); - return 0; -} - int32_t IntelligentWindingRobotCtrl::xy_get_point(int32_t clip_index, int32_t& x, int32_t& y) { - // x = 0; - // y = 0; - int clip_x_off = clip_index / cfg.clip_each_line_num; int clip_y_off = clip_index % cfg.clip_each_line_num; @@ -245,6 +310,8 @@ int32_t IntelligentWindingRobotCtrl::xy_get_point(int32_t clip_index, int32_t& x return 0; } + +#if 0 int32_t IntelligentWindingRobotCtrl::zmove_to(int32_t pos) { ZLOGI(TAG, "zmove_to %d", pos); int32_t nowpos = 0; @@ -264,10 +331,10 @@ int32_t IntelligentWindingRobotCtrl::zmove_to(int32_t pos) { int32_t IntelligentWindingRobotCtrl::zreset() { ZLOGI(TAG, "zreset"); m_dm->motor_enable(ZMOTOR_ID, 1); - m_dm->motor_move_to_zero_backward(4, 450, 300, 2000, 0); wait_module_idle(ZMOTOR_ID); return 0; } +#endif int32_t IntelligentWindingRobotCtrl::xymove_to(int32_t x, int32_t y) { ZLOGI(TAG, "xymove_to %d %d", x, y); @@ -283,6 +350,7 @@ int32_t IntelligentWindingRobotCtrl::xy_reset() { } int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index) { +#if 0 ZLOGI(TAG, "xy_run_to_clip_pos_test %d", clip_index); if (clip_index >= cfg.clip_line * cfg.clip_each_line_num) { ZLOGE(TAG, "clip_index %d out of range", clip_index); @@ -303,6 +371,7 @@ int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index) zmove_to(cfg.z_axis_take_clip_pos); m21_arm_hook_claws_reset(); zmove_to(0); +#endif } int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) { diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index 02cf5cc..bf3fe54 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -117,7 +117,7 @@ class IntelligentWindingRobotCtrl { public: void wait_module_idle(int32_t moduleid); - void wait_modules_idle(...); + 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(); @@ -141,15 +141,18 @@ class IntelligentWindingRobotCtrl { int32_t m23_laxian_motor_move_to_reset_pos(); // block int32_t m23_laxian_motor_move_to_tight_line_pos(); // block + int32_t m4_zreset(); + int32_t m4_zmove_to(int32_t pos); + int32_t device_reset(); - int32_t disable_all_motor(); + int32_t disable_all_module(); + int32_t enable_all_module(); + 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); int32_t xy_reset(); - int32_t zmove_to(int32_t pos); - int32_t zreset(); int32_t xymove_to(int32_t x, int32_t y); void regcb(); diff --git a/usrc/main.cpp b/usrc/main.cpp index 92158e4..1e62e90 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -240,6 +240,12 @@ void Main::run() { g_zmodule_device_manager.registerModule(&g_step_motor45[1]); g_zmodule_device_manager.registerModule(&g_step_motor45[2]); + int32_t status; + g_zmodule_device_manager.module_get_status(21, &status); + + // status *= 2; + // status + g_intelligent_winding_robot_ctrl.initialize(&g_zmodule_device_manager, &g_cmdScheduler); /*******************************************************************************