|
|
@ -26,8 +26,6 @@ void MiniServoCtrlModule::create_default_config(config_t &cfg) { |
|
|
|
cfg.target_pos_tolerance = 15; |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int32_t MiniServoCtrlModule::module_set_reg(int32_t regindex, int32_t val) { |
|
|
|
int32_t ret = 0; |
|
|
|
switch (regindex) { |
|
|
@ -43,6 +41,47 @@ int32_t MiniServoCtrlModule::module_set_reg(int32_t regindex, int32_t val) { |
|
|
|
MODULE_REG_CASE__NOT_SUPPORT(kreg_mini_servo_temperature); // set only
|
|
|
|
MODULE_REG_CASE__NOT_SUPPORT(kreg_mini_servo_loadvalue); // set only
|
|
|
|
MODULE_REG_CASE__SET_REG_TO(kreg_mini_servo_target_pos_tolerance, m_cfg.target_pos_tolerance); |
|
|
|
|
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_firmware_main_version, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_firmware_sub_version, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_main_version, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_sub_version, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_min_angle, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_max_angle, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_max_temp, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_max_voltage, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_min_voltage, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_max_torque, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_unload_condition, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_p, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_d, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_i, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_min_start, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_cw_dead_zone, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_ccw_dead_zone, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_protect_current, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_protect_torque, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_protect_time, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_overload_torque, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_speed_p, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_overload_time, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_speed_i, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_torque_switch, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_acc, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_target_pos, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_run_time, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_run_speed, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_torque_limit, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_lock_flag, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_current_pos, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_current_speed, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_current_load, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_current_voltage, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_current_temp, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_status, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_move_flag, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__SET_REG_BY_FN(kreg_mini_servo_servo_current_current, mini_servo_set_subic_reg(regindex, val)); |
|
|
|
|
|
|
|
default: |
|
|
|
return err::kmodule_not_find_reg; |
|
|
|
break; |
|
|
@ -64,6 +103,47 @@ int32_t MiniServoCtrlModule::module_get_reg(int32_t regindex, int32_t *val) { |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_temperature, mini_servo_read_reg(kreg_mini_servo_temperature, val)); // set only
|
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_loadvalue, mini_servo_read_reg(kreg_mini_servo_loadvalue, val)); // set only
|
|
|
|
MODULE_CFG_CASE__GET_REG_FROM(kreg_mini_servo_target_pos_tolerance, m_cfg.target_pos_tolerance); |
|
|
|
|
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_firmware_main_version, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_firmware_sub_version, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_main_version, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_sub_version, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_min_angle, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_max_angle, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_max_temp, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_max_voltage, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_min_voltage, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_max_torque, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_unload_condition, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_p, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_d, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_i, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_min_start, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_cw_dead_zone, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_ccw_dead_zone, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_protect_current, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_protect_torque, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_protect_time, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_overload_torque, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_speed_p, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_overload_time, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_speed_i, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_torque_switch, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_acc, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_target_pos, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_run_time, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_run_speed, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_torque_limit, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_lock_flag, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_current_pos, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_current_speed, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_current_load, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_current_voltage, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_current_temp, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_status, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_move_flag, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
MODULE_REG_CASE__GET_REG_BY_FN(kreg_mini_servo_servo_current_current, mini_servo_get_subic_reg(regindex, val)); |
|
|
|
|
|
|
|
default: |
|
|
|
return err::kmodule_not_find_reg; |
|
|
|
break; |
|
|
@ -142,6 +222,7 @@ int32_t MiniServoCtrlModule::mini_servo_rotate(int32_t speed) { |
|
|
|
[this, speed]() { |
|
|
|
befor_motor_move(); |
|
|
|
servo_status_t status = {0}; |
|
|
|
osDelay(5); |
|
|
|
|
|
|
|
{ |
|
|
|
m_bus->runInMode1(m_idinbus, m_cfg.limit_torque, speed); |
|
|
@ -178,7 +259,7 @@ int32_t MiniServoCtrlModule::mini_servo_move_to(int32_t pos3600) { |
|
|
|
m_thread.start( |
|
|
|
[this, pos3600]() { |
|
|
|
befor_motor_move(); |
|
|
|
|
|
|
|
osDelay(5); |
|
|
|
{ |
|
|
|
int32_t velocity = m_cfg.limit_velocity; |
|
|
|
servo_status_t status = {0}; |
|
|
@ -225,6 +306,7 @@ int32_t MiniServoCtrlModule::mini_servo_rotate_with_torque(int32_t torque) { |
|
|
|
m_thread.start( |
|
|
|
[this, torque]() { |
|
|
|
befor_motor_move(); |
|
|
|
osDelay(5); |
|
|
|
servo_status_t status = {0}; |
|
|
|
{ |
|
|
|
m_bus->runInMode2(m_idinbus, torque); |
|
|
@ -299,7 +381,7 @@ bool MiniServoCtrlModule::check_when_run(servo_status_t *status) { |
|
|
|
module_errorcode = err::ksubdevice_overtime; |
|
|
|
return false; |
|
|
|
} |
|
|
|
|
|
|
|
osDelay(1); |
|
|
|
if (m_bus->getStatus(m_idinbus) != 0) { |
|
|
|
status->status = m_bus->getStatus(m_idinbus); |
|
|
|
ZLOGE(TAG, "check fail, status = %x", status->status); |
|
|
@ -307,14 +389,29 @@ bool MiniServoCtrlModule::check_when_run(servo_status_t *status) { |
|
|
|
module_detail_errorcode = status->status; |
|
|
|
return false; |
|
|
|
} |
|
|
|
osDelay(1); |
|
|
|
|
|
|
|
status->isMove = moveflag; |
|
|
|
status->pos = nowpos; |
|
|
|
status->status = m_bus->getStatus(m_idinbus); |
|
|
|
ZLOGI(TAG, "moduleid:%d idinbus:%d status %x isMove %d pos %d", module_id, m_idinbus, status->status, status->isMove, status->pos); |
|
|
|
osDelay(1); |
|
|
|
return true; |
|
|
|
} |
|
|
|
|
|
|
|
int32_t MiniServoCtrlModule::mini_servo_set_subic_reg(int32_t regoff, int32_t regval) { |
|
|
|
regoff = regoff - kreg_mini_servo_firmware_main_version; |
|
|
|
bool suc = m_bus->write_reg(m_idinbus, regoff, regval); |
|
|
|
if (!suc) return err::ksubdevice_overtime; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t MiniServoCtrlModule::mini_servo_get_subic_reg(int32_t regoff, int32_t *regval) { |
|
|
|
regoff = regoff - kreg_mini_servo_firmware_main_version; |
|
|
|
bool suc = m_bus->read_reg(m_idinbus, regoff, regval); |
|
|
|
if (!suc) return err::ksubdevice_overtime; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
/***********************************************************************************************************************
|
|
|
|
* INTERNAL * |
|
|
|
***********************************************************************************************************************/ |
|
|
|