update

usrc/intelligent_winding_robot_ctrl.cpp

@@ -77,11 +77,17 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() {
   // 6989 - 4266 2723
   cfg.xy_platform_remove_line_pos_x = 648;
   cfg.xy_platform_remove_line_pos_y = 6092;
+
+  cfg.xy_platform_takeline_pos_x = 37359;
+  cfg.xy_platform_takeline_pos_y = 7047;
+
+  cfg.xy_platform_enter_line_pos_x = 17625;
+  cfg.xy_platform_enter_line_pos_y = 7001;
   //
-  //
-  cfg.z_axis_cook_bullet_pos   = 3377;
-  cfg.z_axis_take_clip_pos     = 6924;
-  cfg.z_axis_process_line_high = 3148;
+  cfg.z_axis_cook_bullet_pos    = 3377;
+  cfg.z_axis_take_clip_pos      = 6924;
+  cfg.z_axis_take_line_high     = 3400;
+  cfg.z_axis_transfer_line_high = 2785;
 
   return 0;
 }
@@ -93,8 +99,9 @@ void IntelligentWindingRobotCtrl::regcb() {
   m_cmdparse->regCMD("disable_all_module", "()", 0, [this](PARAM) { disable_all_module(); });
   // m_cmdparse->regCMD("xy_run_to_clip_pos_test", "()", 1, [this](PARAM) { return xy_run_to_clip_pos_test(atoi(paraV[0])); });
   m_cmdparse->regCMD("step_take_bullet", "()", 1, [this](PARAM) { return step_take_bullet(atoi(paraV[0])); });
-  m_cmdparse->regCMD("step_prepare_remove_line", "()", 0, [this](PARAM) { return step_prepare_remove_line(); });
-  m_cmdparse->regCMD("step_remove_line", "()", 0, [this](PARAM) { return step_remove_line(); });
+  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", "()", 1, [this](PARAM) { return step_winding_prepare(); });
 
   // m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); });
 
@@ -320,7 +327,29 @@ int32_t IntelligentWindingRobotCtrl::step_take_bullet(int32_t bulletindex) {
   WAIT_MODULES_IDLE(4);
 }
 
-int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line() {
+int32_t IntelligentWindingRobotCtrl::step_take_back_bullet(int32_t bulletindex) {
+  m4_zmove_to(0);
+  WAIT_MODULES_IDLE(4);
+
+  xymove_to_bullet_pos(bulletindex);
+
+  WAIT_MODULES_IDLE(3);
+
+  m4_zmove_to(cfg.z_axis_take_clip_pos);
+  WAIT_MODULES_IDLE(4);
+
+  m21_arm_hook_claws_reset();
+  m11_arm_jiaxian_move_to_reset_pos();
+  WAIT_MODULES_IDLE(21, 11);
+
+  m4_zmove_to(0);
+  WAIT_MODULES_IDLE(4);
+
+  m4_zreset();
+  WAIT_MODULES_IDLE(4);
+}
+
+int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line(int32_t bulletindex) {
   m15_paifei_moveto_reset();
   m13_yaxian_move_to_reset_backward();
   stop_probe_bullet_pos();
@@ -358,7 +387,7 @@ int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line() {
     WAIT_MODULES_IDLE(3);
     xymove_to(cfg.xy_platform_remove_line_pos_x, cfg.xy_platform_remove_line_pos_y);
     WAIT_MODULES_IDLE(3);
-    m4_zmove_to(cfg.z_axis_process_line_high);
+    m4_zmove_to(cfg.z_axis_take_line_high);
     WAIT_MODULES_IDLE(4);
     // 排线舵机
     m15_paifei_moveto_press();
@@ -368,11 +397,10 @@ int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line() {
     WAIT_MODULES_IDLE(11);
     m4_zmove_to(0);
     WAIT_MODULES_IDLE(4);
+
+    step_take_back_bullet(bulletindex);
     xymove_to(0, 0);
     WAIT_MODULES_IDLE(3);
-    /**
-     * @brief TODO:如果需要在这里放回子弹壳
-     */
   } else {
     /**
      * @brief TODO:如果没有子弹，需要在这里做的事情
@@ -381,6 +409,7 @@ int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line() {
 }
 
 int32_t IntelligentWindingRobotCtrl::step_remove_line() {
+  ZLOGI(TAG, "step_remove_line");
   start_probe_bullet_pos();
   m15_paifei_moveto_press();
   WAIT_MODULES_IDLE(15);
@@ -395,10 +424,52 @@ 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;
 }
 
+int32_t IntelligentWindingRobotCtrl::step_winding_prepare() {
+  m4_zmove_to(0);
+  WAIT_MODULES_IDLE(4);
+
+  xymove_to(cfg.xy_platform_takeline_pos_x, cfg.xy_platform_takeline_pos_y);
+  WAIT_MODULES_IDLE(3);
+
+  // 线夹张开
+  m11_arm_jiaxian_move_to_reset_pos();
+  WAIT_MODULES_IDLE(11);
+
+  // Z轴移动到夹线位置
+  m4_zmove_to(cfg.z_axis_take_line_high);
+  WAIT_MODULES_IDLE(4);
+
+  // 夹线
+  m11_arm_jiaxian_move_to_clamp_pos();
+  WAIT_MODULES_IDLE(11);
+
+  // 线夹张开
+  m12_jiaxian_move_to_open_pos();
+  WAIT_MODULES_IDLE(12);
+
+  // 移动到转移线高度
+  m16_xianlajin_move_to_line_entry_pos();
+
+  WAIT_MODULES_IDLE(16);
+  m4_zmove_to(cfg.z_axis_transfer_line_high);
+  WAIT_MODULES_IDLE(4);
+  // 移动到转移线位置
+  xymove_to(cfg.xy_platform_takeline_pos_x, cfg.xy_platform_takeline_pos_y + 4000);
+  WAIT_MODULES_IDLE(4);
+  xymove_to(cfg.xy_platform_enter_line_pos_x, cfg.xy_platform_enter_line_pos_y + 4000);
+  WAIT_MODULES_IDLE(4);
+  xymove_to(cfg.xy_platform_enter_line_pos_x, cfg.xy_platform_enter_line_pos_y);
+  WAIT_MODULES_IDLE(3);
+  m_dm->motor_enable(13,0);
+  // m13_yaxian_press_clip();
+  // WAIT_MODULES_IDLE(13);
+
+}
+
 int32_t IntelligentWindingRobotCtrl::main_shaft_run() {
   ZLOGI(TAG, "main_shaft_run");
   DO(m_dm->motor_rotate_acctime(2, 1, 1000, 10000));
@@ -464,6 +535,13 @@ int32_t IntelligentWindingRobotCtrl::zreset() {
 }
 #endif
 
+int32_t IntelligentWindingRobotCtrl::xymove_to_bullet_pos(int32_t bulletindex) {
+  int32_t x = 0;
+  int32_t y = 0;
+  xy_get_point(bulletindex, x, y);
+  return xymove_to(x, y);
+}
+
 int32_t IntelligentWindingRobotCtrl::xymove_to(int32_t x, int32_t y) {
   ZLOGI(TAG, "xymove_to %d %d", x, y);
   int32_t nowx;
@@ -473,7 +551,18 @@ int32_t IntelligentWindingRobotCtrl::xymove_to(int32_t x, int32_t y) {
     xy_reset();
   }
   WAIT_MODULES_IDLE(3);
-  m_dm->xymotor_move_to(XYRobot_ID, x, y, 0);
+
+  if (y > nowy) {
+    m_dm->xymotor_move_to(XYRobot_ID, nowx, y, 0);
+    WAIT_MODULES_IDLE(3);
+    m_dm->xymotor_move_to(XYRobot_ID, x, y, 0);
+    WAIT_MODULES_IDLE(3);
+  } else {
+    m_dm->xymotor_move_to(XYRobot_ID, x, nowy, 0);
+    WAIT_MODULES_IDLE(3);
+    m_dm->xymotor_move_to(XYRobot_ID, x, y, 0);
+    WAIT_MODULES_IDLE(3);
+  }
   return 0;
 }
 int32_t IntelligentWindingRobotCtrl::xy_reset() {

usrc/intelligent_winding_robot_ctrl.hpp

@@ -84,10 +84,12 @@ class IntelligentWindingRobotCtrl {
     /**
      * @brief ZÖᶨλ
      */
-    int32_t z_axis_process_line_high;
+    int32_t z_axis_take_line_high;
     int32_t z_axis_take_clip_pos;
     int32_t z_axis_winding_hight;
+    int32_t z_axis_transfer_line_high;
 
+    // 2585
     // 3377
 
     int32_t z_axis_cook_bullet_pos;
@@ -95,14 +97,19 @@ class IntelligentWindingRobotCtrl {
     /**
      * @brief XYƽ̨
      */
-    int32_t xy_platform_cook_bullet_pos_x;
-    int32_t xy_platform_cook_bullet_pos_y;
+    int32_t xy_platform_cook_bullet_pos_x;  // ÈÆÏß¼Ó¹¤
+    int32_t xy_platform_cook_bullet_pos_y;  // ÈÆÏß¼Ó¹¤
 
-    int32_t xy_platform_remove_line_pos_x;
-    int32_t xy_platform_remove_line_pos_y;
+    int32_t xy_platform_remove_line_pos_x;  // ÒÆÏßλ
+    int32_t xy_platform_remove_line_pos_y;  // ÒÆÏßλ
 
-    int32_t xy_platform_takeline_pos_x;
-    int32_t xy_platform_takeline_pos_y;
+    int32_t xy_platform_takeline_pos_x;  // È¡ÏßλÖÃ
+    int32_t xy_platform_takeline_pos_y;  // È¡ÏßλÖÃ
+
+    int32_t xy_platform_enter_line_pos_x;  // ÈëÏßλ
+    int32_t xy_platform_enter_line_pos_y;  // ÈëÏßλ
+
+    //
 
     int32_t xy_platform_takeline_clip00_pos_x;  //
     int32_t xy_platform_takeline_clip00_pos_y;
@@ -162,8 +169,11 @@ class IntelligentWindingRobotCtrl {
   int32_t enable_all_module();
 
   int32_t step_take_bullet(int32_t bulletindex);
-  int32_t step_prepare_remove_line();
+  int32_t step_take_back_bullet(int32_t bulletindex);
+
+  int32_t step_prepare_remove_line(int32_t bulletindex);
   int32_t step_remove_line();
+  int32_t step_winding_prepare();
 
   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);  // È¡µ¯¼Ð
@@ -171,6 +181,7 @@ class IntelligentWindingRobotCtrl {
   int32_t xy_reset();
 
   int32_t xymove_to(int32_t x, int32_t y);
+  int32_t xymove_to_bullet_pos(int32_t bulletindex);
 
   void    regcb();
   int32_t initialize_device();