update

2 years ago · be08897acc
3 changed files with 41 additions and 16 deletions
--- a/usrc/intelligent_winding_robot_ctrl.cpp
+++ b/usrc/intelligent_winding_robot_ctrl.cpp
@ -234,7 +234,8 @@ 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();
+  wait_module_idle(22);
  /**
   * @brief 优化复位逻辑，超时复位失败，报警
   */
@ -284,8 +285,9 @@ 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 0; }
-int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() { return m_dm->motor_rotate_with_torque(22, 1, 4095); }
+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::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); }
@ -303,7 +305,10 @@ int32_t IntelligentWindingRobotCtrl::m4_zmove_to(int32_t pos) {
  }
 }

-int32_t IntelligentWindingRobotCtrl::substep_zaxis_do_bullet_action(take_bullet_pos_type_t zpos, take_bullet_acktion_t take_bullet_acktion, take_bullet_line_acktion_t take_bullet_line_acktion) {
+int32_t IntelligentWindingRobotCtrl::substep_zaxis_do_bullet_action(take_bullet_pos_type_t     zpos,                      //
+                                                                    take_bullet_acktion_t      take_bullet_acktion,       //
+                                                                    take_bullet_line_acktion_t take_bullet_line_acktion,  //
+                                                                    function<void()>           bottomoperation) {
  if (take_bullet_line_acktion == kTakeLine) {
    m11_arm_jiaxian_move_to_reset_pos();
  } else if (take_bullet_line_acktion == kReleaseLine) {
@ -334,6 +339,11 @@ int32_t IntelligentWindingRobotCtrl::substep_zaxis_do_bullet_action(take_bullet_
    m21_arm_hook_claws_reset();
  }
  WAIT_MODULES_IDLE(11, 21);
+
+  if (bottomoperation) {
+    bottomoperation();
+  }
+
  m4_zmove_to(0);
  WAIT_MODULES_IDLE(4);
 }
@ -623,6 +633,16 @@ mini_servo_move_to 6 1850 600 500
  m16_xianlajin_move_to_cook_lineend_low_pos();                              //
 }

+int32_t IntelligentWindingRobotCtrl::step_winding_take_bullet_from_cooking_to_origin_pos(int bulletindex) {
+  /**
+   * @brief
+   *
+   *  1. 夹住线
+   *  2.
+   *
+   */
+}
+
 int32_t IntelligentWindingRobotCtrl::main_shaft_run() {
  ZLOGI(TAG, "main_shaft_run");
  DO(m_dm->motor_rotate_acctime(2, 1, 1000, 10000));
--- a/usrc/intelligent_winding_robot_ctrl.hpp
+++ b/usrc/intelligent_winding_robot_ctrl.hpp
@ -180,10 +180,11 @@ class IntelligentWindingRobotCtrl {
  int32_t m21_arm_hook_claws_reset();
  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();              // block
-  int32_t m22_scissors_cut();                         // block
-  int32_t m23_laxian_motor_move_to_reset_pos();       // block
-  int32_t m23_laxian_motor_move_to_tight_line_pos();  // block
+  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();  //

  int32_t m4_zreset();
  int32_t m4_zmove_to(int32_t pos);
@ -192,7 +193,10 @@ class IntelligentWindingRobotCtrl {
  int32_t disable_all_module();
  int32_t enable_all_module();

-  int32_t substep_zaxis_do_bullet_action(take_bullet_pos_type_t zpos, take_bullet_acktion_t take_bullet_acktion, take_bullet_line_acktion_t take_bullet_line_acktion);
+  int32_t substep_zaxis_do_bullet_action(take_bullet_pos_type_t     zpos,                      //
+                                         take_bullet_acktion_t      take_bullet_acktion,       //
+                                         take_bullet_line_acktion_t take_bullet_line_acktion,  //
+                                         function<void()>           bottomoperation = nullptr);

  int32_t step_take_bullet(int32_t bulletindex);
  int32_t step_take_back_bullet(int32_t bulletindex);
@ -204,8 +208,9 @@ class IntelligentWindingRobotCtrl {
  int32_t step_winding_prepare();
  int32_t step_winding();
  // int32_t step_load_the_bullet_case();
-  int32_t step_winding_lineend_prepare(int bulletindex);  // 绕线头准备
-  int32_t step_winding_lineend();                         // 绕线头
+  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);  // 取弹夹
--- a/usrc/main.cpp
+++ b/usrc/main.cpp
@ -66,28 +66,28 @@ static StepMotor45::cfg_t cfg1 = {
    .enable_max_pos_limit = false,
    .mirror               = true,

-    .zeroPin       = PG1,
+    .zeroPin       = PB12,
    .ioPollType    = ZGPIO::kMode_pullup,
    .zeroPinMirror = true,

    .driverPin       = {PB15, PD11, PD12, PD13},
    .driverPinMirror = true,
 };
-
 static StepMotor45::cfg_t cfg2 = {
    .max_pos              = -1,
    .enable_zero_limit    = true,
    .enable_max_pos_limit = false,
    .mirror               = true,

-    .zeroPin       = PB13,
+    .zeroPin       = PG1,
    .ioPollType    = ZGPIO::kMode_pullup,
    .zeroPinMirror = true,

    .driverPin       = {PG2, PG3, PG4, PG5},
    .driverPinMirror = true,
 };
-
+// PB13
+#if 0
 static StepMotor45::cfg_t cfg3 = {
    .max_pos              = -1,
    .enable_zero_limit    = true,
@ -101,7 +101,6 @@ static StepMotor45::cfg_t cfg3 = {
    .driverPin       = {PG6, PG7, PG8, PC6},
    .driverPinMirror = true,
 };
-#if 0
 static StepMotor45::cfg_t cfg4 = {
    .max_pos              = -1,
    .enable_zero_limit    = false,
@ -188,6 +187,7 @@ void        regfn() { script_reg_fn(); }

 extern void step_motor_cmd_reg();
 void        Main::run() {
+  // PB13
  /*******************************************************************************
   *                                 ÏµÍ³³õÊ¼»¯                                  *
   *******************************************************************************/