From 0bab1e91685223e515249c244f1aa12b3bbe7e07 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 26 Oct 2023 07:14:37 +0800 Subject: [PATCH] stablev1 --- README.md | 63 +++++++++++++++++++++-- usrc/intelligent_winding_robot_ctrl.cpp | 90 ++++++++++++++++++++++++++------- usrc/intelligent_winding_robot_ctrl.hpp | 2 + usrc/main.cpp | 2 +- 4 files changed, 135 insertions(+), 22 deletions(-) diff --git a/README.md b/README.md index 9da07c6..58bd0e9 100644 --- a/README.md +++ b/README.md @@ -160,14 +160,14 @@ bullet_holder ``` -step_take_bullet 1 -step_prepare_remove_line 1 +step_take_bullet 3 +step_prepare_remove_line 3 step_remove_line step_winding_prepare step_winding -step_winding_lineend_prepare 1 +step_winding_lineend_prepare 3 step_winding_lineend -step_winding_take_bullet_from_cooking_to_origin_pos 1 +step_winding_take_bullet_from_cooking_to_origin_pos 3 辅助 @@ -179,4 +179,59 @@ step_take_back_bullet 1 ``` 异常处理 1. 移除线超时 +``` + +``` +TEST:step_winding_lineend_prepare 1 + +m23_laxian_motor_move_to_reset_pos +m12_jiaxian_move_to_open_pos +step_winding_lineend_prepare 1 +``` + + + +``` +实验准备 +motor_move_to 16 1900 300 0 +motor_move_to_acctime 2 1100 5 50 + +抬升复位 +motor_move_to 16 1900 300 0 +motor_move_to_acctime 2 2200 5 50 # 反转 +m16_xianlajin_move_to_cook_lineend_low_pos +motor_move_to_acctime 2 2300 5 50 # 反转 +motor_move_to_acctime 2 -200 5 50 # 正转 +m16_xianlajin_move_to_cook_lineend_high_pos +motor_move_to_acctime 2 1000 5 50 +m23_laxian_motor_move_to_tight_line_pos +motor_move_to_acctime 2 1000 5 50 +motor_move_to 16 1850 300 0 //m16_xianlajin_move_to_cook_lineend_low_pos +motor_move_to_acctime 2 -200 5 50 +m16_xianlajin_move_to_cook_lineend_high_pos +motor_move_to_acctime 2 1100 5 50 +motor_move_to_torque 23 1600 40 0 +motor_move_to 16 1850 300 0 + + + + + + + + + + + + +1. 剪刀碍事 +2. 卡扣十分容易松 +3. 如果线梭子的线方向放反，将无法退线 + + + + + + + ``` \ No newline at end of file diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index c19017c..1d511bf 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -39,7 +39,7 @@ class WidthDetector { APPDM* m_dm; int32_t m_bullet_distance = 33; // 理论值33,这里取保守值20 - int32_t m_bullet_full_distance = 168; + int32_t m_bullet_full_distance = 127; public: void init(IntelligentWindingRobotCtrl::config_t* cfg, APPDM* dm) { @@ -177,7 +177,7 @@ class WidthDetector { wait_module_idle(14); } - bool isFull() { return g_nowdpos > m_bullet_full_distance - 10; } + bool isFull() { return g_nowdpos > m_bullet_full_distance - 20; } bool isHasBullet() { return g_nowdpos > 5; } bool isRemoveLineEnd() { return g_nowdpos < m_bullet_distance + 10; } }; @@ -224,7 +224,7 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() { cfg.m15_paifei_press_torque = 330; cfg.m16_xianlajin_reset_pos = 2047; - cfg.m16_xianlajin_cook_line_end_ready_pos = 1959; + cfg.m16_xianlajin_cook_line_end_ready_pos = 1900; cfg.m16_xianlajin_tight_line_pos = 1966; cfg.m16_xianlajin_winding_low_pos = 1885; cfg.m16_xianlajin_winding_high_pos = 1815; @@ -245,12 +245,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 = 6700; + cfg.xy_platform_enter_line_pos_y = 6600; // - cfg.z_axis_cook_bullet_pos = 3277; + cfg.z_axis_cook_bullet_pos = 3400; // 3277 cfg.z_axis_take_clip_pos = 6850; cfg.z_axis_take_line_high = 3500; - cfg.z_axis_transfer_line_high = 2675; + cfg.z_axis_transfer_line_high = 2275; cfg.z_axis_remove_line_high = 3567; cfg.m2_zerooff = 1110; @@ -474,9 +474,9 @@ int32_t IntelligentWindingRobotCtrl::m22_scissors_move_reset_pos() { 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::m22_scissors_cut() { return m_dm->motor_move_to(22, 2500, 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::m23_laxian_motor_move_to_tight_line_pos() { return m_dm->motor_move_to_torque(23, 1800, 40, 0); } int32_t IntelligentWindingRobotCtrl::m4_zreset() { m_dm->motor_move_to_zero_backward(4, 450, 300, 2000, 0); } int32_t IntelligentWindingRobotCtrl::m4_zmove_to(int32_t pos) { ZLOGI(TAG, "zmove_to %d", pos); @@ -744,6 +744,7 @@ int32_t IntelligentWindingRobotCtrl::step_winding() { } m_dm->module_stop(2); stop_probe_bullet_pos(); + WAIT_MODULES_IDLE(16); ZLOGI(TAG, "step_winding end...."); return 0; } @@ -764,12 +765,18 @@ int32_t IntelligentWindingRobotCtrl::step_winding_lineend_prepare(int bulletinde m_dm->motor_move_to_acctime(2, cfg.m2_zerooff, 30, 100); wait_module_idle(2); WAIT_MODULES_IDLE(13); + m_dm->motor_move_to_torque(23, 1900, 40, 0); + WAIT_MODULES_IDLE(23); + + m16_xianlajin_move_to_cook_lineend_ready_pos(); + WAIT_MODULES_IDLE(16); 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); - m16_xianlajin_move_to_tight_line_pos(); + WAIT_MODULES_IDLE(16); substep_zaxis_do_bullet_action(kCookPos, kTakeBackBullet, kReleaseLine); + return 0; } @@ -805,18 +812,19 @@ mini_servo_move_to 6 1550 600 500 eq20_move_to 1 1110 30 100 mini_servo_move_to 6 1850 600 500 #endif - m23_laxian_motor_move_to_tight_line_pos(); - + // m23_laxian_motor_move_to_tight_line_pos(); +#if 0 int32_t velocity = 30; int32_t acctime = 10; m_dm->motor_move_to_acctime(2, cfg.m2_zerooff, velocity, acctime); WAIT_MODULES_IDLE(2); - m16_xianlajin_move_to_cook_lineend_low_pos(); // WAIT_MODULES_IDLE(16); // m_dm->motor_move_to_acctime(2, cfg.m2_zerooff + 1000, velocity, acctime); // 沟位置 WAIT_MODULES_IDLE(2); // + m16_xianlajin_move_to_cook_lineend_low_pos(); // + WAIT_MODULES_IDLE(16); // m_dm->motor_move_to_acctime(2, cfg.m2_zerooff - 1600, velocity, acctime); // 逆向上线位置 WAIT_MODULES_IDLE(2); // m16_xianlajin_move_to_cook_lineend_high_pos(); // @@ -832,19 +840,67 @@ mini_servo_move_to 6 1850 600 500 m_dm->motor_move_to_acctime(2, cfg.m2_zerooff, velocity, acctime); // WAIT_MODULES_IDLE(2); // m16_xianlajin_move_to_cook_lineend_low_pos(); // +#endif + +#if 0 + motor_move_to 16 1850 300 0 + motor_move_to_acctime 2 1400 5 50 + motor_move_to 16 1800 300 0 + motor_move_to_acctime 2 2200 5 50 + motor_move_to 16 1800 300 0 + motor_move_to_acctime 2 -200 1 50 + motor_move_to 16 1741 300 0 + motor_move_to_acctime 2 1000 5 50 + motor_move_to 16 1850 300 0 + motor_move_to_acctime 2 -200 5 50 + motor_move_to 16 1741 300 0 + motor_move_to_acctime 2 1100 5 50 + motor_move_to 16 1850 300 0 +#endif + + // m_dm->motor_move_to(16,1980,300,0); + m_dm->motor_move_to(16, 1850, 300, 0); + wait_module_idle(16); + m_dm->motor_move_to_acctime(2, 1400, 5, 50); + wait_module_idle(2); + m_dm->motor_move_to(16, 1850, 300, 0); + wait_module_idle(16); + m_dm->motor_move_to_acctime(2, 2200, 5, 50); + wait_module_idle(2); + m_dm->motor_move_to(16, 1800, 300, 0); + wait_module_idle(16); + m_dm->motor_move_to_acctime(2, -200, 3, 50); + osDelay(4000); + wait_module_idle(2); + m_dm->motor_move_to(16, 1741, 300, 0); + wait_module_idle(16); + m_dm->motor_move_to_acctime(2, 1000, 5, 50); + wait_module_idle(2); + m_dm->motor_move_to(16, 1850, 300, 0); + wait_module_idle(16); + m_dm->motor_move_to_acctime(2, -200, 5, 50); + wait_module_idle(2); + m_dm->motor_move_to(16, 1741, 300, 0); + wait_module_idle(16); + m_dm->motor_move_to_acctime(2, 1100, 5, 50); + wait_module_idle(2); + m_dm->motor_move_to(16, 1850, 300, 0); + wait_module_idle(16); } int32_t IntelligentWindingRobotCtrl::step_winding_take_bullet_from_cooking_to_origin_pos(int bulletindex) { + m16_xianlajin_move_to_reset(); 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(12); + m22_scissors_cut(); + WAIT_MODULES_IDLE(22); + m22_scissors_move_reset_pos(); + + substep_zaxis_do_bullet_action(kCookPos, kTakeBullet, kReleaseLine, [this]() {}); WAIT_MODULES_IDLE(22); xymove_to_bullet_pos(bulletindex); diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index f22ec09..a99e1f1 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -226,6 +226,8 @@ class IntelligentWindingRobotCtrl { 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_reset(); diff --git a/usrc/main.cpp b/usrc/main.cpp index a31f6b4..4783df9 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -75,7 +75,7 @@ static StepMotor45::cfg_t cfg1 = { }; static StepMotor45::cfg_t cfg2 = { .max_pos = -1, - .enable_zero_limit = true, + .enable_zero_limit = false, .enable_max_pos_limit = false, .mirror = true,