add default velocity to servo

components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -110,14 +110,14 @@ int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, action_cb_s
   return 0;
 }
 
-int32_t MiniRobotCtrlModule::move_forward(s32 torque) {
+int32_t MiniRobotCtrlModule::move_forward(s32 velocity, s32 torque) {
   ZLOGI(TAG, "%d move_forward torque:%d", m_module_id, torque);
 
   if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
   limit_input(torque, 0, 1000);
 
   m_thread.stop();
-  if (!m_bus->moveTo(m_id, 4095, 0, torque)) return err::ksubdevice_overtime;
+  if (!m_bus->moveTo(m_id, 4095, velocity, torque)) return err::ksubdevice_overtime;
   m_thread.start([this, torque]() {
     uint8_t moveflag = 0;
     while (true) {
@@ -134,14 +134,14 @@ int32_t MiniRobotCtrlModule::move_forward(s32 torque) {
 
   return 0;
 }
-int32_t MiniRobotCtrlModule::move_backward(s32 torque) {
+int32_t MiniRobotCtrlModule::move_backward(s32 velocity, s32 torque) {
   ZLOGI(TAG, "%d move_backward torque:%d", m_module_id, torque);
 
   if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
   limit_input(torque, 0, 1000);
 
   m_thread.stop();
-  if (!m_bus->moveTo(m_id, 0, 0, torque)) return err::ksubdevice_overtime;
+  if (!m_bus->moveTo(m_id, 0, velocity, torque)) return err::ksubdevice_overtime;
   m_thread.start([this, torque]() {
     uint8_t moveflag = 0;
     while (true) {
@@ -334,6 +334,7 @@ int32_t MiniRobotCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t
   switch (param_id) {
     MODULE_COMMON_PROCESS_REG_CB();
     PROCESS_REG(kreg_motor_default_torque, REG_GET(m_cfg.default_torque), REG_SET(m_cfg.default_torque));
+    PROCESS_REG(kreg_motor_default_velocity, REG_GET(m_cfg.default_velocity), REG_SET(m_cfg.default_velocity));
     PROCESS_REG(kreg_module_input_state, module_readio(&val), ACTION_NONE);
     PROCESS_REG(kreg_robot_pos, motor_read_pos(&val), ACTION_NONE);
     default:
@@ -353,9 +354,9 @@ int32_t MiniRobotCtrlModule::motor_move_by(int32_t distance, int32_t motor_veloc
 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_rotate_with_torque(int32_t direction, int32_t torque) {
   if (direction >= 0) {
-    return move_forward(torque);
+    return move_forward(m_cfg.default_velocity, torque);
   } else {
-    return move_backward(torque);
+    return move_backward(m_cfg.default_velocity, torque);
   }
 }
 void MiniRobotCtrlModule::set_errorcode(int32_t errorcode) { m_com_reg.module_errorcode = errorcode; }
@@ -420,15 +421,15 @@ int32_t MiniRobotCtrlModule::motor_read_pos(int32_t *pos) {
 }
 int32_t MiniRobotCtrlModule::motor_easy_rotate(int32_t direction) {
   if (direction == 1) {
-    return move_forward(m_cfg.default_torque);
+    return move_forward(m_cfg.default_velocity, m_cfg.default_torque);
   } else if (direction == -1) {
-    return move_backward(m_cfg.default_torque);
+    return move_backward(m_cfg.default_velocity, m_cfg.default_torque);
   } else {
     return err::kparam_out_of_range;
   }
 }
-int32_t MiniRobotCtrlModule::motor_easy_move_by(int32_t distance) { return move_by(distance, 0, m_cfg.default_torque, nullptr); }
-int32_t MiniRobotCtrlModule::motor_easy_move_to(int32_t position) { return move_to(position, 0, m_cfg.default_torque, nullptr); }
+int32_t MiniRobotCtrlModule::motor_easy_move_by(int32_t distance) { return move_by(distance, m_cfg.default_velocity, m_cfg.default_torque, nullptr); }
+int32_t MiniRobotCtrlModule::motor_easy_move_to(int32_t position) { return move_to(position, m_cfg.default_velocity, m_cfg.default_torque, nullptr); }
 int32_t MiniRobotCtrlModule::motor_easy_move_to_zero(int32_t direction) { return motor_easy_move_to(0); }
 
 int32_t MiniRobotCtrlModule::motor_easy_set_current_pos(int32_t pos) { return position_calibrate(pos); }

components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp

@@ -13,6 +13,7 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI
  public:
   typedef struct {
     s32 default_torque;
+    s32 default_velocity;
   } flash_config_t;
 
  private:
@@ -29,26 +30,26 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI
   void        initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus, flash_config_t *cfg);
   static void create_default_config(flash_config_t &cfg);
 
-  virtual int32_t enable(u8 enable) override;
-  virtual int32_t stop(u8 stop_type) override;
-  virtual int32_t position_calibrate(s32 nowpos) override;
+  virtual int32_t enable(u8 enable);
+  virtual int32_t stop(u8 stop_type);
+  virtual int32_t position_calibrate(s32 nowpos);
 
-  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 rotate(s32 speed, s32 torque, s32 run_time, action_cb_status_t status_cb);
+  virtual int32_t move_to(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb);
+  virtual int32_t move_by(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb);
 
-  virtual int32_t move_forward(s32 torque);
-  virtual int32_t move_backward(s32 torque);
+  virtual int32_t move_forward(s32 velocity, s32 torque);
+  virtual int32_t move_backward(s32 velocity, 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;
+  virtual int32_t run_with_torque(s32 torque, s32 run_time, action_cb_status_t status_cb);
+  virtual int32_t move_by_nolimit(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb);
 
-  virtual int32_t read_version(version_t &version) override;
-  virtual int32_t read_status(status_t &status) override;
-  virtual int32_t read_detailed_status(detailed_status_t &debug_info) override;
+  virtual int32_t read_version(version_t &version);
+  virtual int32_t read_status(status_t &status);
+  virtual int32_t read_detailed_status(detailed_status_t &debug_info);
 
-  virtual int32_t set_basic_param(basic_param_t &param) override;
-  virtual int32_t get_basic_param(basic_param_t &param) override;
+  virtual int32_t set_basic_param(basic_param_t &param);
+  virtual int32_t get_basic_param(basic_param_t &param);
 
   /*******************************************************************************
    *                               OVERRIDE MODULE                               *

components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp

@@ -2,72 +2,6 @@
 #include "scirpt_cmder_mini_servo_motor_ctrl_module.hpp"
 
 #include <string.h>
+#if 0
 
-// #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(CmdSchedulerV2* 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::kmodule_not_found;
-  }
-  *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___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);
-  REG_CMD_WITH_ACK("mini_servo_", get_basic_param, "(id)", 1, I_MiniServoModule::basic_param_t, ack);
-}
+#endif

components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.hpp

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

components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp

@@ -1,69 +1,5 @@
 #include "zcan_mini_servo_ctrl_module.hpp"
 using namespace iflytop;
 #if 0
-#define TAG "ZCanMiniServoCtrlModule"
-void ZCanMiniServoCtrlModule::initialize(ZCanCmder* cancmder, int id, I_MiniServoModule* module) {
-  m_cancmder = cancmder;
-  m_id       = id;
-  m_module   = module;
-  m_lock.init();
-  cancmder->registerListener(this);
-}
-
-void ZCanMiniServoCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
-
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_enable, m_id) { errorcode = m_module->enable(cmd->enable); }
-  END_PP();
-
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_stop, m_id) { errorcode = m_module->stop(cmd->stop_type); }
-  END_PP();
-
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_position_calibrate, m_id) { errorcode = m_module->position_calibrate(cmd->calibrate_pos); }
-  END_PP();
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_rotate, m_id) {  //
-    errorcode = m_module->rotate(cmd->speed, cmd->torque, cmd->run_time, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_mini_servo_ctrl_rotate); });
-  }
-  END_PP();
-
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_move_to, m_id) {  //
-    errorcode = m_module->move_to(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_mini_servo_ctrl_move_to); });
-  }
-  END_PP();
-
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_move_by, m_id) {  //
-    errorcode = m_module->move_by(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_mini_servo_ctrl_move_by); });
-  }
-  END_PP();
-
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_run_with_torque, m_id) {  //
-    errorcode = m_module->run_with_torque(cmd->torque, cmd->run_time, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_mini_servo_ctrl_run_with_torque); });
-  }
-  END_PP();
-
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_move_by_nolimit, m_id) {  //
-    errorcode = m_module->move_by_nolimit(cmd->pos, cmd->speed, cmd->torque, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_mini_servo_ctrl_move_by_nolimit); });
-  }
-  END_PP();
-
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_read_version, m_id) { errorcode = m_module->read_version(ack->ack); }
-  END_PP();
-
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_read_status, m_id) { errorcode = m_module->read_status(ack->ack); }
-  END_PP();
-
-  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_basic_param, m_id) { errorcode = m_module->set_basic_param(cmd->param); }
-  END_PP();
-
-
-
-  PROCESS_PACKET(kcmd_mini_servo_ctrl_get_basic_param, m_id) { errorcode = m_module->get_basic_param(ack->ack); }
-  END_PP();
-
-}
 
 #endif

components/zcancmder_module/zcan_mini_servo_ctrl_module.hpp

@@ -2,19 +2,5 @@
 #include "sdk/components/zprotocols/zcancmder/api/i_mini_servo_module.hpp"
 #include "sdk\components\zcancmder\zcanreceiver.hpp"
 namespace iflytop {
-#if 0
 
-class ZCanMiniServoCtrlModule : public ZCanCmderListener {
-  ZCanCmder*         m_cancmder = nullptr;
-  int                m_id       = 0;
-  I_MiniServoModule* m_module;
-
-  uint8_t m_txbuf[128] = {0};
-  zmutex  m_lock;
-
- public:
-  void         initialize(ZCanCmder* cancmder, int id, I_MiniServoModule* module);
-  virtual void onRceivePacket(CanPacketRxBuffer* rxcmd);
-};
-#endif
 }  // namespace iflytop

components/zprotocols/zcancmder

@@ -1 +1 @@
-Subproject commit b03e0a12e3ae450cbcdf5fd0f0218b258071f2eb
+Subproject commit 9c2f754b924618706274d948dcda39b5d9282fce