diff --git a/components/eq_20_asb_motor/eq20_servomotor.cpp b/components/eq_20_asb_motor/eq20_servomotor.cpp index d6f7281..12efcba 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/eq20_servomotor.cpp @@ -280,7 +280,8 @@ int32_t Eq20ServoMotor::module_get_reg(int32_t param_id, int32_t *param_value) { int32_t Eq20ServoMotor::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_velocity, REG_GET(m_servo_config.default_velocity), REG_SET(m_servo_config.default_velocity)); + PROCESS_REG(kreg_motor_default_velocity, REG_GET(m_servo_config.default_velocity), // + REG_SET(m_servo_config.default_velocity, 0, m_servo_config.default_velocity_limit)); PROCESS_REG(kreg_module_input_state, module_readio(&val), ACTION_NONE); PROCESS_REG(kreg_robot_pos, motor_read_pos(&val), ACTION_NONE); default: diff --git a/components/eq_20_asb_motor/eq20_servomotor.hpp b/components/eq_20_asb_motor/eq20_servomotor.hpp index 3ca5d2f..e920df8 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.hpp +++ b/components/eq_20_asb_motor/eq20_servomotor.hpp @@ -11,6 +11,7 @@ using namespace std; class Eq20ServoMotor : public ZIModule, public ZIMotor { ENABLE_MODULE(Eq20ServoMotor, kmotor_module, 0x0001); + public: typedef struct { union { @@ -64,9 +65,10 @@ class Eq20ServoMotor : public ZIModule, public ZIMotor { } servo_internal_status_t; typedef struct { - int32_t default_acc_time_ms = 1000; - int32_t default_velocity = 800; - int32_t find_zero_velocity = 100; + int32_t default_acc_time_ms = 1000; + int32_t default_velocity = 800; + int32_t find_zero_velocity = 100; + int32_t default_velocity_limit = 3000; } servo_config_t; typedef enum {