diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp index 536831a..8a34ae8 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/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); } diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp index 0851057..aa2b607 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/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 ¶m) override; - virtual int32_t get_basic_param(basic_param_t ¶m) override; + virtual int32_t set_basic_param(basic_param_t ¶m); + virtual int32_t get_basic_param(basic_param_t ¶m); /******************************************************************************* * OVERRIDE MODULE * diff --git a/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp b/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp index 6e50a3b..2c31844 100644 --- a/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.cpp +++ b/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 +#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 \ No newline at end of file diff --git a/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.hpp b/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.hpp index 75b259a..510e690 100644 --- a/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.hpp +++ b/components/mini_servo_motor/scirpt_cmder_mini_servo_motor_ctrl_module.hpp @@ -1,23 +1,5 @@ #pragma once #include +#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 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 \ No newline at end of file +#endif \ No newline at end of file diff --git a/components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp b/components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp index ec7e505..dce2fb2 100644 --- a/components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp +++ b/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 \ No newline at end of file diff --git a/components/zcancmder_module/zcan_mini_servo_ctrl_module.hpp b/components/zcancmder_module/zcan_mini_servo_ctrl_module.hpp index aa551bc..8e675b6 100644 --- a/components/zcancmder_module/zcan_mini_servo_ctrl_module.hpp +++ b/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 diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index b03e0a1..9c2f754 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit b03e0a12e3ae450cbcdf5fd0f0218b258071f2eb +Subproject commit 9c2f754b924618706274d948dcda39b5d9282fce