diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index fbf47b3..649158d 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -38,8 +38,9 @@ class WidthDetector { IntelligentWindingRobotCtrl::config_t* cfg; APPDM* m_dm; - int32_t m_bullet_distance = 33; // 理论值33,这里取保守值20 - int32_t m_bullet_full_distance = 127; + int32_t m_bullet_distance = 33; // 理论值33,这里取保守值20 + int32_t m_bullet_full_distance = 154; + int32_t lastDistanceChangeTicket = 0; public: void init(IntelligentWindingRobotCtrl::config_t* cfg, APPDM* dm) { @@ -155,7 +156,8 @@ class WidthDetector { if (g_isrunning && getDetectGPIOState() == 0) { stop_run(); m_dm->motor_read_pos(14, &g_nowpos); - g_nowdpos = cfg->m14_raoxiantance_tance_zero_pos - g_nowpos; + g_nowdpos = cfg->m14_raoxiantance_tance_zero_pos - g_nowpos; + lastDistanceChangeTicket = zos_get_tick(); ZLOGI(TAG, "--------------now pos %d bullet width %d", g_nowpos, g_nowdpos); } else if (!g_isrunning && getDetectGPIOState() != 0) { start_run_back(); @@ -167,7 +169,8 @@ class WidthDetector { if (g_isrunning && getDetectGPIOState() != 0) { stop_run(); m_dm->motor_read_pos(14, &g_nowpos); - g_nowdpos = cfg->m14_raoxiantance_tance_zero_pos - g_nowpos; + g_nowdpos = cfg->m14_raoxiantance_tance_zero_pos - g_nowpos; + lastDistanceChangeTicket = zos_get_tick(); ZLOGI(TAG, "--------------now pos %d bullet width %d", g_nowpos, g_nowdpos); } else if (!g_isrunning && getDetectGPIOState() == 0) { start_run_forward_slow(); @@ -185,9 +188,16 @@ class WidthDetector { wait_module_idle(14, 0); } - bool isFull() { return g_nowdpos > m_bullet_full_distance - 20; } + bool isFull() { return g_nowdpos > m_bullet_full_distance; } bool isHasBullet() { return g_nowdpos > 5; } bool isRemoveLineEnd() { return g_nowdpos < m_bullet_distance + 10; } + + bool distanceIsStopChange() { + if (zos_haspassedms(lastDistanceChangeTicket) > 10 * 1000) { + return true; + } + return false; + } }; WidthDetector g_widthDetector; @@ -231,14 +241,16 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() { cfg.m15_paifei_press_direction = 1; cfg.m15_paifei_press_torque = 330; - cfg.m16_xianlajin_reset_pos = 2047; + cfg.m16_xianlajin_reset_pos = 2000; cfg.m16_xianlajin_cook_line_end_ready_pos = 1900; cfg.m16_xianlajin_tight_line_pos = 1966; cfg.m16_xianlajin_winding_low_pos = 1885; cfg.m16_xianlajin_winding_high_pos = 1815; - cfg.m16_xianlajin_line_entry_pos = 1800; - cfg.m16_xianlajin_cook_line_end_low_pos = 1820; - cfg.m16_xianlajin_cook_line_end_high_pos = 1741; + // cfg.m16_xianlajin_winding_low_pos = 1922; + // cfg.m16_xianlajin_winding_high_pos = 1863; + cfg.m16_xianlajin_line_entry_pos = 1800; + cfg.m16_xianlajin_cook_line_end_low_pos = 1820; + cfg.m16_xianlajin_cook_line_end_high_pos = 1741; cfg.m21_arm_hook_claws_full_pos = 2558; cfg.m21_arm_hook_claws_half_pos = 2194; @@ -285,6 +297,7 @@ void IntelligentWindingRobotCtrl::regcb() { 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("start_remove_line", "()", 0, [this](PARAM) { return start_remove_line(); }); // m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); }); @@ -689,8 +702,12 @@ int32_t IntelligentWindingRobotCtrl::step_remove_line() { if (i == overtime - 1) { ZLOGI(TAG, "detect remove line end timeout"); } + if (g_widthDetector.distanceIsStopChange()) { + ZLOGI(TAG, "distanceIsStopChange......"); + break; + } } - osDelay(5000); + osDelay(10 * 1000); m_dm->module_stop(2); // m15_paifei_moveto_reset(); m15_paifei_moveto_reset(); @@ -749,8 +766,13 @@ int32_t IntelligentWindingRobotCtrl::step_winding_prepare() { int32_t IntelligentWindingRobotCtrl::step_winding() { start_probe_bullet_pos(); m_dm->motor_rotate_acctime(2, 1, 1000, 10000); - osDelay(1000); + osDelay(5000); m23_laxian_motor_move_to_reset_pos(); + m13_yaxian_move_to_reset_backward(); + osDelay(800); + m13_yaxian_move_to_reset_forward(); + osDelay(300); + for (size_t i = 0; i < 80; i++) { // TODO:60这里是个常量可能会造成绕线提前释放。 m16_xianlajin_move_to_winding_low_pos(); WAIT_MODULES_IDLE(16); @@ -759,6 +781,11 @@ int32_t IntelligentWindingRobotCtrl::step_winding() { if (g_widthDetector.isFull()) { break; } + + if (g_widthDetector.distanceIsStopChange()) { + ZLOGI(TAG, "distanceIsStopChange......"); + break; + } } m_dm->module_stop(2); stop_probe_bullet_pos(); @@ -1034,6 +1061,55 @@ int32_t IntelligentWindingRobotCtrl::start_winding() { // }); return 0; } +int32_t IntelligentWindingRobotCtrl::start_remove_line() { + // + m_work_thread.start([this]() { + try { + ZLOGI(TAG, "start_winding"); + m_iswinding = true; + m_nowwinding_index = 0; + 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; + step_take_bullet(i); + step_prepare_remove_line(i, hasbullet); + if (!hasbullet) { + continue; + } + step_remove_line(); + stop_probe_bullet_pos(); + substep_zaxis_do_bullet_action(kCookPos, kTakeBullet, kKeepLine, NULL); + xymove_to_bullet_pos(i); + substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBackBullet, kReleaseLine, NULL); + 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 : 完成一个无异常版本 + } + m_iswinding = false; + }); + return 0; +} + int32_t IntelligentWindingRobotCtrl::stop_winding() { m_work_thread.stop(); m_iswinding = false; diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index 3ab01b6..08acac3 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -243,6 +243,8 @@ class IntelligentWindingRobotCtrl { int32_t start_winding(); int32_t stop_winding(); + int32_t start_remove_line(); + int32_t dumpcfg(); int32_t do_winding(int32_t index);