diff --git a/components/eq_20_asb_motor/eq20_servomotor.cpp b/components/eq_20_asb_motor/eq20_servomotor.cpp index 67fccf5..df95dde 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/eq20_servomotor.cpp @@ -9,12 +9,13 @@ using namespace iflytop; #define TAG "Eq20ServoMotor" -void Eq20ServoMotor::init(int moduleid, ModbusBlockHost *modbusBlockHost, int motorid) { +void Eq20ServoMotor::init(int moduleid, ModbusBlockHost *modbusBlockHost, int motorid, servo_config_t servo_config) { // this->com = com; m_modbusBlockHost = modbusBlockHost; m_deviceId = motorid; m_moduleId = moduleid; ZASSERT(m_modbusBlockHost != NULL); + m_servo_config = servo_config; } #define DO(exptr) \ @@ -224,6 +225,8 @@ int32_t Eq20ServoMotor::getid(int32_t *id) { *id = m_moduleId; return 0; } +int32_t Eq20ServoMotor::module_enable(int32_t enable) { return motor_enable(enable); } + int32_t Eq20ServoMotor::module_stop() { m_final_action = kActionType_none; return stop(); @@ -324,3 +327,21 @@ int32_t Eq20ServoMotor::motor_move_to_zero_backward(int32_t findzerospeed, int32 } int32_t Eq20ServoMotor::motor_read_pos(int32_t *pos) { return get_pos(*pos); } + +int32_t Eq20ServoMotor::motor_easy_rotate(int32_t direction) { // + return motor_rotate_acctime(direction, m_servo_config.default_velocity, m_servo_config.default_acc_time_ms); +}; +int32_t Eq20ServoMotor::motor_easy_move_by(int32_t distance) { // + return motor_move_by_acctime(distance, m_servo_config.default_velocity, m_servo_config.default_acc_time_ms); +}; +int32_t Eq20ServoMotor::motor_easy_move_to(int32_t position) { // + return motor_move_to_acctime(position, m_servo_config.default_velocity, m_servo_config.default_acc_time_ms); +}; +int32_t Eq20ServoMotor::motor_easy_move_to_zero(int32_t direction) { // + if (direction > 0) + return move_to_zero_forward(m_servo_config.find_zero_velocity, m_servo_config.find_zero_velocity, m_servo_config.default_acc_time_ms); + else + return move_to_zero_backward(m_servo_config.find_zero_velocity, m_servo_config.find_zero_velocity, m_servo_config.default_acc_time_ms); +}; +int32_t Eq20ServoMotor::motor_easy_set_current_pos(int32_t pos) { return err::koperation_not_support; }; +int32_t Eq20ServoMotor::motor_easy_move_to_io(int32_t ioindex, int32_t direction) { return err::koperation_not_support; }; \ No newline at end of file diff --git a/components/eq_20_asb_motor/eq20_servomotor.hpp b/components/eq_20_asb_motor/eq20_servomotor.hpp index 5ff301e..c3a833e 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.hpp +++ b/components/eq_20_asb_motor/eq20_servomotor.hpp @@ -62,6 +62,12 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor { } data; } servo_internal_status_t; + typedef struct { + int32_t default_acc_time_ms = 1000; + int32_t default_velocity = 800; + int32_t find_zero_velocity = 100; + } servo_config_t; + typedef enum { kActionType_none, kActionType_rotate, @@ -77,11 +83,13 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor { final_action_t m_final_action = kActionType_none; + servo_config_t m_servo_config; + public: Eq20ServoMotor(/* args */){}; ~Eq20ServoMotor(){}; - void init(int moduleid, ModbusBlockHost *modbusBlockHost, int motorid); + void init(int moduleid, ModbusBlockHost *modbusBlockHost, int motorid, servo_config_t servo_config); int32_t enable(int32_t enable); @@ -105,6 +113,8 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor { *******************************************************************************/ virtual int32_t getid(int32_t *id) override; + virtual int32_t module_enable(int32_t enable) override; + virtual int32_t module_ping() { return 0; }; virtual int32_t module_stop() override; @@ -139,6 +149,13 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor { 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); + virtual int32_t motor_easy_set_current_pos(int32_t pos); + virtual int32_t motor_easy_move_to_io(int32_t ioindex, int32_t direction); + public: int32_t write_reg(uint32_t regadd, int32_t value); int32_t read_reg(uint32_t regadd, int32_t &value); 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 728eaa8..bbcfe85 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp @@ -74,7 +74,7 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI *******************************************************************************/ virtual int32_t module_ping() { return 0; }; - virtual int32_t module_enable(int32_t enable) override { motor_enable(enable); }; + 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; @@ -105,4 +105,4 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI int32_t module_xxx_reg(int32_t param_id, bool read, int32_t &val); int32_t do_action(int32_t actioncode); }; -} // namespace iflytop \ No newline at end of file +} // namespace iflytop