iflytop_cancmder/usrc/intelligent_winding_robot_ctrl.cpp

#include "intelligent_winding_robot_ctrl.hpp"

#include <stdarg.h>
#include <stdlib.h>
#include <string.h>

#include <exception>
using namespace std;
using namespace iflytop;

#define TAG "APPDM"

#define DO(exptr)        \
  {                      \
    int32_t ret = exptr; \
    if (ret != 0) {      \
      return ret;        \
    }                    \
  }

#define PARAM int32_t paramN, const char **paraV, ICmdParserACK *ack

void IntelligentWindingRobotCtrl::processError(int32_t err) { ZLOGE(TAG, "processError: %d", err); }

class myexception : public exception {
  virtual const char* what() const throw() { return "My exception happened"; }
};

class WidthDetector {
  ZGPIO   g_detectGpio;
  int32_t g_nowpos    = 0;
  int32_t g_nowdpos   = 0;
  bool    g_isrunning = false;

  ZThread m_detect_thread;
  int32_t m_lastState = false;

  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;
    m_dm      = dm;
    g_detectGpio.initAsInput(PB13, ZGPIO::kMode_pullup, ZGPIO::kIRQ_noIrq, true);
    m_detect_thread.init("detect", 512);
    getDetectGPIOState();
  }
  int32_t getDetectGPIOState() {
    int32_t nowstate = g_detectGpio.getState();
    for (size_t i = 0; i < 5; i++) {
      if (g_detectGpio.getState() != nowstate) {
        return m_lastState;
      }
      osDelay(3);
    }
    m_lastState = nowstate;
    return m_lastState;
  }

  void wait_module_idle(int32_t moduleid) {
    zos_delay(100);
    int i = 0;
    while (true) {
      int32_t status = 0;
      int32_t ecode  = m_dm->module_get_status(moduleid, &status);
      if (ecode != 0) {
        break;
      };
      if (status == 0) {
        ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
        break;
      }
      if (status == 2) {
        break;
      };
      if (i % 30 == 0) {
        ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
      }
      i++;
      zos_delay(10);
    }
  }

  void m14_raoxiantance_move_to_reset() { m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 330, 0); }
  void start_run_back() {
    m_dm->motor_move_to(14, cfg->m14_raoxiantance_reset_pos, 30, 0);
    g_isrunning = true;
  }
  void start_run_forward_slow() {
    m_dm->motor_move_to(14, cfg->m14_raoxiantance_tance_zero_pos + 100, 30, 0);
    g_isrunning = true;
  }
  void start_run_forward() {
    m_dm->motor_move_to(14, cfg->m14_raoxiantance_tance_zero_pos + 100, 330, 0);
    g_isrunning = true;
  }
  void stop_run() {
    m_dm->module_stop(14);
    g_isrunning = false;
  }

  void startDetect(bool forward = false) {
    m_detect_thread.stop();
    m14_raoxiantance_move_to_reset();
    wait_module_idle(14);

    int32_t enterticket = zos_get_tick();

    start_run_forward();
    while (true) {
      if (g_isrunning && getDetectGPIOState() != 0) {
        stop_run();
        break;
      }

      if (zos_haspassedms(enterticket) > 3000) {
        ZLOGE(TAG, "start_probe_bullet_pos timeout");
        /**
         * @brief TODO add exception here
         *
         */
        stop_run();
        break;
      }
      zos_delay(10);
    }

    start_run_back();
    while (true) {
      if (g_isrunning && getDetectGPIOState() == 0) {
        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);
        break;
      }
      zos_delay(3);
    }

    m_detect_thread.start([this, forward]() {
      if (!forward) {
        while (!m_detect_thread.getExitFlag()) {
          if (g_isrunning && getDetectGPIOState() == 0) {
            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);
          } else if (!g_isrunning && getDetectGPIOState() != 0) {
            start_run_back();
          }
          zos_delay(3);
        }
      } else {
        while (!m_detect_thread.getExitFlag()) {
          if (g_isrunning && getDetectGPIOState() != 0) {
            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);
          } else if (!g_isrunning && getDetectGPIOState() == 0) {
            start_run_forward_slow();
          }
          zos_delay(3);
        }
      }
    });
  }

  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;

int32_t IntelligentWindingRobotCtrl::initialize(APPDM* dm, ICmdParser* cmdparse) {
  m_dm       = dm;
  m_cmdparse = cmdparse;
  m_work_thread.init("work", 512);
  // m_detect_thread.init("detect", 512);
  initialize_device();
  regcb();
  g_widthDetector.init(&cfg, dm);
  return 0;
}

int32_t IntelligentWindingRobotCtrl::initialize_device() {
  cfg.xy_platform_takeline_clip00_pos_x = 1833;
  cfg.xy_platform_takeline_clip00_pos_y = 16819;
  cfg.xy_platform_takeline_clipXX_pos_x = 52381;
  cfg.xy_platform_takeline_clipXX_pos_y = 32087;
  cfg.clip_line                         = 12;
  cfg.clip_each_line_num                = 5;

  cfg.m11_arm_jiaxian_reset_pos       = 2619;
  cfg.m11_arm_jiaxian_clamp_torque    = 330;
  cfg.m11_arm_jiaxian_clamp_direction = -1;

  cfg.m12_jiaxian_reset_pos       = 2600;
  cfg.m12_jiaxian_clamp_direction = -1;
  cfg.m12_jiaxian_clamp_torque    = 330;

  cfg.m13_yaxian_forward_reset_pos  = 1015;
  cfg.m13_yaxian_backward_reset_pos = 2885;
  cfg.m13_jiaxian_clamp_direction   = -1;
  cfg.m13_jiaxian_clamp_torque      = 600;

  cfg.m14_raoxiantance_reset_pos      = 2047;
  cfg.m14_raoxiantance_tance_zero_pos = 2829;

  cfg.m15_paifei_reset_pos       = 2046;
  cfg.m15_paifei_press_direction = 1;
  cfg.m15_paifei_press_torque    = 330;

  cfg.m16_xianlajin_reset_pos               = 2047;
  cfg.m16_xianlajin_cook_line_end_ready_pos = 1959;
  cfg.m16_xianlajin_tight_line_pos          = 1966;
  cfg.m16_xianlajin_winding_low_pos         = 1885;
  cfg.m16_xianlajin_winding_high_pos        = 1815;
  cfg.m16_xianlajin_line_entry_pos          = 1800;
  cfg.m16_xianlajin_cook_line_end_low_pos   = 1820;
  cfg.m16_xianlajin_cook_line_end_high_pos  = 1741;

  cfg.m21_arm_hook_claws_full_pos = 2558;
  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 = -599;
  cfg.xy_platform_remove_line_pos_y = 4861;

  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 = 6700;
  //
  cfg.z_axis_cook_bullet_pos    = 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_remove_line_high   = 3567;

  cfg.m2_zerooff = 1110;
  return 0;
}

void IntelligentWindingRobotCtrl::regcb() {
  // device_reset
  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("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) {
    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(); });
  m_cmdparse->regCMD("step_winding_lineend", "()", 0, [this](PARAM) { return step_winding_lineend(); });
  m_cmdparse->regCMD("step_winding_lineend_prepare", "()", 1, [this](PARAM) { return step_winding_lineend_prepare(atoi(paraV[0])); });
  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])); });

  // m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); });

  m_cmdparse->regCMD("m11_arm_jiaxian_move_to_reset_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_reset_pos(); });
  m_cmdparse->regCMD("m11_arm_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_clamp_pos(); });
  m_cmdparse->regCMD("m12_jiaxian_move_to_open_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_open_pos(); });
  m_cmdparse->regCMD("m12_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_clamp_pos(); });
  m_cmdparse->regCMD("m13_yaxian_move_to_reset_forward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_forward(); });
  m_cmdparse->regCMD("m13_yaxian_move_to_reset_backward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_backward(); });
  m_cmdparse->regCMD("m13_yaxian_press_clip", "()", 0, [this](PARAM) { return m13_yaxian_press_clip(); });
  m_cmdparse->regCMD("m14_raoxiantance_move_to_reset", "()", 0, [this](PARAM) { return m14_raoxiantance_move_to_reset(); });
  m_cmdparse->regCMD("m15_paifei_moveto_reset", "()", 0, [this](PARAM) { return m15_paifei_moveto_reset(); });
  m_cmdparse->regCMD("m15_paifei_moveto_press", "()", 0, [this](PARAM) { return m15_paifei_moveto_press(); });
  m_cmdparse->regCMD("m16_xianlajin_move_to_reset", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_reset(); });
  m_cmdparse->regCMD("m16_xianlajin_move_to_tight_line_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_tight_line_pos(); });
  m_cmdparse->regCMD("m16_xianlajin_move_to_winding_low_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_winding_low_pos(); });
  m_cmdparse->regCMD("m16_xianlajin_move_to_winding_up_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_winding_up_pos(); });
  m_cmdparse->regCMD("m16_xianlajin_move_to_line_entry_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_line_entry_pos(); });
  m_cmdparse->regCMD("m16_xianlajin_move_to_cook_lineend_high_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_cook_lineend_high_pos(); });
  m_cmdparse->regCMD("m16_xianlajin_move_to_cook_lineend_low_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_cook_lineend_low_pos(); });
  m_cmdparse->regCMD("m16_xianlajin_move_to_cook_lineend_ready_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_cook_lineend_ready_pos(); });

  m_cmdparse->regCMD("m21_arm_hook_claws_reset", "()", 0, [this](PARAM) { return m21_arm_hook_claws_reset(); });
  m_cmdparse->regCMD("m21_arm_hook_claws_move_to_half_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_half_pos(); });
  m_cmdparse->regCMD("m21_arm_hook_claws_move_to_full_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_full_pos(); });

  m_cmdparse->regCMD("m22_scissors_move_reset_pos", "()", 0, [this](PARAM) { return m22_scissors_move_reset_pos(); });
  m_cmdparse->regCMD("m22_scissors_cut", "()", 0, [this](PARAM) { return m22_scissors_cut(); });
  m_cmdparse->regCMD("m23_laxian_motor_move_to_reset_pos", "()", 0, [this](PARAM) { return m23_laxian_motor_move_to_reset_pos(); });
  m_cmdparse->regCMD("m23_laxian_motor_move_to_tight_line_pos", "()", 0, [this](PARAM) { return m23_laxian_motor_move_to_tight_line_pos(); });
  m_cmdparse->regCMD("start_probe_bullet_pos", "()", 0, [this](PARAM) { return start_probe_bullet_pos(); });
  m_cmdparse->regCMD("start_probe_bullet_pos_forward", "()", 0, [this](PARAM) { return start_probe_bullet_pos_forward(); });
}

void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) {
  zos_delay(100);
  int i = 0;
  while (true) {
    int32_t status = 0;
    int32_t ecode  = m_dm->module_get_status(moduleid, &status);
    if (ecode != 0) {
      processError(ecode);
      break;
    };
    if (status == 0) {
      ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
      break;
    }
    if (status == 2) {
      processError(err::kfail);
      break;
    };
    if (i % 30 == 0) {
      ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
    }
    i++;
    zos_delay(100);
  }
}

void IntelligentWindingRobotCtrl::wait_modules_idle(void* mark, ...) {
  int32_t moduleid = 0;
  va_list args;
  va_start(args, mark);
  moduleid = va_arg(args, int32_t);
  while (moduleid > 0) {
    wait_module_idle(moduleid);
    moduleid = va_arg(args, int32_t);
  }
  va_end(args);
}

int32_t IntelligentWindingRobotCtrl::disable_all_module() {
  m_dm->motor_enable(2, 0);
  m_dm->xymotor_enable(3, 0);
  m_dm->motor_enable(4, 0);
  m_dm->motor_enable(11, 0);
  m_dm->motor_enable(12, 0);
  m_dm->motor_enable(13, 0);
  m_dm->motor_enable(14, 0);
  m_dm->motor_enable(15, 0);
  m_dm->motor_enable(16, 0);

  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() {
  m_dm->motor_enable(2, 1);
  m_dm->xymotor_enable(3, 1);
  m_dm->motor_enable(4, 1);
  m_dm->motor_enable(11, 1);
  m_dm->motor_enable(12, 1);
  m_dm->motor_enable(13, 1);
  m_dm->motor_enable(14, 1);
  m_dm->motor_enable(15, 1);
  m_dm->motor_enable(16, 1);

  m_dm->motor_enable(21, 1);
  m_dm->motor_enable(22, 1);
  m_dm->motor_enable(23, 1);
  return 0;
}

#define WAIT_MODULES_IDLE(...) \
  { wait_modules_idle(NULL, __VA_ARGS__, NULL); }

int32_t IntelligentWindingRobotCtrl::device_reset() {
  // Z�Ḵλ
  /**
   * @
   *
   * 1. Z�ἰ��������λ
   * 2. ����M13�⣬����������λ
   * 3. M13��λ,�ȴ�һ��ʱ�䣬�鿴M13�Ƿ���λ�ɹ�
   * 4. ��е�۸�λ
   */
  m_dm->module_stop(2);
  enable_all_module();
  m4_zreset();
  m11_arm_jiaxian_move_to_reset_pos();
  WAIT_MODULES_IDLE(4, 11);

  m12_jiaxian_move_to_clamp_pos();
  m14_raoxiantance_move_to_reset();
  m15_paifei_moveto_reset();
  m16_xianlajin_move_to_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_reset_pos();
  wait_module_idle(22);
  /**
   * @brief �Ż���λ�߼�����ʱ��λʧ�ܣ�����
   */
  m13_yaxian_move_to_reset_backward();
  wait_module_idle(13);

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

bool IntelligentWindingRobotCtrl::is_hasbullet() { return g_widthDetector.isHasBullet(); }

void IntelligentWindingRobotCtrl::start_probe_bullet_pos() { g_widthDetector.startDetect(); }
void IntelligentWindingRobotCtrl::start_probe_bullet_pos_forward() { g_widthDetector.startDetect(true); }
void IntelligentWindingRobotCtrl::stop_probe_bullet_pos() { g_widthDetector.stopDetect(); }

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_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); }
int32_t IntelligentWindingRobotCtrl::m13_yaxian_press_clip() { return m_dm->motor_rotate_with_torque(13, cfg.m13_jiaxian_clamp_direction, cfg.m13_jiaxian_clamp_torque); }
int32_t IntelligentWindingRobotCtrl::m14_raoxiantance_move_to_reset() { return m_dm->motor_move_to_torque(14, cfg.m14_raoxiantance_reset_pos, 330, 0); }
int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_reset() { return m_dm->motor_move_to_torque(15, cfg.m15_paifei_reset_pos, 330, 0); }
int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_press() { return m_dm->motor_rotate_with_torque(15, cfg.m15_paifei_press_direction, cfg.m15_paifei_press_torque); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_reset() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_reset_pos, 300, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_tight_line_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_tight_line_pos, 300, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_winding_low_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_winding_low_pos, 100, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_winding_up_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_winding_high_pos, 20, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_line_entry_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_line_entry_pos, 20, 0); }

int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_cook_lineend_high_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_cook_line_end_high_pos, 300, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_cook_lineend_low_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_cook_line_end_low_pos, 300, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_cook_lineend_ready_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_cook_line_end_ready_pos, 300, 0); }

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() {
  m_dm->motor_move_to_zero_backward(22, 0, 0, 0, 0);
  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::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); }
int32_t IntelligentWindingRobotCtrl::m4_zmove_to(int32_t pos) {
  ZLOGI(TAG, "zmove_to %d", pos);
  int32_t nowpos = 0;
  m_dm->motor_read_pos(ZMOTOR_ID, &nowpos);
  if (pos >= nowpos) {
    m_dm->motor_move_to(ZMOTOR_ID, pos, 450, 600);
  } else {
    m_dm->motor_move_to(ZMOTOR_ID, pos, 1000, 600);
  }
  if (pos == 0) {
    m4_zreset();
  }
}

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) {
  } else if (take_bullet_line_acktion == kKeepLine) {
  }
  WAIT_MODULES_IDLE(11);
  if (zpos == kCookPos) {
    m4_zmove_to(cfg.z_axis_cook_bullet_pos);
  } else if (zpos == kBulletBulletHolderPos) {
    m4_zmove_to(cfg.z_axis_take_clip_pos);
  }
  WAIT_MODULES_IDLE(4);

  if (take_bullet_line_acktion == kTakeLine) {
    m11_arm_jiaxian_move_to_clamp_pos();
  } else if (take_bullet_line_acktion == kReleaseLine) {
    m11_arm_jiaxian_move_to_reset_pos();
  } else if (take_bullet_line_acktion == kKeepLine) {
  }

  if (take_bullet_acktion == kTakeBullet) {
    m21_arm_hook_claws_move_to_full_pos();
  } else if (take_bullet_acktion == kTakeBackBullet) {
    m21_arm_hook_claws_reset();
  } else if (take_bullet_acktion == kTakeBulletCase) {
    m21_arm_hook_claws_move_to_half_pos();
  } else if (take_bullet_acktion == kTakeBackBulletCase) {
    m21_arm_hook_claws_reset();
  }
  WAIT_MODULES_IDLE(11, 21);

  if (bottomoperation) {
    bottomoperation();
  }

  m4_zmove_to(0);
  WAIT_MODULES_IDLE(4);
}

int32_t IntelligentWindingRobotCtrl::step_take_bullet(int32_t bulletindex) {
  m4_zreset();
  WAIT_MODULES_IDLE(4);

  xy_reset();
  WAIT_MODULES_IDLE(3);

  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::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, bool& hasbullet) {
  m15_paifei_moveto_reset();
  m13_yaxian_move_to_reset_backward();
  stop_probe_bullet_pos();
  WAIT_MODULES_IDLE(13, 15);

  // �ƶ���COOKλ��
  xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y);
  WAIT_MODULES_IDLE(3);

  // Z�ƶ��������ӵ�λ��
  m4_zmove_to(cfg.z_axis_cook_bullet_pos);
  WAIT_MODULES_IDLE(4);

  m21_arm_hook_claws_move_to_half_pos();
  WAIT_MODULES_IDLE(21);

  m4_zmove_to(0);  // Z������
  WAIT_MODULES_IDLE(4);

  start_probe_bullet_pos();

  /**
   * @brief
   *
   *        <------------------point1()
   * removeLinePos            ^
   *                          |2723 // 20mm
   *                          |
   *                        COOK()
   *
   *
   */
  if (is_hasbullet()) {
    xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y - 3623);
    xymove_to(cfg.xy_platform_remove_line_pos_x, cfg.xy_platform_cook_bullet_pos_y - 3623);
    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_remove_line_high);
    WAIT_MODULES_IDLE(4);
    // ���߶���
    m15_paifei_moveto_press();
    // m4_zmove_to(cfg.z_axis_take_clip_pos);
    // WAIT_MODULES_IDLE(4);
    m11_arm_jiaxian_move_to_reset_pos();
    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);
    hasbullet = true;
  } else {
    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);
  }
}

int32_t IntelligentWindingRobotCtrl::step_remove_line() {
  ZLOGI(TAG, "step_remove_line");
  start_probe_bullet_pos_forward();
  m15_paifei_moveto_press();
  WAIT_MODULES_IDLE(15);

  m_dm->motor_rotate_acctime(2, -1, 1000, 1000);
  int overtime = 90;
  for (int i = 0; i < overtime; i++) {
    osDelay(1000);
    if (g_widthDetector.isRemoveLineEnd()) {
      ZLOGI(TAG, "detect remove line end");
      break;
    }
    if (i == overtime - 1) {
      ZLOGI(TAG, "detect remove line end timeout");
    }
  }
  osDelay(5000);
  m_dm->module_stop(2);
  // m15_paifei_moveto_reset();
  m15_paifei_moveto_reset();
  stop_probe_bullet_pos();
  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);
  m16_xianlajin_move_to_line_entry_pos();
  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);

  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);
  m13_yaxian_press_clip();
  WAIT_MODULES_IDLE(13);
  m11_arm_jiaxian_move_to_reset_pos();
  WAIT_MODULES_IDLE(11);
  m16_xianlajin_move_to_tight_line_pos();
  m4_zmove_to(0);
  osDelay(500);
  WAIT_MODULES_IDLE(4);
  xymove_to(0, 0);
  WAIT_MODULES_IDLE(16);

  // WAIT_MODULES_IDLE(13);
}

int32_t IntelligentWindingRobotCtrl::step_winding() {
  start_probe_bullet_pos();
  m_dm->motor_rotate_acctime(2, 1, 1000, 10000);
  osDelay(1000);
  m23_laxian_motor_move_to_reset_pos();
  for (size_t i = 0; i < 80; i++) {  // TODO:60�����Ǹ��������ܻ�����������ǰ�ͷš�
    m16_xianlajin_move_to_winding_low_pos();
    WAIT_MODULES_IDLE(16);
    m16_xianlajin_move_to_winding_up_pos();
    WAIT_MODULES_IDLE(16);
    if (g_widthDetector.isFull()) {
      break;
    }
  }
  m_dm->module_stop(2);
  stop_probe_bullet_pos();
  ZLOGI(TAG, "step_winding end....");
  return 0;
}

int32_t IntelligentWindingRobotCtrl::step_winding_lineend_prepare(int bulletindex) {
  /**
   * @brief
   *
   *  0. ��������
   *  1. �ƶ���BulletHolderPos
   *  2. ȡ����
   *  3. �ƶ���CookPos
   *  4. �ŵ���
   */
  m13_yaxian_move_to_reset_backward();
  m_dm->motor_move_to_zero_forward(2, 2, 2, 0, 0);
  wait_module_idle(2);
  m_dm->motor_move_to_acctime(2, cfg.m2_zerooff, 30, 100);
  wait_module_idle(2);
  WAIT_MODULES_IDLE(13);

  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();
  substep_zaxis_do_bullet_action(kCookPos, kTakeBackBullet, kReleaseLine);
  return 0;
}

int32_t IntelligentWindingRobotCtrl::step_winding_lineend() {
  /**
   * @brief
   *
   *  1. ̧����������λ
   *  2. ��������ת
   *  3. ��������
   *  4. ̧�������ƶ�����λ
   *  5. ��������
   *  6. ̧�������ƶ�����λ
   *  7. ��������
   *  8. ̧�������ƶ�����λ
   *  9. ��������
   *  10.̧�������ƶ�����λ
   *  11.��������
   */

#if 0
eq20_move_by 1 840 30 100
mini_servo_move_to 6 1550 600 500
eq20_move_by 1 -840 30 100


mini_servo_move_to 6 1750 600 500
eq20_move_to 1 280 30 100
mini_servo_move_to 6 1600 600 500

eq20_move_to 1 -560 30 100
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();

  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);                                                      //
  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();                             //
  WAIT_MODULES_IDLE(16);                                                     //
  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 - 1600, velocity, acctime);  //
  WAIT_MODULES_IDLE(2);                                                      //
  m16_xianlajin_move_to_cook_lineend_high_pos();                             //
  WAIT_MODULES_IDLE(16);                                                     //
  m_dm->motor_move_to_acctime(2, cfg.m2_zerooff, velocity, acctime);         //
  WAIT_MODULES_IDLE(2);                                                      //
  m16_xianlajin_move_to_cook_lineend_low_pos();                              //
}

int32_t IntelligentWindingRobotCtrl::step_winding_take_bullet_from_cooking_to_origin_pos(int bulletindex) {
  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(22);

  xymove_to_bullet_pos(bulletindex);
  WAIT_MODULES_IDLE(3);

  substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBackBullet, kReleaseLine);
}
/**
 * @brief
 */

int32_t IntelligentWindingRobotCtrl::xy_get_point(int32_t clip_index, int32_t& x, int32_t& y) {
  int clip_x_off = clip_index / cfg.clip_each_line_num;
  int clip_y_off = clip_index % cfg.clip_each_line_num;

  float eachx = (cfg.xy_platform_takeline_clipXX_pos_x - cfg.xy_platform_takeline_clip00_pos_x) / (cfg.clip_line - 1);
  float eachy = (cfg.xy_platform_takeline_clipXX_pos_y - cfg.xy_platform_takeline_clip00_pos_y) / (cfg.clip_each_line_num - 1);

  x = cfg.xy_platform_takeline_clip00_pos_x + eachx * clip_x_off;
  y = cfg.xy_platform_takeline_clip00_pos_y + eachy * clip_y_off;

  return 0;
}

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;
  int32_t nowy;
  m_dm->xymotor_read_pos(XYRobot_ID, &nowx, &nowy);
  if (nowx == 0 && nowy == 0) {
    xy_reset();
  }
  WAIT_MODULES_IDLE(3);

  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() {
  ZLOGI(TAG, "xy_reset");
  m_dm->xymotor_move_to_zero(XYRobot_ID);
  return 0;
}
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");
      m_iswinding        = true;
      m_nowwinding_index = 0;
      device_reset();
      bool hasbullet = false;
      for (size_t i = 0; i < 5 * 12; i++) {
        m_nowwinding_index = i + 1;
        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 : ����һ�����쳣�汾
    m_iswinding = false;
  });
  return 0;
}
int32_t IntelligentWindingRobotCtrl::stop_winding() {
  m_work_thread.stop();
  m_iswinding = false;
  return 0;
}