update

README.md

@@ -140,7 +140,10 @@ bullet_holder
 ```
 
 ```
-ﾈ｡ｵｯｼﾐ
 
+```
+
+
+```
 
 ```

sdk

@@ -1 +1 @@
-Subproject commit 038b6538f837519f0759a668a2ba533bb7c500d3
+Subproject commit a2715e8c3078ed9ba6ab10f1f270e6e7cd1f2a5c

usrc/intelligent_winding_robot_ctrl.cpp

@@ -82,13 +82,14 @@ void IntelligentWindingRobotCtrl::regcb() {
   m_cmdparse->regCMD("device_reset", "()", 0, [this](PARAM) { device_reset(); });
   m_cmdparse->regCMD("enable_all_module", "()", 0, [this](PARAM) { enable_all_module(); });
   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("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("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); });
 
   m_cmdparse->regCMD("app_m11_arm_jiaxian_move_to_reset_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_reset_pos(); });
   m_cmdparse->regCMD("app_m11_arm_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_clamp_pos(); });
-  m_cmdparse->regCMD("app_m12_jiaxian_move_to_reset_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_reset_pos(); });
+  m_cmdparse->regCMD("app_m12_jiaxian_move_to_open_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_open_pos(); });
   m_cmdparse->regCMD("app_m12_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_clamp_pos(); });
   m_cmdparse->regCMD("app_m13_yaxian_move_to_reset_forward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_forward(); });
   m_cmdparse->regCMD("app_m13_yaxian_move_to_reset_backward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_backward(); });
@@ -200,10 +201,9 @@ int32_t IntelligentWindingRobotCtrl::device_reset() {
   enable_all_module();
   m4_zreset();
   m11_arm_jiaxian_move_to_reset_pos();
-  m21_arm_hook_claws_reset();
-  WAIT_MODULES_IDLE(4, 11, 21);
+  WAIT_MODULES_IDLE(4, 11);
 
-  m12_jiaxian_move_to_reset_pos();
+  m12_jiaxian_move_to_clamp_pos();
   m14_raoxiantance_move_to_reset();
   m15_paifei_moveto_reset();
   m16_xianlajin_move_to_reset();
@@ -220,13 +220,16 @@ int32_t IntelligentWindingRobotCtrl::device_reset() {
   m_dm->xymotor_move_to_zero(XYRobot_ID);
   wait_module_idle(XYRobot_ID);
 
+  m21_arm_hook_claws_reset();
+  wait_module_idle(21);
+
   ZLOGI(TAG, "device_reset finished....");
+  return 0;
 }
 
-
 int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_reset_pos() { return m_dm->motor_move_to_torque(11, cfg.m11_arm_jiaxian_reset_pos, 330, 0); }
 int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_clamp_pos() { return m_dm->motor_rotate_with_torque(11, cfg.m11_arm_jiaxian_clamp_direction, cfg.m11_arm_jiaxian_clamp_torque); }
-int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_reset_pos() { return m_dm->motor_move_to_torque(12, cfg.m12_jiaxian_reset_pos, 330, 0); }
+int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_open_pos() { return m_dm->motor_move_to_torque(12, cfg.m12_jiaxian_reset_pos, 330, 0); }
 int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_clamp_pos() { return m_dm->motor_rotate_with_torque(12, cfg.m12_jiaxian_clamp_direction, cfg.m12_jiaxian_clamp_torque); }
 int32_t IntelligentWindingRobotCtrl::m13_yaxian_move_to_reset_forward() { return m_dm->motor_move_to_torque(13, cfg.m13_yaxian_forward_reset_pos, 330, 0); }
 int32_t IntelligentWindingRobotCtrl::m13_yaxian_move_to_reset_backward() { return m_dm->motor_move_to_torque(13, cfg.m13_yaxian_backward_reset_pos, 100, 0); }
@@ -262,6 +265,46 @@ int32_t IntelligentWindingRobotCtrl::m4_zmove_to(int32_t pos) {
   }
 }
 
+int32_t IntelligentWindingRobotCtrl::step_take_bullet(int32_t bulletindex) {
+#if 0
+0. XY复位
+1. Z轴复位
+2. 钩子复位
+
+3. 移动到指定位置
+
+3. 取子弹
+4. 夹线
+5. Z轴移动到零点
+#endif
+  xy_reset();
+  m4_zreset();
+  m21_arm_hook_claws_reset();
+  m11_arm_jiaxian_move_to_reset_pos();
+
+  WAIT_MODULES_IDLE(3, 4, 11, 21);
+
+  int32_t x = 0;
+  int32_t y = 0;
+  xy_get_point(bulletindex, x, y);
+  xymove_to(x, y);
+
+  WAIT_MODULES_IDLE(3);
+
+  m4_zmove_to(cfg.z_axis_take_clip_pos);
+  WAIT_MODULES_IDLE(4);
+
+  m21_arm_hook_claws_move_to_full_pos();
+  m11_arm_jiaxian_move_to_clamp_pos();
+  WAIT_MODULES_IDLE(21, 11);
+
+  m4_zmove_to(0);
+  WAIT_MODULES_IDLE(4);
+
+  m4_zreset();
+  WAIT_MODULES_IDLE(4);
+}
+
 int32_t IntelligentWindingRobotCtrl::main_shaft_run() {
   ZLOGI(TAG, "main_shaft_run");
   DO(m_dm->motor_rotate_acctime(2, 1, 1000, 10000));
@@ -338,13 +381,11 @@ int32_t IntelligentWindingRobotCtrl::zreset() {
 int32_t IntelligentWindingRobotCtrl::xymove_to(int32_t x, int32_t y) {
   ZLOGI(TAG, "xymove_to %d %d", x, y);
   m_dm->xymotor_move_to(XYRobot_ID, x, y, 0);
-  wait_module_idle(XYRobot_ID);
   return 0;
 }
 int32_t IntelligentWindingRobotCtrl::xy_reset() {
   ZLOGI(TAG, "xy_reset");
   m_dm->xymotor_move_to_zero(XYRobot_ID);
-  wait_module_idle(XYRobot_ID);
   return 0;
 }
 

usrc/intelligent_winding_robot_ctrl.hpp

@@ -116,11 +116,12 @@ class IntelligentWindingRobotCtrl {
   int32_t initialize(APPDM* dm, ICmdParser* cmdparse);
 
  public:
-  void    wait_module_idle(int32_t moduleid);
-  void    wait_modules_idle(void* mark, ...);
+  void wait_module_idle(int32_t moduleid);
+  void wait_modules_idle(void* mark, ...);
+
   int32_t m11_arm_jiaxian_move_to_reset_pos();
   int32_t m11_arm_jiaxian_move_to_clamp_pos();
-  int32_t m12_jiaxian_move_to_reset_pos();
+  int32_t m12_jiaxian_move_to_open_pos();
   int32_t m12_jiaxian_move_to_clamp_pos();
   int32_t m13_yaxian_move_to_reset_forward();
   int32_t m13_yaxian_move_to_reset_backward();
@@ -148,6 +149,8 @@ class IntelligentWindingRobotCtrl {
   int32_t disable_all_module();
   int32_t enable_all_module();
 
+  int32_t step_take_bullet(int32_t 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_run_to_clip_pos_test(int32_t clip_index);