update

README.md

@@ -153,6 +153,9 @@ bullet_holder
   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])); });
 
+
+  
+
 ```
 
 ```
@@ -166,4 +169,9 @@ step_winding_lineend_prepare 1
 step_winding_lineend
 step_winding_take_bullet_from_cooking_to_origin_pos 1
 
+```
+
+```
+异常处理 
+    1. 移除线超时
 ```

usrc/intelligent_winding_robot_ctrl.cpp

@@ -38,6 +38,9 @@ class WidthDetector {
   IntelligentWindingRobotCtrl::config_t* cfg;
   APPDM*                                 m_dm;
 
+  int32_t m_bullet_distance      = 33;  // 理论值33,这里取保守值20
+  int32_t m_bullet_full_distance = 168;
+
  public:
   void init(IntelligentWindingRobotCtrl::config_t* cfg, APPDM* dm) {
     this->cfg = cfg;
@@ -88,7 +91,7 @@ class WidthDetector {
     g_isrunning = true;
   }
   void start_run_forward() {
-    m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_tance_zero_pos+100, 330, 0);
+    m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_tance_zero_pos + 100, 330, 0);
     g_isrunning = true;
   }
   void stop_run() {
@@ -140,7 +143,7 @@ class WidthDetector {
           stop_run();
           m_dm->motor_read_pos(14, &g_nowpos);
           g_nowdpos = cfg->m14_raoxiantance_tance_zero_pos - g_nowpos;
-          ZLOGI(TAG, "--------------now pos %d bullet width %d",g_nowpos, g_nowdpos);
+          ZLOGI(TAG, "--------------now pos %d bullet width %d", g_nowpos, g_nowdpos);
         } else if (!g_isrunning && getDetectGPIOState() != 0) {
           start_run_back();
         }
@@ -148,12 +151,17 @@ class WidthDetector {
       }
     });
   }
+
   void stopDetect() {
     m_detect_thread.stop();
 
     m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 330, 0);
     wait_module_idle(14);
   }
+
+  bool isFull() { return g_nowdpos > m_bullet_full_distance - 10; }
+  bool isHasBullet() { return g_nowdpos > 5; }
+  bool isRemoveLineEnd() { return g_nowdpos < m_bullet_distance + 10; }
 };
 
 WidthDetector g_widthDetector;
@@ -206,13 +214,13 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() {
   cfg.m16_xianlajin_cook_line_end_high_pos = 1741;
 
   cfg.m21_arm_hook_claws_full_pos = 2558;
-  cfg.m21_arm_hook_claws_half_pos = 2294;
+  cfg.m21_arm_hook_claws_half_pos = 2194;
 
   cfg.xy_platform_cook_bullet_pos_x = 21691;
   cfg.xy_platform_cook_bullet_pos_y = 6989;
   // 6989 - 4266 2723
-  cfg.xy_platform_remove_line_pos_x = 648;
-  cfg.xy_platform_remove_line_pos_y = 6092;
+  cfg.xy_platform_remove_line_pos_x = -599;
+  cfg.xy_platform_remove_line_pos_y = 4861;
 
   cfg.xy_platform_takeline_pos_x = 37359;
   cfg.xy_platform_takeline_pos_y = 7047;
@@ -224,6 +232,7 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() {
   cfg.z_axis_take_clip_pos      = 6850;
   cfg.z_axis_take_line_high     = 3500;
   cfg.z_axis_transfer_line_high = 2675;
+  cfg.z_axis_remove_line_high   = 3567;
 
   cfg.m2_zerooff = 1110;
   return 0;
@@ -237,7 +246,10 @@ void IntelligentWindingRobotCtrl::regcb() {
   // 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_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_prepare_remove_line", "()", 1, [this](PARAM) {
+    bool hasbullet = false;
+    return step_prepare_remove_line(atoi(paraV[0]), hasbullet);
+  });
   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(); });
@@ -326,6 +338,22 @@ int32_t IntelligentWindingRobotCtrl::disable_all_module() {
   m_dm->motor_enable(21, 0);
   m_dm->motor_enable(22, 0);
   m_dm->motor_enable(23, 0);
+
+  m_dm->module_stop(2);
+  m_dm->module_stop(3);
+  m_dm->module_stop(4);
+  m_dm->module_stop(11);
+  m_dm->module_stop(12);
+  m_dm->module_stop(13);
+  m_dm->module_stop(14);
+  m_dm->module_stop(15);
+  m_dm->module_stop(16);
+  m_dm->module_stop(21);
+  m_dm->module_stop(22);
+  m_dm->module_stop(23);
+
+  stop_probe_bullet_pos();
+
   return 0;
 }
 int32_t IntelligentWindingRobotCtrl::enable_all_module() {
@@ -389,7 +417,7 @@ int32_t IntelligentWindingRobotCtrl::device_reset() {
   return 0;
 }
 
-bool IntelligentWindingRobotCtrl::is_hasbullet() { return true; }
+bool IntelligentWindingRobotCtrl::is_hasbullet() { return g_widthDetector.isHasBullet(); }
 
 void IntelligentWindingRobotCtrl::start_probe_bullet_pos() { g_widthDetector.startDetect(); }
 void IntelligentWindingRobotCtrl::stop_probe_bullet_pos() { g_widthDetector.stopDetect(); }
@@ -537,7 +565,7 @@ int32_t IntelligentWindingRobotCtrl::step_take_back_bullet(int32_t bulletindex)
   WAIT_MODULES_IDLE(4);
 }
 
-int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line(int32_t bulletindex) {
+int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line(int32_t bulletindex, bool& hasbullet) {
   m15_paifei_moveto_reset();
   m13_yaxian_move_to_reset_backward();
   stop_probe_bullet_pos();
@@ -577,7 +605,7 @@ int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line(int32_t bulletinde
 
     WAIT_MODULES_IDLE(3);
 
-    m4_zmove_to(cfg.z_axis_take_line_high);
+    m4_zmove_to(cfg.z_axis_remove_line_high);
     WAIT_MODULES_IDLE(4);
     // 排线舵机
     m15_paifei_moveto_press();
@@ -591,10 +619,13 @@ int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line(int32_t bulletinde
     step_take_back_bullet(bulletindex);
     xymove_to(0, 0);
     WAIT_MODULES_IDLE(3);
+    hasbullet = true;
   } else {
-    /**
-     * @brief TODO:如果没有子弹，需要在这里做的事情
-     */
+    hasbullet = false;
+    stop_probe_bullet_pos();
+    substep_zaxis_do_bullet_action(kCookPos, kTakeBullet, kKeepLine, NULL);
+    xymove_to_bullet_pos(bulletindex);
+    substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBackBullet, kReleaseLine, NULL);
   }
 }
 
@@ -840,9 +871,38 @@ int32_t IntelligentWindingRobotCtrl::xy_reset() {
 }
 int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) { return 0; }
 int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; }
-
 int32_t IntelligentWindingRobotCtrl::start_winding() {  //
+
+  m_work_thread.start([this]() {
+    try {
+      ZLOGI(TAG, "start_winding");
+      device_reset();
+      bool hasbullet = false;
+      for (size_t i = 0; i < 5 * 12; i++) {
+        xymove_to(0, 0);
+        step_take_bullet(i);
+        step_prepare_remove_line(i, hasbullet);
+        if (!hasbullet) {
+          continue;
+        }
+        step_remove_line();
+        step_winding_prepare();
+        step_winding();
+        step_winding_lineend_prepare(i);
+        step_winding_lineend();
+        step_winding_take_bullet_from_cooking_to_origin_pos(i);
+      }
+
+    } catch (int32_t ecode) {
+      ZLOGE(TAG, "work thread catch exception %d", ecode);
+    } catch (...) {
+      ZLOGE(TAG, "work thread catch unkown exception");
+    }
+    disable_all_module();  // todo : 完成一个无异常版本
+  });
+  return 0;
+}
+int32_t IntelligentWindingRobotCtrl::stop_winding() {
+  m_work_thread.stop();
   return 0;
 }
-int32_t IntelligentWindingRobotCtrl::stop_winding() { return 0; }
-int32_t IntelligentWindingRobotCtrl::reset_and_check_device() { return 0; }

usrc/intelligent_winding_robot_ctrl.hpp

@@ -92,6 +92,7 @@ class IntelligentWindingRobotCtrl {
     int32_t z_axis_take_clip_pos;
     int32_t z_axis_winding_hight;
     int32_t z_axis_transfer_line_high;
+    int32_t z_axis_remove_line_high;
 
     // 2585
     // 3377
@@ -203,7 +204,7 @@ class IntelligentWindingRobotCtrl {
   int32_t step_take_back_bullet(int32_t bulletindex);
   int32_t step_take_bullet_case(int32_t bulletindex);
   int32_t step_take_back_bullet_case(int32_t bulletindex);
-  int32_t step_prepare_remove_line(int32_t bulletindex);
+  int32_t step_prepare_remove_line(int32_t bulletindex, bool& hasbullet);
   int32_t step_remove_line();
   int32_t step_winding_prepare();
   int32_t step_winding();
@@ -223,13 +224,9 @@ class IntelligentWindingRobotCtrl {
    */
   int32_t setcfg(const char* cfgname, int32_t cfgvalue);
 
-
-  
-
  public:
   int32_t start_winding();
   int32_t stop_winding();
-  int32_t reset_and_check_device();
 
   int32_t dumpcfg();