|
@ -40,29 +40,29 @@ int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int speed, action_cb_st |
|
|
ZLOGI(TAG, "move_to x:%d y:%d", x, y); |
|
|
ZLOGI(TAG, "move_to x:%d y:%d", x, y); |
|
|
int runspeed = speed; |
|
|
int runspeed = speed; |
|
|
if (speed == 0) { |
|
|
if (speed == 0) { |
|
|
runspeed = m_cfg.maxspeed; |
|
|
|
|
|
|
|
|
runspeed = m_basecfg.maxspeed; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) { |
|
|
|
|
|
runspeed = m_cfg.maxspeed; |
|
|
|
|
|
|
|
|
if (m_basecfg.maxspeed != 0 && speed > m_basecfg.maxspeed) { |
|
|
|
|
|
runspeed = m_basecfg.maxspeed; |
|
|
} |
|
|
} |
|
|
if (m_cfg.min_x != 0 && x < m_cfg.min_x) { |
|
|
|
|
|
x = m_cfg.min_x; |
|
|
|
|
|
|
|
|
if (m_basecfg.min_x != 0 && x < m_basecfg.min_x) { |
|
|
|
|
|
x = m_basecfg.min_x; |
|
|
} |
|
|
} |
|
|
if (m_cfg.max_x != 0 && x > m_cfg.max_x) { |
|
|
|
|
|
x = m_cfg.max_x; |
|
|
|
|
|
|
|
|
if (m_basecfg.max_x != 0 && x > m_basecfg.max_x) { |
|
|
|
|
|
x = m_basecfg.max_x; |
|
|
} |
|
|
} |
|
|
if (m_cfg.min_y != 0 && y < m_cfg.min_y) { |
|
|
|
|
|
y = m_cfg.min_y; |
|
|
|
|
|
|
|
|
if (m_basecfg.min_y != 0 && y < m_basecfg.min_y) { |
|
|
|
|
|
y = m_basecfg.min_y; |
|
|
} |
|
|
} |
|
|
if (m_cfg.max_y != 0 && y > m_cfg.max_y) { |
|
|
|
|
|
y = m_cfg.max_y; |
|
|
|
|
|
|
|
|
if (m_basecfg.max_y != 0 && y > m_basecfg.max_y) { |
|
|
|
|
|
y = m_basecfg.max_y; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
// int32_t m1pos, m2pos;
|
|
|
// int32_t m1pos, m2pos;
|
|
|
m_thread.stop(); |
|
|
m_thread.stop(); |
|
|
m_thread.start([this, x, y, runspeed, status_cb]() { |
|
|
m_thread.start([this, x, y, runspeed, status_cb]() { |
|
|
_motor_move_to(x, y, runspeed, m_cfg.acc, m_cfg.dec); |
|
|
|
|
|
|
|
|
_motor_move_to(x, y, runspeed, m_basecfg.acc, m_basecfg.dec); |
|
|
while (!_motor_is_reach_target()) { |
|
|
while (!_motor_is_reach_target()) { |
|
|
if (m_thread.getExitFlag()) break; |
|
|
if (m_thread.getExitFlag()) break; |
|
|
vTaskDelay(10); |
|
|
vTaskDelay(10); |
|
@ -82,8 +82,8 @@ int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int speed, a |
|
|
m_thread.stop(); |
|
|
m_thread.stop(); |
|
|
_motor_stop(); |
|
|
_motor_stop(); |
|
|
|
|
|
|
|
|
if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) { |
|
|
|
|
|
speed = m_cfg.maxspeed; |
|
|
|
|
|
|
|
|
if (m_basecfg.maxspeed != 0 && speed > m_basecfg.maxspeed) { |
|
|
|
|
|
speed = m_basecfg.maxspeed; |
|
|
} |
|
|
} |
|
|
int32_t s_x, s_y, to_x, to_y; |
|
|
int32_t s_x, s_y, to_x, to_y; |
|
|
getnowpos(s_x, s_y); |
|
|
getnowpos(s_x, s_y); |
|
@ -91,7 +91,7 @@ int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int speed, a |
|
|
to_y = s_y + dy; |
|
|
to_y = s_y + dy; |
|
|
|
|
|
|
|
|
m_thread.start([this, to_x, to_y, speed, status_cb]() { |
|
|
m_thread.start([this, to_x, to_y, speed, status_cb]() { |
|
|
_motor_move_to(to_x, to_y, speed, m_cfg.acc, m_cfg.dec); |
|
|
|
|
|
|
|
|
_motor_move_to(to_x, to_y, speed, m_basecfg.acc, m_basecfg.dec); |
|
|
|
|
|
|
|
|
while (!_motor_is_reach_target()) { |
|
|
while (!_motor_is_reach_target()) { |
|
|
if (m_thread.getExitFlag()) break; |
|
|
if (m_thread.getExitFlag()) break; |
|
@ -115,28 +115,28 @@ int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, int speed, action_cb_ |
|
|
_motor_stop(); |
|
|
_motor_stop(); |
|
|
|
|
|
|
|
|
if (speed == 0) { |
|
|
if (speed == 0) { |
|
|
speed = m_cfg.maxspeed; |
|
|
|
|
|
|
|
|
speed = m_basecfg.maxspeed; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) { |
|
|
|
|
|
speed = m_cfg.maxspeed; |
|
|
|
|
|
|
|
|
if (m_basecfg.maxspeed != 0 && speed > m_basecfg.maxspeed) { |
|
|
|
|
|
speed = m_basecfg.maxspeed; |
|
|
} |
|
|
} |
|
|
int32_t s_x, s_y, to_x, to_y; |
|
|
int32_t s_x, s_y, to_x, to_y; |
|
|
getnowpos(s_x, s_y); |
|
|
getnowpos(s_x, s_y); |
|
|
to_x = s_x + dx; |
|
|
to_x = s_x + dx; |
|
|
to_y = s_y + dy; |
|
|
to_y = s_y + dy; |
|
|
|
|
|
|
|
|
if (m_cfg.min_x != 0 && to_x < m_cfg.min_x) { |
|
|
|
|
|
to_x = m_cfg.min_x; |
|
|
|
|
|
|
|
|
if (m_basecfg.min_x != 0 && to_x < m_basecfg.min_x) { |
|
|
|
|
|
to_x = m_basecfg.min_x; |
|
|
} |
|
|
} |
|
|
if (m_cfg.max_x != 0 && to_x > m_cfg.max_x) { |
|
|
|
|
|
to_x = m_cfg.max_x; |
|
|
|
|
|
|
|
|
if (m_basecfg.max_x != 0 && to_x > m_basecfg.max_x) { |
|
|
|
|
|
to_x = m_basecfg.max_x; |
|
|
} |
|
|
} |
|
|
if (m_cfg.min_y != 0 && to_y < m_cfg.min_y) { |
|
|
|
|
|
to_y = m_cfg.min_y; |
|
|
|
|
|
|
|
|
if (m_basecfg.min_y != 0 && to_y < m_basecfg.min_y) { |
|
|
|
|
|
to_y = m_basecfg.min_y; |
|
|
} |
|
|
} |
|
|
if (m_cfg.max_y != 0 && to_y > m_cfg.max_y) { |
|
|
|
|
|
to_y = m_cfg.max_y; |
|
|
|
|
|
|
|
|
if (m_basecfg.max_y != 0 && to_y > m_basecfg.max_y) { |
|
|
|
|
|
to_y = m_basecfg.max_y; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
if (to_x == s_x && to_y == s_y) { |
|
|
if (to_x == s_x && to_y == s_y) { |
|
@ -145,7 +145,7 @@ int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, int speed, action_cb_ |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
m_thread.start([this, to_x, to_y, speed, status_cb]() { |
|
|
m_thread.start([this, to_x, to_y, speed, status_cb]() { |
|
|
_motor_move_to(to_x, to_y, speed, m_cfg.acc, m_cfg.dec); |
|
|
|
|
|
|
|
|
_motor_move_to(to_x, to_y, speed, m_basecfg.acc, m_basecfg.dec); |
|
|
|
|
|
|
|
|
while (!_motor_is_reach_target()) { |
|
|
while (!_motor_is_reach_target()) { |
|
|
if (m_thread.getExitFlag()) break; |
|
|
if (m_thread.getExitFlag()) break; |
|
@ -212,8 +212,8 @@ int32_t XYRobotCtrlModule::move_to_zero_with_calibrate(int32_t x, int32_t y, act |
|
|
calibrate_x = dx + x; |
|
|
calibrate_x = dx + x; |
|
|
calibrate_y = dy + y; |
|
|
calibrate_y = dy + y; |
|
|
|
|
|
|
|
|
m_zero_shift_x = calibrate_x; |
|
|
|
|
|
m_zero_shift_y = calibrate_y; |
|
|
|
|
|
|
|
|
m_basecfg.shift_x = calibrate_x; |
|
|
|
|
|
m_basecfg.shift_y = calibrate_y; |
|
|
|
|
|
|
|
|
m_stepM1->setXACTUAL(0); |
|
|
m_stepM1->setXACTUAL(0); |
|
|
m_stepM2->setXACTUAL(0); |
|
|
m_stepM2->setXACTUAL(0); |
|
@ -245,7 +245,7 @@ int32_t XYRobotCtrlModule::stop(uint8_t stopType) { |
|
|
} else { |
|
|
} else { |
|
|
ZLOGI(TAG, "breakStop"); |
|
|
ZLOGI(TAG, "breakStop"); |
|
|
m_thread.stop(); |
|
|
m_thread.stop(); |
|
|
_motor_stop(m_cfg.breakdec); |
|
|
|
|
|
|
|
|
_motor_stop(m_basecfg.breakdec); |
|
|
return 0; |
|
|
return 0; |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
@ -293,91 +293,57 @@ int32_t XYRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info) { |
|
|
} |
|
|
} |
|
|
void XYRobotCtrlModule::create_default_cfg(flash_config_t& cfg) { |
|
|
void XYRobotCtrlModule::create_default_cfg(flash_config_t& cfg) { |
|
|
memset(&cfg, 0, sizeof(cfg)); |
|
|
memset(&cfg, 0, sizeof(cfg)); |
|
|
cfg.robot_type = corexy; |
|
|
|
|
|
cfg.x_shaft = 0; |
|
|
|
|
|
cfg.y_shaft = 0; |
|
|
|
|
|
cfg.ihold = 1; |
|
|
|
|
|
cfg.irun = 3; |
|
|
|
|
|
cfg.iholddelay = 100; |
|
|
|
|
|
cfg.distance_scale = 10000; |
|
|
|
|
|
cfg.shift_x = 0; |
|
|
|
|
|
cfg.shift_y = 0; |
|
|
|
|
|
|
|
|
cfg.basecfg.robot_type = corexy; |
|
|
|
|
|
cfg.basecfg.x_shaft = 0; |
|
|
|
|
|
cfg.basecfg.y_shaft = 0; |
|
|
|
|
|
cfg.basecfg.ihold = 1; |
|
|
|
|
|
cfg.basecfg.irun = 3; |
|
|
|
|
|
cfg.basecfg.iholddelay = 100; |
|
|
|
|
|
cfg.basecfg.distance_scale = 10000; |
|
|
|
|
|
cfg.basecfg.shift_x = 0; |
|
|
|
|
|
cfg.basecfg.shift_y = 0; |
|
|
|
|
|
|
|
|
// limit
|
|
|
// limit
|
|
|
cfg.acc = 300; |
|
|
|
|
|
cfg.dec = 300; |
|
|
|
|
|
cfg.breakdec = 1600; |
|
|
|
|
|
cfg.maxspeed = 800; |
|
|
|
|
|
cfg.min_x = 0; |
|
|
|
|
|
cfg.max_x = 0; |
|
|
|
|
|
cfg.min_y = 0; |
|
|
|
|
|
cfg.max_y = 0; |
|
|
|
|
|
|
|
|
|
|
|
cfg.run_to_zero_max_d = 10000 * 100; |
|
|
|
|
|
cfg.leave_from_zero_max_d = 3 * 10000; |
|
|
|
|
|
cfg.run_to_zero_speed = 300; |
|
|
|
|
|
cfg.run_to_zero_dec = 1600; |
|
|
|
|
|
|
|
|
cfg.basecfg.acc = 300; |
|
|
|
|
|
cfg.basecfg.dec = 300; |
|
|
|
|
|
cfg.basecfg.breakdec = 1600; |
|
|
|
|
|
cfg.basecfg.maxspeed = 800; |
|
|
|
|
|
cfg.basecfg.min_x = 0; |
|
|
|
|
|
cfg.basecfg.max_x = 0; |
|
|
|
|
|
cfg.basecfg.min_y = 0; |
|
|
|
|
|
cfg.basecfg.max_y = 0; |
|
|
|
|
|
|
|
|
|
|
|
cfg.basecfg.run_to_zero_max_d = 10000 * 100; |
|
|
|
|
|
cfg.basecfg.run_to_zero_speed = 200; |
|
|
|
|
|
cfg.basecfg.run_to_zero_dec = 1600; |
|
|
|
|
|
cfg.basecfg.look_zero_edge_max_d = 10000 * 3; |
|
|
|
|
|
cfg.basecfg.look_zero_edge_speed = 10; |
|
|
|
|
|
cfg.basecfg.look_zero_edge_dec = 1600; |
|
|
|
|
|
|
|
|
|
|
|
// cfg.basecfg.run_to_zero_max_d = 10000 * 100;
|
|
|
|
|
|
// cfg.basecfg.leave_from_zero_max_d = 3 * 10000;
|
|
|
|
|
|
// cfg.basecfg.run_to_zero_speed = 300;
|
|
|
|
|
|
// cfg.basecfg.run_to_zero_dec = 1600;
|
|
|
return; |
|
|
return; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
int32_t XYRobotCtrlModule::set_base_param(const base_param_t& inparam) { |
|
|
int32_t XYRobotCtrlModule::set_base_param(const base_param_t& inparam) { |
|
|
zlock_guard lock(m_lock); |
|
|
zlock_guard lock(m_lock); |
|
|
|
|
|
|
|
|
m_cfg.robot_type = inparam.robot_type; |
|
|
|
|
|
m_cfg.x_shaft = inparam.x_shaft; |
|
|
|
|
|
m_cfg.y_shaft = inparam.y_shaft; |
|
|
|
|
|
m_cfg.ihold = inparam.ihold; |
|
|
|
|
|
m_cfg.irun = inparam.irun; |
|
|
|
|
|
m_cfg.iholddelay = inparam.iholddelay; |
|
|
|
|
|
m_cfg.distance_scale = inparam.distance_scale; |
|
|
|
|
|
m_cfg.shift_x = inparam.shift_x; |
|
|
|
|
|
m_cfg.shift_y = inparam.shift_y; |
|
|
|
|
|
m_cfg.acc = inparam.acc; |
|
|
|
|
|
m_cfg.dec = inparam.dec; |
|
|
|
|
|
m_cfg.breakdec = inparam.breakdec; |
|
|
|
|
|
m_cfg.maxspeed = inparam.maxspeed; |
|
|
|
|
|
m_cfg.min_x = inparam.min_x; |
|
|
|
|
|
m_cfg.max_x = inparam.max_x; |
|
|
|
|
|
m_cfg.min_y = inparam.min_y; |
|
|
|
|
|
m_cfg.max_y = inparam.max_y; |
|
|
|
|
|
m_cfg.run_to_zero_max_d = inparam.run_to_zero_max_d; |
|
|
|
|
|
m_cfg.leave_from_zero_max_d = inparam.leave_from_zero_max_d; |
|
|
|
|
|
m_cfg.run_to_zero_speed = inparam.run_to_zero_speed; |
|
|
|
|
|
m_cfg.run_to_zero_dec = inparam.run_to_zero_dec; |
|
|
|
|
|
|
|
|
m_basecfg = inparam; |
|
|
active_cfg(); |
|
|
active_cfg(); |
|
|
return 0; |
|
|
return 0; |
|
|
}; |
|
|
}; |
|
|
|
|
|
|
|
|
void XYRobotCtrlModule::active_cfg() { |
|
|
void XYRobotCtrlModule::active_cfg() { |
|
|
m_stepM1->setScale(m_cfg.distance_scale); |
|
|
|
|
|
m_stepM2->setScale(m_cfg.distance_scale); |
|
|
|
|
|
m_stepM1->setIHOLD_IRUN(m_cfg.ihold, m_cfg.irun, m_cfg.iholddelay); |
|
|
|
|
|
m_stepM2->setIHOLD_IRUN(m_cfg.ihold, m_cfg.irun, m_cfg.iholddelay); |
|
|
|
|
|
|
|
|
m_stepM1->setScale(m_basecfg.distance_scale); |
|
|
|
|
|
m_stepM2->setScale(m_basecfg.distance_scale); |
|
|
|
|
|
m_stepM1->setIHOLD_IRUN(m_basecfg.ihold, m_basecfg.irun, m_basecfg.iholddelay); |
|
|
|
|
|
m_stepM2->setIHOLD_IRUN(m_basecfg.ihold, m_basecfg.irun, m_basecfg.iholddelay); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
int32_t XYRobotCtrlModule::get_base_param(base_param_t& ack) { |
|
|
int32_t XYRobotCtrlModule::get_base_param(base_param_t& ack) { |
|
|
zlock_guard lock(m_lock); |
|
|
zlock_guard lock(m_lock); |
|
|
ack.robot_type = m_cfg.robot_type; |
|
|
|
|
|
ack.x_shaft = m_cfg.x_shaft; |
|
|
|
|
|
ack.y_shaft = m_cfg.y_shaft; |
|
|
|
|
|
ack.ihold = m_cfg.ihold; |
|
|
|
|
|
ack.irun = m_cfg.irun; |
|
|
|
|
|
ack.iholddelay = m_cfg.iholddelay; |
|
|
|
|
|
ack.distance_scale = m_cfg.distance_scale; |
|
|
|
|
|
ack.shift_x = m_cfg.shift_x; |
|
|
|
|
|
ack.shift_y = m_cfg.shift_y; |
|
|
|
|
|
ack.acc = m_cfg.acc; |
|
|
|
|
|
ack.dec = m_cfg.dec; |
|
|
|
|
|
ack.breakdec = m_cfg.breakdec; |
|
|
|
|
|
ack.maxspeed = m_cfg.maxspeed; |
|
|
|
|
|
ack.min_x = m_cfg.min_x; |
|
|
|
|
|
ack.max_x = m_cfg.max_x; |
|
|
|
|
|
ack.min_y = m_cfg.min_y; |
|
|
|
|
|
ack.max_y = m_cfg.max_y; |
|
|
|
|
|
ack.run_to_zero_max_d = m_cfg.run_to_zero_max_d; |
|
|
|
|
|
ack.leave_from_zero_max_d = m_cfg.leave_from_zero_max_d; |
|
|
|
|
|
ack.run_to_zero_speed = m_cfg.run_to_zero_speed; |
|
|
|
|
|
ack.run_to_zero_dec = m_cfg.run_to_zero_dec; |
|
|
|
|
|
|
|
|
ack = m_basecfg; |
|
|
return 0; |
|
|
return 0; |
|
|
}; |
|
|
}; |
|
|
|
|
|
|
|
@ -398,104 +364,103 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task(int32_t& dx, int32_t& dy) { |
|
|
|
|
|
|
|
|
int32_t XYRobotCtrlModule::exec_move_to_zero_task() { |
|
|
int32_t XYRobotCtrlModule::exec_move_to_zero_task() { |
|
|
/*******************************************************************************
|
|
|
/*******************************************************************************
|
|
|
* 远离X零点 * |
|
|
|
|
|
|
|
|
* 移动导零点 * |
|
|
*******************************************************************************/ |
|
|
*******************************************************************************/ |
|
|
if (m_Xgpio->getState()) { |
|
|
|
|
|
/**
|
|
|
|
|
|
* @brief 如果设备已经在零点,则反向移动一定距离远离零点 |
|
|
|
|
|
*/ |
|
|
|
|
|
_motor_move_by(m_cfg.leave_from_zero_max_d, 0, m_cfg.run_to_zero_speed, m_cfg.acc, m_cfg.run_to_zero_dec); |
|
|
|
|
|
while (!_motor_is_reach_target()) { |
|
|
|
|
|
if (m_thread.getExitFlag()) break; |
|
|
|
|
|
|
|
|
if (!m_Xgpio->getState()) { |
|
|
|
|
|
_motor_move_by(-m_basecfg.run_to_zero_max_d, 0, m_basecfg.run_to_zero_speed, m_basecfg.acc, m_basecfg.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); |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
vTaskDelay(1); |
|
|
vTaskDelay(1); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
if (m_thread.getExitFlag()) { |
|
|
if (m_thread.getExitFlag()) { |
|
|
ZLOGI(TAG, "break move to zero thread exit"); |
|
|
ZLOGI(TAG, "break move to zero thread exit"); |
|
|
return 0; |
|
|
|
|
|
|
|
|
return err::kce_break_by_user; |
|
|
} |
|
|
} |
|
|
if (m_Xgpio->getState()) { |
|
|
|
|
|
ZLOGE(TAG, "leave away zero failed"); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (!xreach_zero) { |
|
|
|
|
|
ZLOGE(TAG, "find zero point fail"); |
|
|
return err::kce_x_leave_away_zero_point_fail; |
|
|
return err::kce_x_leave_away_zero_point_fail; |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
/*******************************************************************************
|
|
|
|
|
|
* 移动X轴到零点 * |
|
|
|
|
|
*******************************************************************************/ |
|
|
|
|
|
_motor_move_by(-m_cfg.run_to_zero_max_d, 0, m_cfg.run_to_zero_speed, m_cfg.acc, m_cfg.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); |
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (m_Xgpio->getState()) { |
|
|
|
|
|
_motor_move_by(m_basecfg.look_zero_edge_max_d, 0, m_basecfg.look_zero_edge_speed, m_basecfg.acc, m_basecfg.look_zero_edge_dec); |
|
|
|
|
|
bool xleave_zero = false; |
|
|
|
|
|
while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { |
|
|
|
|
|
if (m_Xgpio->getState()) { |
|
|
|
|
|
xleave_zero = true; |
|
|
|
|
|
_motor_stop(-1); |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
|
|
|
vTaskDelay(1); |
|
|
} |
|
|
} |
|
|
vTaskDelay(1); |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
if (!xreach_zero) { |
|
|
|
|
|
// 触发异常回调
|
|
|
|
|
|
ZLOGE(TAG, "x reach zero failed"); |
|
|
|
|
|
return err::kce_not_found_x_zero_point; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
if (m_thread.getExitFlag()) { |
|
|
|
|
|
ZLOGI(TAG, "break move to zero thread exit"); |
|
|
|
|
|
return err::kce_break_by_user; |
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
if (m_thread.getExitFlag()) { |
|
|
|
|
|
ZLOGI(TAG, "break move to zero thread exit"); |
|
|
|
|
|
return 0; |
|
|
|
|
|
|
|
|
if (!xleave_zero) { |
|
|
|
|
|
ZLOGI(TAG, "leave away zero failed"); |
|
|
|
|
|
return err::kce_x_leave_away_zero_point_fail; |
|
|
|
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
ZLOGI(TAG, "x reach zero"); |
|
|
|
|
|
|
|
|
|
|
|
/*******************************************************************************
|
|
|
/*******************************************************************************
|
|
|
* 远离Y零点 * |
|
|
|
|
|
|
|
|
* 移动Y轴到零点 * |
|
|
*******************************************************************************/ |
|
|
*******************************************************************************/ |
|
|
|
|
|
|
|
|
if (m_Ygpio->getState()) { |
|
|
|
|
|
/**
|
|
|
|
|
|
* @brief 如果设备已经在零点,则反向移动一定距离远离零点 |
|
|
|
|
|
*/ |
|
|
|
|
|
_motor_move_by(0, m_cfg.leave_from_zero_max_d, m_cfg.run_to_zero_speed, m_cfg.acc, m_cfg.dec); |
|
|
|
|
|
while (!_motor_is_reach_target()) { |
|
|
|
|
|
if (m_thread.getExitFlag()) break; |
|
|
|
|
|
|
|
|
if (!m_Ygpio->getState()) { |
|
|
|
|
|
_motor_move_by(0, -m_basecfg.run_to_zero_max_d, m_basecfg.run_to_zero_speed, m_basecfg.acc, m_basecfg.run_to_zero_dec); |
|
|
|
|
|
bool yreach_zero = false; |
|
|
|
|
|
while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { |
|
|
|
|
|
// ZLOGI(TAG, "ygpio %d", m_Ygpio->getState());
|
|
|
|
|
|
if (m_Ygpio->getState()) { |
|
|
|
|
|
yreach_zero = true; |
|
|
|
|
|
_motor_stop(-1); |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
vTaskDelay(1); |
|
|
vTaskDelay(1); |
|
|
} |
|
|
} |
|
|
if (m_thread.getExitFlag()) { |
|
|
if (m_thread.getExitFlag()) { |
|
|
ZLOGI(TAG, "break move to zero thread exit"); |
|
|
ZLOGI(TAG, "break move to zero thread exit"); |
|
|
return 0; |
|
|
|
|
|
|
|
|
return err::kce_break_by_user; |
|
|
} |
|
|
} |
|
|
if (m_Ygpio->getState()) { |
|
|
|
|
|
ZLOGE(TAG, "leave away zero failed"); |
|
|
|
|
|
|
|
|
if (!yreach_zero) { |
|
|
|
|
|
ZLOGE(TAG, "find zero point fail"); |
|
|
return err::kce_y_leave_away_zero_point_fail; |
|
|
return err::kce_y_leave_away_zero_point_fail; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
/*******************************************************************************
|
|
|
|
|
|
* 移动Y轴到零点 * |
|
|
|
|
|
*******************************************************************************/ |
|
|
|
|
|
_motor_move_by(0, -m_cfg.run_to_zero_max_d, m_cfg.run_to_zero_speed, m_cfg.acc, m_cfg.run_to_zero_dec); |
|
|
|
|
|
bool yreach_zero = false; |
|
|
|
|
|
while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { |
|
|
|
|
|
// ZLOGI(TAG, "ygpio %d", m_Ygpio->getState());
|
|
|
|
|
|
if (m_Ygpio->getState()) { |
|
|
|
|
|
yreach_zero = true; |
|
|
|
|
|
_motor_stop(-1); |
|
|
|
|
|
break; |
|
|
|
|
|
|
|
|
if (m_Ygpio->getState()) { |
|
|
|
|
|
_motor_move_by(0, m_basecfg.look_zero_edge_max_d, m_basecfg.look_zero_edge_speed, m_basecfg.acc, m_basecfg.look_zero_edge_dec); |
|
|
|
|
|
bool yleave_zero = false; |
|
|
|
|
|
while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { |
|
|
|
|
|
if (m_Ygpio->getState()) { |
|
|
|
|
|
yleave_zero = true; |
|
|
|
|
|
_motor_stop(-1); |
|
|
|
|
|
break; |
|
|
|
|
|
} |
|
|
|
|
|
vTaskDelay(1); |
|
|
|
|
|
} |
|
|
|
|
|
if (m_thread.getExitFlag()) { |
|
|
|
|
|
ZLOGI(TAG, "break move to zero thread exit"); |
|
|
|
|
|
return err::kce_break_by_user; |
|
|
|
|
|
} |
|
|
|
|
|
if (!yleave_zero) { |
|
|
|
|
|
ZLOGI(TAG, "leave away zero failed"); |
|
|
|
|
|
return err::kce_y_leave_away_zero_point_fail; |
|
|
} |
|
|
} |
|
|
vTaskDelay(1); |
|
|
|
|
|
} |
|
|
|
|
|
if (!yreach_zero) { |
|
|
|
|
|
// 触发异常回调
|
|
|
|
|
|
ZLOGE(TAG, "y reach zero failed"); |
|
|
|
|
|
return err::kce_not_found_y_zero_point; |
|
|
|
|
|
} |
|
|
|
|
|
if (m_thread.getExitFlag()) { |
|
|
|
|
|
ZLOGI(TAG, "break move to zero thread exit"); |
|
|
|
|
|
return 0; |
|
|
|
|
|
} |
|
|
} |
|
|
ZLOGI(TAG, "y reach zero"); |
|
|
|
|
|
ZLOGI(TAG, "move_to_zero success"); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
return 0; |
|
|
return 0; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
@ -553,27 +518,27 @@ void XYRobotCtrlModule::_motor_stop(int32_t dec) { |
|
|
m_stepM2->stop(); |
|
|
m_stepM2->stop(); |
|
|
} |
|
|
} |
|
|
void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y) { |
|
|
void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y) { |
|
|
if (m_cfg.robot_type == corexy) { |
|
|
|
|
|
|
|
|
if (m_basecfg.robot_type == corexy) { |
|
|
x = (m1pos - m2pos); |
|
|
x = (m1pos - m2pos); |
|
|
y = (m1pos + m2pos); |
|
|
y = (m1pos + m2pos); |
|
|
} else { |
|
|
} else { |
|
|
x = m1pos + m2pos; |
|
|
x = m1pos + m2pos; |
|
|
y = m1pos - m2pos; |
|
|
y = m1pos - m2pos; |
|
|
} |
|
|
} |
|
|
if (m_cfg.x_shaft) x = -x; |
|
|
|
|
|
if (m_cfg.y_shaft) y = -y; |
|
|
|
|
|
|
|
|
if (m_basecfg.x_shaft) x = -x; |
|
|
|
|
|
if (m_basecfg.y_shaft) y = -y; |
|
|
|
|
|
|
|
|
x += m_zero_shift_x; |
|
|
|
|
|
y += m_zero_shift_y; |
|
|
|
|
|
|
|
|
x += m_basecfg.shift_x; |
|
|
|
|
|
y += m_basecfg.shift_y; |
|
|
} |
|
|
} |
|
|
void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) { |
|
|
void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) { |
|
|
if (m_cfg.x_shaft) x = -x; |
|
|
|
|
|
if (m_cfg.y_shaft) y = -y; |
|
|
|
|
|
|
|
|
if (m_basecfg.x_shaft) x = -x; |
|
|
|
|
|
if (m_basecfg.y_shaft) y = -y; |
|
|
|
|
|
|
|
|
x -= m_zero_shift_x; |
|
|
|
|
|
y -= m_zero_shift_y; |
|
|
|
|
|
|
|
|
x -= m_basecfg.shift_x; |
|
|
|
|
|
y -= m_basecfg.shift_y; |
|
|
|
|
|
|
|
|
if (m_cfg.robot_type == corexy) { |
|
|
|
|
|
|
|
|
if (m_basecfg.robot_type == corexy) { |
|
|
m1pos = ((x + y) / 2); |
|
|
m1pos = ((x + y) / 2); |
|
|
m2pos = ((y - x) / 2); |
|
|
m2pos = ((y - x) / 2); |
|
|
} else { |
|
|
} else { |
|
@ -588,27 +553,29 @@ void XYRobotCtrlModule::call_status_cb(action_cb_status_t cb, int32_t status) { |
|
|
if (cb) cb(status); |
|
|
if (cb) cb(status); |
|
|
} |
|
|
} |
|
|
void XYRobotCtrlModule::dumpcfg() { |
|
|
void XYRobotCtrlModule::dumpcfg() { |
|
|
ZLOGI(TAG, "robot_type :%d", m_cfg.robot_type); |
|
|
|
|
|
ZLOGI(TAG, "x_shaft :%d", m_cfg.x_shaft); |
|
|
|
|
|
ZLOGI(TAG, "y_shaft :%d", m_cfg.y_shaft); |
|
|
|
|
|
ZLOGI(TAG, "ihold :%d", m_cfg.ihold); |
|
|
|
|
|
ZLOGI(TAG, "irun :%d", m_cfg.irun); |
|
|
|
|
|
ZLOGI(TAG, "iholddelay :%d", m_cfg.iholddelay); |
|
|
|
|
|
ZLOGI(TAG, "distance_scale :%d", m_cfg.distance_scale); |
|
|
|
|
|
ZLOGI(TAG, "shift_x :%d", m_cfg.shift_x); |
|
|
|
|
|
ZLOGI(TAG, "shift_y :%d", m_cfg.shift_y); |
|
|
|
|
|
ZLOGI(TAG, "acc :%d", m_cfg.acc); |
|
|
|
|
|
ZLOGI(TAG, "dec :%d", m_cfg.dec); |
|
|
|
|
|
ZLOGI(TAG, "breakdec :%d", m_cfg.breakdec); |
|
|
|
|
|
ZLOGI(TAG, "maxspeed :%d", m_cfg.maxspeed); |
|
|
|
|
|
ZLOGI(TAG, "min_x :%d", m_cfg.min_x); |
|
|
|
|
|
ZLOGI(TAG, "max_x :%d", m_cfg.max_x); |
|
|
|
|
|
ZLOGI(TAG, "min_y :%d", m_cfg.min_y); |
|
|
|
|
|
ZLOGI(TAG, "max_y :%d", m_cfg.max_y); |
|
|
|
|
|
ZLOGI(TAG, "run_to_zero_max_d :%d", m_cfg.run_to_zero_max_d); |
|
|
|
|
|
ZLOGI(TAG, "leave_from_zero_max_d :%d", m_cfg.leave_from_zero_max_d); |
|
|
|
|
|
ZLOGI(TAG, "run_to_zero_speed :%d", m_cfg.run_to_zero_speed); |
|
|
|
|
|
ZLOGI(TAG, "run_to_zero_dec :%d", m_cfg.run_to_zero_dec); |
|
|
|
|
|
|
|
|
ZLOGI(TAG, "robot_type :%d", m_basecfg.robot_type); |
|
|
|
|
|
ZLOGI(TAG, "x_shaft :%d", m_basecfg.x_shaft); |
|
|
|
|
|
ZLOGI(TAG, "y_shaft :%d", m_basecfg.y_shaft); |
|
|
|
|
|
ZLOGI(TAG, "ihold :%d", m_basecfg.ihold); |
|
|
|
|
|
ZLOGI(TAG, "irun :%d", m_basecfg.irun); |
|
|
|
|
|
ZLOGI(TAG, "iholddelay :%d", m_basecfg.iholddelay); |
|
|
|
|
|
ZLOGI(TAG, "distance_scale :%d", m_basecfg.distance_scale); |
|
|
|
|
|
ZLOGI(TAG, "shift_x :%d", m_basecfg.shift_x); |
|
|
|
|
|
ZLOGI(TAG, "shift_y :%d", m_basecfg.shift_y); |
|
|
|
|
|
ZLOGI(TAG, "acc :%d", m_basecfg.acc); |
|
|
|
|
|
ZLOGI(TAG, "dec :%d", m_basecfg.dec); |
|
|
|
|
|
ZLOGI(TAG, "breakdec :%d", m_basecfg.breakdec); |
|
|
|
|
|
ZLOGI(TAG, "maxspeed :%d", m_basecfg.maxspeed); |
|
|
|
|
|
ZLOGI(TAG, "min_x :%d", m_basecfg.min_x); |
|
|
|
|
|
ZLOGI(TAG, "max_x :%d", m_basecfg.max_x); |
|
|
|
|
|
ZLOGI(TAG, "min_y :%d", m_basecfg.min_y); |
|
|
|
|
|
ZLOGI(TAG, "max_y :%d", m_basecfg.max_y); |
|
|
|
|
|
ZLOGI(TAG, "run_to_zero_max_d :%d", m_basecfg.run_to_zero_max_d); |
|
|
|
|
|
ZLOGI(TAG, "run_to_zero_speed :%d", m_basecfg.run_to_zero_speed); |
|
|
|
|
|
ZLOGI(TAG, "run_to_zero_dec :%d", m_basecfg.run_to_zero_dec); |
|
|
|
|
|
ZLOGI(TAG, "look_zero_edge_max_d :%d", m_basecfg.look_zero_edge_max_d); |
|
|
|
|
|
ZLOGI(TAG, "look_zero_edge_speed :%d", m_basecfg.look_zero_edge_speed); |
|
|
|
|
|
ZLOGI(TAG, "look_zero_edge_dec :%d", m_basecfg.look_zero_edge_dec); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
int32_t XYRobotCtrlModule::flush() { //
|
|
|
int32_t XYRobotCtrlModule::flush() { //
|
|
|