update

components/cmdscheduler/cmd_scheduler.cpp

@@ -133,7 +133,7 @@ int32_t CmdScheduler::callcmd(const char* cmd) {
 
 void CmdScheduler::remove_note(char* input, int inputlen) {
   bool detect_note = false;
-  for (size_t i = 0; i < inputlen; i++) {
+  for (int i = 0; i < inputlen; i++) {
     if (!detect_note && input[i] == '#') {
       detect_note = true;
     }
@@ -144,7 +144,7 @@ void CmdScheduler::remove_note(char* input, int inputlen) {
 }
 
 void CmdScheduler::prase_cmd(char* input, int inputlen, int& argc, char* argv[]) {
-  for (size_t i = 0; input[i] == 0 || i < inputlen; i++) {
+  for (int i = 0; input[i] == 0 || i < inputlen; i++) {
     if (input[i] == ' ' || input[i] == '\r' || input[i] == '\n') {
       input[i] = 0;
     }

components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp

@@ -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());

components/mini_servo_motor/feite_servo_motor.hpp

@@ -141,6 +141,12 @@ class FeiTeServoMotor {
   bool getTorqueSwitch(uint8_t id, bool& on);
 
   bool getNowPos(uint8_t id, int16_t& pos);
+  bool getNowPos(uint8_t id, int32_t& pos) {
+    int16_t pos16;
+    bool    ret = getNowPos(id, pos16);
+    pos         = pos16;
+    return ret;
+  }
   bool setTargetPos(uint8_t id, int16_t pos);
 
   bool getServoCalibration(uint8_t, int16_t& poscalibration);

components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -10,9 +10,11 @@ static void limit_input(s32& input, s32 min, s32 max) {
   if (input < min) input = min;
 }
 
-void MiniRobotCtrlModule::initialize(FeiTeServoMotor* bus, uint8_t id) {
+void MiniRobotCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus) {
   m_bus       = bus;
-  m_id  = id;
+  m_id        = idinbus;
+  m_module_id = module_id;
+
   m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode);
   m_thread.init("MiniRobotCtrlModule", 1024, osPriorityNormal);
 }
@@ -263,3 +265,64 @@ void MiniRobotCtrlModule::call_status_cb(action_cb_status_t cb, int32_t status)
   m_last_exec_status = status;
   if (cb) cb(status);
 }
+
+/*******************************************************************************
+ *                               OVERRIDE MODULE                               *
+ *******************************************************************************/
+
+int32_t MiniRobotCtrlModule::getid(int32_t *id) {
+  *id = m_module_id;
+  return 0;
+}
+int32_t MiniRobotCtrlModule::module_stop() { return stop(0); }
+int32_t MiniRobotCtrlModule::module_break() { return stop(0); }
+int32_t MiniRobotCtrlModule::module_get_last_exec_status(int32_t *status) {
+  *status = m_last_exec_status;
+  return 0;
+}
+int32_t MiniRobotCtrlModule::module_get_status(int32_t *status) {
+  *status = m_thread.isworking();
+  return 0;
+}
+int32_t MiniRobotCtrlModule::module_get_error(int32_t *iserror) {
+  /**
+   * @brief TODO:
+   *  Ìí¼Ó¹ýÔؼì²â
+   */
+  *iserror = 0;
+  return 0;
+}
+int32_t MiniRobotCtrlModule::module_clear_error() { return 0; }
+
+int32_t MiniRobotCtrlModule::module_set_param(int32_t param_id, int32_t param_value) { return err::kmodule_not_find_config_index; }
+int32_t MiniRobotCtrlModule::module_get_param(int32_t param_id, int32_t *param_value) { return err::kmodule_not_find_config_index; }
+int32_t MiniRobotCtrlModule::module_readio(int32_t *io) {
+  *io = 0;
+  return 0;
+}
+int32_t MiniRobotCtrlModule::module_writeio(int32_t io) { return 0; }
+int32_t MiniRobotCtrlModule::module_read_adc(int32_t adcindex, int32_t *adc) {
+  *adc = 0;
+  return 0;
+}
+
+/*******************************************************************************
+ *                                    Motor                                    *
+ *******************************************************************************/
+int32_t MiniRobotCtrlModule::motor_enable(int32_t varenable) { return enable(varenable); }
+int32_t MiniRobotCtrlModule::motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { return rotate(direction, motor_velocity, acc, nullptr); }
+int32_t MiniRobotCtrlModule::motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) { return move_by(distance, motor_velocity, acc, nullptr); }
+int32_t MiniRobotCtrlModule::motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) { return move_to(position, motor_velocity, acc, nullptr); }
+int32_t MiniRobotCtrlModule::motor_move_to_with_torque(int32_t pos, int32_t torque) { return move_to(pos, 0, torque, nullptr); }
+
+int32_t MiniRobotCtrlModule::motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; }
+int32_t MiniRobotCtrlModule::motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; }
+int32_t MiniRobotCtrlModule::motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; }
+
+int32_t MiniRobotCtrlModule::motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return err::koperation_not_support; }
+int32_t MiniRobotCtrlModule::motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return move_to(0, findzerospeed, acc, nullptr); }
+
+int32_t MiniRobotCtrlModule::motor_read_pos(int32_t *pos) {
+  if (!m_bus->getNowPos(m_id, *pos)) return err::ksubdevice_overtime;
+  return 0;
+}

components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp

@@ -4,18 +4,20 @@
 //
 #include "feite_servo_motor.hpp"
 #include "sdk/components/zprotocols/zcancmder/api/i_mini_servo_module.hpp"
+#include "sdk\components\zprotocols\zcancmder_v2\api\api.hpp"
 
 namespace iflytop {
-class MiniRobotCtrlModule : public I_MiniServoModule {
+class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZIMotor {
   FeiTeServoMotor *m_bus;
   uint8_t          m_id;
+  uint16_t         m_module_id;
   ZThread          m_thread;
   s32              m_pos_shift;
 
   s32 m_last_exec_status = 0;
 
  public:
-  void initialize(FeiTeServoMotor* bus, uint8_t id);
+  void initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus);
 
   virtual int32_t enable(u8 enable) override;
   virtual int32_t stop(u8 stop_type) override;
@@ -35,13 +37,46 @@ 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_basic_param(basic_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_basic_param(basic_param_t &param) override;
-  // virtual int32_t get_warning_limit_param(warning_limit_param_t& param) override;
+
+  /*******************************************************************************
+   *                               OVERRIDE MODULE                               *
+   *******************************************************************************/
+
+  virtual int32_t getid(int32_t *id) override;
+  virtual int32_t module_stop() override;
+  virtual int32_t module_break() override;
+  virtual int32_t module_get_last_exec_status(int32_t *status) override;
+  virtual int32_t module_get_status(int32_t *status) override;
+  virtual int32_t module_get_error(int32_t *iserror) override;
+  virtual int32_t module_clear_error() override;
+
+  virtual int32_t module_set_param(int32_t param_id, int32_t param_value) override;
+  virtual int32_t module_get_param(int32_t param_id, int32_t *param_value) override;
+
+  virtual int32_t module_readio(int32_t *io) override;
+  virtual int32_t module_writeio(int32_t io) override;
+
+  virtual int32_t module_read_adc(int32_t adcindex, int32_t *adc) override;
+
+  /*******************************************************************************
+   *                                    Motor                                    *
+   *******************************************************************************/
+  virtual int32_t motor_enable(int32_t enable) override;
+  virtual int32_t motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) override;
+  virtual int32_t motor_move_by(int32_t distance, int32_t motor_velocity, int32_t acc) override;
+  virtual int32_t motor_move_to(int32_t position, int32_t motor_velocity, int32_t acc) override;
+  virtual int32_t motor_move_to_with_torque(int32_t pos, int32_t torque) override;
+
+  virtual int32_t motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) override;
+  virtual int32_t motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) override;
+  virtual int32_t motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) override;
+
+  virtual int32_t motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
+  virtual int32_t motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
+
+  virtual int32_t motor_read_pos(int32_t *pos);
 
  private:
   void call_status_cb(action_cb_status_t cb, int32_t status);

components/zprotocols/zcancmder_v2

@@ -1 +1 @@
-Subproject commit 115dbcfc41877dd4ceb3412df2838322ce7f09e2
+Subproject commit 5abf00ddaeca30ef08bad4b8b69ccfa7b074d161