diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index 00deefb..f88d2a1 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -94,6 +94,7 @@ 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_prepare_remove_line", "()", 0, [this](PARAM) { return step_prepare_remove_line(); }); + m_cmdparse->regCMD("step_remove_line", "()", 0, [this](PARAM) { return step_remove_line(); }); // m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); }); @@ -346,14 +347,14 @@ int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line() { * * <------------------point1() * removeLinePos ^ - * |2723 + * |2723 // 20mm * | * COOK() * * */ if (is_hasbullet()) { - xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y - 2723); + xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y - 3623); WAIT_MODULES_IDLE(3); xymove_to(cfg.xy_platform_remove_line_pos_x, cfg.xy_platform_remove_line_pos_y); WAIT_MODULES_IDLE(3); @@ -372,12 +373,30 @@ int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line() { /** * @brief TODO:如果需要在这里放回子弹壳 */ + } else { + /** + * @brief TODO:如果没有子弹,需要在这里做的事情 + */ } +} + +int32_t IntelligentWindingRobotCtrl::step_remove_line() { + start_probe_bullet_pos(); + m15_paifei_moveto_press(); + WAIT_MODULES_IDLE(15); - // XY移动 - // xymove_to(); + m_dm->motor_rotate_acctime(2, -1, 1000, 1000); - // is_hasbullet + /** + * @TODO:添加排线结束检测 + */ + for (size_t i = 0; i < 10; i++) { + osDelay(1000); + } + m_dm->module_stop(2); + stop_probe_bullet_pos(); + m15_paifei_moveto_reset(); + return 0; } int32_t IntelligentWindingRobotCtrl::main_shaft_run() { diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index 5f3cd46..0945b52 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -163,6 +163,7 @@ class IntelligentWindingRobotCtrl { int32_t step_take_bullet(int32_t bulletindex); int32_t step_prepare_remove_line(); + int32_t step_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); // 取弹夹