diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index a75ae9e..42a030c 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -26,12 +26,146 @@ class myexception : public exception { virtual const char* what() const throw() { return "My exception happened"; } }; +class WidthDetector { + ZGPIO g_detectGpio; + int32_t g_nowpos = 0; + int32_t g_nowdpos = 0; + bool g_isrunning = false; + + ZThread m_detect_thread; + int32_t m_lastState = false; + + IntelligentWindingRobotCtrl::config_t* cfg; + APPDM* m_dm; + + public: + void init(IntelligentWindingRobotCtrl::config_t* cfg, APPDM* dm) { + this->cfg = cfg; + m_dm = dm; + g_detectGpio.initAsInput(PB13, ZGPIO::kMode_pullup, ZGPIO::kIRQ_noIrq, true); + m_detect_thread.init("detect", 512); + getDetectGPIOState(); + } + int32_t getDetectGPIOState() { + int32_t nowstate = g_detectGpio.getState(); + for (size_t i = 0; i < 5; i++) { + if (g_detectGpio.getState() != nowstate) { + return m_lastState; + } + osDelay(3); + } + m_lastState = nowstate; + return m_lastState; + } + + void wait_module_idle(int32_t moduleid) { + zos_delay(100); + int i = 0; + while (true) { + int32_t status = 0; + int32_t ecode = m_dm->module_get_status(moduleid, &status); + if (ecode != 0) { + break; + }; + if (status == 0) { + ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status); + break; + } + if (status == 2) { + break; + }; + if (i % 30 == 0) { + ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status); + } + i++; + zos_delay(10); + } + } + + 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, 30, 0); + g_isrunning = true; + } + void start_run_forward() { + m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_tance_zero_pos+100, 330, 0); + g_isrunning = true; + } + void stop_run() { + m_dm->module_stop(14); + g_isrunning = false; + } + + void startDetect() { + m_detect_thread.stop(); + m14_raoxiantance_move_to_reset(); + wait_module_idle(14); + + int32_t enterticket = zos_get_tick(); + + start_run_forward(); + while (true) { + if (g_isrunning && getDetectGPIOState() != 0) { + stop_run(); + break; + } + + if (zos_haspassedms(enterticket) > 3000) { + ZLOGE(TAG, "start_probe_bullet_pos timeout"); + /** + * @brief TODO add exception here + * + */ + stop_run(); + break; + } + zos_delay(10); + } + + start_run_back(); + while (true) { + 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; + ZLOGI(TAG, "--------------now pos %d bullet width %d", g_nowpos, g_nowdpos); + break; + } + zos_delay(3); + } + + m_detect_thread.start([this]() { + while (!m_detect_thread.getExitFlag()) { + 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; + ZLOGI(TAG, "--------------now pos %d bullet width %d",g_nowpos, g_nowdpos); + } else if (!g_isrunning && getDetectGPIOState() != 0) { + start_run_back(); + } + zos_delay(3); + } + }); + } + void stopDetect() { + m_detect_thread.stop(); + + m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 330, 0); + wait_module_idle(14); + } +}; + +WidthDetector g_widthDetector; + int32_t IntelligentWindingRobotCtrl::initialize(APPDM* dm, ICmdParser* cmdparse) { m_dm = dm; m_cmdparse = cmdparse; - + m_work_thread.init("work", 512); + // m_detect_thread.init("detect", 512); initialize_device(); regcb(); + g_widthDetector.init(&cfg, dm); return 0; } @@ -57,7 +191,7 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() { cfg.m13_jiaxian_clamp_torque = 500; cfg.m14_raoxiantance_reset_pos = 2047; - cfg.m14_raoxiantance_tance_zero_pos = 2517; + cfg.m14_raoxiantance_tance_zero_pos = 2829; cfg.m15_paifei_reset_pos = 2046; cfg.m15_paifei_press_direction = 1; @@ -137,6 +271,7 @@ void IntelligentWindingRobotCtrl::regcb() { m_cmdparse->regCMD("app_m22_scissors_cut", "()", 0, [this](PARAM) { return m22_scissors_cut(); }); m_cmdparse->regCMD("app_m23_laxian_motor_move_to_reset_pos", "()", 0, [this](PARAM) { return m23_laxian_motor_move_to_reset_pos(); }); m_cmdparse->regCMD("app_m23_laxian_motor_move_to_tight_line_pos", "()", 0, [this](PARAM) { return m23_laxian_motor_move_to_tight_line_pos(); }); + m_cmdparse->regCMD("start_probe_bullet_pos", "()", 0, [this](PARAM) { return start_probe_bullet_pos(); }); } void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) { @@ -256,14 +391,8 @@ int32_t IntelligentWindingRobotCtrl::device_reset() { bool IntelligentWindingRobotCtrl::is_hasbullet() { return true; } -void IntelligentWindingRobotCtrl::start_probe_bullet_pos() { - m14_raoxiantance_move_to_reset(); - WAIT_MODULES_IDLE(14); -} -void IntelligentWindingRobotCtrl::stop_probe_bullet_pos() { - m14_raoxiantance_move_to_reset(); - WAIT_MODULES_IDLE(14); -} +void IntelligentWindingRobotCtrl::start_probe_bullet_pos() { g_widthDetector.startDetect(); } +void IntelligentWindingRobotCtrl::stop_probe_bullet_pos() { g_widthDetector.stopDetect(); } int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_reset_pos() { return m_dm->motor_move_to_torque(11, cfg.m11_arm_jiaxian_reset_pos, 330, 0); } 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); } @@ -657,33 +786,9 @@ int32_t IntelligentWindingRobotCtrl::step_winding_take_bullet_from_cooking_to_or substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBackBullet, kReleaseLine); } - -int32_t IntelligentWindingRobotCtrl::main_shaft_run() { - ZLOGI(TAG, "main_shaft_run"); - DO(m_dm->motor_rotate_acctime(2, 1, 1000, 10000)); - return 0; -} -int32_t IntelligentWindingRobotCtrl::main_shaft_stop() { - ZLOGI(TAG, "main_shaft_stop"); - DO(m_dm->module_stop(2)); - return 0; -} -/** - * @brief XY平台 - */ -int32_t IntelligentWindingRobotCtrl::xy_platform_reset() { return 0; } -/** - * @brief Z轴 - */ - -int32_t IntelligentWindingRobotCtrl::do_reset_device() {} -int32_t IntelligentWindingRobotCtrl::do_winding(int32_t index) {} /** * @brief */ -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::xy_get_point(int32_t clip_index, int32_t& x, int32_t& y) { int clip_x_off = clip_index / cfg.clip_each_line_num; @@ -698,31 +803,6 @@ 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; - 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) { - zreset(); - } - wait_module_idle(ZMOTOR_ID); - return 0; -} - -int32_t IntelligentWindingRobotCtrl::zreset() { - ZLOGI(TAG, "zreset"); - m_dm->motor_enable(ZMOTOR_ID, 1); - wait_module_idle(ZMOTOR_ID); - return 0; -} -#endif - int32_t IntelligentWindingRobotCtrl::xymove_to_bullet_pos(int32_t bulletindex) { int32_t x = 0; int32_t y = 0; @@ -758,86 +838,11 @@ int32_t IntelligentWindingRobotCtrl::xy_reset() { m_dm->xymotor_move_to_zero(XYRobot_ID); return 0; } +int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) { return 0; } +int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; } -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); - return err::kparam_out_of_range; - } - zreset(); - m11_arm_jiaxian_move_to_reset_pos(); - m21_arm_hook_claws_reset(); - - int32_t x = 0; - int32_t y = 0; - xy_get_point(clip_index, x, y); - - xymove_to(x, y); - zmove_to(cfg.z_axis_take_clip_pos); - m21_arm_hook_claws_move_to_full_pos(); - zmove_to(0); - 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) { -#if 0 - if (strcmp(cfgname, "paifei_reset_pos") == 0) - cfg.paifei_reset_pos = cfgvalue; - else if (strcmp(cfgname, "paifei_press_pos") == 0) - cfg.paifei_press_pos = cfgvalue; - else if (strcmp(cfgname, "paifei_press_torque") == 0) - cfg.paifei_press_torque = cfgvalue; - else if (strcmp(cfgname, "raoxiantance_reset_pos") == 0) - cfg.raoxiantance_reset_pos = cfgvalue; - else if (strcmp(cfgname, "raoxiantance_tance_zero_pos") == 0) - cfg.raoxiantance_tance_zero_pos = cfgvalue; - else if (strcmp(cfgname, "yaxian_reset_pos") == 0) - cfg.yaxian_reset_pos = cfgvalue; - else if (strcmp(cfgname, "yaxian_press_pos") == 0) - cfg.yaxian_press_pos = cfgvalue; - else if (strcmp(cfgname, "yaxian_press_torque") == 0) - cfg.yaxian_press_torque = cfgvalue; - else if (strcmp(cfgname, "yaxian_wait_for_press_pos") == 0) - cfg.yaxian_wait_for_press_pos = cfgvalue; - else if (strcmp(cfgname, "xianlajin_reset_pos") == 0) - cfg.xianlajin_reset_pos = cfgvalue; - else if (strcmp(cfgname, "xianlajin_line_entry_pos") == 0) - cfg.xianlajin_line_entry_pos = cfgvalue; - else if (strcmp(cfgname, "xianlajin_tight_line_pos") == 0) - cfg.xianlajin_tight_line_pos = cfgvalue; - else if (strcmp(cfgname, "xianlajin_loose_line_pos") == 0) - cfg.xianlajin_loose_line_pos = cfgvalue; - else if (strcmp(cfgname, "jiaxian_reset_pos") == 0) - cfg.jiaxian_reset_pos = cfgvalue; - else if (strcmp(cfgname, "jiaxian_clamp_pos") == 0) - cfg.jiaxian_clamp_pos = cfgvalue; - else if (strcmp(cfgname, "jiaxian_clamp_torque") == 0) - cfg.jiaxian_clamp_torque = cfgvalue; - else if (strcmp(cfgname, "arm_jiaxian_reset_pos") == 0) - cfg.arm_jiaxian_reset_pos = cfgvalue; - else if (strcmp(cfgname, "arm_jiaxian_clamp_pos") == 0) - cfg.arm_jiaxian_clamp_pos = cfgvalue; - else if (strcmp(cfgname, "arm_jiaxian_clamp_torque") == 0) - cfg.arm_jiaxian_clamp_torque = cfgvalue; - else if (strcmp(cfgname, "scissors_reset_pos") == 0) - cfg.scissors_reset_pos = cfgvalue; - else if (strcmp(cfgname, "m22_scissors_cut_pos") == 0) - cfg.m22_scissors_cut_pos = cfgvalue; - else if (strcmp(cfgname, "m21_arm_hook_claws_half_pos") == 0) - cfg.m21_arm_hook_claws_half_pos = cfgvalue; - else if (strcmp(cfgname, "m21_arm_hook_claws_full_pos") == 0) - cfg.m21_arm_hook_claws_full_pos = cfgvalue; - return 0; -#endif +int32_t IntelligentWindingRobotCtrl::start_winding() { // return 0; } -int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; } - -#if 1 - -#endif +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 0ec3da4..3d8f397 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -129,6 +129,9 @@ class IntelligentWindingRobotCtrl { ICmdParser* m_cmdparse; config_t cfg; + ZThread m_work_thread; + ZThread m_detect_thread; + typedef enum { kBulletBulletHolderPos = 0, kCookPos, @@ -191,7 +194,6 @@ class IntelligentWindingRobotCtrl { int32_t device_reset(); int32_t disable_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, // take_bullet_line_acktion_t take_bullet_line_acktion, // @@ -201,46 +203,36 @@ 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_remove_line(); 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); // 取弹夹 - int32_t xy_run_to_clip_pos_test(int32_t clip_index); int32_t xy_reset(); - int32_t xymove_to(int32_t x, int32_t y); int32_t xymove_to_bullet_pos(int32_t bulletindex); - void regcb(); int32_t initialize_device(); - - int32_t main_shaft_run(); - int32_t main_shaft_stop(); - - /** - * @brief XY平台 - */ - int32_t xy_platform_reset(); - /** * @brief */ + 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 setcfg(const char* cfgname, int32_t cfgvalue); int32_t dumpcfg(); - int32_t do_reset_device(); int32_t do_winding(int32_t index); void processError(int32_t err);