diff --git a/components/cmdscheduler/cmd_scheduler.cpp b/components/cmdscheduler/cmd_scheduler.cpp index 855b7de..2d5e3ca 100644 --- a/components/cmdscheduler/cmd_scheduler.cpp +++ b/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; } diff --git a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp index 385ff78..debfa1a 100644 --- a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp +++ b/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()); @@ -77,4 +77,4 @@ void ScriptCmderEq20Servomotor::regcmd() { 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); -} \ No newline at end of file +} diff --git a/components/mini_servo_motor/feite_servo_motor.hpp b/components/mini_servo_motor/feite_servo_motor.hpp index 756be0e..f88e1fe 100644 --- a/components/mini_servo_motor/feite_servo_motor.hpp +++ b/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); @@ -178,7 +184,7 @@ class FeiTeServoMotor { void dump_detailed_status(detailed_status_t* detailed_status); bool getMoveFlag(uint8_t id, uint8_t& moveflag); - + public: bool write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval); bool write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval); 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 271dbc7..2f653d5 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -5,14 +5,16 @@ using namespace iflytop; using namespace std; #define TAG "MiniRobotCtrlModule" -static void limit_input(s32& input, s32 min, s32 max) { +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 = bus; - m_id = id; +void MiniRobotCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus) { + m_bus = bus; + m_id = idinbus; + m_module_id = module_id; + m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode); m_thread.init("MiniRobotCtrlModule", 1024, osPriorityNormal); } @@ -201,13 +203,13 @@ int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, action_cb int32_t MiniRobotCtrlModule::move_by_nolimit(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) { return err::koperation_not_support; } -int32_t MiniRobotCtrlModule::read_version(version_t& version) { +int32_t MiniRobotCtrlModule::read_version(version_t &version) { uint8_t mainversion, subversion, miniserv_mainversion, miniserv_subversion; if (!m_bus->readversion(m_id, mainversion, subversion, miniserv_mainversion, miniserv_subversion)) return err::ksubdevice_overtime; version.version = mainversion << 24 | subversion << 16 | miniserv_mainversion << 8 | miniserv_subversion; return 0; } -int32_t MiniRobotCtrlModule::read_status(status_t& status) { +int32_t MiniRobotCtrlModule::read_status(status_t &status) { if (!m_bus->ping(m_id)) return err::ksubdevice_overtime; FeiTeServoMotor::detailed_status_t feitestatus; @@ -223,7 +225,7 @@ int32_t MiniRobotCtrlModule::read_status(status_t& status) { } return 0; } -int32_t MiniRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info) { +int32_t MiniRobotCtrlModule::read_detailed_status(detailed_status_t &debug_info) { if (!m_bus->ping(m_id)) return err::ksubdevice_overtime; FeiTeServoMotor::detailed_status_t feitestatus; if (!m_bus->read_detailed_status(m_id, &feitestatus)) return err::ksubdevice_overtime; @@ -244,8 +246,8 @@ int32_t MiniRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info) return 0; } -int32_t MiniRobotCtrlModule::set_basic_param(basic_param_t& param) { return err::koperation_not_support; } -int32_t MiniRobotCtrlModule::get_basic_param(basic_param_t& param) { +int32_t MiniRobotCtrlModule::set_basic_param(basic_param_t ¶m) { return err::koperation_not_support; } +int32_t MiniRobotCtrlModule::get_basic_param(basic_param_t ¶m) { ZLOGI(TAG, "get_basic_param"); if (!m_bus->ping(m_id)) return err::ksubdevice_overtime; m_bus->read_u16(m_id, feite::kRegServoCalibration, param.posCalibrate); @@ -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; +} \ No newline at end of file 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 d99fe22..e50c096 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/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 { - FeiTeServoMotor* m_bus; +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; @@ -31,17 +33,50 @@ class MiniRobotCtrlModule : public I_MiniServoModule { 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 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) 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 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_basic_param(basic_param_t ¶m) override; + virtual int32_t get_basic_param(basic_param_t ¶m) 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); diff --git a/components/zprotocols/zcancmder_v2 b/components/zprotocols/zcancmder_v2 index 115dbcf..5abf00d 160000 --- a/components/zprotocols/zcancmder_v2 +++ b/components/zprotocols/zcancmder_v2 @@ -1 +1 @@ -Subproject commit 115dbcfc41877dd4ceb3412df2838322ce7f09e2 +Subproject commit 5abf00ddaeca30ef08bad4b8b69ccfa7b074d161