diff --git a/README.md b/README.md index 076113e..dfdfc86 100644 --- a/README.md +++ b/README.md @@ -153,6 +153,9 @@ bullet_holder m_cmdparse->regCMD("step_winding_lineend", "()", 0, [this](PARAM) { return step_winding_lineend(); }); 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])); }); + + + ``` ``` @@ -166,4 +169,9 @@ step_winding_lineend_prepare 1 step_winding_lineend step_winding_take_bullet_from_cooking_to_origin_pos 1 +``` + +``` +异常处理 + 1. 移除线超时 ``` \ No newline at end of file diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index 42a030c..2f33bd6 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -38,6 +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 = 168; + public: void init(IntelligentWindingRobotCtrl::config_t* cfg, APPDM* dm) { this->cfg = cfg; @@ -88,7 +91,7 @@ class WidthDetector { g_isrunning = true; } void start_run_forward() { - m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_tance_zero_pos+100, 330, 0); + m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_tance_zero_pos + 100, 330, 0); g_isrunning = true; } void stop_run() { @@ -140,7 +143,7 @@ class WidthDetector { stop_run(); m_dm->motor_read_pos(14, &g_nowpos); g_nowdpos = cfg->m14_raoxiantance_tance_zero_pos - g_nowpos; - ZLOGI(TAG, "--------------now pos %d bullet width %d",g_nowpos, g_nowdpos); + ZLOGI(TAG, "--------------now pos %d bullet width %d", g_nowpos, g_nowdpos); } else if (!g_isrunning && getDetectGPIOState() != 0) { start_run_back(); } @@ -148,12 +151,17 @@ class WidthDetector { } }); } + void stopDetect() { m_detect_thread.stop(); m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 330, 0); wait_module_idle(14); } + + bool isFull() { return g_nowdpos > m_bullet_full_distance - 10; } + bool isHasBullet() { return g_nowdpos > 5; } + bool isRemoveLineEnd() { return g_nowdpos < m_bullet_distance + 10; } }; WidthDetector g_widthDetector; @@ -206,13 +214,13 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() { cfg.m16_xianlajin_cook_line_end_high_pos = 1741; cfg.m21_arm_hook_claws_full_pos = 2558; - cfg.m21_arm_hook_claws_half_pos = 2294; + cfg.m21_arm_hook_claws_half_pos = 2194; cfg.xy_platform_cook_bullet_pos_x = 21691; cfg.xy_platform_cook_bullet_pos_y = 6989; // 6989 - 4266 2723 - cfg.xy_platform_remove_line_pos_x = 648; - cfg.xy_platform_remove_line_pos_y = 6092; + cfg.xy_platform_remove_line_pos_x = -599; + cfg.xy_platform_remove_line_pos_y = 4861; cfg.xy_platform_takeline_pos_x = 37359; cfg.xy_platform_takeline_pos_y = 7047; @@ -224,6 +232,7 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() { cfg.z_axis_take_clip_pos = 6850; cfg.z_axis_take_line_high = 3500; cfg.z_axis_transfer_line_high = 2675; + cfg.z_axis_remove_line_high = 3567; cfg.m2_zerooff = 1110; return 0; @@ -237,7 +246,10 @@ void IntelligentWindingRobotCtrl::regcb() { // 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("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_prepare_remove_line", "()", 1, [this](PARAM) { + bool hasbullet = false; + return step_prepare_remove_line(atoi(paraV[0]), hasbullet); + }); 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("step_remove_line", "()", 0, [this](PARAM) { return step_remove_line(); }); @@ -326,6 +338,22 @@ int32_t IntelligentWindingRobotCtrl::disable_all_module() { m_dm->motor_enable(21, 0); 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(); + return 0; } int32_t IntelligentWindingRobotCtrl::enable_all_module() { @@ -389,7 +417,7 @@ int32_t IntelligentWindingRobotCtrl::device_reset() { return 0; } -bool IntelligentWindingRobotCtrl::is_hasbullet() { return true; } +bool IntelligentWindingRobotCtrl::is_hasbullet() { return g_widthDetector.isHasBullet(); } void IntelligentWindingRobotCtrl::start_probe_bullet_pos() { g_widthDetector.startDetect(); } void IntelligentWindingRobotCtrl::stop_probe_bullet_pos() { g_widthDetector.stopDetect(); } @@ -537,7 +565,7 @@ int32_t IntelligentWindingRobotCtrl::step_take_back_bullet(int32_t bulletindex) WAIT_MODULES_IDLE(4); } -int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line(int32_t bulletindex) { +int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line(int32_t bulletindex, bool& hasbullet) { m15_paifei_moveto_reset(); m13_yaxian_move_to_reset_backward(); stop_probe_bullet_pos(); @@ -577,7 +605,7 @@ int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line(int32_t bulletinde WAIT_MODULES_IDLE(3); - m4_zmove_to(cfg.z_axis_take_line_high); + m4_zmove_to(cfg.z_axis_remove_line_high); WAIT_MODULES_IDLE(4); // 排线舵机 m15_paifei_moveto_press(); @@ -591,10 +619,13 @@ int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line(int32_t bulletinde step_take_back_bullet(bulletindex); xymove_to(0, 0); WAIT_MODULES_IDLE(3); + hasbullet = true; } else { - /** - * @brief TODO:如果没有子弹,需要在这里做的事情 - */ + hasbullet = false; + stop_probe_bullet_pos(); + substep_zaxis_do_bullet_action(kCookPos, kTakeBullet, kKeepLine, NULL); + xymove_to_bullet_pos(bulletindex); + substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBackBullet, kReleaseLine, NULL); } } @@ -840,9 +871,38 @@ int32_t IntelligentWindingRobotCtrl::xy_reset() { } int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) { return 0; } int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; } - int32_t IntelligentWindingRobotCtrl::start_winding() { // + + m_work_thread.start([this]() { + try { + ZLOGI(TAG, "start_winding"); + device_reset(); + bool hasbullet = false; + for (size_t i = 0; i < 5 * 12; i++) { + xymove_to(0, 0); + step_take_bullet(i); + step_prepare_remove_line(i, hasbullet); + if (!hasbullet) { + continue; + } + step_remove_line(); + step_winding_prepare(); + step_winding(); + step_winding_lineend_prepare(i); + step_winding_lineend(); + step_winding_take_bullet_from_cooking_to_origin_pos(i); + } + + } catch (int32_t ecode) { + ZLOGE(TAG, "work thread catch exception %d", ecode); + } catch (...) { + ZLOGE(TAG, "work thread catch unkown exception"); + } + disable_all_module(); // todo : 完成一个无异常版本 + }); + return 0; +} +int32_t IntelligentWindingRobotCtrl::stop_winding() { + m_work_thread.stop(); return 0; } -int32_t IntelligentWindingRobotCtrl::stop_winding() { return 0; } -int32_t IntelligentWindingRobotCtrl::reset_and_check_device() { return 0; } \ No newline at end of file diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index 3d8f397..a2192bc 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -92,6 +92,7 @@ class IntelligentWindingRobotCtrl { int32_t z_axis_take_clip_pos; int32_t z_axis_winding_hight; int32_t z_axis_transfer_line_high; + int32_t z_axis_remove_line_high; // 2585 // 3377 @@ -203,7 +204,7 @@ class IntelligentWindingRobotCtrl { int32_t step_take_back_bullet(int32_t bulletindex); int32_t step_take_bullet_case(int32_t bulletindex); int32_t step_take_back_bullet_case(int32_t bulletindex); - int32_t step_prepare_remove_line(int32_t bulletindex); + int32_t step_prepare_remove_line(int32_t bulletindex, bool& hasbullet); int32_t step_remove_line(); int32_t step_winding_prepare(); int32_t step_winding(); @@ -223,13 +224,9 @@ class IntelligentWindingRobotCtrl { */ int32_t setcfg(const char* cfgname, int32_t cfgvalue); - - - public: int32_t start_winding(); int32_t stop_winding(); - int32_t reset_and_check_device(); int32_t dumpcfg();