stablev1

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. 如果线梭子的线方向放反，将无法退线
+
+
+
+
+
+
+
 ```

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

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

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,