diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index cf887c8..00deefb 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -72,7 +72,16 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() { cfg.m21_arm_hook_claws_full_pos = 2558; cfg.m21_arm_hook_claws_half_pos = 2294; - cfg.z_axis_take_clip_pos = 6924; + 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.z_axis_cook_bullet_pos = 3377; + cfg.z_axis_take_clip_pos = 6924; + cfg.z_axis_process_line_high = 3148; return 0; } @@ -84,6 +93,7 @@ void IntelligentWindingRobotCtrl::regcb() { m_cmdparse->regCMD("disable_all_module", "()", 0, [this](PARAM) { disable_all_module(); }); // 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_prepare_remove_line", "()", 0, [this](PARAM) { return step_prepare_remove_line(); }); // m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); }); @@ -227,6 +237,17 @@ int32_t IntelligentWindingRobotCtrl::device_reset() { return 0; } +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); +} + 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); } int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_open_pos() { return m_dm->motor_move_to_torque(12, cfg.m12_jiaxian_reset_pos, 330, 0); } @@ -266,19 +287,12 @@ int32_t IntelligentWindingRobotCtrl::m4_zmove_to(int32_t pos) { } int32_t IntelligentWindingRobotCtrl::step_take_bullet(int32_t bulletindex) { -#if 0 -0. XY复位 -1. Z轴复位 -2. 钩子复位 - -3. 移动到指定位置 + m4_zreset(); + WAIT_MODULES_IDLE(4); -3. 取子弹 -4. 夹线 -5. Z轴移动到零点 -#endif xy_reset(); - m4_zreset(); + WAIT_MODULES_IDLE(3); + m21_arm_hook_claws_reset(); m11_arm_jiaxian_move_to_reset_pos(); @@ -305,6 +319,67 @@ int32_t IntelligentWindingRobotCtrl::step_take_bullet(int32_t bulletindex) { WAIT_MODULES_IDLE(4); } +int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line() { + m15_paifei_moveto_reset(); + m13_yaxian_move_to_reset_backward(); + stop_probe_bullet_pos(); + WAIT_MODULES_IDLE(13, 15); + + // 移动到COOK位置 + xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y); + WAIT_MODULES_IDLE(3); + + // Z移动到放置子弹位置 + m4_zmove_to(cfg.z_axis_cook_bullet_pos); + WAIT_MODULES_IDLE(4); + + m21_arm_hook_claws_move_to_half_pos(); + WAIT_MODULES_IDLE(21); + + m4_zmove_to(0); // Z轴归零 + WAIT_MODULES_IDLE(4); + + start_probe_bullet_pos(); + + /** + * @brief + * + * <------------------point1() + * removeLinePos ^ + * |2723 + * | + * COOK() + * + * + */ + if (is_hasbullet()) { + xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y - 2723); + WAIT_MODULES_IDLE(3); + xymove_to(cfg.xy_platform_remove_line_pos_x, cfg.xy_platform_remove_line_pos_y); + WAIT_MODULES_IDLE(3); + m4_zmove_to(cfg.z_axis_process_line_high); + WAIT_MODULES_IDLE(4); + // 排线舵机 + m15_paifei_moveto_press(); + // m4_zmove_to(cfg.z_axis_take_clip_pos); + // WAIT_MODULES_IDLE(4); + m11_arm_jiaxian_move_to_reset_pos(); + WAIT_MODULES_IDLE(11); + m4_zmove_to(0); + WAIT_MODULES_IDLE(4); + xymove_to(0, 0); + WAIT_MODULES_IDLE(3); + /** + * @brief TODO:如果需要在这里放回子弹壳 + */ + } + + // XY移动 + // xymove_to(); + + // is_hasbullet +} + int32_t IntelligentWindingRobotCtrl::main_shaft_run() { ZLOGI(TAG, "main_shaft_run"); DO(m_dm->motor_rotate_acctime(2, 1, 1000, 10000)); @@ -322,14 +397,6 @@ int32_t IntelligentWindingRobotCtrl::xy_platform_reset() { return 0; } /** * @brief Z轴 */ -int32_t IntelligentWindingRobotCtrl::z_axis_reset() { return 0; } -int32_t IntelligentWindingRobotCtrl::z_axis_move_to(int32_t pos) { return 0; } - -int32_t IntelligentWindingRobotCtrl::xy_move_to_zero() { return 0; } // 移动到零位 -int32_t IntelligentWindingRobotCtrl::xy_take_clip(int32_t index) { return 0; } // 取弹夹 -int32_t IntelligentWindingRobotCtrl::xy_take_line() { return 0; } // 取线 -int32_t IntelligentWindingRobotCtrl::xy_take_back_clip() { return 0; } // 放弹夹 -int32_t IntelligentWindingRobotCtrl::xy_remove_line() { return 0; } // 移除线 int32_t IntelligentWindingRobotCtrl::do_reset_device() {} int32_t IntelligentWindingRobotCtrl::do_winding(int32_t index) {} @@ -380,6 +447,13 @@ int32_t IntelligentWindingRobotCtrl::zreset() { int32_t IntelligentWindingRobotCtrl::xymove_to(int32_t x, int32_t y) { ZLOGI(TAG, "xymove_to %d %d", x, y); + int32_t nowx; + int32_t nowy; + m_dm->xymotor_read_pos(XYRobot_ID, &nowx, &nowy); + if (nowx == 0 && nowy == 0) { + xy_reset(); + } + WAIT_MODULES_IDLE(3); m_dm->xymotor_move_to(XYRobot_ID, x, y, 0); return 0; } diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index 4abe3f0..5f3cd46 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -84,16 +84,22 @@ class IntelligentWindingRobotCtrl { /** * @brief Z轴定位 */ - int32_t z_axis_take_line_pos; + int32_t z_axis_process_line_high; int32_t z_axis_take_clip_pos; int32_t z_axis_winding_hight; + // 3377 + + int32_t z_axis_cook_bullet_pos; + /** * @brief XY平台 */ + int32_t xy_platform_cook_bullet_pos_x; + int32_t xy_platform_cook_bullet_pos_y; - int32_t xy_platform_winding_pos_x; - int32_t xy_platform_winding_pos_y; + int32_t xy_platform_remove_line_pos_x; + int32_t xy_platform_remove_line_pos_y; int32_t xy_platform_takeline_pos_x; int32_t xy_platform_takeline_pos_y; @@ -115,6 +121,12 @@ class IntelligentWindingRobotCtrl { public: int32_t initialize(APPDM* dm, ICmdParser* cmdparse); + private: + void start_probe_bullet_pos(); + void stop_probe_bullet_pos(); + int32_t get_probe_bullet_pos(); + bool is_hasbullet(); + public: void wait_module_idle(int32_t moduleid); void wait_modules_idle(void* mark, ...); @@ -150,6 +162,7 @@ class IntelligentWindingRobotCtrl { int32_t enable_all_module(); int32_t step_take_bullet(int32_t bulletindex); + int32_t step_prepare_remove_line(); 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); // 取弹夹 @@ -168,17 +181,6 @@ class IntelligentWindingRobotCtrl { * @brief XY平台 */ int32_t xy_platform_reset(); - /** - * @brief Z轴 - */ - int32_t z_axis_reset(); - int32_t z_axis_move_to(int32_t pos); - - int32_t xy_move_to_zero(); // 移动到零位 - int32_t xy_take_clip(int32_t index); // 取弹夹 - int32_t xy_take_line(); // 取线 - int32_t xy_take_back_clip(); // 放弹夹 - int32_t xy_remove_line(); // 移除线 /** * @brief diff --git a/usrc/main.cpp b/usrc/main.cpp index 1e62e90..e603e15 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -242,7 +242,7 @@ void Main::run() { int32_t status; g_zmodule_device_manager.module_get_status(21, &status); - + // status *= 2; // status @@ -251,6 +251,8 @@ void Main::run() { /******************************************************************************* * g_xyrobotctrlmodule * *******************************************************************************/ + g_xyrobotctrlmodule.module_set_param(kcfg_motor_x_shift, 0); + g_xyrobotctrlmodule.module_set_param(kcfg_motor_y_shift, 0); g_xyrobotctrlmodule.module_set_param(kcfg_motor_x_one_circle_pulse, 7344); g_xyrobotctrlmodule.module_set_param(kcfg_motor_y_one_circle_pulse, 7344); g_xyrobotctrlmodule.module_set_param(kcfg_motor_run_to_zero_speed, 50); @@ -264,7 +266,6 @@ void Main::run() { g_xyrobotctrlmodule.module_active_cfg(); g_z_step_motor.module_set_param(kcfg_motor_x_shift, 0); - g_z_step_motor.module_set_param(kcfg_motor_x_shaft, 0); g_z_step_motor.module_set_param(kcfg_motor_x_one_circle_pulse, 800); g_z_step_motor.module_active_cfg();