|
|
@ -252,10 +252,7 @@ int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, action_cb_ |
|
|
|
m_thread.stop(); |
|
|
|
m_thread.start([this, lastforms, speed, status_cb]() { |
|
|
|
ZLOGI(TAG, "(in work thread)rotate speed:%d lastforms:%d acc:%d dec:%d", speed, lastforms, m_param.acc, m_param.dec); |
|
|
|
|
|
|
|
m_stepM1->setAcceleration(m_param.acc); |
|
|
|
m_stepM1->setDeceleration(m_param.dec); |
|
|
|
m_stepM1->rotate(speed); |
|
|
|
_rotate(speed, m_param.acc, m_param.dec); |
|
|
|
|
|
|
|
int32_t startticket = zos_get_tick(); |
|
|
|
bool reachtime = false; |
|
|
@ -380,9 +377,9 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { |
|
|
|
if (!m_Xgpio->getState()) { |
|
|
|
{ |
|
|
|
ZLOGI(TAG, "---------STEP1-------- move to zero first"); |
|
|
|
_motor_move_by(-m_param.run_to_zero_max_d, m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec); |
|
|
|
_rotate(-m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec); |
|
|
|
bool xreach_zero = false; |
|
|
|
while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { |
|
|
|
while (!m_thread.getExitFlag()) { |
|
|
|
// ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget());
|
|
|
|
if (m_Xgpio->getState()) { |
|
|
|
xreach_zero = true; |
|
|
@ -414,8 +411,8 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { |
|
|
|
* @brief 如果设备已经在零点,则反向移动一定距离远离零点 |
|
|
|
*/ |
|
|
|
if (m_Xgpio->getState()) { |
|
|
|
_motor_move_by(m_param.look_zero_edge_max_d, m_param.look_zero_edge_speed, m_param.acc, m_param.look_zero_edge_dec); |
|
|
|
while (!_motor_is_reach_target()) { |
|
|
|
_rotate(m_param.look_zero_edge_speed, m_param.acc, m_param.look_zero_edge_dec); |
|
|
|
while (true) { |
|
|
|
if (m_thread.getExitFlag()) break; |
|
|
|
if (!m_Xgpio->getState()) { |
|
|
|
_motor_stop(-1); |
|
|
@ -434,42 +431,6 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { |
|
|
|
return err::kxymotor_x_find_zero_edge_fail; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
/*******************************************************************************
|
|
|
|
* 移动X轴到零点 * |
|
|
|
*******************************************************************************/ |
|
|
|
// {
|
|
|
|
// ZLOGI(TAG, "---------STEP3-------- move to zero");
|
|
|
|
// _motor_move_by(-m_param.run_to_zero_move_to_zero_max_d, m_param.run_to_zero_speed, m_param.acc, m_param.run_to_zero_dec);
|
|
|
|
// bool xreach_zero = false;
|
|
|
|
// while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
|
|
|
|
// // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget());
|
|
|
|
// if (m_Xgpio->getState()) {
|
|
|
|
// xreach_zero = true;
|
|
|
|
// _motor_stop(-1);
|
|
|
|
|
|
|
|
// while (!m_stepM1->isStoped()) { // 等待电机停止
|
|
|
|
// _motor_stop(-1);
|
|
|
|
// vTaskDelay(10);
|
|
|
|
// }
|
|
|
|
|
|
|
|
// break;
|
|
|
|
// }
|
|
|
|
// vTaskDelay(1);
|
|
|
|
// }
|
|
|
|
|
|
|
|
// if (m_thread.getExitFlag()) {
|
|
|
|
// ZLOGI(TAG, "break move to zero thread exit");
|
|
|
|
// return err::kmodule_opeation_break_by_user;
|
|
|
|
// }
|
|
|
|
|
|
|
|
// if (!xreach_zero) {
|
|
|
|
// // 触发异常回调
|
|
|
|
// ZLOGE(TAG, "x reach zero failed");
|
|
|
|
// return err::kxymotor_not_found_x_zero_point;
|
|
|
|
// }
|
|
|
|
// }
|
|
|
|
|
|
|
|
if (m_thread.getExitFlag()) { |
|
|
|
ZLOGI(TAG, "break move to zero thread exit"); |
|
|
|
return 0; |
|
|
@ -480,6 +441,12 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { |
|
|
|
|
|
|
|
return 0; |
|
|
|
} |
|
|
|
void StepMotorCtrlModule::_rotate(int32_t vel, int32_t acc, int32_t dec) { |
|
|
|
ZLOGI(TAG, "m%d _rotate %d %d %d", m_id, vel, acc, dec); |
|
|
|
m_stepM1->setAcceleration(acc); |
|
|
|
m_stepM1->setDeceleration(dec); |
|
|
|
m_stepM1->rotate(vel); |
|
|
|
} |
|
|
|
|
|
|
|
void StepMotorCtrlModule::getnowpos(int32_t& pos) { |
|
|
|
int32_t motor_pos = m_stepM1->getXACTUAL(); |
|
|
@ -638,3 +605,183 @@ void StepMotorCtrlModule::active_cfg() { |
|
|
|
m_stepM1->setScale(m_param.distance_scale); |
|
|
|
m_stepM1->setMotorShaft(m_param.x_shaft); |
|
|
|
} |
|
|
|
|
|
|
|
int32_t StepMotorCtrlModule::getid(int32_t* id) { |
|
|
|
*id = m_id; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
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_lastexecstatus; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t StepMotorCtrlModule::module_get_status(int32_t* status) { |
|
|
|
*status = m_thread.isworking() ? 1 : 0; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t StepMotorCtrlModule::module_get_error(int32_t* iserror) { |
|
|
|
*iserror = 0; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
int32_t StepMotorCtrlModule::module_set_param(int32_t param_id, int32_t param_value) { |
|
|
|
if (param_id == kcfg_motor_x_shift) { |
|
|
|
m_param.shift_x = param_value; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
if (param_id == kcfg_motor_x_shaft) { |
|
|
|
m_param.x_shaft = param_value; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
if (param_id == kcfg_motor_x_one_circle_pulse) { |
|
|
|
m_param.distance_scale = param_value; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
if (param_id == k_cfg_stepmotor_ihold) { |
|
|
|
m_param.ihold = param_value; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
if (param_id == k_cfg_stepmotor_irun) { |
|
|
|
m_param.irun = param_value; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
if (param_id == k_cfg_stepmotor_iholddelay) { |
|
|
|
m_param.iholddelay = param_value; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
return err::kmodule_not_find_config_index; |
|
|
|
} |
|
|
|
int32_t StepMotorCtrlModule::module_get_param(int32_t param_id, int32_t* param_value) { |
|
|
|
if (param_id == kcfg_motor_x_shift) { |
|
|
|
*param_value = m_param.shift_x; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
if (param_id == kcfg_motor_x_shaft) { |
|
|
|
*param_value = m_param.x_shaft; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
if (param_id == kcfg_motor_x_one_circle_pulse) { |
|
|
|
*param_value = m_param.distance_scale; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
if (param_id == k_cfg_stepmotor_ihold) { |
|
|
|
*param_value = m_param.ihold; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
if (param_id == k_cfg_stepmotor_irun) { |
|
|
|
*param_value = m_param.irun; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
if (param_id == k_cfg_stepmotor_iholddelay) { |
|
|
|
*param_value = m_param.iholddelay; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
return err::kmodule_not_find_config_index; |
|
|
|
} |
|
|
|
|
|
|
|
int32_t StepMotorCtrlModule::module_clear_error() { return 0; } |
|
|
|
|
|
|
|
int32_t StepMotorCtrlModule::module_readio(int32_t* io) { |
|
|
|
*io = 0; |
|
|
|
for (size_t i = 0; i < m_nio; i++) { |
|
|
|
if (m_iotable[i].getState()) { |
|
|
|
*io |= (0x01 << i); |
|
|
|
} |
|
|
|
} |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t StepMotorCtrlModule::module_writeio(int32_t io) { return 0; } |
|
|
|
|
|
|
|
int32_t StepMotorCtrlModule::module_read_adc(int32_t adcindex, int32_t* adc) { |
|
|
|
*adc = 0; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
int32_t StepMotorCtrlModule::module_factory_reset() { return factory_reset(); } |
|
|
|
int32_t StepMotorCtrlModule::module_flush_cfg() { return flush(); } |
|
|
|
int32_t StepMotorCtrlModule::module_active_cfg() { |
|
|
|
active_cfg(); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
int32_t StepMotorCtrlModule::motor_enable(int32_t enable) { |
|
|
|
zlock_guard l(m_lock); |
|
|
|
ZLOGI(TAG, "m%d motor_enable %d", m_id, enable); |
|
|
|
if (enable != 0) { |
|
|
|
m_stepM1->enable(true); |
|
|
|
} else { |
|
|
|
m_stepM1->enable(false); |
|
|
|
} |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t StepMotorCtrlModule::motor_rotate(int32_t direction, int32_t motor_velocity, int32_t acc) { |
|
|
|
ZLOGI(TAG, "m%d motor_rotate %d", m_id, direction); |
|
|
|
m_status_cb = nullptr; |
|
|
|
m_thread.stop(); |
|
|
|
_rotate(direction * motor_velocity, acc, acc); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t StepMotorCtrlModule::motor_move_by(int32_t dx, int32_t motor_velocity, int32_t acc) { |
|
|
|
zlock_guard lock(m_lock); |
|
|
|
int32_t xnow = 0; |
|
|
|
getnowpos(xnow); |
|
|
|
int32_t toxnow = xnow + dx; |
|
|
|
return motor_move_to(toxnow, motor_velocity, acc); |
|
|
|
} |
|
|
|
int32_t StepMotorCtrlModule::motor_move_to(int32_t tox, int32_t motor_velocity, int32_t acc) { |
|
|
|
zlock_guard lock(m_lock); |
|
|
|
ZLOGI(TAG, "m%d motor_move_to %d", m_id, tox); |
|
|
|
m_status_cb = nullptr; |
|
|
|
m_thread.stop(); |
|
|
|
|
|
|
|
if (m_param.min_x != 0 && tox < m_param.min_x) { |
|
|
|
tox = m_param.min_x; |
|
|
|
} |
|
|
|
if (m_param.max_x != 0 && tox > m_param.max_x) { |
|
|
|
tox = m_param.max_x; |
|
|
|
} |
|
|
|
m_thread.start( |
|
|
|
[this, tox, motor_velocity, acc]() { |
|
|
|
_motor_move_to(tox, motor_velocity, acc, acc); |
|
|
|
while (!_motor_is_reach_target()) { |
|
|
|
if (m_thread.getExitFlag()) break; |
|
|
|
vTaskDelay(10); |
|
|
|
} |
|
|
|
call_exec_status_cb(0, nullptr); |
|
|
|
}, |
|
|
|
[this, tox]() { _motor_stop(); }); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t StepMotorCtrlModule::motor_move_to_with_torque(int32_t pos, int32_t torque) { return err::koperation_not_support; } |
|
|
|
|
|
|
|
int32_t StepMotorCtrlModule::motor_rotate_acctime(int32_t direction, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } |
|
|
|
int32_t StepMotorCtrlModule::motor_move_by_acctime(int32_t distance, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } |
|
|
|
int32_t StepMotorCtrlModule::motor_move_to_acctime(int32_t position, int32_t motor_velocity, int32_t acctime) { return err::koperation_not_support; } |
|
|
|
|
|
|
|
int32_t StepMotorCtrlModule::motor_move_to_zero_forward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { return err::koperation_not_support; } |
|
|
|
int32_t StepMotorCtrlModule::motor_move_to_zero_backward(int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { |
|
|
|
ZLOGI(TAG, "motor_move_to_zero_backward"); |
|
|
|
m_param.run_to_zero_speed = findzerospeed; |
|
|
|
m_param.look_zero_edge_speed = findzeroedge_speed; |
|
|
|
m_param.look_zero_edge_dec = acc; |
|
|
|
m_param.run_to_zero_dec = acc; |
|
|
|
return move_to_zero(nullptr); |
|
|
|
} |
|
|
|
|
|
|
|
int32_t StepMotorCtrlModule::motor_read_pos(int32_t* pos) { |
|
|
|
int32_t xnow = 0; |
|
|
|
getnowpos(xnow); |
|
|
|
*pos = xnow; |
|
|
|
return 0; |
|
|
|
} |