add some cfg cmd

components/errorcode/errorcode.hpp

@@ -0,0 +1,72 @@
+#pragma once
+
+namespace iflytop {
+namespace err {
+using namespace std;
+
+#define ERROR_CODE(errortype, suberrorcode) (errortype + suberrorcode)
+
+typedef enum {
+  ksucc = ERROR_CODE(0, 0),
+  kfail = ERROR_CODE(0, 1),
+
+  /**
+   * @brief 通信错误
+   */
+  kce_overtime       = ERROR_CODE(1000, 0),
+  kce_noack          = ERROR_CODE(1000, 1),
+  kce_errorack       = ERROR_CODE(1000, 2),
+  kce_device_offline = ERROR_CODE(1000, 3),
+  kce_parse_json_err = ERROR_CODE(1000, 4),
+
+  /**
+   * @brief 数据库错误
+   */
+  kdbe_user_not_exist  = ERROR_CODE(2000, 0),
+  kdbe_catch_exception = ERROR_CODE(2000, 1),
+
+  /**
+   * @brief 硬件错误
+   */
+  kharde_unfound = ERROR_CODE(3000, 0),
+  /**
+   * @brief 程序运行错误
+   */
+  kre_catch_exception = ERROR_CODE(4000, 0),
+  /**
+   * @brief 应用交互基本错误
+   */
+  kinteraction_error_passwd_error   = ERROR_CODE(5000, 0),  // 密码错误
+  kinteraction_error_user_not_exist = ERROR_CODE(5000, 1),  // 用户不存在
+  /**
+   * @brief 系统错误
+   */
+  ksys_error             = ERROR_CODE(6000, 0),
+  ksys_create_file_error = ERROR_CODE(6000, 1),
+  ksys_create_dir_error  = ERROR_CODE(6000, 2),
+  ksys_open_file_error   = ERROR_CODE(6000, 3),
+  ksys_open_dir_error    = ERROR_CODE(6000, 4),
+  ksys_read_file_error   = ERROR_CODE(6000, 5),
+  ksys_write_file_error  = ERROR_CODE(6000, 6),
+  ksys_close_file_error  = ERROR_CODE(6000, 7),
+  ksys_close_dir_error   = ERROR_CODE(6000, 8),
+  ksys_delete_file_error = ERROR_CODE(6000, 9),
+  ksys_delete_dir_error  = ERROR_CODE(6000, 10),
+  ksys_copy_file_error   = ERROR_CODE(6000, 11),
+
+  /**
+   * @brief 通用，但未归类错误
+   * 50000
+   */
+
+  kcommon_error_device_not_zero        = ERROR_CODE(50000, 0),  // 设备未归零
+  kcommon_error_over_temperature       = ERROR_CODE(50000, 1),  // 过温
+  kcommon_error_over_voltage           = ERROR_CODE(50000, 2),  // 过压
+  kcommon_error_param_out_of_range     = ERROR_CODE(50000, 3),  // 参数超出范围
+  kcommon_error_not_found_zero_point   = ERROR_CODE(50000, 4),  // 未找到零点
+  kcommon_error_not_found_x_zero_point = ERROR_CODE(50000, 5),  // 未找到零点
+  kcommon_error_not_found_y_zero_point = ERROR_CODE(50000, 6),  // 未找到零点
+
+} error_t;
+}  // namespace err
+}  // namespace iflytop

components/tmc/basic/tmc_ic_interface.hpp

@@ -58,8 +58,9 @@ class IStepperMotor {
   virtual bool isStoped()      = 0;
 
   virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) = 0;
+  virtual void setDistanceK(float distanceK)                                   = 0;
 
-  virtual void enable(bool enable)  = 0;
+  virtual void enable(bool enable) = 0;
 };
 
 }  // namespace iflytop

components/tmc/ic/ztmc4361A.cpp

@@ -141,6 +141,7 @@ void TMC4361A::setAcceleration(float accelerationpps2) {
    *      AMAX [pps2] = AMAX / 237 ?? fCLK^2
    */
 
+  accelerationpps2 *= m_distanceK;
   int32_t acc = (int32_t)accelerationpps2;
   writeInt(TMC4361A_AMAX, acc << 2);
 }
@@ -155,7 +156,7 @@ void TMC4361A::setDeceleration(float accelerationpps2) {
    *      a[?v per clk_cycle]= AMAX / 2^37
    *      AMAX [pps2] = AMAX / 237 ?? fCLK^2
    */
-
+  accelerationpps2 *= m_distanceK;
   int32_t acc = (int32_t)accelerationpps2;
   writeInt(TMC4361A_DMAX, acc << 2);
 }
@@ -197,8 +198,11 @@ void TMC4361A::initialize(cfg_t *cfg) {
   zchip_clock_early_delayus(300 * 1000);
   zchip_clock_early_delayus(300 * 1000);
 
-  driverIC_setIHOLD_IRUN(1, 3, 0);  
+  driverIC_setIHOLD_IRUN(1, 3, 0);
 }
+
+void TMC4361A::setDistanceK(float distanceK) { m_distanceK = distanceK; }
+
 uint8_t TMC4361A::reset() {
   // Pulse the low-active hardware reset pin
   stop();
@@ -243,13 +247,13 @@ uint8_t TMC4361A::reset() {
 }
 uint8_t TMC4361A::restore() { return 1; }
 
-int32_t TMC4361A::getXACTUAL() { return readInt(TMC4361A_XACTUAL); }
-void    TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, value); }
-int32_t TMC4361A::getVACTUAL() { return readInt(TMC4361A_VACTUAL); }
-int32_t TMC4361A::getXTARGET() { return readInt(TMC4361A_X_TARGET); }
-int32_t TMC4361A::getENC_POS() { return readInt(TMC4361A_ENC_POS); }
-void    TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, value); }
-int32_t TMC4361A::getENC_POS_DEV() { return readInt(TMC4361A_ENC_POS_DEV_RD); }
+int32_t TMC4361A::getXACTUAL() { return readInt(TMC4361A_XACTUAL) / m_distanceK; }
+void    TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, value * m_distanceK); }
+int32_t TMC4361A::getVACTUAL() { return readInt(TMC4361A_VACTUAL) / m_distanceK; }
+int32_t TMC4361A::getXTARGET() { return readInt(TMC4361A_X_TARGET) / m_distanceK; }
+int32_t TMC4361A::getENC_POS() { return readInt(TMC4361A_ENC_POS) / m_distanceK; }
+void    TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, value * m_distanceK); }
+int32_t TMC4361A::getENC_POS_DEV() { return readInt(TMC4361A_ENC_POS_DEV_RD) / m_distanceK; }
 void    TMC4361A::enableIC(bool enable) {
   SET_PIN(m_ennPin, !enable);
   SET_PIN(m_driverIC_ennPin, !enable);
@@ -262,12 +266,16 @@ int32_t tmc4361A_discardVelocityDecimals(int32_t value) {
   return value << 8;
 }
 void TMC4361A::rotate(int32_t velocity) {
+  velocity *= m_distanceK;
   m_motor_mode = kvelmode;
   PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 0);
   writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocity));
 }
 void TMC4361A::stop() { rotate(0); }
 void TMC4361A::moveTo(int32_t position, uint32_t velocityMax) {
+  position *= m_distanceK;
+  velocityMax *= m_distanceK;
+
   m_motor_mode = kposmode;
   PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 1);
   writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocityMax));
@@ -300,6 +308,7 @@ uint32_t TMC4361A::haspassedms(uint32_t now, uint32_t last) {
 }
 bool TMC4361A::isReachTarget() {
   uint32_t value = readInt(TMC4361A_STATUS);
+  // printf("TMC4361A_STATUS:%08x\n", value);
   if (m_motor_mode == kposmode) {
     return (value & TMC4361A_TARGET_REACHED_F_MASK) > 0;
   } else {

components/tmc/ic/ztmc4361A.hpp

@@ -74,6 +74,8 @@ class TMC4361A : public IStepperMotor {
 
   motor_mode_t m_motor_mode = kvelmode;
 
+  float m_distanceK = 1.0;
+
  public:
   TMC4361A(/* args */);
 
@@ -114,6 +116,8 @@ class TMC4361A : public IStepperMotor {
   virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_setIHOLD_IRUN(ihold, irun, iholddelay); }
   virtual void setMotorShaft(bool reverse) { driverIC_setMotorShaft(reverse); }
 
+  virtual void setDistanceK(float distanceK);
+
   /*******************************************************************************
    *                              Çý¶¯Æ÷³õʼ»¯·½·¨                               *
    *******************************************************************************/

components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp

@@ -1,4 +1,6 @@
 #include "xy_robot_ctrl_module.hpp"
+
+#include "sdk/components/errorcode/errorcode.hpp"
 using namespace iflytop;
 using namespace std;
 #define TAG "XYRobotCtrlModule"
@@ -7,91 +9,295 @@ void XYRobotCtrlModule::initialize(IStepperMotor* stepM1,  //
                                    ZGPIO*         Xgpio,   //
                                    ZGPIO*         Ygpio,   //
                                    float          distanceK) {
-  m_stepM1    = stepM1;
-  m_stepM2    = stepM2;
-  m_Xgpio     = Xgpio;
-  m_Ygpio     = Ygpio;
-  m_distanceK = distanceK;
+  m_stepM1 = stepM1;
+  m_stepM2 = stepM2;
+  m_Xgpio  = Xgpio;
+  m_Ygpio  = Ygpio;
+  ZASSERT(m_stepM1);
+  ZASSERT(m_stepM2);
+  ZASSERT(m_Xgpio);
+  ZASSERT(m_Ygpio);
+  m_stepM1->setDistanceK(distanceK);
+  m_stepM2->setDistanceK(distanceK);
+  m_lock.init();
+  m_statu_lock.init();
   m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal);
 }
 
+void XYRobotCtrlModule::dumpcfg(const char* tag) {
+  ZLOGI(TAG, "======dumpcfg:%s=========", tag);
+  ZLOGI(TAG, "= m_cfg_acc                   :%d", m_cfg_acc);
+  ZLOGI(TAG, "= m_cfg_dec                   :%d", m_cfg_dec);
+  ZLOGI(TAG, "= m_cfg_velocity              :%d", m_cfg_velocity);
+  ZLOGI(TAG, "= m_cfg_runtozero_maxX        :%d", m_cfg_runtozero_maxX);
+  ZLOGI(TAG, "= m_cfg_runtozero_maxY        :%d", m_cfg_runtozero_maxY);
+  ZLOGI(TAG, "= m_cfg_runtozero_speed       :%d", m_cfg_runtozero_speed);
+  ZLOGI(TAG, "= m_cfg_runtozero_dec         :%d", m_cfg_runtozero_dec);
+  ZLOGI(TAG, "=");
+}
+int32_t XYRobotCtrlModule::set_zero_robottype(RobotType_t robot_type) {
+  m_robot_type = robot_type;
+  return 0;
+}
+int32_t XYRobotCtrlModule::set_x_shaft(bool x_shaft) {
+  m_x_shaft = x_shaft;
+  return 0;
+}
+int32_t XYRobotCtrlModule::set_y_shaft(bool y_shaft) {
+  m_y_shaft = y_shaft;
+  return 0;
+}
+
 int32_t XYRobotCtrlModule::enable(bool venable) {
+  zlock_guard lock(m_lock);
+
   ZLOGI(TAG, "enable:%d", venable);
   m_stepM1->enable(venable);
   m_stepM2->enable(venable);
   return 0;
 }
 
-int32_t XYRobotCtrlModule::getMotor1Pos() { return m_stepM1->getXACTUAL() * m_distanceK; }
-int32_t XYRobotCtrlModule::getMotor2Pos() { return m_stepM2->getXACTUAL() * m_distanceK; }
-
-void XYRobotCtrlModule::getNowPos(int32_t& x, int32_t& y) {
-  // y=M1+M2;
-  // X=m1-m2;
-  x = getMotor1Pos() - getMotor2Pos();
-  y = getMotor1Pos() + getMotor2Pos();
-}
-
 int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y) {
+  zlock_guard lock(m_lock);
   ZLOGI(TAG, "move_to x:%d y:%d", x, y);
   int32_t m1pos, m2pos;
-  inverse_kinematics(x, y, m1pos, m2pos);
-
   m_thread.stop();
-  m_thread.start([this,m1pos,m2pos]() {
-    ZLOGI("XYRobotCtrlModule", "m1pos:%d m2pos:%d", m1pos, m2pos);
+  /**
+   * @TODO:添加限位检查
+   */
+  updateStatus(1);
+  m_thread.start([this, x, y]() {
+    _motor_move_to(x, y, m_cfg_velocity, m_cfg_acc, m_cfg_dec);
+    while (!_motor_is_reach_target()) {
+      if (m_thread.getExitFlag()) break;
+      vTaskDelay(10);
+    }
+    _motor_stop();
+    updateStatus(0);
   });
   return 0;
 }
+
 int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy) {
+  zlock_guard lock(m_lock);
   ZLOGI(TAG, "move_by dx:%d dy:%d", dx, dy);
-  return 0;
+  int32_t now_x, now_y;
+  getnowpos(now_x, now_y);
+  return move_to(now_x + dx, now_y + dy);
 }
+
 int32_t XYRobotCtrlModule::move_to_zero() {
+  zlock_guard lock(m_lock);
   ZLOGI(TAG, "move_to_zero");
+
+  updateStatus(1);
+  m_thread.stop();
+  m_thread.start([this]() {
+    int32_t erro = exec_move_to_zero_task();
+    if (erro != 0) {
+      ZLOGI(TAG, "exec_move_to_zero_task failed:%d", erro);
+      updateStatus(0);
+      return;
+    }
+    m_stepM1->setXACTUAL(0);
+    m_stepM2->setXACTUAL(0);
+    updateStatus(0);
+    return;
+  });
+
   return 0;
 }
 int32_t XYRobotCtrlModule::move_to_with_calibrate(int32_t x, int32_t y) {
+  zlock_guard lock(m_lock);
   ZLOGI(TAG, "move_to_with_calibrate x:%d y:%d", x, y);
   return 0;
 }
 int32_t XYRobotCtrlModule::stop() {
+  zlock_guard lock(m_lock);
   ZLOGI(TAG, "stop");
+  m_thread.stop();
   return 0;
 }
-
 uint8_t XYRobotCtrlModule::read_status(uint8_t* module_statu, int32_t* x, int32_t* y) { return 0; }
-
 int32_t XYRobotCtrlModule::set_acc(int32_t acc) {
+  zlock_guard lock(m_lock);
   ZLOGI(TAG, "set_acc acc:%d", acc);
+  m_cfg_acc = acc;
   return 0;
 }
 int32_t XYRobotCtrlModule::set_dec(int32_t dec) {
+  zlock_guard lock(m_lock);
   ZLOGI(TAG, "set_dec dec:%d", dec);
+  m_cfg_dec = dec;
   return 0;
 }
 int32_t XYRobotCtrlModule::set_speed(int32_t speed) {
+  zlock_guard lock(m_lock);
   ZLOGI(TAG, "set_speed speed:%d", speed);
+  m_cfg_velocity = speed;
+  return 0;
+}
+int32_t XYRobotCtrlModule::set_runtozero_maxX(int32_t maxX) {
+  zlock_guard lock(m_lock);
+  ZLOGI(TAG, "set_runtozero_maxX:%d", maxX);
+  m_cfg_runtozero_maxX = maxX;
+  return 0;
+}
+int32_t XYRobotCtrlModule::set_runtozero_maxY(int32_t maxY) {
+  zlock_guard lock(m_lock);
+  ZLOGI(TAG, "set_runtozero_maxY:%d", maxY);
+  m_cfg_runtozero_maxY = maxY;
+  return 0;
+}
+int32_t XYRobotCtrlModule::set_runtozero_speed(int32_t speed) {
+  zlock_guard lock(m_lock);
+  ZLOGI(TAG, "set_runtozero_speed:%d", speed);
+  m_cfg_runtozero_speed = speed;
+  return 0;
+}
+int32_t XYRobotCtrlModule::set_runtozero_dec(int32_t dec) {
+  zlock_guard lock(m_lock);
+  ZLOGI(TAG, "set_runtozero_dec:%d", dec);
+  m_cfg_runtozero_dec = dec;
+  return 0;
+}
+
+int32_t XYRobotCtrlModule::set_runtozero_leave_away_zero_maxXY(int32_t maxXY) {
+  zlock_guard lock(m_lock);
+  ZLOGI(TAG, "set_runtozero_leave_away_zero_maxXY:%d", maxXY);
+  m_cfg_runtozero_leave_away_zero_maxXY = maxXY;
+  return 0;
+}
+int32_t XYRobotCtrlModule::set_distance_scale(float distance_scale) {
+  zlock_guard lock(m_lock);
+  ZLOGI(TAG, "set_distance_scale:%f", distance_scale);
+  m_stepM1->setDistanceK(distance_scale);
+  m_stepM2->setDistanceK(distance_scale);
   return 0;
 }
 
 void XYRobotCtrlModule::loop() {}
 
-void XYRobotCtrlModule::forward_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y) {
-#if 1
-  x = m1pos - m2pos;
-  y = m1pos + m2pos;
-#else
-  x     = m1pos + m2pos;
-  y     = m1pos - m2pos;
-#endif
-}
-void XYRobotCtrlModule::inverse_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) {
-#if 1
-  m1pos = (x + y) / 2;
-  m2pos = (y - x) / 2;
-#else
-  m1pos = (x - y) / 2;
-  m2pos = (x + y) / 2;
-#endif
-}
+int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
+  /*******************************************************************************
+   *                                移动X轴到零点                                *
+   *******************************************************************************/
+  _motor_move_by(m_cfg_runtozero_maxX, 0, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec);
+  bool xreach_zero = false;
+  while (true) {
+    // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget());
+    if (m_Xgpio->getState()) {
+      xreach_zero = true;
+      _motor_stop(-1);
+      break;
+    }
+    vTaskDelay(1);
+  }
+
+  if (!xreach_zero) {
+    // 触发异常回调
+    ZLOGE(TAG, "x reach zero failed");
+    return err::kcommon_error_not_found_x_zero_point;
+  }
+  ZLOGI(TAG, "x reach zero");
+  /*******************************************************************************
+   *                                移动Y轴到零点                                *
+   *******************************************************************************/
+  _motor_move_by(0, m_cfg_runtozero_maxY, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec);
+  bool yreach_zero = false;
+  while (!_motor_is_reach_target()) {
+    // ZLOGI(TAG, "ygpio %d", m_Ygpio->getState());
+    if (m_Ygpio->getState()) {
+      yreach_zero = true;
+      _motor_stop(-1);
+      break;
+    }
+    vTaskDelay(1);
+  }
+  if (!yreach_zero) {
+    // 触发异常回调
+    ZLOGE(TAG, "y reach zero failed");
+    return err::kcommon_error_not_found_y_zero_point;
+  }
+  ZLOGI(TAG, "y reach zero");
+  ZLOGI(TAG, "move_to_zero success");
+  return 0;
+}
+
+void XYRobotCtrlModule::updateStatus(uint8_t status) {
+  zlock_guard lock(m_statu_lock);
+  m_status = status;
+}
+void XYRobotCtrlModule::getnowpos(int32_t& x, int32_t& y) {
+  int32_t m1pos, m2pos;
+  m1pos = m_stepM1->getXACTUAL();
+  m2pos = m_stepM1->getXACTUAL();
+  inverse_kinematics(m1pos, m2pos, x, y);
+}
+void XYRobotCtrlModule::_motor_move_to(int32_t x, int32_t y, int32_t maxv, int32_t acc, int32_t dec) {
+  ZLOGI(TAG, "_motor_move_to x:%d y:%d maxv:%d,acc:%d,dec:%d", x, y, maxv, acc, dec);
+  int32_t target_m1pos, target_m2pos;
+  forward_kinematics(x, y, target_m1pos, target_m2pos);
+  m_stepM1->setAcceleration(acc);
+  m_stepM2->setAcceleration(acc);
+
+  m_stepM1->setDeceleration(dec);
+  m_stepM2->setDeceleration(dec);
+
+  m_stepM1->moveTo(target_m1pos, maxv);
+  m_stepM2->moveTo(target_m2pos, maxv);
+}
+
+void XYRobotCtrlModule::_motor_move_by(int32_t dx, int32_t dy, int32_t maxv, int32_t acc, int32_t dec) {
+  // ZLOGI(TAG, "_motor_move_by dx:%d dy:%d", dx, dy);
+  ZLOGI(TAG, "_motor_move_by dx:%d dy:%d maxv:%d acc:%d dec:%d ", dx, dy, maxv, acc, dec);
+  int32_t t_x, t_y;
+  getnowpos(t_x, t_y);
+  t_x = t_x + dx;
+  t_y = t_y + dy;
+
+  int32_t target_m1pos, target_m2pos;
+  forward_kinematics(t_x, t_y, target_m1pos, target_m2pos);
+
+  m_stepM1->setAcceleration(acc);
+  m_stepM2->setAcceleration(acc);
+
+  m_stepM1->setDeceleration(dec);
+  m_stepM2->setDeceleration(dec);
+  ZLOGI(TAG, "_motor_move_by target_m1pos:%d target_m2pos:%d", target_m1pos, target_m2pos);
+  m_stepM1->moveTo(target_m1pos, maxv);
+  m_stepM2->moveTo(target_m2pos, maxv);
+}
+void XYRobotCtrlModule::_motor_stop(int32_t dec) {
+  if (dec > 0) {
+    m_stepM1->setDeceleration(dec);
+    m_stepM2->setDeceleration(dec);
+  }
+  m_stepM1->stop();
+  m_stepM2->stop();
+}
+void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y) {
+  if (m_robot_type == corexy) {
+    x = (m1pos - m2pos);
+    y = (m1pos + m2pos);
+  } else {
+    x = m1pos + m2pos;
+    y = m1pos - m2pos;
+  }
+  if (m_x_shaft) x = -x;
+  if (m_y_shaft) y = -y;
+}
+void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) {
+  if (m_x_shaft) x = -x;
+  if (m_y_shaft) y = -y;
+
+  if (m_robot_type == corexy) {
+    m1pos = ((x + y) / 2);
+    m2pos = ((y - x) / 2);
+  } else {
+    m1pos = (x - y) / 2;
+    m2pos = (x + y) / 2;
+  }
+}
+
+bool XYRobotCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget() && m_stepM2->isReachTarget(); }

components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp

@@ -6,6 +6,10 @@
 
 namespace iflytop {
 class XYRobotCtrlModule {
+ public:
+  typedef enum { hbot, corexy } RobotType_t;
+
+ private:
   IStepperMotor* m_stepM1;
   IStepperMotor* m_stepM2;
 
@@ -14,11 +18,31 @@ class XYRobotCtrlModule {
 
   ZThread m_thread;
 
-  float m_distanceK = 1.0f;
-
   int m_x = 0;
   int m_y = 0;
 
+  uint8_t m_status = 0;
+
+  int32_t m_zero_shift_x = 0;
+  int32_t m_zero_shift_y = 0;
+
+  zmutex m_lock;
+  zmutex m_statu_lock;
+
+  int32_t m_cfg_acc      = 300000;
+  int32_t m_cfg_dec      = 300000;
+  int32_t m_cfg_velocity = 300000;
+
+  int32_t m_cfg_runtozero_maxX                  = 5120000;
+  int32_t m_cfg_runtozero_maxY                  = 5120000;
+  int32_t m_cfg_runtozero_speed                 = 30000;
+  int32_t m_cfg_runtozero_dec                   = 300000;
+  int32_t m_cfg_runtozero_leave_away_zero_maxXY = 51200 * 5;
+
+  RobotType_t m_robot_type = corexy;
+  bool        m_x_shaft    = false;
+  bool        m_y_shaft    = false;
+
  public:
   void initialize(IStepperMotor* stepM1,  //
                   IStepperMotor* stepM2,  //
@@ -26,6 +50,22 @@ class XYRobotCtrlModule {
                   ZGPIO*         Ygpio,   //
                   float          distanceK);
 
+  int32_t set_acc(int32_t acc);
+  int32_t set_dec(int32_t dec);
+  int32_t set_speed(int32_t speed);
+  int32_t set_zero_robottype(RobotType_t robot_type);
+  int32_t set_x_shaft(bool x_shaft);
+  int32_t set_y_shaft(bool y_shaft);
+
+  int32_t set_runtozero_maxX(int32_t maxX);
+  int32_t set_runtozero_maxY(int32_t maxY);
+  int32_t set_runtozero_speed(int32_t speed);
+  int32_t set_runtozero_dec(int32_t dec);
+  int32_t set_runtozero_leave_away_zero_maxXY(int32_t maxXY);
+  int32_t set_distance_scale(float distance_scale);
+
+  void dumpcfg(const char* tag);
+
   int32_t enable(bool venable);
 
   int32_t move_to(int32_t x, int32_t y);
@@ -36,20 +76,22 @@ class XYRobotCtrlModule {
 
   uint8_t read_status(uint8_t* module_statu, int32_t* x, int32_t* y);
 
-  int32_t set_acc(int32_t acc);
-  int32_t set_dec(int32_t dec);
-  int32_t set_speed(int32_t speed);
-
   void loop();
 
  private:
-  int32_t getMotor1Pos();
-  int32_t getMotor2Pos();
+  void updateStatus(uint8_t status);
 
-  void getNowPos(int32_t& x, int32_t& y);
   void computeTargetMotorPos();
+  void getnowpos(int32_t& x, int32_t& y);
+
+  int32_t exec_move_to_zero_task();
+
+  void inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y);
+  void forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos);
 
-  void forward_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y);
-  void inverse_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos);
+  void _motor_move_to(int32_t dx, int32_t dy, int32_t maxv, int32_t acc, int32_t dec);
+  void _motor_move_by(int32_t dx, int32_t dy, int32_t maxv, int32_t acc, int32_t dec);
+  void _motor_stop(int32_t dec = -1);
+  bool _motor_is_reach_target();
 };
 }  // namespace iflytop

components/zcancmder/cmd/basic_bean.hpp

@@ -66,16 +66,27 @@ typedef enum {
   /*******************************************************************************
    *                                    1006                                     *
    *******************************************************************************/
-  kcmd_xy_robot_ctrl_enable                 = CMDID(1006, 0),
-  kcmd_xy_robot_ctrl_stop                   = CMDID(1006, 1),
-  kcmd_xy_robot_ctrl_move_to_zero           = CMDID(1006, 2),
-  kcmd_xy_robot_ctrl_move_to_with_calibrate = CMDID(1006, 3),
-  kcmd_xy_robot_ctrl_move_to                = CMDID(1006, 4),
-  kcmd_xy_robot_ctrl_move_by                = CMDID(1006, 5),
-  kcmd_xy_robot_ctrl_read_status            = CMDID(1006, 50),
-  kcmd_xy_robot_ctrl_set_acc                = CMDID(1006, 100),
-  kcmd_xy_robot_ctrl_set_dec                = CMDID(1006, 101),
-  kcmd_xy_robot_ctrl_set_speed              = CMDID(1006, 102),
+  kcmd_xy_robot_ctrl_enable                              = CMDID(1006, 0),
+  kcmd_xy_robot_ctrl_stop                                = CMDID(1006, 1),
+  kcmd_xy_robot_ctrl_move_to_zero                        = CMDID(1006, 2),
+  kcmd_xy_robot_ctrl_move_to_with_calibrate              = CMDID(1006, 3),
+  kcmd_xy_robot_ctrl_move_to                             = CMDID(1006, 4),
+  kcmd_xy_robot_ctrl_move_by                             = CMDID(1006, 5),
+  kcmd_xy_robot_ctrl_read_status                         = CMDID(1006, 50),
+  kcmd_xy_robot_ctrl_set_acc                             = CMDID(1006, 100),
+  kcmd_xy_robot_ctrl_set_dec                             = CMDID(1006, 101),
+  kcmd_xy_robot_ctrl_set_speed                           = CMDID(1006, 102),
+  kcmd_xy_robot_ctrl_set_x_shaft                         = CMDID(1006, 103),
+  kcmd_xy_robot_ctrl_set_y_shaft                         = CMDID(1006, 104),
+  kcmd_xy_robot_ctrl_set_zero_robottype                  = CMDID(1006, 105),
+  kcmd_xy_robot_ctrl_set_runtozero_maxX                  = CMDID(1006, 106),
+  kcmd_xy_robot_ctrl_set_runtozero_maxY                  = CMDID(1006, 107),
+  kcmd_xy_robot_ctrl_set_runtozero_speed                 = CMDID(1006, 108),
+  kcmd_xy_robot_ctrl_set_runtozero_dec                   = CMDID(1006, 109),
+  kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_maxXY = CMDID(1006, 110),
+  kcmd_xy_robot_ctrl_set_distance_scale                  = CMDID(1006, 111),
+  //
+
 } CmdID_t;
 
 }  // namespace zcr

components/zcancmder/cmd/c1006_module_cmd.hpp

@@ -22,6 +22,16 @@ ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_status, cmd, uint8_t id; uint8_t _pad;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_acc, cmd, uint8_t id; uint8_t _pad; int32_t acc;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_dec, cmd, uint8_t id; uint8_t _pad; int32_t dec;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_speed, cmd, uint8_t id; uint8_t _pad; int32_t speed;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_x_shaft, cmd, uint8_t id; uint8_t _pad; uint8_t x_shaft;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_y_shaft, cmd, uint8_t id; uint8_t _pad; uint8_t y_shaft;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_zero_robottype, cmd, uint8_t id; uint8_t _pad; uint8_t zero_robottype;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_maxX, cmd, uint8_t id; uint8_t _pad; int32_t maxX;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_maxY, cmd, uint8_t id; uint8_t _pad; int32_t maxY;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_speed, cmd, uint8_t id; uint8_t _pad; int32_t speed;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_dec, cmd, uint8_t id; uint8_t _pad; int32_t dec;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_maxXY, cmd, uint8_t id; uint8_t _pad; int32_t maxXY;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_distance_scale, cmd, uint8_t id; uint8_t _pad; float distance_scale;);
+
 /*******************************************************************************
  *                                     ACK                                     *
  *******************************************************************************/
@@ -35,6 +45,16 @@ ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_status, ack, uint8_t id; uint8_t module_s
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_acc, ack, uint8_t id; uint8_t _pad;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_dec, ack, uint8_t id; uint8_t _pad;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_speed, ack, uint8_t id; uint8_t _pad;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_x_shaft, ack, uint8_t id; uint8_t _pad;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_y_shaft, ack, uint8_t id; uint8_t _pad;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_zero_robottype, ack, uint8_t id; uint8_t _pad;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_maxX, ack, uint8_t id; uint8_t _pad;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_maxY, ack, uint8_t id; uint8_t _pad;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_speed, ack, uint8_t id; uint8_t _pad;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_dec, ack, uint8_t id; uint8_t _pad;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_maxXY, ack, uint8_t id; uint8_t _pad;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_distance_scale, ack, uint8_t id; uint8_t _pad;);
+
 /*******************************************************************************
  *                                status_report                                *
  *******************************************************************************/

components/zcancmder_module/zcan_xy_robot_module.cpp

@@ -50,4 +50,40 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
     errorcode = m_xyRobotCtrlModule->set_speed(cmd->speed);
   }
   END_PROCESS_PACKET();
+  PROCESS_PACKET(kcmd_xy_robot_ctrl_set_x_shaft, m_id) {  //
+    errorcode = m_xyRobotCtrlModule->set_x_shaft(cmd->x_shaft);
+  }
+  END_PROCESS_PACKET();
+  PROCESS_PACKET(kcmd_xy_robot_ctrl_set_y_shaft, m_id) {  //
+    errorcode = m_xyRobotCtrlModule->set_y_shaft(cmd->y_shaft);
+  }
+  END_PROCESS_PACKET();
+  PROCESS_PACKET(kcmd_xy_robot_ctrl_set_zero_robottype, m_id) {  //
+    errorcode = m_xyRobotCtrlModule->set_zero_robottype((XYRobotCtrlModule::RobotType_t)cmd->zero_robottype);
+  }
+  END_PROCESS_PACKET();
+  PROCESS_PACKET(kcmd_xy_robot_ctrl_set_runtozero_maxX, m_id) {  //
+    errorcode = m_xyRobotCtrlModule->set_runtozero_maxX(cmd->maxX);
+  }
+  END_PROCESS_PACKET();
+  PROCESS_PACKET(kcmd_xy_robot_ctrl_set_runtozero_maxY, m_id) {  //
+    errorcode = m_xyRobotCtrlModule->set_runtozero_maxY(cmd->maxY);
+  }
+  END_PROCESS_PACKET();
+  PROCESS_PACKET(kcmd_xy_robot_ctrl_set_runtozero_speed, m_id) {  //
+    errorcode = m_xyRobotCtrlModule->set_runtozero_speed(cmd->speed);
+  }
+  END_PROCESS_PACKET();
+  PROCESS_PACKET(kcmd_xy_robot_ctrl_set_runtozero_dec, m_id) {  //
+    errorcode = m_xyRobotCtrlModule->set_runtozero_dec(cmd->dec);
+  }
+  END_PROCESS_PACKET();
+  PROCESS_PACKET(kcmd_xy_robot_ctrl_set_runtozero_leave_away_zero_maxXY, m_id) {  //
+    errorcode = m_xyRobotCtrlModule->set_runtozero_leave_away_zero_maxXY(cmd->maxXY);
+  }
+  END_PROCESS_PACKET();
+  PROCESS_PACKET(kcmd_xy_robot_ctrl_set_distance_scale, m_id) {  //
+    errorcode = m_xyRobotCtrlModule->set_distance_scale(cmd->distance_scale);
+  }
+  END_PROCESS_PACKET();
 }

os/zthread.cpp

@@ -44,6 +44,8 @@ void ZThread::init(const char *threadname, int stack_size, osPriority priority)
   ZASSERT(m_defaultTaskHandle != NULL);
 }
 void ZThread::start(zosthread_cb_t cb) {
+  stop();
+
   m_taskfunction = cb;
 
   ZASSERT(m_taskfunction);

os/zthread.hpp

@@ -26,6 +26,8 @@ class ZThread {
   void start(zosthread_cb_t cb);
   void stop();
 
+  bool getExitFlag() { return !m_threadisworkingFlagCallSide; }
+
   void sleep(uint32_t ms);
   void wake();
 };