diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index 7823918..a47b9ec 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -234,7 +234,8 @@ 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(); + wait_module_idle(22); /** * @brief 优化复位逻辑,超时复位失败,报警 */ @@ -284,8 +285,9 @@ 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 0; } -int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() { return m_dm->motor_rotate_with_torque(22, 1, 4095); } +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::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); } @@ -303,7 +305,10 @@ int32_t IntelligentWindingRobotCtrl::m4_zmove_to(int32_t pos) { } } -int32_t IntelligentWindingRobotCtrl::substep_zaxis_do_bullet_action(take_bullet_pos_type_t zpos, take_bullet_acktion_t take_bullet_acktion, take_bullet_line_acktion_t take_bullet_line_acktion) { +int32_t IntelligentWindingRobotCtrl::substep_zaxis_do_bullet_action(take_bullet_pos_type_t zpos, // + take_bullet_acktion_t take_bullet_acktion, // + take_bullet_line_acktion_t take_bullet_line_acktion, // + function bottomoperation) { if (take_bullet_line_acktion == kTakeLine) { m11_arm_jiaxian_move_to_reset_pos(); } else if (take_bullet_line_acktion == kReleaseLine) { @@ -334,6 +339,11 @@ int32_t IntelligentWindingRobotCtrl::substep_zaxis_do_bullet_action(take_bullet_ m21_arm_hook_claws_reset(); } WAIT_MODULES_IDLE(11, 21); + + if (bottomoperation) { + bottomoperation(); + } + m4_zmove_to(0); WAIT_MODULES_IDLE(4); } @@ -623,6 +633,16 @@ mini_servo_move_to 6 1850 600 500 m16_xianlajin_move_to_cook_lineend_low_pos(); // } +int32_t IntelligentWindingRobotCtrl::step_winding_take_bullet_from_cooking_to_origin_pos(int bulletindex) { + /** + * @brief + * + * 1. 夹住线 + * 2. + * + */ +} + int32_t IntelligentWindingRobotCtrl::main_shaft_run() { ZLOGI(TAG, "main_shaft_run"); DO(m_dm->motor_rotate_acctime(2, 1, 1000, 10000)); diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index 68d4865..d2f89c4 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -180,10 +180,11 @@ class IntelligentWindingRobotCtrl { 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(); - int32_t m22_scissors_move_reset_pos(); // block - int32_t m22_scissors_cut(); // block - int32_t m23_laxian_motor_move_to_reset_pos(); // block - int32_t m23_laxian_motor_move_to_tight_line_pos(); // block + 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(); // int32_t m4_zreset(); int32_t m4_zmove_to(int32_t pos); @@ -192,7 +193,10 @@ class IntelligentWindingRobotCtrl { int32_t disable_all_module(); int32_t enable_all_module(); - int32_t substep_zaxis_do_bullet_action(take_bullet_pos_type_t zpos, take_bullet_acktion_t take_bullet_acktion, take_bullet_line_acktion_t take_bullet_line_acktion); + int32_t substep_zaxis_do_bullet_action(take_bullet_pos_type_t zpos, // + take_bullet_acktion_t take_bullet_acktion, // + take_bullet_line_acktion_t take_bullet_line_acktion, // + function bottomoperation = nullptr); int32_t step_take_bullet(int32_t bulletindex); int32_t step_take_back_bullet(int32_t bulletindex); @@ -204,8 +208,9 @@ class IntelligentWindingRobotCtrl { int32_t step_winding_prepare(); int32_t step_winding(); // int32_t step_load_the_bullet_case(); - int32_t step_winding_lineend_prepare(int bulletindex); // 绕线头准备 - int32_t step_winding_lineend(); // 绕线头 + 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); // 取弹夹 diff --git a/usrc/main.cpp b/usrc/main.cpp index da9b7b1..cb279b3 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -66,28 +66,28 @@ static StepMotor45::cfg_t cfg1 = { .enable_max_pos_limit = false, .mirror = true, - .zeroPin = PG1, + .zeroPin = PB12, .ioPollType = ZGPIO::kMode_pullup, .zeroPinMirror = true, .driverPin = {PB15, PD11, PD12, PD13}, .driverPinMirror = true, }; - static StepMotor45::cfg_t cfg2 = { .max_pos = -1, .enable_zero_limit = true, .enable_max_pos_limit = false, .mirror = true, - .zeroPin = PB13, + .zeroPin = PG1, .ioPollType = ZGPIO::kMode_pullup, .zeroPinMirror = true, .driverPin = {PG2, PG3, PG4, PG5}, .driverPinMirror = true, }; - +// PB13 +#if 0 static StepMotor45::cfg_t cfg3 = { .max_pos = -1, .enable_zero_limit = true, @@ -101,7 +101,6 @@ static StepMotor45::cfg_t cfg3 = { .driverPin = {PG6, PG7, PG8, PC6}, .driverPinMirror = true, }; -#if 0 static StepMotor45::cfg_t cfg4 = { .max_pos = -1, .enable_zero_limit = false, @@ -188,6 +187,7 @@ void regfn() { script_reg_fn(); } extern void step_motor_cmd_reg(); void Main::run() { + // PB13 /******************************************************************************* * 系统初始化 * *******************************************************************************/