重构指令列表

components/cmdscheduler/cmd_scheduler.cpp

@@ -3,18 +3,34 @@
 
 #include <stdlib.h>
 #include <string.h>
+
+#include "sdk\components\zprotocols\errorcode\errorcode.hpp"
 using namespace iflytop;
 
 #define TAG "CmdScheduler"
 
-void CmdScheduler::registerCmd(std::string cmd, call_cmd_t call_cmd) { m_cmdMap[cmd] = call_cmd; }
+void CmdScheduler::registerCmd(std::string cmd, const char* helpinfo, int npara, call_cmd_t call_cmd) {
+  CMD cmdinfo;
+  cmdinfo.call_cmd  = call_cmd;
+  cmdinfo.help_info = helpinfo;
+  m_cmdMap[cmd]     = cmdinfo;
+  cmdinfo.npara     = npara;
+}
+
+void CmdScheduler::registerCmd(std::string cmd, call_cmd_t call_cmd) {
+  CMD cmdinfo;
+  cmdinfo.call_cmd  = call_cmd;
+  cmdinfo.help_info = "...";
+  m_cmdMap[cmd]     = cmdinfo;
+}
 void CmdScheduler::regbasiccmd() {
-  this->registerCmd("help", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
+  this->registerCmd("help", "", 0, [this](Context* context) {
     ZLOGI(TAG, "help");
     ZLOGI(TAG, "cmdlist:");
     for (auto it = m_cmdMap.begin(); it != m_cmdMap.end(); it++) {
-      ZLOGI(TAG, "  %s", it->first.c_str());
+      ZLOGI(TAG, "  %s %s", it->first.c_str(), it->second.help_info.c_str());
     }
+    return (int32_t)0;
   });
 }
 
@@ -57,16 +73,14 @@ void CmdScheduler::schedule() {
       rxbuf[i] = '\0';
     }
   }
-  // ZLOGI(TAG, "cmdstr2: %s %d %d", cmdstr, strlen(cmdstr),m_rxsize);
   for (int i = 0; i < m_rxsize; i++) {
     if (rxbuf[i] != '\0') {
       ZLOGI(TAG, "docmd: %s", &rxbuf[i]);
-      int               inext = strlen(&rxbuf[i]) + i;
-      CmdProcessContext context;
-      callcmd(&rxbuf[i], context);
-      i = inext;
-      if (context.breakflag) {
-        ZLOGE(TAG, "do break");
+      int     inext = strlen(&rxbuf[i]) + i;
+      int32_t ecode = callcmd(&rxbuf[i]);
+      i             = inext;
+      if (ecode != 0) {
+        ZLOGE(TAG, "callcmd %s fail:%s(%d)", &rxbuf[i], err::error2str(ecode), ecode);
         break;
       }
     }
@@ -75,34 +89,54 @@ void CmdScheduler::schedule() {
   m_dataisready = false;
 }
 
-void CmdScheduler::tx(const char* data, int len) { m_uart->tx((uint8_t*)data, len); }
-
-void CmdScheduler::callcmd(const char* cmd, CmdProcessContext& context) {
+void    CmdScheduler::tx(const char* data, int len) { m_uart->tx((uint8_t*)data, len); }
+int32_t CmdScheduler::callcmd(const char* cmd) {
   int   argc     = 0;
   char* argv[10] = {0};
   memset(cmdcache, 0, sizeof(cmdcache));
   argc = 0;
   memset(argv, 0, sizeof(argv));
   strcpy(cmdcache, cmd);
+  remove_note(cmdcache, strlen(cmdcache));
   prase_cmd(cmdcache, strlen(cmdcache), argc, argv);
   if (argc == 0) {
-    return;
+    return (int32_t)0;
   }
+
   // printf("argc:%d\n", argc);
   // for (size_t i = 0; i < argc; i++) {
   //   printf("argv[%d]:%s\n", i, argv[i]);
   // }
-#if 1
   /**
    * @brief ÔÚÕâÀï´¦ÀíÖ¸Áî
    */
-  if (m_cmdMap.find(string(argv[0])) != m_cmdMap.end()) {
-    context.breakflag = false;
-    m_cmdMap.find(string(argv[0]))->second(argc, argv, &context);
+  auto cmder = m_cmdMap.find(string(argv[0]));
+  if (cmder != m_cmdMap.end()) {
+    Context context;
+    context.argc = argc;
+    context.argv = argv;
+    if (cmder->second.npara != context.argc - 1) return err::kce_cmd_param_num_error;
+    int32_t ret = cmder->second.call_cmd(&context);
+    if (ret == 0) {
+      ZLOGI(TAG, "success", argv[0]);
+    }
+    return ret;
+
   } else {
-    ZLOGE(TAG, "cmd not found");
+    return err::kce_cmd_not_found;
+  }
+}
+
+void CmdScheduler::remove_note(char* input, int inputlen) {
+  bool detect_note = false;
+  for (size_t i = 0; i < inputlen; i++) {
+    if (!detect_note && input[i] == '#') {
+      detect_note = true;
+    }
+    if (detect_note) {
+      input[i] = 0;
+    }
   }
-#endif
 }
 
 void CmdScheduler::prase_cmd(char* input, int inputlen, int& argc, char* argv[]) {

components/cmdscheduler/cmd_scheduler.hpp

@@ -10,15 +10,50 @@ using namespace std;
 
 class CmdScheduler {
  public:
-  class CmdProcessContext {
+  class Context {
    public:
-    bool breakflag = false;
+    int    argc;
+    char** argv;
+
+    int getInt(int index, int defaultvalue = 0) {
+      if (index >= argc) {
+        return defaultvalue;
+      }
+      return atoi(argv[index]);
+    }
+
+    bool getBool(int index, bool defaultvalue = false) {
+      if (index >= argc) {
+        return defaultvalue;
+      }
+      return atoi(argv[index]) != 0;
+    }
+
+    float getFloat(int index, float defaultvalue = 0) {
+      if (index >= argc) {
+        return defaultvalue;
+      }
+      return atof(argv[index]);
+    }
+    const char* getString(int index, const char* defaultvalue = "") {
+      if (index >= argc) {
+        return defaultvalue;
+      }
+      return argv[index];
+    }
   };
 
-  typedef function<void(int, char**, CmdProcessContext*)> call_cmd_t;
+  typedef function<int32_t(Context* context)> call_cmd_t;
+
+  class CMD {
+   public:
+    call_cmd_t call_cmd;
+    string     help_info;
+    int        npara;
+  };
 
  private:
-  map<string, call_cmd_t> m_cmdMap;
+  map<string, CMD> m_cmdMap;
 
   ZUART*  m_uart;
   char*   rxbuf;
@@ -33,14 +68,45 @@ class CmdScheduler {
  public:
   void initialize(UART_HandleTypeDef* huart, uint32_t rxbufsize);
   void registerCmd(std::string cmd, call_cmd_t call_cmd);
+  void registerCmd(std::string cmd, const char* helpinfo, int npara, call_cmd_t call_cmd);
   void tx(const char* data, int len);
 
   void schedule();
 
  private:
-  void regbasiccmd();
-  void callcmd(const char* cmd, CmdProcessContext& context);
-  void prase_cmd(char* input, int inputlen, int& argc, char* argv[]);
+  void    regbasiccmd();
+  int32_t callcmd(const char* cmd);
+  void    prase_cmd(char* input, int inputlen, int& argc, char* argv[]);
+  void    remove_note(char* input, int inputlen);
 };
 
+#define DO_CMD(cond)    \
+  {                     \
+    int32_t ret = cond; \
+    if (ret != 0) {     \
+      return ret;       \
+    }                   \
+  }
+#define IMPL_CMD(cmd, ...)                     \
+  DO_CMD(findmodule(con->getInt(1), &module)); \
+  DO_CMD(module->cmd(__VA_ARGS__));            \
+  return (int32_t)0;
+
+#define IMPL_READ_STATE(cmd, ...)              \
+  DO_CMD(findmodule(con->getInt(1), &module)); \
+  DO_CMD(module->cmd(__VA_ARGS__));            \
+  cmd_dump_ack(ack);                           \
+  return (int32_t)0;
+
+#define REG_CMD(prefix, cmd, para, npara, ...)                                                                           /**/ \
+  m_cmdScheduler->registerCmd(prefix #cmd, para, npara, [this](CmdScheduler::Context* con) {                             /**/ \
+                                                                                             IMPL_CMD(cmd, __VA_ARGS__); /**/ \
+  });
+
+#define REG_DUMP_ACK_CMD(prefix, cmd, para, npara, acktype, ...)                                                                /**/ \
+  m_cmdScheduler->registerCmd(prefix #cmd, para, npara, [this](CmdScheduler::Context* con) {                                    /**/ \
+                                                                                             acktype ack;                       /**/ \
+                                                                                             IMPL_READ_STATE(cmd, __VA_ARGS__); /**/ \
+  });
+
 }  // namespace iflytop

components/mini_servo_motor/feite_servo_motor.cpp

@@ -108,6 +108,8 @@ bool FeiTeServoMotor::moveTo(uint8_t id, int16_t pos, int16_t speed, uint16_t to
    */
   DO(setmode(id, kServoMode));
   if (torque == 0) torque = 1000;
+  if (torque > 1000) torque = 1000;
+
   DO(write_u16(id, kRegServoTorqueLimit, torque));
   DO(write_s16(id, kRegServoRunSpeed, 15, speed));
   DO(setTargetPos(id, pos));
@@ -219,6 +221,13 @@ bool FeiTeServoMotor::reCalibration(int id, int16_t pos) {
  *******************************************************************************/
 bool FeiTeServoMotor::write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval) { return write_reg(id, false, add, &regval, 1); }
 bool FeiTeServoMotor::read_u8(uint8_t id, feite::reg_add_e add, uint8_t& regval) { return read_reg(id, add, &regval, 1); }
+bool FeiTeServoMotor::read_u8(uint8_t id, feite::reg_add_e add, uint16_t& regval) {
+  uint8_t val = 0;
+  bool    ret = read_u8(id, add, val);
+  regval      = val;
+  return ret;
+}
+
 bool FeiTeServoMotor::write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval) { return write_reg(id, false, add, (uint8_t*)&regval, 2); }
 bool FeiTeServoMotor::read_u16(uint8_t id, feite::reg_add_e add, uint16_t& regval) { return read_reg(id, add, (uint8_t*)&regval, 2); }
 bool FeiTeServoMotor::async_write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval) { return write_reg(id, true, add, &regval, 1); }

components/mini_servo_motor/feite_servo_motor.hpp

@@ -187,6 +187,7 @@ class FeiTeServoMotor {
   bool async_write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t regval);
 
   bool read_u8(uint8_t id, feite::reg_add_e add, uint8_t& regval);
+  bool read_u8(uint8_t id, feite::reg_add_e add, uint16_t& regval);
   bool read_u16(uint8_t id, feite::reg_add_e add, uint16_t& regval);
   bool read_s16(uint8_t id, feite::reg_add_e add, uint8_t signbitoff, int16_t& regval);
 

components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -5,6 +5,11 @@ using namespace iflytop;
 using namespace std;
 #define TAG "MiniRobotCtrlModule"
 
+static void limit_input(s32& input, s32 min, s32 max) {
+  if (input > max) input = max;
+  if (input < min) input = min;
+}
+
 void MiniRobotCtrlModule::initialize(FeiTeServoMotor* bus, uint8_t id) { m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode); }
 
 int32_t MiniRobotCtrlModule::enable(u8 enable) {
@@ -37,25 +42,17 @@ int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, action_
   ZLOGI(TAG, "rotate speed:%d torque:%d run_time:%d", speed, torque, run_time);
   m_thread.stop();
   if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
-  if (torque > 1000) torque = 1000;
-  if (torque < 0) torque = 0;
+
+  limit_input(torque, 0, 1000);
+  limit_input(speed, -200000, 200000);
 
   if (!m_bus->rotate(m_id, speed, torque)) return err::kce_subdevice_overtime;
 
   m_thread.start([this, speed, torque, run_time, status_cb]() {
     int32_t entertime = zos_get_tick();
     while (true) {
-      if (m_thread.getExitFlag() && run_time != 0) {
-        stop(0);
-        call_status_cb(status_cb, err::kce_break_by_user);
-        break;
-      }
-
-      if (m_thread.getExitFlag() && run_time == 0) {
-        stop(0);
-        call_status_cb(status_cb, 0);
-        break;
-      }
+      if (m_thread.getExitFlag() && run_time != 0) break;
+      if (m_thread.getExitFlag() && run_time == 0) break;
 
       if (run_time != 0 && zos_haspassedms(entertime) > run_time) {
         stop(0);
@@ -64,6 +61,7 @@ int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, action_
       }
       m_thread.sleep(10);
     }
+    ZLOGI(TAG, "rotate stop");
   });
 
   return 0;
@@ -73,11 +71,9 @@ int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, action_cb_s
   m_thread.stop();
 
   if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
-  if (torque > 1000) torque = 1000;
-  if (torque < 0) torque = 0;
 
-  if (pos > 4095) pos = 4095;
-  if (pos < 0) pos = 0;
+  limit_input(torque, 0, 1000);
+  limit_input(pos, 0, 4095);
 
   // m_bus->moveTo(m_id, pos, speed, torque);
   if (!m_bus->moveTo(m_id, pos, speed, torque)) return err::kce_subdevice_overtime;
@@ -87,18 +83,54 @@ int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, action_cb_s
       uint8_t moveflag = 0;
       m_bus->getMoveFlag(m_id, moveflag);
       if (moveflag == 0) {
-        stop(0);
         call_status_cb(status_cb, 0);
         break;
       }
 
-      if (m_thread.getExitFlag()) {
-        stop(0);
-        call_status_cb(status_cb, err::kce_break_by_user);
-        break;
-      }
+      if (m_thread.getExitFlag()) break;
       m_thread.sleep(10);
     }
+    stop(0);
+    ZLOGI(TAG, "move_to stop");
+  });
+
+  return 0;
+}
+
+int32_t MiniRobotCtrlModule::move_forward(s32 torque) {
+  ZLOGI(TAG, "move_forward torque:%d", torque);
+
+  if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
+  limit_input(torque, 0, 1000);
+
+  m_thread.stop();
+  if (!m_bus->moveTo(m_id, 4095, 0, torque)) return err::kce_subdevice_overtime;
+  m_thread.start([this, torque]() {
+    while (true) {
+      if (m_thread.getExitFlag()) break;
+      m_thread.sleep(10);
+    }
+    stop(0);
+    ZLOGI(TAG, "move_forward stop");
+  });
+
+  return 0;
+}
+int32_t MiniRobotCtrlModule::move_backward(s32 torque) {
+  ZLOGI(TAG, "move_backward torque:%d", torque);
+
+  if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
+  limit_input(torque, 0, 1000);
+
+  m_thread.stop();
+  if (!m_bus->moveTo(m_id, 0, 0, torque)) return err::kce_subdevice_overtime;
+  m_thread.start([this, torque]() {
+    while (true) {
+      if (m_thread.getExitFlag()) break;
+      m_thread.sleep(10);
+    }
+    stop(0);
+    ZLOGI(TAG, "move_forward stop");
   });
 
   return 0;
@@ -108,26 +140,23 @@ int32_t MiniRobotCtrlModule::move_by(s32 dpos, s32 speed, s32 torque, action_cb_
   m_thread.stop();
 
   if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
-  if (torque > 1000) torque = 1000;
-  if (torque < 0) torque = 0;
+  limit_input(torque, 0, 1000);
+  limit_input(speed, -200000, 200000);
 
   int16_t nowpos = 0;
   m_bus->getNowPos(m_id, nowpos);
 
   int32_t targetpos = nowpos + dpos;
-  if (targetpos > 4095) targetpos = 4095;
-  if (targetpos < 0) targetpos = 0;
+  limit_input(targetpos, 0, 4095);
 
   move_to(targetpos, speed, torque, [this, nowpos, status_cb](int32_t status) {
-    // move_by_cb_status_t moveby_status = {0};
-    // moveby_status.dpos                = status.pos - nowpos;
-    // moveby_status.exec_status         = status.exec_status;
     if (status_cb) status_cb(status);
   });
-
   return 0;
 }
 int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, action_cb_status_t status_cb) {
+  return err::kce_operation_not_support;
+#if 0
   ZLOGI(TAG, "run_with_torque torque:%d run_time:%d", torque, run_time);
   m_thread.stop();
 
@@ -161,7 +190,7 @@ int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, action_cb
     stop(0);
     if (status_cb) status_cb(0);
   });
-
+#endif
   return 0;
 }
 
@@ -178,10 +207,10 @@ int32_t MiniRobotCtrlModule::read_status(status_t& status) {
 
   FeiTeServoMotor::detailed_status_t feitestatus;
   if (!m_bus->read_detailed_status(m_id, &feitestatus)) return err::kce_subdevice_overtime;
-  status.torque   = feitestatus.torque;
-  status.speed    = feitestatus.vel;
-  status.pos      = feitestatus.pos;
-  status.io_state = 0;
+  status.torque  = feitestatus.torque;
+  status.speed   = feitestatus.vel;
+  status.pos     = feitestatus.pos;
+  status.ioState = 0;
   if (feitestatus.state != 0) {
     status.status = 3;
   } else {
@@ -199,7 +228,7 @@ int32_t MiniRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info)
   } else {
     debug_info.status = m_thread.isworking();
   }
-  debug_info.io_state    = 0;
+  debug_info.ioState     = 0;
   debug_info.torque      = feitestatus.torque;
   debug_info.speed       = feitestatus.vel;
   debug_info.pos         = feitestatus.pos;
@@ -210,13 +239,20 @@ int32_t MiniRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info)
   return 0;
 }
 
-int32_t MiniRobotCtrlModule::set_run_param(run_param_t& param) { return 0; }
-int32_t MiniRobotCtrlModule::set_basic_param(basic_param_t& param) { return 0; }
-int32_t MiniRobotCtrlModule::set_warning_limit_param(warning_limit_param_t& param) { return 0; }
-
-int32_t MiniRobotCtrlModule::get_run_param(run_param_t& param) { return 0; }
-int32_t MiniRobotCtrlModule::get_basic_param(basic_param_t& param) { return 0; }
-int32_t MiniRobotCtrlModule::get_warning_limit_param(warning_limit_param_t& param) { return 0; }
+int32_t MiniRobotCtrlModule::set_basic_param(basic_param_t& param) { return err::kce_operation_not_support; }
+int32_t MiniRobotCtrlModule::get_basic_param(basic_param_t& param) {
+  ZLOGI(TAG, "get_basic_param");
+  if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
+  m_bus->read_u16(m_id, feite::kRegServoCalibration, param.posCalibrate);
+  m_bus->read_u16(m_id, feite::kRegServoMinAngle, param.minlimit);
+  m_bus->read_u16(m_id, feite::kRegServoMaxAngle, param.maxlimit);
+  m_bus->read_u8(m_id, feite::kRegServoMaxTemp, param.maxtemp);
+  m_bus->read_u8(m_id, feite::kRegServoMinVoltage, param.minvoltage);
+  m_bus->read_u8(m_id, feite::kRegServoMaxVoltage, param.maxvoltage);
+  m_bus->read_u16(m_id, feite::kRegServoProtectCurrent, param.protectCurrent);
+  m_bus->read_u8(m_id, feite::kRegServoProtectTorque, param.protectTorque);
+  return 0;
+}
 
 void MiniRobotCtrlModule::call_status_cb(action_cb_status_t cb, int32_t status) {
   m_last_exec_status = status;

components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp

@@ -24,6 +24,10 @@ class MiniRobotCtrlModule : public I_MiniServoModule {
   virtual int32_t rotate(s32 speed, s32 torque, s32 run_time, action_cb_status_t status_cb) override;
   virtual int32_t move_to(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) override;
   virtual int32_t move_by(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) override;
+
+  virtual int32_t move_forward(s32 torque);
+  virtual int32_t move_backward(s32 torque);
+
   virtual int32_t run_with_torque(s32 torque, s32 run_time, action_cb_status_t status_cb) override;
   virtual int32_t move_by_nolimit(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) override;
 
@@ -31,13 +35,13 @@ class MiniRobotCtrlModule : public I_MiniServoModule {
   virtual int32_t read_status(status_t& status) override;
   virtual int32_t read_detailed_status(detailed_status_t& debug_info) override;
 
-  virtual int32_t set_run_param(run_param_t& param) override;
+  // virtual int32_t set_run_param(run_param_t& param) override;
   virtual int32_t set_basic_param(basic_param_t& param) override;
-  virtual int32_t set_warning_limit_param(warning_limit_param_t& param) override;
+  // virtual int32_t set_warning_limit_param(warning_limit_param_t& param) override;
 
-  virtual int32_t get_run_param(run_param_t& param) override;
+  // virtual int32_t get_run_param(run_param_t& param) override;
   virtual int32_t get_basic_param(basic_param_t& param) override;
-  virtual int32_t get_warning_limit_param(warning_limit_param_t& param) override;
+  // virtual int32_t get_warning_limit_param(warning_limit_param_t& param) override;
 
  private:
   void call_status_cb(action_cb_status_t cb, int32_t status);

components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp

@@ -0,0 +1,73 @@
+
+#include "scirpt_cmder_mini_servo_motor_ctrl_module.hpp"
+
+#include <string.h>
+
+// #include "sdk\components\zprotocols\zcancmder\zcancmder_protocol.hpp"
+using namespace iflytop;
+#define TAG "CMD"
+static void cmd_dump_ack(I_MiniServoModule::version_t& ack) {  //
+  ZLOGI(TAG, "version:%d", ack.version);
+}
+static void cmd_dump_ack(I_MiniServoModule::status_t& ack) {  //
+  ZLOGI(TAG, "status    :%d", ack.status);
+  ZLOGI(TAG, "ioState   :%d", ack.ioState);
+  ZLOGI(TAG, "torque    :%d", ack.torque);
+  ZLOGI(TAG, "speed     :%d", ack.speed);
+  ZLOGI(TAG, "pos       :%d", ack.pos);
+}
+static void cmd_dump_ack(I_MiniServoModule::detailed_status_t& ack) {  //
+  ZLOGI(TAG, "status    :%d", ack.status);
+  ZLOGI(TAG, "ioState   :%d", ack.ioState);
+  ZLOGI(TAG, "torque    :%d", ack.torque);
+  ZLOGI(TAG, "speed     :%d", ack.speed);
+  ZLOGI(TAG, "pos       :%d", ack.pos);
+}
+static void cmd_dump_ack(I_MiniServoModule::basic_param_t& ack) {  //
+  ZLOGI(TAG, "posCalibrate:   %d", ack.posCalibrate);
+  ZLOGI(TAG, "minlimit:       %d", ack.minlimit);
+  ZLOGI(TAG, "maxlimit:       %d", ack.maxlimit);
+  ZLOGI(TAG, "maxtemp:        %d", ack.maxtemp);
+  ZLOGI(TAG, "minvoltage:     %d", ack.minvoltage);
+  ZLOGI(TAG, "maxvoltage:     %d", ack.maxvoltage);
+  ZLOGI(TAG, "protectCurrent: %d", ack.protectCurrent);
+  ZLOGI(TAG, "protectTorque:  %d", ack.protectTorque);
+}
+
+void ScirptCmderMiniServoMotorCtrlModule::initialize(CmdScheduler* cmdScheduler) {
+  m_cmdScheduler = cmdScheduler;
+  ZASSERT(m_cmdScheduler != nullptr);
+  regcmd();
+}
+
+int32_t ScirptCmderMiniServoMotorCtrlModule::findmodule(int id, I_MiniServoModule** module) {
+  auto it = moduler.find(id);
+  if (it == moduler.end()) {
+    return err::kce_no_such_module;
+  }
+  *module = it->second;
+  return 0;
+}
+
+static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; }
+
+void ScirptCmderMiniServoMotorCtrlModule::regmodule(int id, I_MiniServoModule* robot) {
+  ZASSERT(moduler.find(id) == moduler.end());
+  ZASSERT(robot != nullptr);
+  moduler[id] = robot;
+}
+
+void ScirptCmderMiniServoMotorCtrlModule::regcmd() {
+  REG_CMD("mini_servo_", enable, "(id,en)", 2, con->getBool(2));
+  REG_CMD("mini_servo_", stop, "(id,stop_type)", 2, con->getInt(2));
+  REG_CMD("mini_servo_", position_calibrate, "(id,pos)", 2, con->getInt(2));
+  REG_CMD("mini_servo_", rotate, "(id,speed,torque,time)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t ecode) { ZLOGI(TAG, "mini_servo_rotate done %d", ecode); });
+  REG_CMD("mini_servo_", move_to, "(id,pos,speed,torque)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t ecode) { ZLOGI(TAG, "mini_servo_move_to done %d", ecode); });
+  REG_CMD("mini_servo_", move_by, "(id,pos,speed,torque)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t ecode) { ZLOGI(TAG, "mini_servo_move_by done %d", ecode); });
+  REG_CMD("mini_servo_", move_forward, "(id,torque)", 2, con->getInt(2));
+  REG_CMD("mini_servo_", move_backward, "(id,torque)", 2, con->getInt(2));
+  REG_DUMP_ACK_CMD("mini_servo_", read_version, "(id)", 1, I_MiniServoModule::version_t, ack);
+  REG_DUMP_ACK_CMD("mini_servo_", read_status, "(id)", 1, I_MiniServoModule::status_t, ack);
+  REG_DUMP_ACK_CMD("mini_servo_", read_detailed_status, "(id)", 1, I_MiniServoModule::detailed_status_t, ack);
+  REG_DUMP_ACK_CMD("mini_servo_", get_basic_param, "(id)", 1, I_MiniServoModule::basic_param_t, ack);
+}

components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.hpp

@@ -0,0 +1,23 @@
+#pragma once
+#include <map>
+
+#include "sdk/os/zos.hpp"
+#include "sdk\components\cmdscheduler\cmd_scheduler.hpp"
+#include "sdk\components\zprotocols\zcancmder\api\i_mini_servo_module.hpp"
+namespace iflytop {
+
+class ScirptCmderMiniServoMotorCtrlModule {
+  CmdScheduler*                    m_cmdScheduler;
+  map<uint8_t, I_MiniServoModule*> moduler;
+
+  I_MiniServoModule* module;
+
+ public:
+  void initialize(CmdScheduler* cmdScheduler);
+  void regmodule(int id, I_MiniServoModule* robot);
+  void regcmd();
+
+ private:
+  int32_t findmodule(int id, I_MiniServoModule** module);
+};
+}  // namespace iflytop

components/step_motor_45/script_cmder_step_motor_45_scheduler.hpp

@@ -0,0 +1,22 @@
+#pragma once
+#include <map>
+
+#include "sdk/os/zos.hpp"
+#include "sdk\components\cmdscheduler\cmd_scheduler.hpp"
+#include "step_motor_45.hpp"
+namespace iflytop {
+
+class ScriptCmderStepMotor45Scheduler {
+  CmdScheduler*              m_cmdScheduler;
+  map<uint8_t, StepMotor45*> moduler;
+
+ public:
+  void initialize(CmdScheduler* cmdScheduler);
+  void regmodule(int id, StepMotor45* robot);
+  void regcmd();
+
+ private:
+  StepMotor45* findModule(int id);
+};
+
+}  // namespace iflytop

components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.cpp

@@ -31,13 +31,63 @@ void StepMotorCtrlScriptCmderModule::initialize(CmdScheduler* cmdScheduler) {
   ZASSERT(m_cmdScheduler != nullptr);
   regcmd();
 }
-
-I_StepMotorCtrlModule* StepMotorCtrlScriptCmderModule::findXYRobot(int id) {
+int32_t StepMotorCtrlScriptCmderModule::findmodule(int id, I_StepMotorCtrlModule** module) {
   auto it = moduler.find(id);
   if (it == moduler.end()) {
-    return nullptr;
+    return err::kce_no_such_module;
   }
-  return it->second;
+  *module = it->second;
+
+  return 0;
+}
+#define GET_BIT(x, n) ((x >> n) & 0x01)
+
+static void cmd_dump_ack(I_StepMotorCtrlModule::base_param_t& ack) {
+  ZLOGI(TAG, "base_param:");
+  ZLOGI(TAG, "  x_shaft:                           %d", ack.x_shaft);
+  ZLOGI(TAG, "  distance_scale:                    %d", ack.distance_scale);
+  ZLOGI(TAG, "  ihold:                             %d", ack.ihold);
+  ZLOGI(TAG, "  irun:                              %d", ack.irun);
+  ZLOGI(TAG, "  iholddelay:                        %d", ack.iholddelay);
+  ZLOGI(TAG, "  acc:                               %d", ack.acc);
+  ZLOGI(TAG, "  dec:                               %d", ack.dec);
+  ZLOGI(TAG, "  maxspeed:                          %d", ack.maxspeed);
+  ZLOGI(TAG, "  min_x:                             %d", ack.min_x);
+  ZLOGI(TAG, "  max_x:                             %d", ack.max_x);
+  ZLOGI(TAG, "  shift_x:                           %d", ack.shift_x);
+  ZLOGI(TAG, "  run_to_zero_move_to_zero_max_d:    %d", ack.run_to_zero_move_to_zero_max_d);
+  ZLOGI(TAG, "  run_to_zero_leave_from_zero_max_d: %d", ack.run_to_zero_leave_from_zero_max_d);
+  ZLOGI(TAG, "  run_to_zero_speed:                 %d", ack.run_to_zero_speed);
+  ZLOGI(TAG, "  run_to_zero_dec:                   %d", ack.run_to_zero_dec);
+}
+
+static void cmd_dump_ack(I_StepMotorCtrlModule::logic_point_t& ack) {
+  ZLOGI(TAG, "logic_point:");
+  ZLOGI(TAG, "  index:    %d", ack.index);
+  ZLOGI(TAG, "  acc:      %d", ack.acc);
+  ZLOGI(TAG, "  dec:      %d", ack.dec);
+  ZLOGI(TAG, "  velocity: %d", ack.velocity);
+  ZLOGI(TAG, "  x:        %d", ack.x);
+}
+
+static void cmd_dump_ack(I_StepMotorCtrlModule::status_t& ack) {
+  ZLOGI(TAG, "status:");
+  ZLOGI(TAG, "  status:   :%d", ack.status);
+  ZLOGI(TAG, "  io_state  :%d,%d,%d,%d,%d,%d,%d,%d", GET_BIT(ack.io_state, 0), GET_BIT(ack.io_state, 1), GET_BIT(ack.io_state, 2), GET_BIT(ack.io_state, 3), GET_BIT(ack.io_state, 4), GET_BIT(ack.io_state, 5), GET_BIT(ack.io_state, 6), GET_BIT(ack.io_state, 7));
+  ZLOGI(TAG, "  x:        :%d", ack.x);
+}
+
+static void cmd_dump_ack(I_StepMotorCtrlModule::detailed_status_t& ack) {
+  ZLOGI(TAG, "detailed_status:");
+  ZLOGI(TAG, "  status:           %d", ack.status);
+  ZLOGI(TAG, "  io_state         :%d,%d,%d,%d,%d,%d,%d,%d", GET_BIT(ack.io_state, 0), GET_BIT(ack.io_state, 1), GET_BIT(ack.io_state, 2), GET_BIT(ack.io_state, 3), GET_BIT(ack.io_state, 4), GET_BIT(ack.io_state, 5), GET_BIT(ack.io_state, 6), GET_BIT(ack.io_state, 7));
+  ZLOGI(TAG, "  x:                %d", ack.x);
+  ZLOGI(TAG, "  last_exec_status: %d", ack.last_exec_status);
+}
+
+static void cmd_dump_ack(I_StepMotorCtrlModule::version_t& ack) {
+  ZLOGI(TAG, "version:");
+  ZLOGI(TAG, "  version: %d", ack.version);
 }
 
 static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; }
@@ -50,317 +100,127 @@ void StepMotorCtrlScriptCmderModule::regmodule(int id, I_StepMotorCtrlModule* ro
 void StepMotorCtrlScriptCmderModule::regcmd() {  //
   ZASSERT(m_cmdScheduler != nullptr);
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_is_busy", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_ARGC(1);
-
-    int id = atoi(argv[1]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    bool ack = module->isbusy();
-    ZLOGI(TAG, "is busy:%d", ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_is_busy", "(id)", 1, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    ZLOGI(TAG, "is busy:%d", module->isbusy());
+    return (int32_t)0;
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_get_last_exec_status", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_ARGC(1);
-
-    int id = atoi(argv[1]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->get_last_exec_status();
-    ZLOGI(TAG, "last exec status:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_get_last_exec_status", "(id)", 1, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    ZLOGI(TAG, "last exec status:%s(%d)", err::error2str(module->get_last_exec_status()), module->get_last_exec_status());
+    return (int32_t)0;
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_move_to", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_MIN_ARGC(2);
-
-    int     id    = atoi(argv[1]);
-    int32_t x     = atoi(argv[2]);
-    int32_t speed = 0;
-    if (argc >= 4) {
-      int32_t speed = atoi(argv[3]);
-    }
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->move_to(x, speed, [](int32_t status) { ZLOGI(TAG, "move to exec end status:%s(%d)", err::error2str(status), status); });
-    ZLOGI(TAG, "move to:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_move_to", "(id,x,speed)", 3, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    IMPL_CMD(move_to, con->getInt(1), con->getInt(2),  //
+             [this](int32_t status) { ZLOGI(TAG, "move to exec end status:%s(%d)", err::error2str(status), status); });
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_move_by", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_MIN_ARGC(2);
-
-    int     id    = atoi(argv[1]);
-    int32_t dx    = atoi(argv[2]);
-    int32_t speed = 0;
-    if (argc >= 4) {
-      int32_t speed = atoi(argv[3]);
-    }
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->move_by(dx, speed, [](int32_t status) { ZLOGI(TAG, "move by exec end status:%s(%d)", err::error2str(status), status); });
-    ZLOGI(TAG, "move by:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_move_by", "(id,dx,speed)", 3, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    IMPL_CMD(move_by, con->getInt(1), con->getInt(2),  //
+             [this](int32_t status) { ZLOGI(TAG, "move by exec end status:%s(%d)", err::error2str(status), status); });
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_zero", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_ARGC(1);
-
-    int id = atoi(argv[1]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->move_to_zero([](int32_t status) { ZLOGI(TAG, "move to zero exec end status:%s(%d)", err::error2str(status), status); });
-    ZLOGI(TAG, "move to zero:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_zero", "(id)", 1, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    IMPL_CMD(move_to_zero, [this](int32_t status) { ZLOGI(TAG, "move to zero exec end status:%s(%d)", err::error2str(status), status); });
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_zero_with_calibrate", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_MIN_ARGC(2);
-
-    int     id = atoi(argv[1]);
-    int32_t x  = atoi(argv[2]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->move_to_zero_with_calibrate(x, [](int32_t status) { ZLOGI(TAG, "move to zero with calibrate exec end status:%s(%d)", err::error2str(status), status); });
-    ZLOGI(TAG, "move to zero with calibrate:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_zero_with_calibrate", "(id,x)", 2, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    IMPL_CMD(move_to_zero_with_calibrate, con->getInt(1),  //
+             [this](int32_t status) { ZLOGI(TAG, "move to zero with calibrate exec end status:%s(%d)", err::error2str(status), status); });
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_rotate", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_MIN_ARGC(2);
-
-    int     id        = atoi(argv[1]);
-    int32_t speed     = atoi(argv[2]);
-    int32_t lastforms = 0;
-    if (argc >= 4) {
-      int32_t lastforms = atoi(argv[3]);
-    }
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->rotate(speed, lastforms, [](int32_t status) { ZLOGI(TAG, "rotate exec end status:%s(%d)", err::error2str(status), status); });
-    ZLOGI(TAG, "rotate:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_rotate", "(id,speed,lastforms)", 3, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    IMPL_CMD(rotate, con->getInt(1), con->getInt(2),  //
+             [this](int32_t status) { ZLOGI(TAG, "rotate exec end status:%s(%d)", err::error2str(status), status); });
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_stop", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_MIN_ARGC(2);
-
-    int     id       = atoi(argv[1]);
-    uint8_t stopType = atoi(argv[2]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->stop(stopType);
-    ZLOGI(TAG, "stop:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_stop", "(id,stop_type)", 2, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    IMPL_CMD(stop, con->getInt(1));
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_force_change_current_pos", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_MIN_ARGC(2);
-
-    int     id = atoi(argv[1]);
-    int32_t x  = atoi(argv[2]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->force_change_current_pos(x);
-    ZLOGI(TAG, "force change current pos:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_force_change_current_pos", "(id,x)", 2, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    IMPL_CMD(force_change_current_pos, con->getInt(1));
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_logic_point", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_MIN_ARGC(2);
-
-    int     id             = atoi(argv[1]);
-    int32_t logic_point_id = atoi(argv[2]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->move_to_logic_point(logic_point_id, [](int32_t status) { ZLOGI(TAG, "move to logic point exec end status:%s(%d)", err::error2str(status), status); });
-    ZLOGI(TAG, "move to logic point:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_logic_point", "(id,logic_point_id)", 2, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    IMPL_CMD(move_to_logic_point, con->getInt(1),  //
+             [this](int32_t status) { ZLOGI(TAG, "move to logic point exec end status:%s(%d)", err::error2str(status), status); });
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_set_logic_point", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_MIN_ARGC(6);
-
-    int     id             = atoi(argv[1]);
-    int32_t logic_point_id = atoi(argv[2]);
-    int32_t x              = atoi(argv[3]);
-    int32_t vel            = atoi(argv[4]);
-    int32_t acc            = atoi(argv[5]);
-    int32_t dec            = atoi(argv[6]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->set_logic_point(logic_point_id, x, vel, acc, dec);
-    ZLOGI(TAG, "set logic point:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_set_logic_point", "(id,logic_point_id,x,vel,acc,dec)", 6, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    IMPL_CMD(set_logic_point, con->getInt(1), con->getInt(2), con->getInt(3), con->getInt(4), con->getInt(5));
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_set_logic_point_simplify", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_MIN_ARGC(2);
-
-    int     id             = atoi(argv[1]);
-    int32_t logic_point_id = atoi(argv[2]);
-    int32_t x              = 0;
-    int32_t vel            = 0;
-    int32_t acc            = 0;
-    int32_t dec            = 0;
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    x = module->read_pos();
-
-    int32_t ack = module->set_logic_point(logic_point_id, x, vel, acc, dec);
-    ZLOGI(TAG, "set logic point:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_set_logic_point_simplify", "(id,logic_point_id)", 2, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    IMPL_CMD(set_logic_point, con->getInt(1), module->read_pos(), 0, 0, 0);
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_get_logic_point", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_MIN_ARGC(2);
-
-    int     id             = atoi(argv[1]);
-    int32_t logic_point_id = atoi(argv[2]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    I_StepMotorCtrlModule::logic_point_t logic_point;
-    int32_t                              ack = module->get_logic_point(logic_point_id, logic_point);
-    if (ack != 0) {
-      ZLOGI(TAG, "get logic point:%s(%d)", err::error2str(ack), ack);
-      return;
-    }
-
-    ZLOGI(TAG, "logic_point_id          :%d", logic_point.index);
-    ZLOGI(TAG, "logic_point.x           :%d", logic_point.x);
-    ZLOGI(TAG, "logic_point.vel         :%d", logic_point.velocity);
-    ZLOGI(TAG, "logic_point.acc         :%d", logic_point.acc);
-    ZLOGI(TAG, "logic_point.dec         :%d", logic_point.dec);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_get_logic_point", "(id,logic_point_id)", 2, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    I_StepMotorCtrlModule::logic_point_t ack;
+    IMPL_READ_STATE(get_logic_point, con->getInt(1), ack);
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_flush", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_ARGC(1);
-
-    int id = atoi(argv[1]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->flush();
-    ZLOGI(TAG, "flush:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_get_base_param", "(id)", 1, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    I_StepMotorCtrlModule::base_param_t ack;
+    IMPL_READ_STATE(get_base_param, ack);
   });
-  m_cmdScheduler->registerCmd("step_motor_ctrl_enable", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_ARGC(2);
-
-    int id = atoi(argv[1]);
-    int enable = atoi(argv[2]);
 
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->enable(enable);
-    ZLOGI(TAG, "flush:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_flush", "(id)", 1, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    IMPL_CMD(flush);
   });
-  m_cmdScheduler->registerCmd("step_motor_ctrl_factory_reset", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_ARGC(1);
-
-    int id = atoi(argv[1]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
 
-    int32_t ack = module->factory_reset();
-    ZLOGI(TAG, "factory reset:%s(%d)", err::error2str(ack), ack);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_enable", "(id,en)", 2, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    IMPL_CMD(enable, con->getBool(1));
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_read_version", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_ARGC(1);
-
-    int id = atoi(argv[1]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    I_StepMotorCtrlModule::version_t version;
-    int32_t                          ack = module->read_version(version);
-    if (ack != 0) {
-      ZLOGI(TAG, "read version:%s(%d)", err::error2str(ack), ack);
-      return;
-    }
-
-    ZLOGI(TAG, "version.version:%d", version.version);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_factory_reset", "(id)", 1, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    IMPL_CMD(factory_reset);
   });
 
-#define GET_BIT(x, n) ((x >> n) & 0x01)
-
-  m_cmdScheduler->registerCmd("step_motor_ctrl_read_status", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_ARGC(1);
-
-    int id = atoi(argv[1]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    I_StepMotorCtrlModule::status_t status;
-    int32_t                         ack = module->read_status(status);
-    if (ack != 0) {
-      ZLOGI(TAG, "read status:%s(%d)", err::error2str(ack), ack);
-      return;
-    }
-
-    ZLOGI(TAG, "status.status      :%d", status.status);
-    ZLOGI(TAG, "status.io_state    :%d,%d,%d,%d,%d,%d,%d,%d", GET_BIT(status.io_state, 0), GET_BIT(status.io_state, 1), GET_BIT(status.io_state, 2), GET_BIT(status.io_state, 3), GET_BIT(status.io_state, 4), GET_BIT(status.io_state, 5), GET_BIT(status.io_state, 6), GET_BIT(status.io_state, 7));
-    ZLOGI(TAG, "status.x           :%d", status.x);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_read_version", "(id)", 1, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    I_StepMotorCtrlModule::version_t ack;
+    IMPL_READ_STATE(read_version, ack);
   });
-  // virtual int32_t read_detailed_status(detailed_status_t& debug_info) = 0;
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_read_detailed_status", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-        CHECK_ARGC(1);
-
-    int id = atoi(argv[1]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    I_StepMotorCtrlModule::detailed_status_t status;
-    int32_t                         ack = module->read_detailed_status(status);
-    if (ack != 0) {
-      ZLOGI(TAG, "read status:%s(%d)", err::error2str(ack), ack);
-      return;
-    }
-
-    ZLOGI(TAG, "status.status      :%d", status.status);
-    ZLOGI(TAG, "status.io_state    :%d,%d,%d,%d,%d,%d,%d,%d", GET_BIT(status.io_state, 0), GET_BIT(status.io_state, 1), GET_BIT(status.io_state, 2), GET_BIT(status.io_state, 3), GET_BIT(status.io_state, 4), GET_BIT(status.io_state, 5), GET_BIT(status.io_state, 6), GET_BIT(status.io_state, 7));
-    ZLOGI(TAG, "status.x           :%d", status.x);
-    ZLOGI(TAG, "status.last_exec_status :%d", status.last_exec_status);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_read_status", "(id)", 1, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    I_StepMotorCtrlModule::status_t ack;
+    IMPL_READ_STATE(read_status, ack);
   });
 
-  m_cmdScheduler->registerCmd("step_motor_ctrl_set_base_param", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_MIN_ARGC(3);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_read_detailed_status", "(id)", 1, [this](CmdScheduler::Context* con) {
+    DO_CMD(findmodule(con->getInt(0), &module));
+    I_StepMotorCtrlModule::detailed_status_t ack;
+    IMPL_READ_STATE(read_detailed_status, ack);
+  });
 
-    int         id        = atoi(argv[1]);
-    const char* paramName = argv[2];
-    int32_t     value     = atoi(argv[3]);
+  m_cmdScheduler->registerCmd("step_motor_ctrl_set_base_param", "(id,paramName,val)", 3, [this](CmdScheduler::Context* con) {
+    const char* paramName = con->getString(2);
+    int32_t     value     = con->getInt(3);
 
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
+    DO_CMD(findmodule(con->getInt(0), &module));
 
     I_StepMotorCtrlModule::base_param_t param;
-    int32_t                             ack = module->get_base_param(param);
-    if (ack != 0) {
-      ZLOGE(TAG, "get_base_param failed:%d", ack);
-      return;
-    }
+    DO_CMD(module->get_base_param(param));
+
     if (streq(paramName, "x_shaft")) {
       param.x_shaft = value;
     } else if (streq(paramName, "distance_scale")) {
@@ -393,46 +253,9 @@ void StepMotorCtrlScriptCmderModule::regcmd() {  //
       param.run_to_zero_dec = value;
     } else {
       ZLOGE(TAG, "invalid param name:%s", paramName);
-      return;
-    }
-    ack = module->set_base_param(param);
-    if (ack != 0) {
-      ZLOGE(TAG, "set_base_param failed:%s(%d)", err::error2str(ack), ack);
-      return;
+      return (int32_t)err::kce_param_out_of_range;
     }
-    ZLOGI(TAG, "set_base_param ok");
-    return;
-  });
-
-  m_cmdScheduler->registerCmd("step_motor_ctrl_get_base_param", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    CHECK_ARGC(1);
-
-    int id = atoi(argv[1]);
-
-    I_StepMotorCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    I_StepMotorCtrlModule::base_param_t param;
-    int32_t                             ack = module->get_base_param(param);
-    if (ack != 0) {
-      ZLOGE(TAG, "get_base_param failed:%d", ack);
-      return;
-    }
-
-    ZLOGI(TAG, "param.x_shaft                       :%d", param.x_shaft);
-    ZLOGI(TAG, "param.distance_scale                :%d", param.distance_scale);
-    ZLOGI(TAG, "param.ihold                         :%d", param.ihold);
-    ZLOGI(TAG, "param.irun                          :%d", param.irun);
-    ZLOGI(TAG, "param.iholddelay                    :%d", param.iholddelay);
-    ZLOGI(TAG, "param.acc                           :%d", param.acc);
-    ZLOGI(TAG, "param.dec                           :%d", param.dec);
-    ZLOGI(TAG, "param.maxspeed                      :%d", param.maxspeed);
-    ZLOGI(TAG, "param.min_x                         :%d", param.min_x);
-    ZLOGI(TAG, "param.max_x                         :%d", param.max_x);
-    ZLOGI(TAG, "param.shift_x                       :%d", param.shift_x);
-    ZLOGI(TAG, "param.run_to_zero_move_to_zero_max_d:%d", param.run_to_zero_move_to_zero_max_d);
-    ZLOGI(TAG, "param.run_to_zero_leave_from_zero_max_d:%d", param.run_to_zero_leave_from_zero_max_d);
-    ZLOGI(TAG, "param.run_to_zero_speed             :%d", param.run_to_zero_speed);
-    ZLOGI(TAG, "param.run_to_zero_dec               :%d", param.run_to_zero_dec);
+    DO_CMD(module->set_base_param(param));
+    return (int32_t)0;
   });
 }

components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.hpp

@@ -9,6 +9,7 @@ namespace iflytop {
 class StepMotorCtrlScriptCmderModule {
   CmdScheduler*                        m_cmdScheduler;
   map<uint8_t, I_StepMotorCtrlModule*> moduler;
+  I_StepMotorCtrlModule*               module;
 
  public:
   void initialize(CmdScheduler* cmdScheduler);
@@ -16,7 +17,7 @@ class StepMotorCtrlScriptCmderModule {
   void regcmd();
 
  private:
-  I_StepMotorCtrlModule* findXYRobot(int id);
+  int32_t findmodule(int id, I_StepMotorCtrlModule** module);
 };
 
 }  // namespace iflytop

components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp

@@ -6,36 +6,63 @@
 using namespace iflytop;
 #define TAG "XYRobotScriptCmderModule"
 
-#define CHECK_ARGC(n)            \
-  if (argc != (n + 1)) {         \
-    ZLOGE(TAG, "argc != %d", n); \
-    context->breakflag = true;   \
-    return;                      \
-  }
-
-#define SCRIPT_CMDER_CHECK(exptr)          \
-  if (!(exptr)) {                          \
-    ZLOGE(TAG, "check %s failed", #exptr); \
-    context->breakflag = true;             \
-    return;                                \
-  }
-
 void XYRobotScriptCmderModule::initialize(CmdScheduler* cmdScheduler) {
   m_cmdScheduler = cmdScheduler;
   ZASSERT(m_cmdScheduler != nullptr);
   regcmd();
 }
-
-I_XYRobotCtrlModule* XYRobotScriptCmderModule::findXYRobot(int id) {
+int32_t XYRobotScriptCmderModule::findmodule(int id, I_XYRobotCtrlModule** module) {
   auto it = moduler.find(id);
   if (it == moduler.end()) {
-    return nullptr;
+    return err::kce_no_such_module;
   }
-  return it->second;
+  *module = it->second;
+  return 0;
 }
-
 static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; }
 
+static void cmd_dump_ack(I_XYRobotCtrlModule::version_t& ack) {  //
+  ZLOGI(TAG, "version:%d", ack.version);
+}
+
+static void cmd_dump_ack(I_XYRobotCtrlModule::status_t& ack) {  //
+  ZLOGI(TAG, "status    :%d", ack.status);
+  ZLOGI(TAG, "x         :%d", ack.x);
+  ZLOGI(TAG, "y         :%d", ack.y);
+  ZLOGI(TAG, "iostate   :%d", ack.iostate);
+}
+
+static void cmd_dump_ack(I_XYRobotCtrlModule::detailed_status_t& ack) {  //
+  ZLOGI(TAG, "status    :%d", ack.status);
+  ZLOGI(TAG, "x         :%d", ack.x);
+  ZLOGI(TAG, "y         :%d", ack.y);
+  ZLOGI(TAG, "iostate   :%d", ack.iostate);
+}
+
+static void cmd_dump_ack(I_XYRobotCtrlModule::base_param_t& ack) {
+  ZLOGI(TAG, "  robot_type            :%d", ack.robot_type);
+  ZLOGI(TAG, "  x_shaft               :%d", ack.x_shaft);
+  ZLOGI(TAG, "  y_shaft               :%d", ack.y_shaft);
+  ZLOGI(TAG, "  ihold                 :%d", ack.ihold);
+  ZLOGI(TAG, "  irun                  :%d", ack.irun);
+  ZLOGI(TAG, "  iholddelay            :%d", ack.iholddelay);
+  ZLOGI(TAG, "  distance_scale        :%d", ack.distance_scale);
+  ZLOGI(TAG, "  shift_x               :%d", ack.shift_x);
+  ZLOGI(TAG, "  shift_y               :%d", ack.shift_y);
+  ZLOGI(TAG, "  acc                   :%d", ack.acc);
+  ZLOGI(TAG, "  dec                   :%d", ack.dec);
+  ZLOGI(TAG, "  breakdec              :%d", ack.breakdec);
+  ZLOGI(TAG, "  maxspeed              :%d", ack.maxspeed);
+  ZLOGI(TAG, "  min_x                 :%d", ack.min_x);
+  ZLOGI(TAG, "  max_x                 :%d", ack.max_x);
+  ZLOGI(TAG, "  min_y                 :%d", ack.min_y);
+  ZLOGI(TAG, "  max_y                 :%d", ack.max_y);
+  ZLOGI(TAG, "  run_to_zero_max_d     :%d", ack.run_to_zero_max_d);
+  ZLOGI(TAG, "  leave_from_zero_max_d :%d", ack.leave_from_zero_max_d);
+  ZLOGI(TAG, "  run_to_zero_speed     :%d", ack.run_to_zero_speed);
+  ZLOGI(TAG, "  run_to_zero_dec       :%d", ack.run_to_zero_dec);
+}
+
 void XYRobotScriptCmderModule::regXYRobot(int id, I_XYRobotCtrlModule* robot) {
   ZASSERT(moduler.find(id) == moduler.end());
   ZASSERT(robot != nullptr);
@@ -44,171 +71,34 @@ void XYRobotScriptCmderModule::regXYRobot(int id, I_XYRobotCtrlModule* robot) {
 void XYRobotScriptCmderModule::regcmd() {  //
   ZASSERT(m_cmdScheduler != nullptr);
 
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_enable", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 3);
-
-    int  id     = atoi(argv[1]);
-    bool enable = atoi(argv[2]);
-
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->enable(enable);
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
-  });
-
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_stop", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 3);
-
-    int     id       = atoi(argv[1]);
-    uint8_t stopType = atoi(argv[2]);
-
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->stop(stopType);
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
-  });
-
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_move_to_zero", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 2);
-
-    int id = atoi(argv[1]);
-
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->move_to_zero([this](int32_t status) { ZLOGI(TAG, "move_to_zero status:%d", status); });
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
-  });
-
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_move_to_zero_with_calibrate", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 4);
-
-    int32_t id   = atoi(argv[1]);
-    int32_t nowx = atoi(argv[2]);
-    int32_t nowy = atoi(argv[3]);
-
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->move_to_zero_with_calibrate(nowx, nowy, [this](int32_t status) { ZLOGI(TAG, "move_to_zero_with_calibrate status:%d", status); });
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
-  });
-
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_move_to", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 5);
-
-    int32_t id    = atoi(argv[1]);
-    int32_t x     = atoi(argv[2]);
-    int32_t y     = atoi(argv[3]);
-    int32_t speed = atoi(argv[4]);
-
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->move_to(x, y, speed, [this](int32_t status) { ZLOGI(TAG, "move_to status:%d", status); });
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
-  });
-
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_move_by", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 5);
-
-    int32_t id = atoi(argv[1]);
-    int32_t dx = atoi(argv[2]);
-    int32_t dy = atoi(argv[3]);
-    int32_t v  = atoi(argv[4]);
-
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->move_by(dx, dy, v, [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); });
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
-  });
-
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_force_change_current_pos", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 4);
-
-    int32_t id = atoi(argv[1]);
-    int32_t x  = atoi(argv[2]);
-    int32_t y  = atoi(argv[3]);
-
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->force_change_current_pos(x, y);
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
-  });
-
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_read_version", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 2);
-
-    int32_t id = atoi(argv[1]);
-
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    I_XYRobotCtrlModule::version_t version;
-    int32_t                        ack = module->read_version(version);
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
-    if (ack == 0) {
-      ZLOGI(TAG, "version:%d", version.version);
-    }
-  });
-
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_read_status", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 2);
-
-    int32_t id = atoi(argv[1]);
-
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    I_XYRobotCtrlModule::status_t status;
-    int32_t                       ack = module->read_status(status);
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
-    if (ack == 0) {
-      ZLOGI(TAG, "  status:%d", status.status);
-      ZLOGI(TAG, "  x:%d", status.x);
-      ZLOGI(TAG, "  y:%d", status.y);
-      ZLOGI(TAG, "  iostate:%d", status.iostate);
-    }
-  });
-
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_read_detailed_status", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 2);
-
-    int32_t id = atoi(argv[1]);
-
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    I_XYRobotCtrlModule::detailed_status_t status;
-    int32_t                                ack = module->read_detailed_status(status);
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
-    if (ack == 0) {
-      ZLOGI(TAG, "  status:%d", status.status);
-      ZLOGI(TAG, "  x     :%d", status.x);
-      ZLOGI(TAG, "  y     :%d", status.y);
-      ZLOGI(TAG, "  iostate:%d", status.iostate);
-    }
-  });
-
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_set_base_param", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 4);
-    int32_t     id        = atoi(argv[1]);
-    const char* paramName = argv[2];
-    int32_t     paramVal  = atoi(argv[3]);
-
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
+#define PREIX "xy_robot_ctrl_"
+
+  REG_CMD(PREIX, enable, "(id,en)", 2, con->getBool(2));
+  REG_CMD(PREIX, stop, "(id,stop_type)", 2, con->getInt(2));
+  REG_CMD(PREIX, move_to_zero, "(id)", 1, [this](int32_t status) { ZLOGI(TAG, "move_to_zero status:%d", status); });
+  REG_CMD(PREIX, move_to_zero_with_calibrate, "(id,nowx,nowy)", 3, con->getInt(2), con->getInt(3), [this](int32_t status) { ZLOGI(TAG, "move_to_zero_with_calibrate status:%d", status); });
+  REG_CMD(PREIX, move_to, "(id,x,y,speed)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_to status:%d", status); });
+  REG_CMD(PREIX, move_by, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); });
+  REG_CMD(PREIX, force_change_current_pos, "(id,x,y)", 3, con->getInt(2), con->getInt(3));
+  REG_DUMP_ACK_CMD(PREIX, read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack);
+  REG_DUMP_ACK_CMD(PREIX, read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack);
+  REG_DUMP_ACK_CMD(PREIX, read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack);
+  REG_DUMP_ACK_CMD(PREIX, get_base_param, "(id)", 1, I_XYRobotCtrlModule::base_param_t, ack);
+  REG_DUMP_ACK_CMD("xy_robot_ctrl_", read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack);
+  REG_DUMP_ACK_CMD("xy_robot_ctrl_", read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack);
+  REG_DUMP_ACK_CMD("xy_robot_ctrl_", read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack);
+  REG_CMD("xy_robot_ctrl_", flush, "(id)", 1);
+  REG_CMD("xy_robot_ctrl_", factory_reset, "(id)", 1);
+  REG_CMD("xy_robot_ctrl_", move_by_no_limit, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by_no_limit status:%d", status); });
+
+  m_cmdScheduler->registerCmd("xy_robot_ctrl_set_base_param", "(id,paramName,val)", 3, [this](CmdScheduler::Context* con) {
+    const char* paramName = con->getString(2);
+    int32_t     paramVal  = con->getInt(3);
+
+    DO_CMD(findmodule(con->getInt(0), &module));
 
     I_XYRobotCtrlModule::base_param_t status;
-    int32_t                           ack = module->get_base_param(status);
-    if (ack != 0) {
-      ZLOGE(TAG, "get_base_param failed:%d", ack);
-      return;
-    }
+    DO_CMD(module->get_base_param(status));
 
     if (streq(paramName, "robot_type")) {
       status.robot_type = paramVal;
@@ -254,85 +144,11 @@ void XYRobotScriptCmderModule::regcmd() {  //
       status.run_to_zero_dec = paramVal;
     } else {
       ZLOGE(TAG, "invalid param name:%s", paramName);
-      return;
+   return(int32_t) err::kce_param_out_of_range;
     }
-    ack = module->set_base_param(status);
-    if (ack != 0) {
-      ZLOGE(TAG, "set_base_param failed:%d,%s", ack, err::error2str(ack));
-      return;
-    }
-    ZLOGI(TAG, "set_base_param ok");
-    return;
-  });
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_get_base_param", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 2);
-
-    int32_t              id     = atoi(argv[1]);
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    I_XYRobotCtrlModule::base_param_t status;
-    int32_t                           ack = module->get_base_param(status);
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
-    if (ack == 0) {
-      ZLOGI(TAG, "  robot_type            :%d", status.robot_type);
-      ZLOGI(TAG, "  x_shaft               :%d", status.x_shaft);
-      ZLOGI(TAG, "  y_shaft               :%d", status.y_shaft);
-      ZLOGI(TAG, "  ihold                 :%d", status.ihold);
-      ZLOGI(TAG, "  irun                  :%d", status.irun);
-      ZLOGI(TAG, "  iholddelay            :%d", status.iholddelay);
-      ZLOGI(TAG, "  distance_scale        :%d", status.distance_scale);
-      ZLOGI(TAG, "  shift_x               :%d", status.shift_x);
-      ZLOGI(TAG, "  shift_y               :%d", status.shift_y);
-      ZLOGI(TAG, "  acc                   :%d", status.acc);
-      ZLOGI(TAG, "  dec                   :%d", status.dec);
-      ZLOGI(TAG, "  breakdec              :%d", status.breakdec);
-      ZLOGI(TAG, "  maxspeed              :%d", status.maxspeed);
-      ZLOGI(TAG, "  min_x                 :%d", status.min_x);
-      ZLOGI(TAG, "  max_x                 :%d", status.max_x);
-      ZLOGI(TAG, "  min_y                 :%d", status.min_y);
-      ZLOGI(TAG, "  max_y                 :%d", status.max_y);
-      ZLOGI(TAG, "  run_to_zero_max_d     :%d", status.run_to_zero_max_d);
-      ZLOGI(TAG, "  leave_from_zero_max_d :%d", status.leave_from_zero_max_d);
-      ZLOGI(TAG, "  run_to_zero_speed     :%d", status.run_to_zero_speed);
-      ZLOGI(TAG, "  run_to_zero_dec       :%d", status.run_to_zero_dec);
-    }
-  });
-
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_flush", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 2);
 
-    int32_t              id     = atoi(argv[1]);
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
+    DO_CMD(module->set_base_param(status));
+    return (int32_t)0;
 
-    int32_t ack = module->flush();
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
   });
-
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_factory_reset", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 2);
-
-    int32_t              id     = atoi(argv[1]);
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->factory_reset();
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
-  });
-
-  m_cmdScheduler->registerCmd("xy_robot_ctrl_move_by_no_limit", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
-    SCRIPT_CMDER_CHECK(argc == 5);
-
-    int32_t id = atoi(argv[1]);
-    int32_t dx = atoi(argv[2]);
-    int32_t dy = atoi(argv[3]);
-    int32_t v  = atoi(argv[4]);
-
-    I_XYRobotCtrlModule* module = findXYRobot(id);
-    SCRIPT_CMDER_CHECK(module != nullptr);
-
-    int32_t ack = module->move_by_no_limit(dx, dy, v, [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); });
-    ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
-  });
-}
+}

components/xy_robot_ctrl_module/xy_robot_script_cmder_module.hpp

@@ -9,6 +9,7 @@ namespace iflytop {
 class XYRobotScriptCmderModule {
   CmdScheduler*                      m_cmdScheduler;
   map<uint8_t, I_XYRobotCtrlModule*> moduler;
+  I_XYRobotCtrlModule*               module;
 
  public:
   void initialize(CmdScheduler* cmdScheduler);
@@ -16,7 +17,7 @@ class XYRobotScriptCmderModule {
   void regcmd();
 
  private:
-  I_XYRobotCtrlModule* findXYRobot(int id);
+  int32_t findmodule(int id, I_XYRobotCtrlModule** module);
 };
 
 }  // namespace iflytop

components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp

@@ -54,21 +54,14 @@ void ZCanMiniServoCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
   PROCESS_PACKET(kcmd_mini_servo_ctrl_read_detailed_status, m_id) { errorcode = m_module->read_detailed_status(ack->ack); }
   END_PP();
 
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_set_run_param, m_id) { errorcode = m_module->set_run_param(cmd->param); }
-  END_PP();
+
 
   PROCESS_PACKET(kcmd_mini_servo_ctrl_set_basic_param, m_id) { errorcode = m_module->set_basic_param(cmd->param); }
   END_PP();
 
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_set_warning_limit_param, m_id) { errorcode = m_module->set_warning_limit_param(cmd->param); }
-  END_PP();
 
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_get_run_param, m_id) { errorcode = m_module->get_run_param(ack->ack); }
-  END_PP();
 
   PROCESS_PACKET(kcmd_mini_servo_ctrl_get_basic_param, m_id) { errorcode = m_module->get_basic_param(ack->ack); }
   END_PP();
 
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_get_warning_limit_param, m_id) { errorcode = m_module->get_warning_limit_param(ack->ack); }
-  END_PP();
 }

components/zprotocols/errorcode

@@ -1 +1 @@
-Subproject commit d6cf422a398a4a0a1883f2fbd17c33def6d3beac
+Subproject commit 0d0b37363452b7ae7ca442bf924bf8ba021b5a62

os/zoslogger.cpp

@@ -10,6 +10,7 @@ extern "C" {
 
 void zos_loggger_init() { glog_mutex.init(); }
 void zos_log(const char* fmt, ...) {
+  iflytop::zlock_guard lock(glog_mutex);
   if (g_enable_log) {
     va_list args;
     va_start(args, fmt);

os/zoslogger.hpp

@@ -11,28 +11,23 @@ extern bool g_enable_log;
 
 #define ZLOG_RELEASE(TAG, fmt, ...)         \
   if (g_enable_log) {                       \
-    iflytop::zlock_guard lock(glog_mutex);  \
     zos_log(TAG "" fmt "\n", ##__VA_ARGS__); \
   }
 #define ZLOGI(TAG, fmt, ...)                                                             \
   if (g_enable_log) {                                                                    \
-    iflytop::zlock_guard lock(glog_mutex);                                               \
     zos_log("%08lu INFO [%-8s] " fmt "\n", zchip_clock_get_ticket(), TAG, ##__VA_ARGS__); \
   }
 #define ZLOGD(TAG, fmt, ...)                                                             \
   if (g_enable_log) {                                                                    \
-    iflytop::zlock_guard lock(glog_mutex);                                               \
     zos_log("%08lu DEBU [%-8s] " fmt "\n", zchip_clock_get_ticket(), TAG, ##__VA_ARGS__); \
   }
 #define ZLOGE(TAG, fmt, ...)                                                             \
   if (g_enable_log) {                                                                    \
-    iflytop::zlock_guard lock(glog_mutex);                                               \
     zos_log("%08lu ERRO [%-8s] " fmt "\n", zchip_clock_get_ticket(), TAG, ##__VA_ARGS__); \
   }
 
 #define ZLOGW(TAG, fmt, ...)                                                             \
   if (g_enable_log) {                                                                    \
-    iflytop::zlock_guard lock(glog_mutex);                                               \
     zos_log("%08lu WARN [%-8s] " fmt "\n", zchip_clock_get_ticket(), TAG, ##__VA_ARGS__); \
   }