update

components/cmdscheduler/cmd_scheduler_v2.hpp

@@ -46,4 +46,65 @@ class CmdSchedulerV2 : public ICmdParser {
   void    dumpack(ICmdParserACK* ack);
 };
 
+class __Params {
+  int32_t      m_paramN;
+  const char** m_paraV;
+
+ public:
+  __Params(int32_t paramN, const char** paraV) : m_paramN(paramN), m_paraV(paraV) {}
+
+  int32_t getInt(int32_t index) {
+    index -= 1;
+    if (index >= m_paramN) {
+      return 0;
+    }
+    if (index < 0) {
+      return 0;
+    }
+    return atoi(m_paraV[index]);
+  }
+
+  bool getBool(int32_t index) {
+    index -= 1;
+    if (index >= m_paramN) {
+      return false;
+    }
+    if (index < 0) {
+      return false;
+    }
+    return atoi(m_paraV[index]) != 0;
+  }
+};
+
+#define DO_CMD(cond)    \
+  {                     \
+    int32_t ret = cond; \
+    if (ret != 0) {     \
+      return ret;       \
+    }                   \
+  }
+#define IMPL_CMD(cmd, ...)                    \
+  __Params con(paramN, paraV);                \
+  DO_CMD(findmodule(con.getInt(1), &module)); \
+  DO_CMD(module->cmd(__VA_ARGS__));           \
+  return (int32_t)0;
+
+#define IMPL_READ_STATE(cmd, ...)             \
+  __Params con(paramN, paraV);                \
+  DO_CMD(findmodule(con.getInt(1), &module)); \
+  DO_CMD(module->cmd(__VA_ARGS__));           \
+  cmd_dump_ack(ack);                          \
+  return (int32_t)0;
+
+#define REG_CMD___NO_ACK(prefix, cmd, para, npara, ...)                                                                                                /**/ \
+  m_cmdScheduler->registerCmd(prefix #cmd, para, npara, [this](int32_t paramN, const char** paraV, ICmdParserACK* __ack) {                             /**/ \
+                                                                                                                           IMPL_CMD(cmd, __VA_ARGS__); /**/ \
+  });
+
+#define REG_CMD_WITH_ACK(prefix, cmd, para, npara, acktype, ...)                                                                                              /**/ \
+  m_cmdScheduler->registerCmd(prefix #cmd, para, npara, [this](int32_t paramN, const char** paraV, ICmdParserACK* __ack) {                                    /**/ \
+                                                                                                                           acktype ack;                       /**/ \
+                                                                                                                           IMPL_READ_STATE(cmd, __VA_ARGS__); /**/ \
+  });
+
 }  // namespace iflytop

components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp

@@ -7,7 +7,7 @@
 using namespace iflytop;
 #define TAG "CMD"
 
-void ScriptCmderEq20Servomotor::initialize(CmdScheduler* cmdScheduler) {
+void ScriptCmderEq20Servomotor::initialize(CmdSchedulerV2* cmdScheduler) {
   m_cmdScheduler = cmdScheduler;
   ZASSERT(m_cmdScheduler != nullptr);
   regcmd();
@@ -48,7 +48,7 @@ static void cmd_dump_ack(Eq20ServoMotor::servo_internal_status_t ack) {  //
   ZLOGI(TAG, "encoder_battery_warning:%d", ack.data.sbit.encoder_battery_warning);
   ZLOGI(TAG, "encoder_battery_report :%d", ack.data.sbit.encoder_battery_report);
 }
-//static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; }
+// static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; }
 
 void ScriptCmderEq20Servomotor::regmodule(int id, Eq20ServoMotor* robot) {
   ZASSERT(moduler.find(id) == moduler.end());
@@ -57,24 +57,24 @@ void ScriptCmderEq20Servomotor::regmodule(int id, Eq20ServoMotor* robot) {
 }
 
 void ScriptCmderEq20Servomotor::regcmd() {
-  REG_CMD___NO_ACK("eq20_", enable, "(id,en)", 2, con->getBool(2));
-  REG_CMD___NO_ACK("eq20_", move_to, "(id,pos,rpm,acctime)", 4, con->getInt(2), con->getInt(3), con->getInt(4));
-  REG_CMD___NO_ACK("eq20_", move_by, "(id,pos,rpm,acctime)", 4, con->getInt(2), con->getInt(3), con->getInt(4));
-  REG_CMD___NO_ACK("eq20_", rotate, "(id,rpm,acctime)", 3, con->getInt(2), con->getInt(3));
-  REG_CMD___NO_ACK("eq20_", move_to_zero_forward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, con->getInt(2), con->getInt(3), con->getInt(4));
-  REG_CMD___NO_ACK("eq20_", move_to_zero_backward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, con->getInt(2), con->getInt(3), con->getInt(4));
+  REG_CMD___NO_ACK("eq20_", enable, "(id,en)", 2, con.getBool(2));
+  REG_CMD___NO_ACK("eq20_", move_to, "(id,pos,rpm,acctime)", 4, con.getInt(2), con.getInt(3), con.getInt(4));
+  REG_CMD___NO_ACK("eq20_", move_by, "(id,pos,rpm,acctime)", 4, con.getInt(2), con.getInt(3), con.getInt(4));
+  REG_CMD___NO_ACK("eq20_", rotate, "(id,rpm,acctime)", 3, con.getInt(2), con.getInt(3));
+  REG_CMD___NO_ACK("eq20_", move_to_zero_forward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, con.getInt(2), con.getInt(3), con.getInt(4));
+  REG_CMD___NO_ACK("eq20_", move_to_zero_backward, "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, con.getInt(2), con.getInt(3), con.getInt(4));
   REG_CMD___NO_ACK("eq20_", stop, "(id)", 1);
   REG_CMD_WITH_ACK("eq20_", get_pos, "(id)", 1, int32_t, ack);
   REG_CMD_WITH_ACK("eq20_", get_servo_internal_state, "(id)", 1, Eq20ServoMotor::servo_internal_status_t, ack);
   REG_CMD_WITH_ACK("eq20_", get_io_state, "(id)", 1, int32_t, ack);
 
-  REG_CMD_WITH_ACK("eq20_", read_pn, "(id,pnadd)", 2, int32_t, con->getInt(2), ack);
-  REG_CMD___NO_ACK("eq20_", write_pn, "(id,pnadd,value)", 3, con->getInt(2), con->getInt(3));
-  REG_CMD___NO_ACK("eq20_", write_pn_bit, "(id,pnadd,off,value)", 4, con->getInt(2), con->getInt(3), con->getInt(4));
-  REG_CMD_WITH_ACK("eq20_", read_pn_bit, "(id,pnadd,off)", 3, int32_t, con->getInt(2), con->getInt(3), ack);
+  REG_CMD_WITH_ACK("eq20_", read_pn, "(id,pnadd)", 2, int32_t, con.getInt(2), ack);
+  REG_CMD___NO_ACK("eq20_", write_pn, "(id,pnadd,value)", 3, con.getInt(2), con.getInt(3));
+  REG_CMD___NO_ACK("eq20_", write_pn_bit, "(id,pnadd,off,value)", 4, con.getInt(2), con.getInt(3), con.getInt(4));
+  REG_CMD_WITH_ACK("eq20_", read_pn_bit, "(id,pnadd,off)", 3, int32_t, con.getInt(2), con.getInt(3), ack);
 
-  REG_CMD___NO_ACK("eq20_", write_reg, "(id,regadd,value)", 3, con->getInt(2), con->getInt(3));
-  REG_CMD_WITH_ACK("eq20_", read_reg, "(id,regadd)", 2, int32_t, con->getInt(2), ack);
-  REG_CMD___NO_ACK("eq20_", write_reg_bit, "(id,regadd,off,value)", 4, con->getInt(2), con->getInt(3), con->getInt(4));
-  REG_CMD_WITH_ACK("eq20_", read_reg_bit, "(id,regadd,off)", 3, int32_t, con->getInt(2), con->getInt(3), ack);
+  REG_CMD___NO_ACK("eq20_", write_reg, "(id,regadd,value)", 3, con.getInt(2), con.getInt(3));
+  REG_CMD_WITH_ACK("eq20_", read_reg, "(id,regadd)", 2, int32_t, con.getInt(2), ack);
+  REG_CMD___NO_ACK("eq20_", write_reg_bit, "(id,regadd,off,value)", 4, con.getInt(2), con.getInt(3), con.getInt(4));
+  REG_CMD_WITH_ACK("eq20_", read_reg_bit, "(id,regadd,off)", 3, int32_t, con.getInt(2), con.getInt(3), ack);
 }

components/eq_20_asb_motor/script_cmder_eq20_servomotor.hpp

@@ -3,17 +3,18 @@
 
 #include "eq20_servomotor.hpp"
 #include "sdk/os/zos.hpp"
-#include "sdk\components\cmdscheduler\cmd_scheduler.hpp"
+// #include "sdk\components\cmdscheduler\cmd_scheduler.hpp"
+#include "sdk/components/cmdscheduler/cmd_scheduler_v2.hpp"
 namespace iflytop {
 
 class ScriptCmderEq20Servomotor {
-  CmdScheduler*                 m_cmdScheduler;
+  CmdSchedulerV2*                 m_cmdScheduler;
   map<uint8_t, Eq20ServoMotor*> moduler;
 
   Eq20ServoMotor* module;
 
  public:
-  void initialize(CmdScheduler* cmdScheduler);
+  void initialize(CmdSchedulerV2* cmdScheduler);
   void regmodule(int id, Eq20ServoMotor* robot);
   void regcmd();
 

components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp

@@ -34,7 +34,7 @@ static void cmd_dump_ack(I_MiniServoModule::basic_param_t& ack) {  //
   ZLOGI(TAG, "protectTorque:  %d", ack.protectTorque);
 }
 
-void ScirptCmderMiniServoMotorCtrlModule::initialize(CmdScheduler* cmdScheduler) {
+void ScirptCmderMiniServoMotorCtrlModule::initialize(CmdSchedulerV2* cmdScheduler) {
   m_cmdScheduler = cmdScheduler;
   ZASSERT(m_cmdScheduler != nullptr);
   regcmd();
@@ -58,14 +58,14 @@ void ScirptCmderMiniServoMotorCtrlModule::regmodule(int id, I_MiniServoModule* r
 }
 
 void ScirptCmderMiniServoMotorCtrlModule::regcmd() {
-  REG_CMD___NO_ACK("mini_servo_", enable, "(id,en)", 2, con->getBool(2));
-  REG_CMD___NO_ACK("mini_servo_", stop, "(id,stop_type)", 2, con->getInt(2));
-  REG_CMD___NO_ACK("mini_servo_", position_calibrate, "(id,pos)", 2, con->getInt(2));
-  REG_CMD___NO_ACK("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___NO_ACK("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___NO_ACK("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___NO_ACK("mini_servo_", move_forward, "(id,torque)", 2, con->getInt(2));
-  REG_CMD___NO_ACK("mini_servo_", move_backward, "(id,torque)", 2, con->getInt(2));
+  REG_CMD___NO_ACK("mini_servo_", enable, "(id,en)", 2, con.getBool(2));
+  REG_CMD___NO_ACK("mini_servo_", stop, "(id,stop_type)", 2, con.getInt(2));
+  REG_CMD___NO_ACK("mini_servo_", position_calibrate, "(id,pos)", 2, con.getInt(2));
+  REG_CMD___NO_ACK("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___NO_ACK("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___NO_ACK("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___NO_ACK("mini_servo_", move_forward, "(id,torque)", 2, con.getInt(2));
+  REG_CMD___NO_ACK("mini_servo_", move_backward, "(id,torque)", 2, con.getInt(2));
   REG_CMD_WITH_ACK("mini_servo_", read_version, "(id)", 1, I_MiniServoModule::version_t, ack);
   REG_CMD_WITH_ACK("mini_servo_", read_status, "(id)", 1, I_MiniServoModule::status_t, ack);
   REG_CMD_WITH_ACK("mini_servo_", read_detailed_status, "(id)", 1, I_MiniServoModule::detailed_status_t, ack);

components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.hpp

@@ -2,18 +2,18 @@
 #include <map>
 
 #include "sdk/os/zos.hpp"
-#include "sdk\components\cmdscheduler\cmd_scheduler.hpp"
+#include "sdk\components\cmdscheduler\cmd_scheduler_v2.hpp"
 #include "sdk\components\zprotocols\zcancmder\api\i_mini_servo_module.hpp"
 namespace iflytop {
 
 class ScirptCmderMiniServoMotorCtrlModule {
-  CmdScheduler*                    m_cmdScheduler;
+  CmdSchedulerV2*                    m_cmdScheduler;
   map<uint8_t, I_MiniServoModule*> moduler;
 
   I_MiniServoModule* module;
 
  public:
-  void initialize(CmdScheduler* cmdScheduler);
+  void initialize(CmdSchedulerV2* cmdScheduler);
   void regmodule(int id, I_MiniServoModule* robot);
   void regcmd();
 

components/step_motor_45/script_cmder_step_motor_45.cpp

@@ -7,7 +7,7 @@
 using namespace iflytop;
 #define TAG "CMD"
 
-void ScriptCmderStepMotor45::initialize(CmdScheduler* cmdScheduler) {
+void ScriptCmderStepMotor45::initialize(CmdSchedulerV2* cmdScheduler) {
   m_cmdScheduler = cmdScheduler;
   ZASSERT(m_cmdScheduler != nullptr);
   regcmd();
@@ -36,17 +36,17 @@ static void cmd_dump_ack(bool& ack) { ZLOGI(TAG, "ack %d", ack); }
 void ScriptCmderStepMotor45::regcmd() {
 #define PREFIX "step_motor_45_"
 
-  REG_CMD___NO_ACK(PREFIX, rotate, "(id,direction)", 2, con->getInt(2));
-  REG_CMD___NO_ACK(PREFIX, move_by, "(id,pos)", 2, con->getInt(2));
-  REG_CMD___NO_ACK(PREFIX, move_to, "(id,pos)", 2, con->getInt(2));
-  REG_CMD___NO_ACK(PREFIX, set_default_speed, "(id,speed)", 2, con->getInt(2));
+  REG_CMD___NO_ACK(PREFIX, rotate, "(id,direction)", 2, con.getInt(2));
+  REG_CMD___NO_ACK(PREFIX, move_by, "(id,pos)", 2, con.getInt(2));
+  REG_CMD___NO_ACK(PREFIX, move_to, "(id,pos)", 2, con.getInt(2));
+  REG_CMD___NO_ACK(PREFIX, set_default_speed, "(id,speed)", 2, con.getInt(2));
   REG_CMD_WITH_ACK(PREFIX, get_default_speed, "(id)", 1, int32_t, ack);
   REG_CMD_WITH_ACK(PREFIX, is_reach_target_pos, "(id)", 1, bool, ack);
   REG_CMD___NO_ACK(PREFIX, stop, "(id)", 1);
   REG_CMD___NO_ACK(PREFIX, zero_calibration, "(id)", 1);
   REG_CMD_WITH_ACK(PREFIX, get_pos, "(id)", 1, int32_t, ack);
-  REG_CMD___NO_ACK(PREFIX, set_pos, "(id,pos)", 2, con->getInt(2));
+  REG_CMD___NO_ACK(PREFIX, set_pos, "(id,pos)", 2, con.getInt(2));
   REG_CMD_WITH_ACK(PREFIX, get_zero_pin_state, "(id,pos)", 1, int32_t, ack);
 
-  // REG_CMD___NO_ACK(PREFIX, initialize, "(id,driverPin1,driverPin2,driverPin3,driverPin4,zeroPin)", 6, con->getInt(2), con->getInt(3), con->getInt(4), con->getInt(5), con->getInt(6), con->getInt(7));
+  // REG_CMD___NO_ACK(PREFIX, initialize, "(id,driverPin1,driverPin2,driverPin3,driverPin4,zeroPin)", 6, con.getInt(2), con.getInt(3), con.getInt(4), con.getInt(5), con.getInt(6), con.getInt(7));
 }

components/step_motor_45/script_cmder_step_motor_45.hpp

@@ -2,17 +2,17 @@
 #include <map>
 
 #include "sdk/os/zos.hpp"
-#include "sdk\components\cmdscheduler\cmd_scheduler.hpp"
+#include "sdk\components\cmdscheduler\cmd_scheduler_v2.hpp"
 #include "step_motor_45.hpp"
 namespace iflytop {
 
 class ScriptCmderStepMotor45 {
-  CmdScheduler*              m_cmdScheduler;
+  CmdSchedulerV2*              m_cmdScheduler;
   map<uint8_t, StepMotor45*> moduler;
   StepMotor45* module;
 
  public:
-  void initialize(CmdScheduler* cmdScheduler);
+  void initialize(CmdSchedulerV2* cmdScheduler);
   void regmodule(int id, StepMotor45* robot);
   void regcmd();