From a48319c7f1a2978794bd5ebb6b6f248fbfbe81a0 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 25 Oct 2023 18:00:20 +0800 Subject: [PATCH] update --- README.md | 17 ++++++++-- usrc/intelligent_winding_robot_ctrl.cpp | 59 +++++++++++++++++++++------------ usrc/intelligent_winding_robot_ctrl.hpp | 1 - 3 files changed, 52 insertions(+), 25 deletions(-) diff --git a/README.md b/README.md index a387a29..076113e 100644 --- a/README.md +++ b/README.md @@ -143,14 +143,27 @@ bullet_holder ``` +```c + 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_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(); }); + 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])); }); + +``` ``` step_take_bullet 1 step_prepare_remove_line 1 -step_remove_line 1 +step_remove_line step_winding_prepare step_winding - +step_winding_lineend_prepare 1 +step_winding_lineend +step_winding_take_bullet_from_cooking_to_origin_pos 1 ``` \ No newline at end of file diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index a47b9ec..d55786c 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -54,7 +54,7 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() { cfg.m13_yaxian_forward_reset_pos = 1015; cfg.m13_yaxian_backward_reset_pos = 2885; cfg.m13_jiaxian_clamp_direction = -1; - cfg.m13_jiaxian_clamp_torque = 200; + cfg.m13_jiaxian_clamp_torque = 500; cfg.m14_raoxiantance_reset_pos = 2047; cfg.m14_raoxiantance_tance_zero_pos = 2517; @@ -84,12 +84,12 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() { cfg.xy_platform_takeline_pos_y = 7047; cfg.xy_platform_enter_line_pos_x = 17625; - cfg.xy_platform_enter_line_pos_y = 6851; + cfg.xy_platform_enter_line_pos_y = 6700; // cfg.z_axis_cook_bullet_pos = 3377; cfg.z_axis_take_clip_pos = 6850; cfg.z_axis_take_line_high = 3400; - cfg.z_axis_transfer_line_high = 2785; + cfg.z_axis_transfer_line_high = 2635; cfg.m2_zerooff = 1110; return 0; @@ -108,6 +108,8 @@ void IntelligentWindingRobotCtrl::regcb() { m_cmdparse->regCMD("step_winding", "()", 0, [this](PARAM) { return step_winding(); }); m_cmdparse->regCMD("step_remove_line", "()", 0, [this](PARAM) { return step_remove_line(); }); m_cmdparse->regCMD("step_winding_lineend", "()", 0, [this](PARAM) { return step_winding_lineend(); }); + m_cmdparse->regCMD("step_winding_lineend_prepare", "()", 1, [this](PARAM) { return step_winding_lineend_prepare(atoi(paraV[0])); }); + 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])); }); // m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); }); @@ -234,7 +236,7 @@ int32_t IntelligentWindingRobotCtrl::device_reset() { m22_scissors_move_reset_pos(); m23_laxian_motor_move_to_reset_pos(); WAIT_MODULES_IDLE(12, 14, 15, 16, 22, 23); - m22_scissors_move_idle_pos(); + m22_scissors_move_reset_pos(); wait_module_idle(22); /** * @brief 优化复位逻辑，超时复位失败，报警 @@ -285,9 +287,12 @@ int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_cook_lineend_low_pos( int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_reset() { return m_dm->motor_move_to_zero_backward(21, 0, 0, 0, 0); } int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_half_pos() { return m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_half_pos, 0, 0); } int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_full_pos() { return m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_full_pos, 0, 0); } -int32_t IntelligentWindingRobotCtrl::m22_scissors_move_reset_pos() { return m_dm->motor_move_to_zero_backward(22, 0, 0, 0, 0); } -int32_t IntelligentWindingRobotCtrl::m22_scissors_move_idle_pos() { return m_dm->motor_move_to(22, 1250, 0, 0); } -int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() { return m_dm->motor_move_to(22, 1, 2070); } +int32_t IntelligentWindingRobotCtrl::m22_scissors_move_reset_pos() { + m_dm->motor_move_to_zero_backward(22, 0, 0, 0, 0); + WAIT_MODULES_IDLE(22); + m_dm->motor_move_to(22, 1250, 0, 0); +} +int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() { return m_dm->motor_move_to(22, 2070, 0, 0); } int32_t IntelligentWindingRobotCtrl::m23_laxian_motor_move_to_reset_pos() { return m_dm->motor_move_to_torque(23, 2040, 200, 0); } int32_t IntelligentWindingRobotCtrl::m23_laxian_motor_move_to_tight_line_pos() { return m_dm->motor_rotate_with_torque(23, -1, 40); } int32_t IntelligentWindingRobotCtrl::m4_zreset() { m_dm->motor_move_to_zero_backward(4, 450, 300, 2000, 0); } @@ -481,6 +486,7 @@ int32_t IntelligentWindingRobotCtrl::step_remove_line() { m_dm->module_stop(2); stop_probe_bullet_pos(); // m15_paifei_moveto_reset(); + m15_paifei_moveto_reset(); return 0; } @@ -535,6 +541,7 @@ int32_t IntelligentWindingRobotCtrl::step_winding_prepare() { int32_t IntelligentWindingRobotCtrl::step_winding() { m_dm->motor_rotate_acctime(2, 1, 1000, 10000); osDelay(1000); + m23_laxian_motor_move_to_reset_pos(); for (size_t i = 0; i < 60; i++) { m16_xianlajin_move_to_winding_low_pos(); WAIT_MODULES_IDLE(16); @@ -557,18 +564,17 @@ int32_t IntelligentWindingRobotCtrl::step_winding_lineend_prepare(int bulletinde * 3. 移动到CookPos * 4. 放弹壳 */ + m13_yaxian_move_to_reset_backward(); m_dm->motor_move_to_zero_forward(2, 2, 2, 0, 0); - xymove_to_bullet_pos(bulletindex); - substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBackBulletCase, kReleaseLine); - - xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y); wait_module_idle(2); m_dm->motor_move_to_acctime(2, cfg.m2_zerooff, 30, 100); - substep_zaxis_do_bullet_action(kCookPos, kTakeBackBullet, kReleaseLine); - /** - * @TODO:线拉紧舵机，拉紧线? - */ + wait_module_idle(2); + WAIT_MODULES_IDLE(13); + xymove_to_bullet_pos(bulletindex); + substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBulletCase, kReleaseLine); + xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y); + substep_zaxis_do_bullet_action(kCookPos, kTakeBackBullet, kReleaseLine); return 0; } @@ -634,13 +640,22 @@ mini_servo_move_to 6 1850 600 500 } int32_t IntelligentWindingRobotCtrl::step_winding_take_bullet_from_cooking_to_origin_pos(int bulletindex) { - /** - * @brief - * - * 1. 夹住线 - * 2. - * - */ + m4_zmove_to(0); + WAIT_MODULES_IDLE(4); + xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y); + WAIT_MODULES_IDLE(3); + m12_jiaxian_move_to_clamp_pos(); + substep_zaxis_do_bullet_action(kCookPos, kTakeBullet, kTakeLine, [this]() { + m22_scissors_cut(); + WAIT_MODULES_IDLE(22); + m22_scissors_move_reset_pos(); + }); + WAIT_MODULES_IDLE(22); + + xymove_to_bullet_pos(bulletindex); + WAIT_MODULES_IDLE(3); + + substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBackBullet, kReleaseLine); } int32_t IntelligentWindingRobotCtrl::main_shaft_run() { diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index d2f89c4..0ec3da4 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -181,7 +181,6 @@ class IntelligentWindingRobotCtrl { int32_t m21_arm_hook_claws_move_to_half_pos(); int32_t m21_arm_hook_claws_move_to_full_pos(); int32_t m22_scissors_move_reset_pos(); // - int32_t m22_scissors_move_idle_pos(); // int32_t m22_scissors_cut(); // int32_t m23_laxian_motor_move_to_reset_pos(); // int32_t m23_laxian_motor_move_to_tight_line_pos(); //