update

components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp

@@ -35,29 +35,8 @@ void XYRobotCtrlModule::dumpcfg(const char* tag) {
   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::move_to(int32_t x, int32_t y) {
+int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, function<void(int32_t exec_status, int32_t tox, int32_t toy)> status_cb) {
   zlock_guard lock(m_lock);
   ZLOGI(TAG, "move_to x:%d y:%d", x, y);
   int32_t m1pos, m2pos;
@@ -66,59 +45,194 @@ int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y) {
    * @TODO:添加限位检查
    */
   updateStatus(1);
-  m_thread.start([this, x, y]() {
+  m_thread.start([this, x, y, status_cb]() {
     _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();
+    int32_t xnow, ynow;
+    getnowpos(xnow, ynow);
+    if (status_cb) status_cb(0, xnow, ynow);
     updateStatus(0);
   });
   return 0;
 }
 
-int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy) {
+int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, function<void(int32_t exec_status, int32_t tox, int32_t toy)> status_cb) {
   zlock_guard lock(m_lock);
   ZLOGI(TAG, "move_by dx:%d dy:%d", dx, dy);
   int32_t now_x, now_y;
   getnowpos(now_x, now_y);
-  return move_to(now_x + dx, now_y + dy);
+  return move_to(now_x + dx, now_y + dy, status_cb);
 }
 
-int32_t XYRobotCtrlModule::move_to_zero() {
+int32_t XYRobotCtrlModule::move_to_zero(function<void(int32_t exec_status, int32_t dx, int32_t dy)> status_cb) {
   zlock_guard lock(m_lock);
   ZLOGI(TAG, "move_to_zero");
+  m_thread.stop();
 
   updateStatus(1);
-  m_thread.stop();
-  m_thread.start([this]() {
-    int32_t erro = exec_move_to_zero_task();
+  m_thread.start([this, status_cb]() {
+    int32_t dx, dy;
+
+    int32_t erro = exec_move_to_zero_task(dx, dy);
     if (erro != 0) {
       ZLOGI(TAG, "exec_move_to_zero_task failed:%d", erro);
+      _motor_stop();
+      if (status_cb) status_cb(erro, dx, dy);
       updateStatus(0);
       return;
     }
     m_stepM1->setXACTUAL(0);
     m_stepM2->setXACTUAL(0);
+    if (status_cb) status_cb(erro, dx, dy);
     updateStatus(0);
     return;
   });
 
   return 0;
 }
-int32_t XYRobotCtrlModule::move_to_with_calibrate(int32_t x, int32_t y) {
+
+int32_t XYRobotCtrlModule::move_to_zero_with_calibrate(int32_t x, int32_t y,
+                                                       function<void(int32_t exec_status, int32_t zero_shift_x, int32_t zero_shift_y)> status_cb) {
   zlock_guard lock(m_lock);
-  ZLOGI(TAG, "move_to_with_calibrate x:%d y:%d", x, y);
+  ZLOGI(TAG, "move_to_zero_with_calibrate x:%d y:%d", x, y);
+  m_thread.stop();
+  updateStatus(1);
+
+  m_thread.start([this, x, y, status_cb]() {
+    int32_t dx, dy;
+    int32_t erro = exec_move_to_zero_task(dx, dy);
+    if (erro != 0) {
+      ZLOGI(TAG, "exec_move_to_zero_task failed:%d", erro);
+      _motor_stop();
+      if (status_cb) status_cb(erro, 0, 0);
+      updateStatus(0);
+      return;
+    }
+
+    int32_t calibrate_x, calibrate_y;
+    calibrate_x = dx + x;
+    calibrate_y = dy + y;
+
+    m_zero_shift_x = calibrate_x;
+    m_zero_shift_y = calibrate_y;
+
+    m_stepM1->setXACTUAL(0);
+    m_stepM2->setXACTUAL(0);
+    if (status_cb) status_cb(erro, m_zero_shift_x, m_zero_shift_y);
+    updateStatus(0);
+    return;
+  });
+
+  return 0;
+}
+int32_t XYRobotCtrlModule::exec_move_to_zero_task(int32_t& dx, int32_t& dy) {
+  int32_t xnow, ynow;
+  getnowpos(xnow, ynow);
+
+  int32_t ret = exec_move_to_zero_task();
+
+  int32_t xnow2, ynow2;
+  getnowpos(xnow2, ynow2);
+
+  dx = xnow2 - xnow;
+  dy = ynow2 - ynow;
+
+  return ret;
+}
+
+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 (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
+    // 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;
+  }
+
+  if (m_thread.getExitFlag()) {
+    ZLOGI(TAG, "break move to zero thread exit");
+    return 0;
+  }
+
+  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 (!m_thread.getExitFlag() && !_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;
+  }
+  if (m_thread.getExitFlag()) {
+    ZLOGI(TAG, "break move to zero thread exit");
+    return 0;
+  }
+  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_stepM2->getXACTUAL();
+  inverse_kinematics(m1pos, m2pos, x, y);
+}
+
 int32_t XYRobotCtrlModule::stop() {
   zlock_guard lock(m_lock);
   ZLOGI(TAG, "stop");
   m_thread.stop();
+  _motor_stop();
+  return 0;
+}
+int32_t XYRobotCtrlModule::read_status(uint8_t* module_statu, int32_t* x, int32_t* y) {
+  zlock_guard lock(m_statu_lock);
+  *module_statu = m_status;
+
+  if (x != nullptr && y != nullptr) {
+    getnowpos(*x, *y);
+  }
+  ZLOGI(TAG, "read_status:%d %d %d", *module_statu, *x, *y);
   return 0;
 }
-uint8_t XYRobotCtrlModule::read_status(uint8_t* module_statu, int32_t* x, int32_t* y) { return 0; }
+
+/*******************************************************************************
+ *                                     SET                                     *
+ *******************************************************************************/
 int32_t XYRobotCtrlModule::set_acc(int32_t acc) {
   zlock_guard lock(m_lock);
   ZLOGI(TAG, "set_acc acc:%d", acc);
@@ -173,67 +287,57 @@ int32_t XYRobotCtrlModule::set_distance_scale(float distance_scale) {
   ZLOGI(TAG, "set_distance_scale:%f", distance_scale);
   m_stepM1->setDistanceK(distance_scale);
   m_stepM2->setDistanceK(distance_scale);
+  m_cfg_distance_scale = distance_scale;
   return 0;
 }
 
 void XYRobotCtrlModule::loop() {}
 
-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);
-  }
+int32_t XYRobotCtrlModule::set_zero_robottype(RobotType_t robot_type) {
+  zlock_guard lock(m_lock);
+  ZLOGI(TAG, "set_zero_robottype:%d", robot_type);
+  m_robot_type = robot_type;
+  return 0;
+}
+int32_t XYRobotCtrlModule::set_x_shaft(bool x_shaft) {
+  zlock_guard lock(m_lock);
+  ZLOGI(TAG, "set_x_shaft:%d", x_shaft);
+  m_x_shaft = x_shaft;
+  return 0;
+}
+int32_t XYRobotCtrlModule::set_y_shaft(bool y_shaft) {
+  zlock_guard lock(m_lock);
+  ZLOGI(TAG, "set_y_shaft:%d", y_shaft);
+  m_y_shaft = y_shaft;
+  return 0;
+}
 
-  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");
+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;
 }
 
-void XYRobotCtrlModule::updateStatus(uint8_t status) {
-  zlock_guard lock(m_statu_lock);
-  m_status = status;
+int32_t XYRobotCtrlModule::set_zero_shift(int32_t x, int32_t y) {
+  zlock_guard lock(m_lock);
+  ZLOGI(TAG, "set_zero_shift x:%d y:%d", x, y);
+  m_zero_shift_x = x;
+  m_zero_shift_y = y;
+  return 0;
 }
-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);
+int32_t XYRobotCtrlModule::get_zero_shift(int32_t* x, int32_t* y) {
+  zlock_guard lock(m_lock);
+  *x = m_zero_shift_x;
+  *y = m_zero_shift_y;
+  return 0;
 }
+
+/*******************************************************************************
+ *                                BASIC_ACTION                                 *
+ *******************************************************************************/
 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;
@@ -244,6 +348,7 @@ void XYRobotCtrlModule::_motor_move_to(int32_t x, int32_t y, int32_t maxv, int32
   m_stepM1->setDeceleration(dec);
   m_stepM2->setDeceleration(dec);
 
+  ZLOGI(TAG, "_motor_move_to target_m1pos:%d target_m2pos:%d", target_m1pos, target_m2pos);
   m_stepM1->moveTo(target_m1pos, maxv);
   m_stepM2->moveTo(target_m2pos, maxv);
 }
@@ -286,11 +391,17 @@ void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t
   }
   if (m_x_shaft) x = -x;
   if (m_y_shaft) y = -y;
+
+  x += m_zero_shift_x;
+  y += m_zero_shift_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;
 
+  x -= m_zero_shift_x;
+  y -= m_zero_shift_y;
+
   if (m_robot_type == corexy) {
     m1pos = ((x + y) / 2);
     m2pos = ((y - x) / 2);

components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp

@@ -38,6 +38,7 @@ class XYRobotCtrlModule {
   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;
+  float   m_cfg_distance_scale                  = 1.0f;
 
   RobotType_t m_robot_type = corexy;
   bool        m_x_shaft    = false;
@@ -57,6 +58,8 @@ class XYRobotCtrlModule {
   int32_t set_x_shaft(bool x_shaft);
   int32_t set_y_shaft(bool y_shaft);
 
+  int32_t set_zero_shift(int32_t x, int32_t y);
+
   int32_t set_runtozero_maxX(int32_t maxX);
   int32_t set_runtozero_maxY(int32_t maxY);
   int32_t set_runtozero_speed(int32_t speed);
@@ -67,14 +70,22 @@ class XYRobotCtrlModule {
   void dumpcfg(const char* tag);
 
   int32_t enable(bool venable);
-
-  int32_t move_to(int32_t x, int32_t y);
-  int32_t move_by(int32_t dx, int32_t dy);
-  int32_t move_to_zero();
-  int32_t move_to_with_calibrate(int32_t x, int32_t y);
   int32_t stop();
 
-  uint8_t read_status(uint8_t* module_statu, int32_t* x, int32_t* y);
+  /*******************************************************************************
+   *                                   ACTION                                    *
+   *******************************************************************************/
+  int32_t move_to(int32_t x, int32_t y, function<void(int32_t exec_status, int32_t tox, int32_t toy)> status_cb);
+  int32_t move_by(int32_t dx, int32_t dy, function<void(int32_t exec_status, int32_t tox, int32_t toy)> status_cb);
+  int32_t move_to_zero(function<void(int32_t exec_status, int32_t dx, int32_t dy)> status_cb);
+  int32_t move_to_zero_with_calibrate(int32_t x, int32_t y, function<void(int32_t exec_status, int32_t zero_shift_x, int32_t zero_shift_y)> status_cb);
+
+  /*******************************************************************************
+   *                                    READ                                     *
+   *******************************************************************************/
+
+  int32_t read_status(uint8_t* module_statu, int32_t* x, int32_t* y);
+  int32_t get_zero_shift(int32_t* x, int32_t* y);
 
   void loop();
 
@@ -84,6 +95,7 @@ class XYRobotCtrlModule {
   void computeTargetMotorPos();
   void getnowpos(int32_t& x, int32_t& y);
 
+  int32_t exec_move_to_zero_task(int32_t& dx, int32_t& dy);
   int32_t exec_move_to_zero_task();
 
   void inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y);

components/zcancmder/cmd/basic_bean.hpp

@@ -12,6 +12,7 @@
   if (rxcmd->iscmd(ordername)) {                                      \
     auto*    cmd       = rxcmd->get_data_as<ordername##_##cmd##_t>(); \
     auto*    ack       = (ordername##_##ack##_t*)m_txbuf;             \
+    auto     cmdheader = rxcmd->get_cmdheader();                      \
     uint32_t errorcode = 0;                                           \
     if (cmd->id == varid) {                                           \
       ack->id = cmd->id;
@@ -69,7 +70,7 @@ typedef enum {
   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_zero_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),

components/zcancmder/cmd/c1006_module_cmd.hpp

@@ -15,7 +15,7 @@ namespace zcr {
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_enable, cmd, uint8_t id; uint8_t enable;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_stop, cmd, uint8_t id; uint8_t _pad;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero, cmd, uint8_t id; uint8_t _pad;);
-ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_with_calibrate, cmd, uint8_t id; uint8_t _pad; int32_t x; int32_t y;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, cmd, uint8_t id; uint8_t _pad; int32_t x; int32_t y;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to, cmd, uint8_t id; uint8_t _pad; int32_t x; int32_t y;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_by, cmd, uint8_t id; uint8_t _pad; int32_t dx; int32_t dy;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_status, cmd, uint8_t id; uint8_t _pad;);
@@ -38,7 +38,7 @@ ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_distance_scale, cmd, uint8_t id; uint8_t _
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_enable, ack, uint8_t id; uint8_t _pad;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_stop, ack, uint8_t id; uint8_t _pad;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero, ack, uint8_t id; uint8_t _pad;);
-ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_with_calibrate, ack, uint8_t id; uint8_t _pad;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, ack, uint8_t id; uint8_t _pad;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to, ack, uint8_t id; uint8_t _pad;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_by, ack, uint8_t id; uint8_t _pad;);
 ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_status, ack, uint8_t id; uint8_t module_statu; int32_t x; int32_t y;);
@@ -58,10 +58,11 @@ ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_distance_scale, ack, uint8_t id; uint8_t _
 /*******************************************************************************
  *                                status_report                                *
  *******************************************************************************/
-ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero, status_report, uint8_t sensorid; uint8_t _pad;);
-ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_with_calibrate, status_report, uint8_t sensorid; uint8_t _pad;);
-ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to, status_report, uint8_t sensorid; uint8_t _pad;);
-ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_by, status_report, uint8_t sensorid; uint8_t _pad;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero, status_report, uint8_t id; uint8_t _pad; int32_t exec_status; int32_t dx; int32_t dy;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, status_report, uint8_t id; uint8_t _pad; int32_t exec_status; int32_t zero_shift_x;
+               int32_t zero_shift_y;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to, status_report, uint8_t id; uint8_t _pad; int32_t exec_status; int32_t tox; int32_t toy;);
+ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_by, status_report, uint8_t id; uint8_t _pad; int32_t exec_status; int32_t tox; int32_t toy;);
 
 #pragma pack(pop)
 

components/zcancmder/zcanreceiver.cpp

@@ -176,6 +176,13 @@ void ZCanCmder::sendAck(Cmdheader_t *cmdheader, uint8_t *data, size_t len) {
   memcpy(txheader->data, data, len);
   sendPacket(txbuff, sizeof(Cmdheader_t) + len);
 }
+void ZCanCmder::sendExecStatusReport(Cmdheader_t *rxcmdheader, uint8_t *data, size_t len) {
+  Cmdheader_t *txheader = (Cmdheader_t *)txbuff;
+  memcpy(txheader, rxcmdheader, sizeof(Cmdheader_t));
+  txheader->packetType = kpt_cmd_exec_status_report;
+  memcpy(txheader->data, data, len);
+  sendPacket(txbuff, sizeof(Cmdheader_t) + len);
+}
 
 void ZCanCmder::sendErrorAck(Cmdheader_t *cmdheader, uint32_t errcode) {
   Cmdheader_t *txheader = (Cmdheader_t *)txbuff;

components/zcancmder/zcanreceiver.hpp

@@ -45,7 +45,6 @@ class CanPacketRxBuffer {
 
   bool iscmd(CmdID_t id);
 
-
   template <typename T>
   T *get_data_as() {
     return (T *)get_data();
@@ -104,6 +103,7 @@ class ZCanCmder : public ZCanIRQListener {
   void regListener(zcan_cmder_listener_t listener);
 
   void    sendPacket(uint8_t *packet, size_t len);
+  void    sendExecStatusReport(Cmdheader_t *rxcmdheader, uint8_t *data, size_t len);
   void    sendAck(Cmdheader_t *rxcmdheader, uint8_t *data, size_t len);
   void    sendErrorAck(Cmdheader_t *cmdheader, uint32_t errcode);
   bool    sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems);

components/zcancmder_module/zcan_xy_robot_module.cpp

@@ -1,6 +1,8 @@
 #include "zcan_xy_robot_module.hpp"
 using namespace iflytop;
 
+#define TAG "ZCANXYRobotCtrlModule"
+
 void ZCANXYRobotCtrlModule::initialize(ZCanCmder* cancmder, int id, XYRobotCtrlModule* xyRobotCtrlModule) {
   m_cancmder          = cancmder;
   m_id                = id;
@@ -19,19 +21,52 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
   }
   END_PROCESS_PACKET();
   PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_zero, m_id) {  //
-    errorcode = m_xyRobotCtrlModule->move_to_zero();
-  }
-  END_PROCESS_PACKET();
-  PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_with_calibrate, m_id) {  //
-    errorcode = m_xyRobotCtrlModule->move_to_with_calibrate(cmd->x, cmd->y);
+    errorcode = m_xyRobotCtrlModule->move_to_zero([this, cmdheader](int32_t exec_status, int32_t dx, int32_t dy) {
+      ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to_zero exec_status:%d dx:%d dy:%d", exec_status, dx, dy);
+      kcmd_xy_robot_ctrl_move_to_zero_status_report_t report;
+      report.id          = m_id;
+      report.exec_status = exec_status;
+      report.dx          = dx;
+      report.dy          = dy;
+      m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report));
+    });
+  }
+  END_PROCESS_PACKET();
+  PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, m_id) {  //
+    errorcode =
+        m_xyRobotCtrlModule->move_to_zero_with_calibrate(cmd->x, cmd->y, [this, cmdheader](int32_t exec_status, int32_t zero_shift_x, int32_t zero_shift_y) {
+          ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to_zero_with_calibrate exec_status:%d zero_shift_x:%d zero_shift_y:%d", exec_status, zero_shift_x, zero_shift_y);
+          kcmd_xy_robot_ctrl_move_to_zero_with_calibrate_status_report_t report;
+          report.id           = m_id;
+          report.exec_status  = exec_status;
+          report.zero_shift_x = zero_shift_x;
+          report.zero_shift_y = zero_shift_y;
+          m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report));
+        });
   }
   END_PROCESS_PACKET();
   PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to, m_id) {  //
-    errorcode = m_xyRobotCtrlModule->move_to(cmd->x, cmd->y);
+    errorcode = m_xyRobotCtrlModule->move_to(cmd->x, cmd->y, [this, cmdheader](int32_t exec_status, int32_t tox, int32_t toy) {
+      ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to exec_status:%d tox:%d toy:%d", exec_status, tox, toy);
+      kcmd_xy_robot_ctrl_move_to_status_report_t report;
+      report.id          = m_id;
+      report.exec_status = exec_status;
+      report.tox         = tox;
+      report.toy         = toy;
+      m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report));
+    });
   }
   END_PROCESS_PACKET();
   PROCESS_PACKET(kcmd_xy_robot_ctrl_move_by, m_id) {  //
-    errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy);
+    errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, [this, cmdheader](int32_t exec_status, int32_t tox, int32_t toy) {
+      ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_by exec_status:%d tox:%d toy:%d", exec_status, tox, toy);
+      kcmd_xy_robot_ctrl_move_by_status_report_t report;
+      report.id          = m_id;
+      report.exec_status = exec_status;
+      report.tox         = tox;
+      report.toy         = toy;
+      m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report));
+    });
   }
   END_PROCESS_PACKET();
   PROCESS_PACKET(kcmd_xy_robot_ctrl_read_status, m_id) {  //