|
|
@ -219,7 +219,8 @@ int32_t StepMotorCtrlModule::enable(bool venable) { |
|
|
|
zlock_guard l(m_lock); |
|
|
|
ZLOGI(TAG, "m%d enable %d", m_id, venable); |
|
|
|
m_stepM1->enable(venable); |
|
|
|
m_enable = venable; |
|
|
|
// m_enable = venable;
|
|
|
|
m_com_reg.module_enable = venable; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t StepMotorCtrlModule::stop(uint8_t stopType) { |
|
|
@ -295,7 +296,7 @@ int32_t StepMotorCtrlModule::read_detailed_status(detailed_status_t& debug_info) |
|
|
|
debug_info.io_state |= (0x01 << i); |
|
|
|
} |
|
|
|
} |
|
|
|
debug_info.last_exec_status = m_module_last_cmd_exec_status; |
|
|
|
debug_info.last_exec_status = m_com_reg.module_last_cmd_exec_status; |
|
|
|
getnowpos(debug_info.x); |
|
|
|
return 0; |
|
|
|
} |
|
|
@ -563,8 +564,8 @@ int32_t StepMotorCtrlModule::move_by_block(int32_t dx, int32_t velocity, int ove |
|
|
|
} |
|
|
|
|
|
|
|
void StepMotorCtrlModule::call_exec_status_cb(int32_t status, action_cb_status_t status_cb) { |
|
|
|
m_module_last_cmd_exec_status = status; |
|
|
|
auto cache_status_cb = m_status_cb; |
|
|
|
m_com_reg.module_last_cmd_exec_status = status; |
|
|
|
auto cache_status_cb = m_status_cb; |
|
|
|
if (cache_status_cb) cache_status_cb(status); |
|
|
|
} |
|
|
|
void StepMotorCtrlModule::create_default_cfg(flash_config_t& cfg) { |
|
|
@ -620,11 +621,11 @@ int32_t StepMotorCtrlModule::getid(int32_t* id) { |
|
|
|
int32_t StepMotorCtrlModule::module_stop() { return stop(0); } |
|
|
|
int32_t StepMotorCtrlModule::module_break() { return stop(0); } |
|
|
|
int32_t StepMotorCtrlModule::module_get_last_exec_status(int32_t* status) { |
|
|
|
*status = m_module_last_cmd_exec_status; |
|
|
|
*status = m_com_reg.module_last_cmd_exec_status; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t StepMotorCtrlModule::module_get_status(int32_t* status) { |
|
|
|
//ZLOGI(TAG, "[%d]-module_get_status %d", m_id, m_thread.isworking());
|
|
|
|
// ZLOGI(TAG, "[%d]-module_get_status %d", m_id, m_thread.isworking());
|
|
|
|
*status = m_thread.isworking() ? 1 : 0; |
|
|
|
return 0; |
|
|
|
} |
|
|
@ -668,7 +669,7 @@ int32_t StepMotorCtrlModule::module_get_error(int32_t* iserror) { |
|
|
|
case param_id: \ |
|
|
|
*val = modulereg; \ |
|
|
|
break; |
|
|
|
|
|
|
|
#if 0
|
|
|
|
int32_t StepMotorCtrlModule::module_set_reg(int32_t param_id, int32_t val) { |
|
|
|
switch (param_id) { |
|
|
|
SET_REG(kreg_motor_shift, m_param.shift_x); |
|
|
@ -723,19 +724,42 @@ int32_t StepMotorCtrlModule::module_get_reg(int32_t param_id, int32_t* val) { |
|
|
|
GET_REG(kreg_module_enableflag, m_enable); |
|
|
|
GET_REG(kreg_robot_pos, nowpos); |
|
|
|
|
|
|
|
GET_REG(kreg_module_last_cmd_exec_status, m_module_last_cmd_exec_status); |
|
|
|
GET_REG(kreg_module_last_cmd_exec_val0, m_module_last_cmd_exec_val0); |
|
|
|
GET_REG(kreg_module_last_cmd_exec_val1, m_module_last_cmd_exec_val1); |
|
|
|
GET_REG(kreg_module_last_cmd_exec_val2, m_module_last_cmd_exec_val2); |
|
|
|
GET_REG(kreg_module_last_cmd_exec_val3, m_module_last_cmd_exec_val3); |
|
|
|
GET_REG(kreg_module_last_cmd_exec_val4, m_module_last_cmd_exec_val4); |
|
|
|
GET_REG(kreg_module_last_cmd_exec_val5, m_module_last_cmd_exec_val5); |
|
|
|
default: |
|
|
|
return err::kmodule_not_find_config_index; |
|
|
|
} |
|
|
|
return 0; |
|
|
|
} |
|
|
|
#endif
|
|
|
|
int32_t StepMotorCtrlModule::module_set_reg(int32_t param_id, int32_t param_value) { return module_xxx_reg(param_id, false, param_value); } |
|
|
|
int32_t StepMotorCtrlModule::module_get_reg(int32_t param_id, int32_t* param_value) { return module_xxx_reg(param_id, true, *param_value); } |
|
|
|
int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { |
|
|
|
switch (param_id) { |
|
|
|
MODULE_COMMON_PROCESS_REG_CB(); |
|
|
|
|
|
|
|
PROCESS_REG(kreg_motor_shift, REG_GET(m_param.shift_x), REG_SET(m_param.shift_x)); |
|
|
|
PROCESS_REG(kreg_motor_shaft, REG_GET(m_param.x_shaft), REG_SET(m_param.x_shaft)); |
|
|
|
PROCESS_REG(kreg_motor_one_circle_pulse, REG_GET(m_param.distance_scale), REG_SET(m_param.distance_scale)); |
|
|
|
PROCESS_REG(kreg_motor_one_circle_pulse_denominator, REG_GET(m_param.distance_scale_denominator), REG_SET(m_param.distance_scale_denominator)); |
|
|
|
PROCESS_REG(kreg_motor_default_velocity, REG_GET(m_param.maxspeed), REG_SET(m_param.maxspeed)); |
|
|
|
PROCESS_REG(kreg_motor_default_acc, REG_GET(m_param.acc), REG_SET(m_param.acc)); |
|
|
|
PROCESS_REG(kreg_motor_default_dec, REG_GET(m_param.dec), REG_SET(m_param.dec)); |
|
|
|
PROCESS_REG(kreg_motor_run_to_zero_max_d, REG_GET(m_param.run_to_zero_max_d), REG_SET(m_param.run_to_zero_max_d)); |
|
|
|
PROCESS_REG(kreg_motor_look_zero_edge_max_d, REG_GET(m_param.look_zero_edge_max_d), REG_SET(m_param.look_zero_edge_max_d)); |
|
|
|
PROCESS_REG(kreg_motor_run_to_zero_speed, REG_GET(m_param.run_to_zero_speed), REG_SET(m_param.run_to_zero_speed)); |
|
|
|
PROCESS_REG(kreg_motor_run_to_zero_dec, REG_GET(m_param.run_to_zero_dec), REG_SET(m_param.run_to_zero_dec)); |
|
|
|
PROCESS_REG(kreg_motor_look_zero_edge_speed, REG_GET(m_param.look_zero_edge_speed), REG_SET(m_param.look_zero_edge_speed)); |
|
|
|
PROCESS_REG(kreg_motor_look_zero_edge_dec, REG_GET(m_param.look_zero_edge_dec), REG_SET(m_param.look_zero_edge_dec)); |
|
|
|
PROCESS_REG(kreg_stepmotor_ihold, REG_GET(m_param.ihold), REG_SET(m_param.ihold)); |
|
|
|
PROCESS_REG(kreg_stepmotor_irun, REG_GET(m_param.irun), REG_SET(m_param.irun)); |
|
|
|
PROCESS_REG(kreg_stepmotor_iholddelay, REG_GET(m_param.iholddelay), REG_SET(m_param.iholddelay)); |
|
|
|
|
|
|
|
default: |
|
|
|
return err::kmodule_not_find_config_index; |
|
|
|
break; |
|
|
|
} |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t StepMotorCtrlModule::do_action(int32_t actioncode) {} |
|
|
|
|
|
|
|
int32_t StepMotorCtrlModule::module_clear_error() { return 0; } |
|
|
|
|
|
|
@ -819,7 +843,7 @@ int32_t StepMotorCtrlModule::motor_move_to(int32_t tox, int32_t motor_velocity, |
|
|
|
call_exec_status_cb(0, nullptr); |
|
|
|
}, |
|
|
|
[this, tox]() { _motor_stop(); }); |
|
|
|
//ZLOGI(TAG, "m%d motor_move_to %d end", m_id, tox);
|
|
|
|
// ZLOGI(TAG, "m%d motor_move_to %d end", m_id, tox);
|
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t StepMotorCtrlModule::motor_rotate_with_torque(int32_t direction, int32_t torque) { return err::koperation_not_support; } |
|
|
@ -853,14 +877,13 @@ int32_t StepMotorCtrlModule::motor_read_pos(int32_t* pos) { |
|
|
|
*pos = xnow; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
void StepMotorCtrlModule::set_last_exec_status(int32_t ecode, int32_t val0, int32_t val1, int32_t val2, int32_t val3, int32_t val4, int32_t val5) { |
|
|
|
m_module_last_cmd_exec_status = ecode; |
|
|
|
m_module_last_cmd_exec_val0 = val0; |
|
|
|
m_module_last_cmd_exec_val1 = val1; |
|
|
|
m_module_last_cmd_exec_val2 = val2; |
|
|
|
m_module_last_cmd_exec_val3 = val3; |
|
|
|
m_module_last_cmd_exec_val4 = val4; |
|
|
|
m_module_last_cmd_exec_val5 = val5; |
|
|
|
void StepMotorCtrlModule::set_last_exec_status(int32_t ecode, int32_t val0, int32_t val1, int32_t val2, int32_t val3, int32_t val4) { |
|
|
|
m_com_reg.module_last_cmd_exec_status = ecode; |
|
|
|
m_com_reg.module_last_cmd_exec_val0 = val0; |
|
|
|
m_com_reg.module_last_cmd_exec_val1 = val1; |
|
|
|
m_com_reg.module_last_cmd_exec_val2 = val2; |
|
|
|
m_com_reg.module_last_cmd_exec_val3 = val3; |
|
|
|
m_com_reg.module_last_cmd_exec_val4 = val4; |
|
|
|
} |
|
|
|
|
|
|
|
int32_t StepMotorCtrlModule::motor_calculated_pos_by_move_to_zero() { |
|
|
@ -876,7 +899,7 @@ int32_t StepMotorCtrlModule::motor_calculated_pos_by_move_to_zero() { |
|
|
|
if (erro != 0) { |
|
|
|
set_last_exec_status(erro); |
|
|
|
} else { |
|
|
|
m_module_last_cmd_exec_status = erro; |
|
|
|
m_com_reg.module_last_cmd_exec_status = erro; |
|
|
|
set_last_exec_status(erro, dx + m_param.shift_x); |
|
|
|
m_stepM1->setXACTUAL(0); |
|
|
|
} |
|
|
@ -894,9 +917,6 @@ int32_t StepMotorCtrlModule::motor_easy_rotate(int32_t direction) { |
|
|
|
return 0; |
|
|
|
}; |
|
|
|
|
|
|
|
// int32_t StepMotorCtrlModule::motor_move_by(int32_t dx, int32_t motor_velocity, int32_t acc) {
|
|
|
|
// int32_t StepMotorCtrlModule::motor_move_to(int32_t tox, int32_t motor_velocity, int32_t acc) {
|
|
|
|
|
|
|
|
int32_t StepMotorCtrlModule::motor_easy_move_by(int32_t distance) { |
|
|
|
zlock_guard lock(m_lock); |
|
|
|
ZLOGI(TAG, "m%d motor_easy_move_by %d", m_id, distance); |
|
|
|