diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index 0c037c2..7ef9367 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -19,6 +19,7 @@ void IntelligentWindingRobotCtrl::processError(int32_t err) { ZLOGE(TAG, "proces void IntelligentWindingRobotCtrl::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); @@ -26,40 +27,68 @@ void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) { processError(ecode); break; }; - ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status); - if (status == 0) break; + if (status == 0) { + ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status); + break; + } if (status == 2) { processError(err::kfail); break; }; - zos_delay(1000); + if (i % 30 == 0) { + ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status); + break; + } + i++; + zos_delay(100); + } +} + +void IntelligentWindingRobotCtrl::wait_modules_idle(...) { + int32_t moduleid = 0; + va_list args; + va_start(args, moduleid); + while (moduleid != -1) { + wait_module_idle(moduleid); + moduleid = va_arg(args, int32_t); } + va_end(args); +} + +int32_t IntelligentWindingRobotCtrl::device_reset() { + // Z轴复位 + zreset(); + m21_arm_hook_claws_reset(); + m11_arm_jiaxian_duoji_move_to_reset_pos(); + + xy_reset(); + ZLOGI(TAG, "device_reset finished...."); } int32_t IntelligentWindingRobotCtrl::initialize_device() { return 0; } // 排废舵机 -int32_t IntelligentWindingRobotCtrl::paifei_duoji_moveto_reset() { - ZLOGI(TAG, "paifei_duoji_moveto_reset %d %d %d", 15, cfg.paifei_duoji_reset_pos, 330); +int32_t IntelligentWindingRobotCtrl::m15_paifei_duoji_moveto_reset() { + ZLOGI(TAG, "m15_paifei_duoji_moveto_reset %d %d %d", 15, cfg.paifei_duoji_reset_pos, 330); DO(m_dm->motor_move_to(15, cfg.paifei_duoji_reset_pos, 2000, 0)); wait_module_idle(15); return 0; } -int32_t IntelligentWindingRobotCtrl::paifei_duoji_moveto_press() { - ZLOGI(TAG, "paifei_duoji_moveto_press %d %d %d", 15, cfg.paifei_duoji_press_pos, cfg.paifei_duoji_press_torque); +int32_t IntelligentWindingRobotCtrl::m15_paifei_duoji_moveto_press() { + ZLOGI(TAG, "m15_paifei_duoji_moveto_press %d %d %d", 15, cfg.paifei_duoji_press_pos, cfg.paifei_duoji_press_torque); DO(m_dm->motor_move_to_with_torque(15, cfg.paifei_duoji_press_pos, cfg.paifei_duoji_press_torque)); wait_module_idle(15); } /** * @brief 绕线探测舵机 */ -int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_reset() { - ZLOGI(TAG, "raoxiantance_duoji_move_to_reset %d %d %d", 14, cfg.raoxiantance_duoji_reset_pos, 330); +int32_t IntelligentWindingRobotCtrl::m14_raoxiantance_duoji_move_to_reset() { + ZLOGI(TAG, "m14_raoxiantance_duoji_move_to_reset %d %d %d", 14, cfg.raoxiantance_duoji_reset_pos, 330); DO(m_dm->motor_move_to(14, cfg.raoxiantance_duoji_reset_pos, 2000, 0)); wait_module_idle(14); return 0; } -int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_get_thickness(int32_t* thickness) { - ZLOGI(TAG, "raoxiantance_duoji_move_to_get_thickness %d %d", 14, cfg.raoxiantance_duoji_tance_zero_pos); +int32_t IntelligentWindingRobotCtrl::m14_raoxiantance_duoji_move_to_get_thickness(int32_t* thickness) { + ZLOGI(TAG, "m14_raoxiantance_duoji_move_to_get_thickness %d %d", 14, cfg.raoxiantance_duoji_tance_zero_pos); DO(m_dm->motor_move_to_with_torque(14, cfg.raoxiantance_duoji_tance_zero_pos, 200)); zos_delay(2000); @@ -67,26 +96,26 @@ int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_get_thickness(in DO(m_dm->motor_read_pos(14, &nowpos)); *thickness = cfg.raoxiantance_duoji_tance_zero_pos - nowpos; - DO(raoxiantance_duoji_move_to_reset()); + DO(m14_raoxiantance_duoji_move_to_reset()); return 0; } /** * @brief 压线舵机 */ -int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_reset() { - ZLOGI(TAG, "yaxian_duoji_move_to_reset %d %d %d", 13, cfg.yaxian_duoji_reset_pos, 330); +int32_t IntelligentWindingRobotCtrl::m13_yaxian_duoji_move_to_reset() { + ZLOGI(TAG, "m13_yaxian_duoji_move_to_reset %d %d %d", 13, cfg.yaxian_duoji_reset_pos, 330); DO(m_dm->motor_move_to(13, cfg.yaxian_duoji_reset_pos, 2000, 0)); wait_module_idle(13); return 0; } -int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_press_pos() { - ZLOGI(TAG, "yaxian_duoji_move_to_press_pos %d %d %d", 13, cfg.yaxian_duoji_press_pos, cfg.yaxian_duoji_press_torque); +int32_t IntelligentWindingRobotCtrl::m13_yaxian_duoji_move_to_press_pos() { + ZLOGI(TAG, "m13_yaxian_duoji_move_to_press_pos %d %d %d", 13, cfg.yaxian_duoji_press_pos, cfg.yaxian_duoji_press_torque); DO(m_dm->motor_move_to_with_torque(13, cfg.yaxian_duoji_press_pos, cfg.yaxian_duoji_press_torque)); wait_module_idle(13); return 0; } -int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_wait_for_press_pos() { - ZLOGI(TAG, "yaxian_duoji_move_to_wait_for_press_pos %d %d %d", 13, cfg.yaxian_duoji_wait_for_press_pos, cfg.yaxian_duoji_press_torque); +int32_t IntelligentWindingRobotCtrl::m13_yaxian_duoji_move_to_wait_for_press_pos() { + ZLOGI(TAG, "m13_yaxian_duoji_move_to_wait_for_press_pos %d %d %d", 13, cfg.yaxian_duoji_wait_for_press_pos, cfg.yaxian_duoji_press_torque); DO(m_dm->motor_move_to_with_torque(13, cfg.yaxian_duoji_wait_for_press_pos, cfg.yaxian_duoji_press_torque)); wait_module_idle(13); return 0; @@ -94,26 +123,26 @@ int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_wait_for_press_pos() { /** * @brief 线拉紧舵机 */ -int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_reset() { - ZLOGI(TAG, "xianlajin_duoji_move_to_reset %d %d %d", 12, cfg.xianlajin_duoji_reset_pos, 330); +int32_t IntelligentWindingRobotCtrl::m16_xianlajin_duoji_move_to_reset() { + ZLOGI(TAG, "m16_xianlajin_duoji_move_to_reset %d %d %d", 12, cfg.xianlajin_duoji_reset_pos, 330); DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_reset_pos, 2000, 0)); wait_module_idle(16); return 0; } // 零位 -int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_line_entry_pos() { - ZLOGI(TAG, "xianlajin_duoji_move_to_line_entry_pos %d %d %d", 16, cfg.xianlajin_duoji_line_entry_pos, 330); +int32_t IntelligentWindingRobotCtrl::m16_xianlajin_duoji_move_to_line_entry_pos() { + ZLOGI(TAG, "m16_xianlajin_duoji_move_to_line_entry_pos %d %d %d", 16, cfg.xianlajin_duoji_line_entry_pos, 330); DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_line_entry_pos, 2000, 0)); wait_module_idle(16); return 0; } // 入线位 -int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_tight_line_pos() { - ZLOGI(TAG, "xianlajin_duoji_move_to_tight_line_pos %d %d %d", 16, cfg.xianlajin_duoji_tight_line_pos, 330); +int32_t IntelligentWindingRobotCtrl::m16_xianlajin_duoji_move_to_tight_line_pos() { + ZLOGI(TAG, "m16_xianlajin_duoji_move_to_line_entry_pos %d %d %d", 16, cfg.xianlajin_duoji_tight_line_pos, 330); DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_tight_line_pos, 2000, 0)); wait_module_idle(16); return 0; } // 拉线位置 -int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_loose_line_pos() { - ZLOGI(TAG, "xianlajin_duoji_move_to_loose_line_pos %d %d %d", 16, cfg.xianlajin_duoji_loose_line_pos, 330); +int32_t IntelligentWindingRobotCtrl::m16_xianlajin_duoji_move_to_loose_line_pos() { + ZLOGI(TAG, "m16_xianlajin_duoji_move_to_loose_line_pos %d %d %d", 16, cfg.xianlajin_duoji_loose_line_pos, 330); DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_loose_line_pos, 2000, 0)); wait_module_idle(16); return 0; @@ -121,14 +150,14 @@ int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_loose_line_pos() { /** * @brief 夹线舵机12 */ -int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_reset_pos() { - ZLOGI(TAG, "jiaxian_duoji_move_to_reset_pos %d %d %d", 12, cfg.jiaxian_duoji_reset_pos, 330); +int32_t IntelligentWindingRobotCtrl::m12_jiaxian_duoji_move_to_reset_pos() { + ZLOGI(TAG, "m12_jiaxian_duoji_move_to_reset_pos %d %d %d", 12, cfg.jiaxian_duoji_reset_pos, 330); DO(m_dm->motor_move_to(12, cfg.jiaxian_duoji_reset_pos, 2000, 0)); wait_module_idle(12); return 0; } -int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_clamp_pos() { - ZLOGI(TAG, "jiaxian_duoji_move_to_clamp_pos %d %d %d", 12, cfg.jiaxian_duoji_clamp_pos, cfg.jiaxian_duoji_clamp_torque); +int32_t IntelligentWindingRobotCtrl::m12_jiaxian_duoji_move_to_clamp_pos() { + ZLOGI(TAG, "m12_jiaxian_duoji_move_to_clamp_pos %d %d %d", 12, cfg.jiaxian_duoji_clamp_pos, cfg.jiaxian_duoji_clamp_torque); DO(m_dm->motor_move_to_with_torque(12, cfg.jiaxian_duoji_clamp_pos, cfg.jiaxian_duoji_clamp_torque)); wait_module_idle(12); return 0; @@ -136,13 +165,13 @@ int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_clamp_pos() { /** * @brief 剪刀 */ -int32_t IntelligentWindingRobotCtrl::scissors_move_reset_pos() { - // ZLOGI(TAG, "scissors_move_reset_pos %d", 11); - ZLOGI(TAG, "scissors_move_reset_pos"); +int32_t IntelligentWindingRobotCtrl::m22_scissors_move_reset_pos() { + // ZLOGI(TAG, "m22_scissors_move_reset_pos %d", 11); + ZLOGI(TAG, "m22_scissors_move_reset_pos"); return 0; } // block -int32_t IntelligentWindingRobotCtrl::scissors_cut() { - ZLOGI(TAG, "scissors_cut %d", 22); +int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() { + ZLOGI(TAG, "m22_scissors_cut %d", 22); DO(m_dm->motor_move_by(22, 4095, 0, 0)); wait_module_idle(22); return 0; @@ -150,14 +179,14 @@ int32_t IntelligentWindingRobotCtrl::scissors_cut() { /** * @brief 机械臂夹线舵机 */ -int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_reset_pos() { - ZLOGI(TAG, "arm_jiaxian_duoji_move_to_reset_pos"); +int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_duoji_move_to_reset_pos() { + ZLOGI(TAG, "m11_arm_jiaxian_duoji_move_to_reset_pos"); DO(m_dm->motor_move_to(11, cfg.arm_jiaxian_duoji_reset_pos, 2000, 0)); wait_module_idle(11); return 0; } -int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_clamp_pos() { - ZLOGI(TAG, "arm_jiaxian_duoji_move_to_clamp_pos"); +int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_duoji_move_to_clamp_pos() { + ZLOGI(TAG, "m11_arm_jiaxian_duoji_move_to_clamp_pos"); DO(m_dm->motor_move_to_with_torque(11, cfg.arm_jiaxian_duoji_clamp_direction, cfg.arm_jiaxian_duoji_clamp_torque)); wait_module_idle(11); return 0; @@ -166,21 +195,21 @@ int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_clamp_pos() { /** * @brief 机械臂钩爪 */ -int32_t IntelligentWindingRobotCtrl::arm_hook_claws_reset() { - ZLOGI(TAG, "arm_hook_claws_reset"); +int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_reset() { + ZLOGI(TAG, "m21_arm_hook_claws_reset"); DO(m_dm->motor_move_to_zero_backward(21, 0, 0, 0, 0)); wait_module_idle(21); return 0; } -int32_t IntelligentWindingRobotCtrl::arm_hook_claws_move_to_half_pos() { - ZLOGI(TAG, "arm_hook_claws_move_to_half_pos"); - DO(m_dm->motor_move_to(21, cfg.arm_hook_claws_half_pos, 0, 0)); +int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_half_pos() { + ZLOGI(TAG, "m21_arm_hook_claws_move_to_half_pos"); + DO(m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_half_pos, 0, 0)); wait_module_idle(21); return 0; } -int32_t IntelligentWindingRobotCtrl::arm_hook_claws_move_to_full_pos() { - ZLOGI(TAG, "arm_hook_claws_move_to_full_pos"); - DO(m_dm->motor_move_to(21, cfg.arm_hook_claws_full_pos, 0, 0)); +int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_full_pos() { + ZLOGI(TAG, "m21_arm_hook_claws_move_to_full_pos"); + DO(m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_full_pos, 0, 0)); wait_module_idle(21); return 0; } @@ -220,15 +249,6 @@ 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::device_reset() { - // Z轴复位 - zreset(); - arm_hook_claws_reset(); - arm_jiaxian_duoji_move_to_reset_pos(); - xy_reset(); - ZLOGI(TAG, "device_reset finished...."); -} - int32_t IntelligentWindingRobotCtrl::disable_all_motor() { m_dm->motor_enable(2, 0); m_dm->xymotor_enable(XYRobot_ID, 0); @@ -269,7 +289,7 @@ int32_t IntelligentWindingRobotCtrl::zmove_to(int32_t pos) { } else { m_dm->motor_move_to(ZMOTOR_ID, pos, 1000, 600); } - if(pos == 0){ + if (pos == 0) { zreset(); } wait_module_idle(ZMOTOR_ID); @@ -304,8 +324,8 @@ int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index) return err::kparam_out_of_range; } zreset(); - arm_jiaxian_duoji_move_to_reset_pos(); - arm_hook_claws_reset(); + m11_arm_jiaxian_duoji_move_to_reset_pos(); + m21_arm_hook_claws_reset(); int32_t x = 0; int32_t y = 0; @@ -313,10 +333,10 @@ int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index) xymove_to(x, y); zmove_to(cfg.z_axis_take_clip_pos); - arm_hook_claws_move_to_full_pos(); + m21_arm_hook_claws_move_to_full_pos(); zmove_to(0); zmove_to(cfg.z_axis_take_clip_pos); - arm_hook_claws_reset(); + m21_arm_hook_claws_reset(); zmove_to(0); } @@ -362,12 +382,12 @@ int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalu cfg.arm_jiaxian_duoji_clamp_torque = cfgvalue; else if (strcmp(cfgname, "scissors_reset_pos") == 0) cfg.scissors_reset_pos = cfgvalue; - else if (strcmp(cfgname, "scissors_cut_pos") == 0) - cfg.scissors_cut_pos = cfgvalue; - else if (strcmp(cfgname, "arm_hook_claws_half_pos") == 0) - cfg.arm_hook_claws_half_pos = cfgvalue; - else if (strcmp(cfgname, "arm_hook_claws_full_pos") == 0) - cfg.arm_hook_claws_full_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 return 0; @@ -394,9 +414,9 @@ int32_t IntelligentWindingRobotCtrl::dumpcfg() { ZLOGI(TAG, "arm_jiaxian_duoji_clamp_pos %d", cfg.arm_jiaxian_duoji_clamp_pos); ZLOGI(TAG, "arm_jiaxian_duoji_clamp_torque %d", cfg.arm_jiaxian_duoji_clamp_torque); ZLOGI(TAG, "scissors_reset_pos %d", cfg.scissors_reset_pos); - ZLOGI(TAG, "scissors_cut_pos %d", cfg.scissors_cut_pos); - ZLOGI(TAG, "arm_hook_claws_half_pos %d", cfg.arm_hook_claws_half_pos); - ZLOGI(TAG, "arm_hook_claws_full_pos %d", cfg.arm_hook_claws_full_pos); + ZLOGI(TAG, "m22_scissors_cut_pos %d", cfg.m22_scissors_cut_pos); + ZLOGI(TAG, "m21_arm_hook_claws_half_pos %d", cfg.m21_arm_hook_claws_half_pos); + ZLOGI(TAG, "m21_arm_hook_claws_full_pos %d", cfg.m21_arm_hook_claws_full_pos); #endif return 0; } @@ -418,8 +438,8 @@ int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdPa cfg.arm_jiaxian_duoji_clamp_direction = -1; cfg.z_axis_take_clip_pos = 6924; - cfg.arm_hook_claws_full_pos = 2458; - cfg.arm_hook_claws_half_pos = 2294; + cfg.m21_arm_hook_claws_full_pos = 2458; + cfg.m21_arm_hook_claws_half_pos = 2294; regcb(); return 0; @@ -435,31 +455,31 @@ void IntelligentWindingRobotCtrl::regcb() { m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); }); #if 0 - m_cmdparse->regCMD("app_paifei_duoji_moveto_reset", "()", 0, [this](PARAM) { return paifei_duoji_moveto_reset(); }); - m_cmdparse->regCMD("app_paifei_duoji_moveto_press", "()", 0, [this](PARAM) { return paifei_duoji_moveto_press(); }); - m_cmdparse->regCMD("app_raoxiantance_duoji_move_to_reset", "()", 0, [this](PARAM) { return raoxiantance_duoji_move_to_reset(); }); - m_cmdparse->regCMD("app_raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](PARAM) { + m_cmdparse->regCMD("app_m15_paifei_duoji_moveto_reset", "()", 0, [this](PARAM) { return m15_paifei_duoji_moveto_reset(); }); + m_cmdparse->regCMD("app_m15_paifei_duoji_moveto_press", "()", 0, [this](PARAM) { return m15_paifei_duoji_moveto_press(); }); + m_cmdparse->regCMD("app_m14_raoxiantance_duoji_move_to_reset", "()", 0, [this](PARAM) { return m14_raoxiantance_duoji_move_to_reset(); }); + m_cmdparse->regCMD("app_m14_raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](PARAM) { int32_t thickness = 0; - int32_t err = raoxiantance_duoji_move_to_get_thickness(ack->getAck(1)); + int32_t err = m14_raoxiantance_duoji_move_to_get_thickness(ack->getAck(1)); ack->acktype = ICmdParserACK::kAckType_int32; ack->rawlen = sizeof(int32_t); }); - m_cmdparse->regCMD("app_yaxian_duoji_move_to_reset", "()", 0, [this](PARAM) { return yaxian_duoji_move_to_reset(); }); - m_cmdparse->regCMD("app_yaxian_duoji_move_to_press_pos", "()", 0, [this](PARAM) { return yaxian_duoji_move_to_press_pos(); }); - m_cmdparse->regCMD("app_yaxian_duoji_move_to_wait_for_press_pos", "()", 0, [this](PARAM) { return yaxian_duoji_move_to_wait_for_press_pos(); }); - m_cmdparse->regCMD("app_xianlajin_duoji_move_to_reset", "()", 0, [this](PARAM) { return xianlajin_duoji_move_to_reset(); }); - m_cmdparse->regCMD("app_xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](PARAM) { return xianlajin_duoji_move_to_line_entry_pos(); }); - m_cmdparse->regCMD("app_xianlajin_duoji_move_to_tight_line_pos", "()", 0, [this](PARAM) { return xianlajin_duoji_move_to_tight_line_pos(); }); - m_cmdparse->regCMD("app_xianlajin_duoji_move_to_loose_line_pos", "()", 0, [this](PARAM) { return xianlajin_duoji_move_to_loose_line_pos(); }); - m_cmdparse->regCMD("app_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](PARAM) { return jiaxian_duoji_move_to_reset_pos(); }); - m_cmdparse->regCMD("app_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](PARAM) { return jiaxian_duoji_move_to_clamp_pos(); }); - m_cmdparse->regCMD("app_scissors_move_reset_pos", "()", 0, [this](PARAM) { return scissors_move_reset_pos(); }); - m_cmdparse->regCMD("app_scissors_cut", "()", 0, [this](PARAM) { return scissors_cut(); }); - m_cmdparse->regCMD("app_arm_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](PARAM) { return arm_jiaxian_duoji_move_to_reset_pos(); }); - m_cmdparse->regCMD("app_arm_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](PARAM) { return arm_jiaxian_duoji_move_to_clamp_pos(); }); - m_cmdparse->regCMD("app_arm_hook_claws_reset", "()", 0, [this](PARAM) { return arm_hook_claws_reset(); }); - m_cmdparse->regCMD("app_arm_hook_claws_move_to_half_pos", "()", 0, [this](PARAM) { return arm_hook_claws_move_to_half_pos(); }); - m_cmdparse->regCMD("app_arm_hook_claws_move_to_full_pos", "()", 0, [this](PARAM) { return arm_hook_claws_move_to_full_pos(); }); + m_cmdparse->regCMD("app_m13_yaxian_duoji_move_to_reset", "()", 0, [this](PARAM) { return m13_yaxian_duoji_move_to_reset(); }); + m_cmdparse->regCMD("app_m13_yaxian_duoji_move_to_press_pos", "()", 0, [this](PARAM) { return m13_yaxian_duoji_move_to_press_pos(); }); + m_cmdparse->regCMD("app_m13_yaxian_duoji_move_to_wait_for_press_pos", "()", 0, [this](PARAM) { return m13_yaxian_duoji_move_to_wait_for_press_pos(); }); + m_cmdparse->regCMD("app_m16_xianlajin_duoji_move_to_reset", "()", 0, [this](PARAM) { return m16_xianlajin_duoji_move_to_reset(); }); + m_cmdparse->regCMD("app_m16_xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](PARAM) { return m16_xianlajin_duoji_move_to_line_entry_pos(); }); + m_cmdparse->regCMD("app_m16_xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](PARAM) { return m16_xianlajin_duoji_move_to_line_entry_pos(); }); + m_cmdparse->regCMD("app_m16_xianlajin_duoji_move_to_loose_line_pos", "()", 0, [this](PARAM) { return m16_xianlajin_duoji_move_to_loose_line_pos(); }); + m_cmdparse->regCMD("app_m12_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](PARAM) { return m12_jiaxian_duoji_move_to_reset_pos(); }); + m_cmdparse->regCMD("app_m12_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](PARAM) { return m12_jiaxian_duoji_move_to_clamp_pos(); }); + m_cmdparse->regCMD("app_m22_scissors_move_reset_pos", "()", 0, [this](PARAM) { return m22_scissors_move_reset_pos(); }); + m_cmdparse->regCMD("app_m22_scissors_cut", "()", 0, [this](PARAM) { return m22_scissors_cut(); }); + m_cmdparse->regCMD("app_m11_arm_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_duoji_move_to_reset_pos(); }); + m_cmdparse->regCMD("app_m11_arm_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_duoji_move_to_clamp_pos(); }); + m_cmdparse->regCMD("app_m21_arm_hook_claws_reset", "()", 0, [this](PARAM) { return m21_arm_hook_claws_reset(); }); + m_cmdparse->regCMD("app_m21_arm_hook_claws_move_to_half_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_half_pos(); }); + m_cmdparse->regCMD("app_m21_arm_hook_claws_move_to_full_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_full_pos(); }); m_cmdparse->regCMD("app_main_shaft_run", "()", 0, [this](PARAM) { return main_shaft_run(); }); m_cmdparse->regCMD("app_main_shaft_stop", "()", 0, [this](PARAM) { return main_shaft_stop(); }); m_cmdparse->regCMD("app_xy_platform_reset", "()", 0, [this](PARAM) { return xy_platform_reset(); }); diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index 3733e27..2237c4d 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -9,6 +9,7 @@ namespace iflytop { using namespace std; + class IntelligentWindingRobotCtrl { public: typedef struct { @@ -59,13 +60,13 @@ class IntelligentWindingRobotCtrl { * @brief 剪刀 */ int32_t scissors_reset_pos; - int32_t scissors_cut_pos; + int32_t m22_scissors_cut_pos; /** * @brief 钩爪 */ - int32_t arm_hook_claws_half_pos; - int32_t arm_hook_claws_full_pos; + int32_t m21_arm_hook_claws_half_pos; + int32_t m21_arm_hook_claws_full_pos; /** * @brief Z轴定位 @@ -84,17 +85,6 @@ class IntelligentWindingRobotCtrl { int32_t xy_platform_takeline_pos_x; int32_t xy_platform_takeline_pos_y; - /** - * @brief - * - * - * 00 - * - * - * - * XX - */ - int32_t xy_platform_takeline_clip00_pos_x; // int32_t xy_platform_takeline_clip00_pos_y; @@ -110,64 +100,75 @@ class IntelligentWindingRobotCtrl { config_t cfg; private: - int32_t device_reset(); - int32_t disable_all_motor(); - 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_duoji_reset = true); // 取弹夹 - int32_t xy_run_to_clip_pos_test(int32_t clip_index); - int32_t xy_reset(); - - int32_t zmove_to(int32_t pos); - int32_t zreset(); - int32_t xymove_to(int32_t x, int32_t y); + void wait_module_idle(int32_t moduleid); + void wait_modules_idle(...); - public: - int32_t initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse); - void regcb(); - int32_t initialize_device(); - // 排废舵机 - int32_t paifei_duoji_moveto_reset(); - int32_t paifei_duoji_moveto_press(); /** - * @brief 绕线探测舵机 + * @brief 机械臂夹线舵机 + */ + int32_t m11_arm_jiaxian_duoji_move_to_reset_pos(); + int32_t m11_arm_jiaxian_duoji_move_to_clamp_pos(); + /** + * @brief 夹线舵机 */ - int32_t raoxiantance_duoji_move_to_reset(); - int32_t raoxiantance_duoji_move_to_get_thickness(int32_t* thickness); + int32_t m12_jiaxian_duoji_move_to_reset_pos(); + int32_t m12_jiaxian_duoji_move_to_clamp_pos(); /** * @brief 压线舵机 */ - int32_t yaxian_duoji_move_to_reset(); - int32_t yaxian_duoji_move_to_press_pos(); - int32_t yaxian_duoji_move_to_wait_for_press_pos(); + int32_t m13_yaxian_duoji_move_to_reset(); + int32_t m13_yaxian_duoji_move_to_press_pos(); + int32_t m13_yaxian_duoji_move_to_wait_for_press_pos(); + /** + * @brief 绕线探测舵机 + */ + int32_t m14_raoxiantance_duoji_move_to_reset(); + int32_t m14_raoxiantance_duoji_move_to_get_thickness(int32_t* thickness); + // 排废舵机 + int32_t m15_paifei_duoji_moveto_reset(); + int32_t m15_paifei_duoji_moveto_press(); /** * @brief 线拉紧舵机 */ - int32_t xianlajin_duoji_move_to_reset(); // 零位 - int32_t xianlajin_duoji_move_to_line_entry_pos(); // 入线位 - int32_t xianlajin_duoji_move_to_tight_line_pos(); // 拉线位置 - int32_t xianlajin_duoji_move_to_loose_line_pos(); // 放线位置 + int32_t m16_xianlajin_duoji_move_to_reset(); // 零位 + int32_t m16_xianlajin_duoji_move_to_line_entry_pos(); // 入线位 + int32_t m16_xianlajin_duoji_move_to_tight_line_pos(); // 拉线位置 + int32_t m16_xianlajin_duoji_move_to_loose_line_pos(); // 放线位置 /** - * @brief 夹线舵机 + * @brief 机械臂钩爪 */ - int32_t jiaxian_duoji_move_to_reset_pos(); - int32_t jiaxian_duoji_move_to_clamp_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(); /** * @brief 剪刀 */ - int32_t scissors_move_reset_pos(); // block - int32_t scissors_cut(); // block + int32_t m22_scissors_move_reset_pos(); // block + int32_t m22_scissors_cut(); // block /** - * @brief 机械臂夹线舵机 + * @brief 拉线电机 */ - int32_t arm_jiaxian_duoji_move_to_reset_pos(); - int32_t arm_jiaxian_duoji_move_to_clamp_pos(); + int32_t m23_laxian_motor_move_to_reset_pos(); // block + int32_t m23_laxian_motor_move_to_tight_line_pos(); // block - /** - * @brief 机械臂钩爪 - */ - int32_t arm_hook_claws_reset(); - int32_t arm_hook_claws_move_to_half_pos(); - int32_t arm_hook_claws_move_to_full_pos(); + + + + int32_t device_reset(); + int32_t disable_all_motor(); + 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_duoji_reset = true); // 取弹夹 + int32_t xy_run_to_clip_pos_test(int32_t clip_index); + int32_t xy_reset(); + + int32_t zmove_to(int32_t pos); + int32_t zreset(); + int32_t xymove_to(int32_t x, int32_t y); + + public: + int32_t initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse); + void regcb(); + int32_t initialize_device(); int32_t main_shaft_run(); int32_t main_shaft_stop(); @@ -202,8 +203,6 @@ class IntelligentWindingRobotCtrl { int32_t do_winding(int32_t index); void processError(int32_t err); - - void wait_module_idle(int32_t moduleid); }; } // namespace iflytop diff --git a/usrc/main.cpp b/usrc/main.cpp index b991240..7b589a5 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -252,7 +252,7 @@ void Main::run() { g_xyrobotctrlmodule.module_set_param(kcfg_motor_default_velocity, 600); g_xyrobotctrlmodule.module_set_param(kcfg_motor_default_acc, 1000); g_xyrobotctrlmodule.module_set_param(kcfg_motor_default_dec, 1000); - g_xyrobotctrlmodule.module_set_param(k_cfg_stepmotor_irun, 2); + g_xyrobotctrlmodule.module_set_param(k_cfg_stepmotor_irun, 4); g_xyrobotctrlmodule.module_active_cfg(); g_z_step_motor.module_set_param(kcfg_motor_x_shift, 0); @@ -303,3 +303,4 @@ void Main::run() { osDelay(1); } } +