update

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
 
 ```

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() {

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();  //