diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index ddc6a93..8d31e6c 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -39,7 +39,7 @@ class WidthDetector { APPDM* m_dm; int32_t m_bullet_distance = 33; // 理论值33,这里取保守值20 - int32_t m_bullet_full_distance = 144; + int32_t m_bullet_full_distance = 140; int32_t lastDistanceChangeTicket = 0; public: @@ -100,15 +100,15 @@ class WidthDetector { void m14_raoxiantance_move_to_reset() { m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 330, 0); } void start_run_back() { - m_dm->motor_move_to(14, cfg->m14_raoxiantance_reset_pos, 330, 0); + m_dm->motor_move_to(14, cfg->m14_raoxiantance_reset_pos, 30, 0); g_isrunning = true; } void start_run_forward_slow() { - m_dm->motor_move_to(14, cfg->m14_raoxiantance_tance_zero_pos , 330, 0); + m_dm->motor_move_to(14, cfg->m14_raoxiantance_tance_zero_pos + 100, 30, 0); g_isrunning = true; } void start_run_forward() { - m_dm->motor_move_to(14, cfg->m14_raoxiantance_tance_zero_pos , 330, 0); + m_dm->motor_move_to(14, cfg->m14_raoxiantance_tance_zero_pos + 100, 330, 0); g_isrunning = true; } void stop_run() { @@ -123,7 +123,6 @@ class WidthDetector { int32_t enterticket = zos_get_tick(); - start_run_forward(); while (true) { if (g_isrunning && getDetectGPIOState() != 0) { @@ -168,7 +167,7 @@ class WidthDetector { start_run_back(); } // ZLOGI(TAG, "%d",getDetectGPIOState()); - zos_delay(3); + zos_delay(1); } } else { while (!m_detect_thread.getExitFlag()) { @@ -181,7 +180,7 @@ class WidthDetector { } else if (!g_isrunning && getDetectGPIOState() == 0) { start_run_forward_slow(); } - zos_delay(3); + zos_delay(1); } } }); @@ -196,10 +195,10 @@ class WidthDetector { 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 isRemoveLineEnd() { return g_nowdpos < m_bullet_distance + 30; } bool distanceIsStopChange() { - if (zos_haspassedms(lastDistanceChangeTicket) > 20 * 1000) { + if (zos_haspassedms(lastDistanceChangeTicket) > 10 * 1000) { return true; } return false; @@ -258,8 +257,8 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() { 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_winding_low_pos = 1875; + cfg.m16_xianlajin_winding_high_pos = 1825; // cfg.m16_xianlajin_winding_low_pos = 1922; // cfg.m16_xianlajin_winding_high_pos = 1863; cfg.m16_xianlajin_line_entry_pos = 1800; @@ -348,6 +347,8 @@ int32_t IntelligentWindingRobotCtrl::stop_all_module() { m_dm->module_stop(22); m_dm->module_stop(23); stop_probe_bullet_pos(); + + m12_jiaxian_move_to_clamp_pos(); } int32_t IntelligentWindingRobotCtrl::disable_all_module() { @@ -368,7 +369,6 @@ int32_t IntelligentWindingRobotCtrl::disable_all_module() { m_dm->motor_enable(22, 0); m_dm->motor_enable(23, 0); - return 0; } int32_t IntelligentWindingRobotCtrl::enable_all_module() { @@ -631,6 +631,7 @@ int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line(int32_t bulletinde // WAIT_MODULES_IDLE(4); m11_arm_jiaxian_move_to_reset_pos(); WAIT_MODULES_IDLE(11); + osDelay(1000); m4_zmove_to(0); WAIT_MODULES_IDLE(4); @@ -653,7 +654,7 @@ int32_t IntelligentWindingRobotCtrl::step_remove_line() { WAIT_MODULES_IDLE(15); m_dm->motor_rotate_acctime(2, -1, 1000, 1000); - #if 0 +#if 0 int overtime = 30; for (int i = 0; i < overtime; i++) { @@ -670,14 +671,13 @@ int32_t IntelligentWindingRobotCtrl::step_remove_line() { break; } } - #endif - prv_sleep(30 * 1000); +#endif + prv_sleep(60 * 1000); m_dm->module_stop(2); start_probe_bullet_pos_forward(); if (!g_widthDetector.isRemoveLineEnd()) { throw (int32_t)0; } - // m15_paifei_moveto_reset(); m15_paifei_moveto_reset(); @@ -736,7 +736,7 @@ 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(5000); + osDelay(2500); m23_laxian_motor_move_to_reset_pos(); m13_yaxian_move_to_reset_backward(); osDelay(800); @@ -746,6 +746,7 @@ int32_t IntelligentWindingRobotCtrl::step_winding() { for (size_t i = 0; i < 80; i++) { // TODO:60这里是个常量可能会造成绕线提前释放。 m16_xianlajin_move_to_winding_low_pos(); WAIT_MODULES_IDLE(16); + m16_xianlajin_move_to_winding_up_pos(); WAIT_MODULES_IDLE(16); if (g_widthDetector.isFull()) {