|
|
|
@ -1,5 +1,7 @@ |
|
|
|
#include "xy_robot_ctrl_module.hpp"
|
|
|
|
|
|
|
|
#include <string.h>
|
|
|
|
|
|
|
|
#include "sdk/components/errorcode/errorcode.hpp"
|
|
|
|
using namespace iflytop; |
|
|
|
using namespace std; |
|
|
|
@ -20,11 +22,11 @@ void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, // |
|
|
|
m_stepM1->setScale(scale); |
|
|
|
m_stepM2->setScale(scale); |
|
|
|
m_lock.init(); |
|
|
|
m_statu_lock.init(); |
|
|
|
// m_statu_lock.init();
|
|
|
|
m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); |
|
|
|
} |
|
|
|
|
|
|
|
int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, function<void(move_to_cb_status_t& status)> status_cb) { |
|
|
|
int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, action_cb_status_t status_cb) { |
|
|
|
zlock_guard lock(m_lock); |
|
|
|
ZLOGI(TAG, "move_to x:%d y:%d", x, y); |
|
|
|
int32_t m1pos, m2pos; |
|
|
|
@ -32,9 +34,8 @@ int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, function<void(move_to_c |
|
|
|
/**
|
|
|
|
* @TODO:添加限位检查 |
|
|
|
*/ |
|
|
|
updateStatus(1); |
|
|
|
m_thread.start([this, x, y, status_cb]() { |
|
|
|
_motor_move_to(x, y, m_cfg_velocity, m_cfg_acc, m_cfg_dec); |
|
|
|
_motor_move_to(x, y, m_cfg.maxspeed, m_cfg.acc, m_cfg.dec); |
|
|
|
while (!_motor_is_reach_target()) { |
|
|
|
if (m_thread.getExitFlag()) break; |
|
|
|
vTaskDelay(10); |
|
|
|
@ -42,18 +43,12 @@ int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, function<void(move_to_c |
|
|
|
_motor_stop(); |
|
|
|
int32_t xnow, ynow; |
|
|
|
getnowpos(xnow, ynow); |
|
|
|
move_to_cb_status_t status = {0}; |
|
|
|
status.exec_status = 0; |
|
|
|
status.tox = xnow; |
|
|
|
status.toy = ynow; |
|
|
|
|
|
|
|
if (status_cb) status_cb(status); |
|
|
|
updateStatus(0); |
|
|
|
call_status_cb(status_cb, 0); |
|
|
|
}); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, function<void(move_by_cb_status_t& status)> status_cb) { |
|
|
|
int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, action_cb_status_t status_cb) { |
|
|
|
zlock_guard lock(m_lock); |
|
|
|
ZLOGI(TAG, "move_by dx:%d dy:%d", dx, dy); |
|
|
|
|
|
|
|
@ -62,13 +57,12 @@ int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, function<void(move_by |
|
|
|
/**
|
|
|
|
* @TODO:添加限位检查 |
|
|
|
*/ |
|
|
|
updateStatus(1); |
|
|
|
m_thread.start([this, dx, dy, status_cb]() { |
|
|
|
int32_t s_x, s_y, to_x, to_y; |
|
|
|
getnowpos(s_x, s_y); |
|
|
|
to_x = s_x + dx; |
|
|
|
to_y = s_y + dy; |
|
|
|
_motor_move_to(to_x, to_y, m_cfg_velocity, m_cfg_acc, m_cfg_dec); |
|
|
|
_motor_move_to(to_x, to_y, m_cfg.maxspeed, m_cfg.acc, m_cfg.dec); |
|
|
|
|
|
|
|
while (!_motor_is_reach_target()) { |
|
|
|
if (m_thread.getExitFlag()) break; |
|
|
|
@ -78,60 +72,56 @@ int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, function<void(move_by |
|
|
|
_motor_stop(); |
|
|
|
int32_t end_x, end_y; |
|
|
|
getnowpos(end_x, end_y); |
|
|
|
|
|
|
|
call_status_cb(status_cb, 0); |
|
|
|
#if 0
|
|
|
|
move_by_cb_status_t status = {0}; |
|
|
|
status.exec_status = 0; |
|
|
|
status.dx = end_x - s_x; |
|
|
|
status.dx = end_y - s_y; |
|
|
|
if (status_cb) status_cb(status); |
|
|
|
updateStatus(0); |
|
|
|
#endif
|
|
|
|
}); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t XYRobotCtrlModule::move_to_zero(function<void(move_to_zero_cb_status_t& status)> status_cb) { |
|
|
|
int32_t XYRobotCtrlModule::move_to_zero(action_cb_status_t status_cb) { |
|
|
|
zlock_guard lock(m_lock); |
|
|
|
ZLOGI(TAG, "move_to_zero"); |
|
|
|
m_thread.stop(); |
|
|
|
|
|
|
|
updateStatus(1); |
|
|
|
m_thread.start([this, status_cb]() { |
|
|
|
int32_t dx, dy; |
|
|
|
move_to_zero_cb_status_t status = {0}; |
|
|
|
int32_t dx, dy; |
|
|
|
|
|
|
|
int32_t erro = exec_move_to_zero_task(dx, dy); |
|
|
|
if (erro != 0) { |
|
|
|
ZLOGI(TAG, "exec_move_to_zero_task failed:%d", erro); |
|
|
|
_motor_stop(); |
|
|
|
status.exec_status = erro; |
|
|
|
if (status_cb) status_cb(status); |
|
|
|
updateStatus(0); |
|
|
|
call_status_cb(status_cb, erro); |
|
|
|
return; |
|
|
|
} |
|
|
|
m_stepM1->setXACTUAL(0); |
|
|
|
m_stepM2->setXACTUAL(0); |
|
|
|
status.exec_status = 0; |
|
|
|
if (status_cb) status_cb(status); |
|
|
|
updateStatus(0); |
|
|
|
call_status_cb(status_cb, 0); |
|
|
|
|
|
|
|
return; |
|
|
|
}); |
|
|
|
|
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t XYRobotCtrlModule::move_to_zero_with_calibrate(int32_t x, int32_t y, function<void(move_to_zero_with_calibrate_cb_status_t& status)> status_cb) { |
|
|
|
int32_t XYRobotCtrlModule::move_to_zero_with_calibrate(int32_t x, int32_t y, action_cb_status_t status_cb) { |
|
|
|
zlock_guard lock(m_lock); |
|
|
|
ZLOGI(TAG, "move_to_zero_with_calibrate x:%d y:%d", x, y); |
|
|
|
m_thread.stop(); |
|
|
|
updateStatus(1); |
|
|
|
|
|
|
|
m_thread.start([this, x, y, status_cb]() { |
|
|
|
move_to_zero_with_calibrate_cb_status_t cb_status = {0}; |
|
|
|
int32_t dx, dy; |
|
|
|
int32_t erro = exec_move_to_zero_task(dx, dy); |
|
|
|
int32_t dx, dy; |
|
|
|
int32_t erro = exec_move_to_zero_task(dx, dy); |
|
|
|
if (erro != 0) { |
|
|
|
ZLOGI(TAG, "exec_move_to_zero_task failed:%d", erro); |
|
|
|
_motor_stop(); |
|
|
|
cb_status.exec_status = erro; |
|
|
|
if (status_cb) status_cb(cb_status); |
|
|
|
updateStatus(0); |
|
|
|
// cb_status.exec_status = erro;
|
|
|
|
// if (status_cb) status_cb(cb_status);
|
|
|
|
call_status_cb(status_cb, erro); |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
@ -144,12 +134,12 @@ int32_t XYRobotCtrlModule::move_to_zero_with_calibrate(int32_t x, int32_t y, fun |
|
|
|
|
|
|
|
m_stepM1->setXACTUAL(0); |
|
|
|
m_stepM2->setXACTUAL(0); |
|
|
|
cb_status.exec_status = 0; |
|
|
|
cb_status.zero_shift_x = m_zero_shift_x; |
|
|
|
cb_status.zero_shift_y = m_zero_shift_y; |
|
|
|
call_status_cb(status_cb, 0); |
|
|
|
// cb_status.exec_status = 0;
|
|
|
|
// cb_status.zero_shift_x = m_zero_shift_x;
|
|
|
|
// cb_status.zero_shift_y = m_zero_shift_y;
|
|
|
|
|
|
|
|
if (status_cb) status_cb(cb_status); |
|
|
|
updateStatus(0); |
|
|
|
// if (status_cb) status_cb(cb_status);
|
|
|
|
return; |
|
|
|
}); |
|
|
|
|
|
|
|
@ -172,7 +162,7 @@ int32_t XYRobotCtrlModule::stop(uint8_t stopType) { |
|
|
|
} else { |
|
|
|
ZLOGI(TAG, "breakStop"); |
|
|
|
m_thread.stop(); |
|
|
|
_motor_stop(m_cfg_breakdec); |
|
|
|
_motor_stop(m_cfg.breakdec); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
} |
|
|
|
@ -191,9 +181,9 @@ int32_t XYRobotCtrlModule::read_version(version_t& version) { |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t XYRobotCtrlModule::read_status(status_t& status) { |
|
|
|
zlock_guard lock(m_statu_lock); |
|
|
|
zlock_guard lock(m_lock); |
|
|
|
status = {0}; |
|
|
|
status.status = m_status; |
|
|
|
status.status = m_thread.isworking() ? 0x01 : 0x00; |
|
|
|
if (m_Xgpio) status.iostate |= m_Xgpio->getState() ? 0x01 : 0x00; |
|
|
|
if (m_Ygpio) status.iostate |= m_Ygpio->getState() ? 0x02 : 0x00; |
|
|
|
getnowpos(status.x, status.y); |
|
|
|
@ -205,106 +195,54 @@ int32_t XYRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info) { |
|
|
|
if (m_Xgpio) debug_info.iostate |= m_Xgpio->getState() ? 0x01 : 0x00; |
|
|
|
if (m_Ygpio) debug_info.iostate |= m_Ygpio->getState() ? 0x02 : 0x00; |
|
|
|
|
|
|
|
debug_info.status = m_status; |
|
|
|
debug_info.status = m_thread.isworking() ? 0x01 : 0x00; |
|
|
|
getnowpos(debug_info.x, debug_info.y); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
int32_t XYRobotCtrlModule::read_run_param(run_param_t& param) { return set_run_param(kset_cmd_type_read, param, param); } |
|
|
|
int32_t XYRobotCtrlModule::read_run_to_zero_param(run_to_zero_param_t& param) { return set_run_to_zero_param(kset_cmd_type_read, param, param); } |
|
|
|
int32_t XYRobotCtrlModule::read_warning_limit_param(warning_limit_param_t& param) { return set_warning_limit_param(kset_cmd_type_read, param, param); } |
|
|
|
|
|
|
|
int32_t XYRobotCtrlModule::set_run_param(run_param_t& param) { return set_run_param(kset_cmd_type_set, param, param); } |
|
|
|
int32_t XYRobotCtrlModule::set_run_to_zero_param(run_to_zero_param_t& param) { return set_run_to_zero_param(kset_cmd_type_set, param, param); } |
|
|
|
int32_t XYRobotCtrlModule::set_warning_limit_param(warning_limit_param_t& param) { return set_warning_limit_param(kset_cmd_type_set, param, param); } |
|
|
|
|
|
|
|
int32_t XYRobotCtrlModule::set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) { |
|
|
|
zlock_guard lock(m_lock); |
|
|
|
if (operation == kset_cmd_type_set) { |
|
|
|
m_robot_type = (RobotType_t)param.robot_type; |
|
|
|
m_x_shaft = param.x_shaft; |
|
|
|
m_y_shaft = param.y_shaft; |
|
|
|
m_cfg_ihold = param.ihold; |
|
|
|
m_cfg_irun = param.irun; |
|
|
|
m_cfg_iholddelay = param.iholddelay; |
|
|
|
m_cfg_distance_scale = param.distance_scale / 1000.0; |
|
|
|
m_zero_shift_x = param.shift_x; |
|
|
|
m_zero_shift_y = param.shift_y; |
|
|
|
m_cfg_acc = param.acc; |
|
|
|
m_cfg_dec = param.dec; |
|
|
|
m_cfg_velocity = param.maxspeed; |
|
|
|
m_min_x = param.min_x; |
|
|
|
m_max_x = param.max_x; |
|
|
|
m_min_y = param.min_y; |
|
|
|
m_max_y = param.max_y; |
|
|
|
ZLOGI(TAG, "================set_run_param===================="); |
|
|
|
ZLOGI(TAG, "= robot_type :%d", m_robot_type); |
|
|
|
ZLOGI(TAG, "= x_shaft :%d", m_x_shaft); |
|
|
|
ZLOGI(TAG, "= y_shaft :%d", m_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 :%f", m_cfg_distance_scale); |
|
|
|
ZLOGI(TAG, "= shift_x :%d", m_zero_shift_x); |
|
|
|
ZLOGI(TAG, "= shift_y :%d", m_zero_shift_y); |
|
|
|
ZLOGI(TAG, "= acc :%d", m_cfg_acc); |
|
|
|
ZLOGI(TAG, "= dec :%d", m_cfg_dec); |
|
|
|
ZLOGI(TAG, "= maxspeed :%d", m_cfg_velocity); |
|
|
|
ZLOGI(TAG, "= min_x :%d", m_min_x); |
|
|
|
ZLOGI(TAG, "= max_x :%d", m_max_x); |
|
|
|
ZLOGI(TAG, "= min_y :%d", m_min_y); |
|
|
|
ZLOGI(TAG, "= max_y :%d", m_max_y); |
|
|
|
ZLOGI(TAG, "==================================================="); |
|
|
|
|
|
|
|
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); |
|
|
|
} |
|
|
|
|
|
|
|
ack.robot_type = (u8)m_robot_type; |
|
|
|
ack.x_shaft = m_x_shaft; |
|
|
|
ack.y_shaft = m_y_shaft; |
|
|
|
ack.ihold = m_cfg_ihold; |
|
|
|
ack.irun = m_cfg_irun; |
|
|
|
ack.iholddelay = m_cfg_iholddelay; |
|
|
|
ack.distance_scale = m_cfg_distance_scale * 1000; |
|
|
|
ack.shift_x = m_zero_shift_x; |
|
|
|
ack.shift_y = m_zero_shift_y; |
|
|
|
ack.acc = m_cfg_acc; |
|
|
|
ack.dec = m_cfg_dec; |
|
|
|
ack.maxspeed = m_cfg_velocity; |
|
|
|
ack.min_x = m_min_x; |
|
|
|
ack.max_x = m_max_x; |
|
|
|
ack.min_y = m_min_y; |
|
|
|
ack.max_y = m_max_y; |
|
|
|
return 0; |
|
|
|
void XYRobotCtrlModule::create_default_cfg(base_param_t& 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 = 1000; |
|
|
|
cfg.shift_x = 0; |
|
|
|
cfg.shift_y = 0; |
|
|
|
|
|
|
|
// limit
|
|
|
|
cfg.acc = 300000; |
|
|
|
cfg.dec = 300000; |
|
|
|
cfg.breakdec = 300000; |
|
|
|
cfg.maxspeed = 300000; |
|
|
|
cfg.min_x = 0; |
|
|
|
cfg.max_x = 0; |
|
|
|
cfg.min_y = 0; |
|
|
|
cfg.max_y = 0; |
|
|
|
|
|
|
|
cfg.run_to_zero_max_d = 5120000; |
|
|
|
cfg.leave_from_zero_max_d = 51200; |
|
|
|
cfg.run_to_zero_speed = 30000; |
|
|
|
cfg.run_to_zero_dec = 600000; |
|
|
|
return; |
|
|
|
} |
|
|
|
int32_t XYRobotCtrlModule::set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) { |
|
|
|
if (operation == kset_cmd_type_set) { |
|
|
|
m_cfg_runtozero_maxX = param.move_to_zero_max_d; |
|
|
|
m_cfg_runtozero_maxY = param.move_to_zero_max_d; |
|
|
|
m_cfg_runtozero_leave_away_zero_maxXY = param.leave_from_zero_max_d; |
|
|
|
m_cfg_runtozero_speed = param.speed; |
|
|
|
m_cfg_runtozero_dec = param.dec; |
|
|
|
ZLOGI(TAG, "================set_run_to_zero_param===================="); |
|
|
|
ZLOGI(TAG, "= move_to_zero_max_d :%d", m_cfg_runtozero_maxX); |
|
|
|
ZLOGI(TAG, "= leave_from_zero_max_d :%d", m_cfg_runtozero_leave_away_zero_maxXY); |
|
|
|
ZLOGI(TAG, "= speed :%d", m_cfg_runtozero_speed); |
|
|
|
ZLOGI(TAG, "= dec :%d", m_cfg_runtozero_dec); |
|
|
|
ZLOGI(TAG, "==========================================================="); |
|
|
|
} |
|
|
|
|
|
|
|
ack.move_to_zero_max_d = m_cfg_runtozero_maxX; |
|
|
|
ack.leave_from_zero_max_d = m_cfg_runtozero_leave_away_zero_maxXY; |
|
|
|
ack.speed = m_cfg_runtozero_speed; |
|
|
|
ack.dec = m_cfg_runtozero_dec; |
|
|
|
int32_t XYRobotCtrlModule::set_base_param(const base_param_t& param) { |
|
|
|
zlock_guard lock(m_lock); |
|
|
|
m_cfg = param; |
|
|
|
m_stepM1->setScale(m_cfg.distance_scale / 1000.0); |
|
|
|
m_stepM2->setScale(m_cfg.distance_scale / 1000.0); |
|
|
|
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); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
int32_t XYRobotCtrlModule::set_warning_limit_param(uint8_t operation, const warning_limit_param_t& param, warning_limit_param_t& ack) { //
|
|
|
|
ZLOGI(TAG, "set_warning_limit_param %d", operation); |
|
|
|
}; |
|
|
|
int32_t XYRobotCtrlModule::get_base_param(base_param_t& ack) { |
|
|
|
zlock_guard lock(m_lock); |
|
|
|
ack = m_cfg; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
}; |
|
|
|
|
|
|
|
int32_t XYRobotCtrlModule::exec_move_to_zero_task(int32_t& dx, int32_t& dy) { |
|
|
|
int32_t xnow, ynow; |
|
|
|
getnowpos(xnow, ynow); |
|
|
|
@ -328,7 +266,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { |
|
|
|
/**
|
|
|
|
* @brief 如果设备已经在零点，则反向移动一定距离远离零点 |
|
|
|
*/ |
|
|
|
_motor_move_by(m_cfg_runtozero_leave_away_zero_maxXY, 0, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); |
|
|
|
_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; |
|
|
|
vTaskDelay(1); |
|
|
|
@ -346,7 +284,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { |
|
|
|
/*******************************************************************************
|
|
|
|
* 移动X轴到零点 * |
|
|
|
*******************************************************************************/ |
|
|
|
_motor_move_by(-m_cfg_runtozero_maxX, 0, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); |
|
|
|
_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());
|
|
|
|
@ -379,7 +317,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { |
|
|
|
/**
|
|
|
|
* @brief 如果设备已经在零点，则反向移动一定距离远离零点 |
|
|
|
*/ |
|
|
|
_motor_move_by(0, m_cfg_runtozero_leave_away_zero_maxXY, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_dec); |
|
|
|
_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; |
|
|
|
vTaskDelay(1); |
|
|
|
@ -397,7 +335,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { |
|
|
|
/*******************************************************************************
|
|
|
|
* 移动Y轴到零点 * |
|
|
|
*******************************************************************************/ |
|
|
|
_motor_move_by(0, -m_cfg_runtozero_maxY, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); |
|
|
|
_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());
|
|
|
|
@ -423,10 +361,6 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
void XYRobotCtrlModule::updateStatus(uint8_t status) { |
|
|
|
zlock_guard lock(m_statu_lock); |
|
|
|
m_status = status; |
|
|
|
} |
|
|
|
void XYRobotCtrlModule::getnowpos(int32_t& x, int32_t& y) { |
|
|
|
int32_t m1pos, m2pos; |
|
|
|
m1pos = m_stepM1->getXACTUAL(); |
|
|
|
@ -481,27 +415,27 @@ void XYRobotCtrlModule::_motor_stop(int32_t dec) { |
|
|
|
m_stepM2->stop(); |
|
|
|
} |
|
|
|
void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y) { |
|
|
|
if (m_robot_type == corexy) { |
|
|
|
if (m_cfg.robot_type == corexy) { |
|
|
|
x = (m1pos - m2pos); |
|
|
|
y = (m1pos + m2pos); |
|
|
|
} else { |
|
|
|
x = m1pos + m2pos; |
|
|
|
y = m1pos - m2pos; |
|
|
|
} |
|
|
|
if (m_x_shaft) x = -x; |
|
|
|
if (m_y_shaft) y = -y; |
|
|
|
if (m_cfg.x_shaft) x = -x; |
|
|
|
if (m_cfg.y_shaft) y = -y; |
|
|
|
|
|
|
|
x += m_zero_shift_x; |
|
|
|
y += m_zero_shift_y; |
|
|
|
} |
|
|
|
void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) { |
|
|
|
if (m_x_shaft) x = -x; |
|
|
|
if (m_y_shaft) y = -y; |
|
|
|
if (m_cfg.x_shaft) x = -x; |
|
|
|
if (m_cfg.y_shaft) y = -y; |
|
|
|
|
|
|
|
x -= m_zero_shift_x; |
|
|
|
y -= m_zero_shift_y; |
|
|
|
|
|
|
|
if (m_robot_type == corexy) { |
|
|
|
if (m_cfg.robot_type == corexy) { |
|
|
|
m1pos = ((x + y) / 2); |
|
|
|
m2pos = ((y - x) / 2); |
|
|
|
} else { |
|
|
|
@ -511,3 +445,9 @@ void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, |
|
|
|
} |
|
|
|
|
|
|
|
bool XYRobotCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget() && m_stepM2->isReachTarget(); } |
|
|
|
|
|
|
|
void XYRobotCtrlModule::call_status_cb(action_cb_status_t cb, int32_t status) { |
|
|
|
if (cb) { |
|
|
|
cb(status); |
|
|
|
} |
|
|
|
} |
