From 2afccd83886638dcede624d33d9bbc339ddc0b29 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 12 Nov 2023 19:46:41 +0800 Subject: [PATCH] update --- .../mini_servo_motor_ctrl_module.cpp | 21 ++++++++++++++++++++- .../mini_servo_motor_ctrl_module.hpp | 17 ++++++++++++++++- ...ro_computer_module_device_script_cmder_paser.cpp | 4 ++++ components/zprotocols/zcancmder_v2 | 2 +- 4 files changed, 41 insertions(+), 3 deletions(-) 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 f68b8b0..084a2cb 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp @@ -10,18 +10,24 @@ static void limit_input(s32 &input, s32 min, s32 max) { if (input < min) input = min; } -void MiniRobotCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus) { +void MiniRobotCtrlModule::initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus, flash_config_t *cfg) { m_bus = bus; m_id = idinbus; m_module_id = module_id; + m_cfg = *cfg; + m_default_cfg = *cfg; + m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode); m_thread.init("MiniRobotCtrlModule", 1024, osPriorityNormal); + + enable(1); } int32_t MiniRobotCtrlModule::enable(u8 enable) { bool suc = m_bus->write_u8(m_id, feite::reg_add_e::kRegServoTorqueSwitch, enable); if (!suc) return err::ksubdevice_overtime; + m_com_reg.module_enable = enable; return err::ksucc; } int32_t MiniRobotCtrlModule::stop(u8 stop_type) { @@ -326,6 +332,7 @@ int32_t MiniRobotCtrlModule::module_read_adc(int32_t adcindex, int32_t *adc) { int32_t MiniRobotCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t &val) { 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_module_input_state, module_readio(&val), ACTION_NONE); PROCESS_REG(kreg_robot_pos, motor_read_pos(&val), ACTION_NONE); default: @@ -410,3 +417,15 @@ int32_t MiniRobotCtrlModule::motor_read_pos(int32_t *pos) { if (!m_bus->getNowPos(m_id, *pos)) return err::ksubdevice_overtime; return 0; } +int32_t MiniRobotCtrlModule::motor_easy_rotate(int32_t direction) { + if (direction == 1) { + return move_forward(m_cfg.default_torque); + } else if (direction == -1) { + return move_backward(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_to_zero(int32_t direction) { return motor_easy_move_to(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 2de4d0e..847daf0 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp @@ -10,14 +10,24 @@ namespace iflytop { class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZIMotor { ENABLE_MODULE(StepMotorCtrlModule, kmini_servo_motor_module, 0x0001); + public: + typedef struct { + s32 default_torque; + } flash_config_t; + + private: FeiTeServoMotor *m_bus; uint8_t m_id; uint16_t m_module_id; ZThread m_thread; s32 m_pos_shift; + flash_config_t m_cfg = {0}; + flash_config_t m_default_cfg = {0}; + public: - void initialize(uint16_t module_id, FeiTeServoMotor *bus, uint8_t idinbus); + 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; @@ -80,6 +90,11 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI virtual int32_t motor_read_pos(int32_t *pos); + virtual int32_t motor_easy_rotate(int32_t direction); + virtual int32_t motor_easy_move_by(int32_t distance); + virtual int32_t motor_easy_move_to(int32_t position); + virtual int32_t motor_easy_move_to_zero(int32_t direction); + private: void call_status_cb(action_cb_status_t cb, int32_t status); void set_errorcode(int32_t errorcode); diff --git a/components/zprotocol_helper/micro_computer_module_device_script_cmder_paser.cpp b/components/zprotocol_helper/micro_computer_module_device_script_cmder_paser.cpp index 801f442..266db8e 100644 --- a/components/zprotocol_helper/micro_computer_module_device_script_cmder_paser.cpp +++ b/components/zprotocol_helper/micro_computer_module_device_script_cmder_paser.cpp @@ -201,6 +201,7 @@ void MicroComputerModuleDeviceScriptCmderPaser::app_dump_reg(int32_t moduleId, i DUMP_CONFIG("motor_run_to_zero_dec", kreg_motor_run_to_zero_dec); DUMP_CONFIG("motor_look_zero_edge_speed", kreg_motor_look_zero_edge_speed); DUMP_CONFIG("motor_look_zero_edge_dec", kreg_motor_look_zero_edge_dec); + DUMP_CONFIG("motor_default_torque", kreg_motor_default_torque); /******************************************************************************* * MOTOR_X * @@ -226,6 +227,7 @@ void MicroComputerModuleDeviceScriptCmderPaser::app_dump_reg(int32_t moduleId, i DUMP_CONFIG("motor_x_run_to_zero_dec", kreg_motor_x_run_to_zero_dec); DUMP_CONFIG("motor_x_look_zero_edge_speed", kreg_motor_x_look_zero_edge_speed); DUMP_CONFIG("motor_x_look_zero_edge_dec", kreg_motor_x_look_zero_edge_dec); + DUMP_CONFIG("motor_x_default_torque", kreg_motor_x_default_torque); /******************************************************************************* * MOTOR_Y * @@ -251,6 +253,7 @@ void MicroComputerModuleDeviceScriptCmderPaser::app_dump_reg(int32_t moduleId, i DUMP_CONFIG("motor_y_run_to_zero_dec", kreg_motor_y_run_to_zero_dec); DUMP_CONFIG("motor_y_look_zero_edge_speed", kreg_motor_y_look_zero_edge_speed); DUMP_CONFIG("motor_y_look_zero_edge_dec", kreg_motor_y_look_zero_edge_dec); + DUMP_CONFIG("motor_y_default_torque", kreg_motor_y_default_torque); /******************************************************************************* * MOTOR_Z * @@ -276,6 +279,7 @@ void MicroComputerModuleDeviceScriptCmderPaser::app_dump_reg(int32_t moduleId, i DUMP_CONFIG("motor_z_run_to_zero_dec", kreg_motor_z_run_to_zero_dec); DUMP_CONFIG("motor_z_look_zero_edge_speed", kreg_motor_z_look_zero_edge_speed); DUMP_CONFIG("motor_z_look_zero_edge_dec", kreg_motor_z_look_zero_edge_dec); + DUMP_CONFIG("motor_z_default_torque", kreg_motor_z_default_torque); DUMP_CONFIG("xyrobot_robot_type", kreg_xyrobot_robot_type); diff --git a/components/zprotocols/zcancmder_v2 b/components/zprotocols/zcancmder_v2 index 9265a6c..d4ad18b 160000 --- a/components/zprotocols/zcancmder_v2 +++ b/components/zprotocols/zcancmder_v2 @@ -1 +1 @@ -Subproject commit 9265a6c2718afb6ab8c4877a729e73c6d05416ed +Subproject commit d4ad18bf21a66dec4fdbbdcbd078c43df88ca923