From 0a01347d1eb151486aa2866bd011f69ab6546e3e Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 26 Oct 2023 08:05:28 +0800 Subject: [PATCH] stable v2 --- usrc/intelligent_winding_robot_ctrl.cpp | 98 ++++++++++++++++++++++----------- usrc/intelligent_winding_robot_ctrl.hpp | 11 ++-- 2 files changed, 72 insertions(+), 37 deletions(-) diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index 1d511bf..fbf47b3 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -61,20 +61,28 @@ class WidthDetector { return m_lastState; } - void wait_module_idle(int32_t moduleid) { + void wait_module_idle(int32_t moduleid, int32_t timeout_ms) { zos_delay(100); - int i = 0; + int i = 0; + int32_t enterticket = zos_get_tick(); while (true) { int32_t status = 0; int32_t ecode = m_dm->module_get_status(moduleid, &status); if (ecode != 0) { break; }; + + if (timeout_ms != 0 && zos_haspassedms(enterticket) > timeout_ms) { + ZLOGE(TAG, "wait_module_idle timeout"); + throw (int32_t)err::kmotor_run_overtime; + } + if (status == 0) { ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status); break; } if (status == 2) { + throw (int32_t)err::kcatch_exception; break; }; if (i % 30 == 0) { @@ -106,7 +114,7 @@ class WidthDetector { void startDetect(bool forward = false) { m_detect_thread.stop(); m14_raoxiantance_move_to_reset(); - wait_module_idle(14); + wait_module_idle(14, 0); int32_t enterticket = zos_get_tick(); @@ -174,7 +182,7 @@ class WidthDetector { m_detect_thread.stop(); m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 330, 0); - wait_module_idle(14); + wait_module_idle(14, 0); } bool isFull() { return g_nowdpos > m_bullet_full_distance - 20; } @@ -275,6 +283,8 @@ void IntelligentWindingRobotCtrl::regcb() { m_cmdparse->regCMD("step_winding_lineend", "()", 0, [this](PARAM) { return step_winding_lineend(); }); m_cmdparse->regCMD("step_winding_lineend_prepare", "()", 1, [this](PARAM) { return step_winding_lineend_prepare(atoi(paraV[0])); }); m_cmdparse->regCMD("step_winding_take_bullet_from_cooking_to_origin_pos", "()", 1, [this](PARAM) { return step_winding_take_bullet_from_cooking_to_origin_pos(atoi(paraV[0])); }); + m_cmdparse->regCMD("start_winding", "()", 0, [this](PARAM) { return start_winding(); }); + m_cmdparse->regCMD("stop_winding", "()", 0, [this](PARAM) { return stop_winding(); }); // m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); }); @@ -309,29 +319,35 @@ void IntelligentWindingRobotCtrl::regcb() { m_cmdparse->regCMD("start_probe_bullet_pos_forward", "()", 0, [this](PARAM) { return start_probe_bullet_pos_forward(); }); } -void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) { +void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid, int32_t timeout_ms) { zos_delay(100); - int i = 0; + int i = 0; + int32_t enterticket = zos_get_tick(); while (true) { int32_t status = 0; int32_t ecode = m_dm->module_get_status(moduleid, &status); if (ecode != 0) { - processError(ecode); break; }; + + if (timeout_ms != 0 && zos_haspassedms(enterticket) > timeout_ms) { + ZLOGE(TAG, "wait_module_idle timeout"); + throw (int32_t)err::kmotor_run_overtime; + } + if (status == 0) { ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status); break; } if (status == 2) { - processError(err::kfail); + throw (int32_t)err::kcatch_exception; break; }; if (i % 30 == 0) { ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status); } i++; - zos_delay(100); + zos_delay(10); } } @@ -346,6 +362,21 @@ void IntelligentWindingRobotCtrl::wait_modules_idle(void* mark, ...) { } va_end(args); } +int32_t IntelligentWindingRobotCtrl::stop_all_module() { + m_dm->module_stop(2); + m_dm->module_stop(3); + m_dm->module_stop(4); + m_dm->module_stop(11); + m_dm->module_stop(12); + m_dm->module_stop(13); + m_dm->module_stop(14); + m_dm->module_stop(15); + m_dm->module_stop(16); + m_dm->module_stop(21); + m_dm->module_stop(22); + m_dm->module_stop(23); + stop_probe_bullet_pos(); +} int32_t IntelligentWindingRobotCtrl::disable_all_module() { m_dm->motor_enable(2, 0); @@ -362,20 +393,7 @@ int32_t IntelligentWindingRobotCtrl::disable_all_module() { m_dm->motor_enable(22, 0); m_dm->motor_enable(23, 0); - m_dm->module_stop(2); - m_dm->module_stop(3); - m_dm->module_stop(4); - m_dm->module_stop(11); - m_dm->module_stop(12); - m_dm->module_stop(13); - m_dm->module_stop(14); - m_dm->module_stop(15); - m_dm->module_stop(16); - m_dm->module_stop(21); - m_dm->module_stop(22); - m_dm->module_stop(23); - - stop_probe_bullet_pos(); + stop_all_module(); return 0; } @@ -428,7 +446,7 @@ int32_t IntelligentWindingRobotCtrl::device_reset() { * @brief 优化复位逻辑，超时复位失败，报警 */ m13_yaxian_move_to_reset_backward(); - wait_module_idle(13); + wait_module_idle(13, 5000); m_dm->xymotor_move_to_zero(XYRobot_ID); wait_module_idle(XYRobot_ID); @@ -536,11 +554,11 @@ int32_t IntelligentWindingRobotCtrl::substep_zaxis_do_bullet_action(take_bullet_ } int32_t IntelligentWindingRobotCtrl::step_take_bullet(int32_t bulletindex) { - m4_zreset(); - WAIT_MODULES_IDLE(4); + // m4_zreset(); + // WAIT_MODULES_IDLE(4); - xy_reset(); - WAIT_MODULES_IDLE(3); + // xy_reset(); + // WAIT_MODULES_IDLE(3); m21_arm_hook_claws_reset(); m11_arm_jiaxian_move_to_reset_pos(); @@ -964,7 +982,6 @@ int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalu int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; } int32_t IntelligentWindingRobotCtrl::start_winding() { // - m_work_thread.start([this]() { try { ZLOGI(TAG, "start_winding"); @@ -973,27 +990,46 @@ int32_t IntelligentWindingRobotCtrl::start_winding() { // device_reset(); bool hasbullet = false; for (size_t i = 0; i < 5 * 12; i++) { + if (m_work_thread.getExitFlag()) { + break; + } + if (!(i / 5 == 0 || i / 5 == 6 || i / 5 == 11)) { + continue; + } + m_nowwinding_index = i + 1; - xymove_to(0, 0); step_take_bullet(i); step_prepare_remove_line(i, hasbullet); if (!hasbullet) { continue; } step_remove_line(); + if (m_work_thread.getExitFlag()) break; step_winding_prepare(); + if (m_work_thread.getExitFlag()) break; step_winding(); + if (m_work_thread.getExitFlag()) break; step_winding_lineend_prepare(i); + if (m_work_thread.getExitFlag()) break; step_winding_lineend(); + if (m_work_thread.getExitFlag()) break; step_winding_take_bullet_from_cooking_to_origin_pos(i); + if (m_work_thread.getExitFlag()) break; } + m4_zmove_to(0); + wait_module_idle(4); + xymove_to(0, 0); + wait_module_idle(3); + stop_all_module(); + } catch (int32_t ecode) { ZLOGE(TAG, "work thread catch exception %d", ecode); + disable_all_module(); // todo : 完成一个无异常版本 } catch (...) { ZLOGE(TAG, "work thread catch unkown exception"); + disable_all_module(); // todo : 完成一个无异常版本 } - disable_all_module(); // todo : 完成一个无异常版本 m_iswinding = false; }); return 0; diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index a99e1f1..3ab01b6 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -171,7 +171,7 @@ class IntelligentWindingRobotCtrl { bool is_hasbullet(); public: - void wait_module_idle(int32_t moduleid); + void wait_module_idle(int32_t moduleid, int32_t timeout_ms = 0); void wait_modules_idle(void* mark, ...); int32_t m11_arm_jiaxian_move_to_reset_pos(); @@ -194,7 +194,6 @@ class IntelligentWindingRobotCtrl { int32_t m16_xianlajin_move_to_cook_lineend_low_pos(); int32_t m16_xianlajin_move_to_cook_lineend_ready_pos(); - int32_t m21_arm_hook_claws_reset(); int32_t m21_arm_hook_claws_move_to_half_pos(); int32_t m21_arm_hook_claws_move_to_full_pos(); @@ -208,6 +207,7 @@ class IntelligentWindingRobotCtrl { int32_t device_reset(); int32_t disable_all_module(); + int32_t stop_all_module(); int32_t enable_all_module(); int32_t substep_zaxis_do_bullet_action(take_bullet_pos_type_t zpos, // take_bullet_acktion_t take_bullet_acktion, // @@ -223,10 +223,9 @@ class IntelligentWindingRobotCtrl { int32_t step_winding_prepare(); int32_t step_winding(); // int32_t step_load_the_bullet_case(); - int32_t step_winding_lineend_prepare(int bulletindex); // 绕线头准备 - int32_t step_winding_lineend(); // 绕线头 - int32_t step_winding_take_bullet_from_cooking_to_origin_pos(int bulletindex); // 绕线头 - + int32_t step_winding_lineend_prepare(int bulletindex); // 绕线头准备 + int32_t step_winding_lineend(); // 绕线头 + int32_t step_winding_take_bullet_from_cooking_to_origin_pos(int 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); // 取弹夹