4 changed files with 168 additions and 896 deletions
-
218components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
-
668components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp.bak
-
42components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
-
136components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp.bak
@ -1,668 +0,0 @@ |
|||
#include "xy_robot_ctrl_module.hpp" |
|||
|
|||
#include <string.h> |
|||
|
|||
#include "a8000_protocol\protocol.hpp" |
|||
#include "sdk\components\flash\znvs.hpp" |
|||
using namespace iflytop; |
|||
using namespace std; |
|||
#define TAG "XYRobotCtrlModule" |
|||
void XYRobotCtrlModule::initialize(int32_t id, IStepperMotor* stepM1, IStepperMotor* stepM2, // |
|||
ZGPIO* Xgpio, int ngpio, // |
|||
flash_config_t& default_cfg, const char* flashmark) { |
|||
m_stepM1 = stepM1; |
|||
m_stepM2 = stepM2; |
|||
m_Xgpio = &Xgpio[0]; |
|||
m_Ygpio = &Xgpio[1]; |
|||
m_gpiotable = Xgpio; |
|||
m_ngpio = ngpio; |
|||
m_moduleId = id; |
|||
ZASSERT(m_stepM1); |
|||
ZASSERT(m_stepM2); |
|||
ZASSERT(m_Xgpio); |
|||
ZASSERT(m_Ygpio); |
|||
|
|||
m_lock.init(); |
|||
|
|||
m_cfg = default_cfg; |
|||
enable(false); |
|||
active_cfg(); |
|||
m_stepM1->setScaleDenominator(10); |
|||
m_stepM2->setScaleDenominator(10); |
|||
m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); |
|||
} |
|||
|
|||
int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int32_t speed, action_cb_status_t status_cb) { |
|||
zlock_guard lock(m_lock); |
|||
if (!m_enable) enable(true); |
|||
ZLOGI(TAG, "move_to x:%d y:%d", x, y); |
|||
int runspeed = speed; |
|||
if (speed == 0) { |
|||
runspeed = m_cfg.maxspeed; |
|||
} |
|||
|
|||
if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) { |
|||
runspeed = m_cfg.maxspeed; |
|||
} |
|||
if (m_cfg.min_x != 0 && x < m_cfg.min_x) { |
|||
x = m_cfg.min_x; |
|||
} |
|||
if (m_cfg.max_x != 0 && x > m_cfg.max_x) { |
|||
x = m_cfg.max_x; |
|||
} |
|||
if (m_cfg.min_y != 0 && y < m_cfg.min_y) { |
|||
y = m_cfg.min_y; |
|||
} |
|||
if (m_cfg.max_y != 0 && y > m_cfg.max_y) { |
|||
y = m_cfg.max_y; |
|||
} |
|||
|
|||
// int32_t m1pos, m2pos; |
|||
m_thread.stop(); |
|||
m_thread.start([this, x, y, runspeed, status_cb]() { |
|||
_motor_move_to(x, y, runspeed, m_cfg.acc, m_cfg.dec); |
|||
while (!_motor_is_reach_target()) { |
|||
if (m_thread.getExitFlag()) break; |
|||
vTaskDelay(10); |
|||
} |
|||
_motor_stop(); |
|||
int32_t xnow, ynow; |
|||
getnowpos(xnow, ynow); |
|||
call_status_cb(status_cb, 0); |
|||
}); |
|||
return 0; |
|||
} |
|||
|
|||
int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb) { |
|||
zlock_guard lock(m_lock); |
|||
if (!m_enable) enable(true); |
|||
ZLOGI(TAG, "move_by_no_limit dx:%d dy:%d", dx, dy); |
|||
|
|||
m_thread.stop(); |
|||
_motor_stop(); |
|||
|
|||
if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) { |
|||
speed = m_cfg.maxspeed; |
|||
} |
|||
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; |
|||
|
|||
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); |
|||
|
|||
while (!_motor_is_reach_target()) { |
|||
if (m_thread.getExitFlag()) break; |
|||
vTaskDelay(10); |
|||
} |
|||
|
|||
_motor_stop(); |
|||
int32_t end_x, end_y; |
|||
getnowpos(end_x, end_y); |
|||
|
|||
call_status_cb(status_cb, 0); |
|||
}); |
|||
return 0; |
|||
}; |
|||
|
|||
int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb) { |
|||
zlock_guard lock(m_lock); |
|||
if (!m_enable) enable(true); |
|||
|
|||
ZLOGI(TAG, "move_by dx:%d dy:%d %d", dx, dy, speed); |
|||
|
|||
m_thread.stop(); |
|||
_motor_stop(); |
|||
|
|||
if (speed == 0) { |
|||
speed = m_cfg.maxspeed; |
|||
} |
|||
|
|||
if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) { |
|||
speed = m_cfg.maxspeed; |
|||
} |
|||
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; |
|||
|
|||
if (m_cfg.min_x != 0 && to_x < m_cfg.min_x) { |
|||
to_x = m_cfg.min_x; |
|||
} |
|||
if (m_cfg.max_x != 0 && to_x > m_cfg.max_x) { |
|||
to_x = m_cfg.max_x; |
|||
} |
|||
if (m_cfg.min_y != 0 && to_y < m_cfg.min_y) { |
|||
to_y = m_cfg.min_y; |
|||
} |
|||
if (m_cfg.max_y != 0 && to_y > m_cfg.max_y) { |
|||
to_y = m_cfg.max_y; |
|||
} |
|||
|
|||
if (to_x == s_x && to_y == s_y) { |
|||
call_status_cb(status_cb, 0); |
|||
return 0; |
|||
} |
|||
|
|||
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); |
|||
|
|||
while (!_motor_is_reach_target()) { |
|||
if (m_thread.getExitFlag()) break; |
|||
vTaskDelay(10); |
|||
} |
|||
|
|||
_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); |
|||
#endif |
|||
}); |
|||
return 0; |
|||
} |
|||
|
|||
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(); |
|||
|
|||
m_thread.start([this, status_cb]() { |
|||
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(); |
|||
call_status_cb(status_cb, erro); |
|||
return; |
|||
} |
|||
m_stepM1->setXACTUAL(0); |
|||
m_stepM2->setXACTUAL(0); |
|||
call_status_cb(status_cb, 0); |
|||
|
|||
return; |
|||
}); |
|||
|
|||
return 0; |
|||
} |
|||
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(); |
|||
|
|||
m_thread.start([this, x, y, status_cb]() { |
|||
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); |
|||
call_status_cb(status_cb, erro); |
|||
return; |
|||
} |
|||
|
|||
int32_t calibrate_x, calibrate_y; |
|||
calibrate_x = dx + x; |
|||
calibrate_y = dy + y; |
|||
|
|||
m_cfg.shift_x = calibrate_x; |
|||
m_cfg.shift_y = calibrate_y; |
|||
|
|||
m_stepM1->setXACTUAL(0); |
|||
m_stepM2->setXACTUAL(0); |
|||
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); |
|||
return; |
|||
}); |
|||
|
|||
return 0; |
|||
} |
|||
int32_t XYRobotCtrlModule::enable(bool venable) { |
|||
zlock_guard lock(m_lock); |
|||
ZLOGI(TAG, "enable:%d", venable); |
|||
m_stepM1->enable(venable); |
|||
m_stepM2->enable(venable); |
|||
m_enable = venable; |
|||
return 0; |
|||
} |
|||
int32_t XYRobotCtrlModule::stop(uint8_t stopType) { |
|||
zlock_guard lock(m_lock); |
|||
if (stopType == 0) { |
|||
ZLOGI(TAG, "stop"); |
|||
m_thread.stop(); |
|||
_motor_stop(); |
|||
return 0; |
|||
} else { |
|||
ZLOGI(TAG, "breakStop"); |
|||
m_thread.stop(); |
|||
_motor_stop(m_cfg.breakdec); |
|||
return 0; |
|||
} |
|||
} |
|||
int32_t XYRobotCtrlModule::force_change_current_pos(int32_t x, int32_t y) { |
|||
zlock_guard lock(m_lock); |
|||
ZLOGI(TAG, "set_current_pos x:%d y:%d", x, y); |
|||
int32_t m1pos, m2pos; |
|||
forward_kinematics(x, y, m1pos, m2pos); |
|||
m_stepM1->setXACTUAL(m1pos); |
|||
m_stepM2->setXACTUAL(m2pos); |
|||
return 0; |
|||
} |
|||
|
|||
void XYRobotCtrlModule::create_default_cfg(flash_config_t& cfg) { |
|||
memset(&cfg, 0, sizeof(cfg)); |
|||
cfg.robot_type = kcorexy; |
|||
cfg.x_shaft = 0; |
|||
cfg.y_shaft = 0; |
|||
cfg.ihold = 1; |
|||
cfg.irun = 3; |
|||
cfg.iholddelay = 100; |
|||
cfg.distance_scale = 7344; |
|||
cfg.shift_x = 0; |
|||
cfg.shift_y = 0; |
|||
|
|||
// 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_speed = 200; |
|||
cfg.run_to_zero_dec = 1600; |
|||
cfg.look_zero_edge_max_d = 10000 * 3; |
|||
cfg.look_zero_edge_speed = 10; |
|||
cfg.look_zero_edge_dec = 1600; |
|||
return; |
|||
} |
|||
|
|||
void XYRobotCtrlModule::active_cfg() { |
|||
m_stepM1->setScale(m_cfg.distance_scale / 2); |
|||
m_stepM2->setScale(m_cfg.distance_scale / 2); |
|||
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); |
|||
} |
|||
|
|||
int32_t XYRobotCtrlModule::exec_move_to_zero_task(int32_t& dx, int32_t& dy) { |
|||
int32_t xnow, ynow; |
|||
getnowpos(xnow, ynow); |
|||
|
|||
int32_t ret = exec_move_to_zero_task(); |
|||
|
|||
int32_t xnow2, ynow2; |
|||
getnowpos(xnow2, ynow2); |
|||
|
|||
dx = xnow2 - xnow; |
|||
dy = ynow2 - ynow; |
|||
|
|||
return ret; |
|||
} |
|||
|
|||
int32_t XYRobotCtrlModule::exec_move_to_zero_task() { |
|||
/******************************************************************************* |
|||
* 移动导零点 * |
|||
*******************************************************************************/ |
|||
if (!m_Xgpio->getState()) { |
|||
_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; |
|||
} |
|||
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, "find zero point fail"); |
|||
return err::kxymotor_x_find_zero_edge_fail; |
|||
} |
|||
ZLOGI(TAG, "step1 reach x zero ok"); |
|||
} |
|||
|
|||
if (m_Xgpio->getState()) { |
|||
_motor_move_by(m_cfg.look_zero_edge_max_d, 0, m_cfg.look_zero_edge_speed, m_cfg.acc, m_cfg.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); |
|||
} |
|||
|
|||
if (m_thread.getExitFlag()) { |
|||
ZLOGI(TAG, "break move to zero thread exit"); |
|||
return err::kmodule_opeation_break_by_user; |
|||
} |
|||
|
|||
if (!xleave_zero) { |
|||
ZLOGI(TAG, "leave away zero failed"); |
|||
return err::kxymotor_x_find_zero_edge_fail; |
|||
} |
|||
ZLOGI(TAG, "step2 leave x zero ok"); |
|||
} |
|||
|
|||
/******************************************************************************* |
|||
* 移动Y轴到零点 * |
|||
*******************************************************************************/ |
|||
|
|||
if (!m_Ygpio->getState()) { |
|||
_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; |
|||
} |
|||
vTaskDelay(1); |
|||
} |
|||
if (m_thread.getExitFlag()) { |
|||
ZLOGI(TAG, "break move to zero thread exit"); |
|||
return err::kmodule_opeation_break_by_user; |
|||
} |
|||
if (!yreach_zero) { |
|||
ZLOGE(TAG, "find zero point fail"); |
|||
return err::kxymotor_y_find_zero_edge_fail; |
|||
} |
|||
ZLOGI(TAG, "step3 reach y zero ok"); |
|||
} |
|||
|
|||
if (m_Ygpio->getState()) { |
|||
_motor_move_by(0, m_cfg.look_zero_edge_max_d, m_cfg.look_zero_edge_speed, m_cfg.acc, m_cfg.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::kmodule_opeation_break_by_user; |
|||
} |
|||
if (!yleave_zero) { |
|||
ZLOGI(TAG, "leave away zero failed"); |
|||
return err::kxymotor_y_find_zero_edge_fail; |
|||
} |
|||
ZLOGI(TAG, "step4 leave y zero ok"); |
|||
} |
|||
ZLOGI(TAG, "move to zero ok"); |
|||
|
|||
return 0; |
|||
} |
|||
|
|||
void XYRobotCtrlModule::getnowpos(int32_t& x, int32_t& y) { |
|||
int32_t m1pos, m2pos; |
|||
m1pos = m_stepM1->getXACTUAL(); |
|||
m2pos = m_stepM2->getXACTUAL(); |
|||
inverse_kinematics(m1pos, m2pos, x, y); |
|||
} |
|||
|
|||
/******************************************************************************* |
|||
* BASIC_ACTION * |
|||
*******************************************************************************/ |
|||
void XYRobotCtrlModule::_motor_move_to(int32_t x, int32_t y, int32_t maxv, int32_t acc, int32_t dec) { |
|||
ZLOGI(TAG, "_motor_move_to x:%d y:%d maxv:%d,acc:%d,dec:%d", x, y, maxv, acc, dec); |
|||
int32_t target_m1pos, target_m2pos; |
|||
forward_kinematics(x, y, target_m1pos, target_m2pos); |
|||
m_stepM1->setAcceleration(acc); |
|||
m_stepM2->setAcceleration(acc); |
|||
|
|||
m_stepM1->setDeceleration(dec); |
|||
m_stepM2->setDeceleration(dec); |
|||
|
|||
// ZLOGI(TAG, "_motor_move_to target_m1pos:%d target_m2pos:%d", target_m1pos, target_m2pos); |
|||
m_stepM1->moveTo(target_m1pos, maxv); |
|||
m_stepM2->moveTo(target_m2pos, maxv); |
|||
} |
|||
|
|||
void XYRobotCtrlModule::_motor_move_by(int32_t dx, int32_t dy, int32_t maxv, int32_t acc, int32_t dec) { |
|||
// ZLOGI(TAG, "_motor_move_by dx:%d dy:%d", dx, dy); |
|||
ZLOGI(TAG, "_motor_move_by dx:%d dy:%d maxv:%d acc:%d dec:%d ", dx, dy, maxv, acc, dec); |
|||
int32_t t_x, t_y; |
|||
getnowpos(t_x, t_y); |
|||
t_x = t_x + dx; |
|||
t_y = t_y + dy; |
|||
|
|||
int32_t target_m1pos, target_m2pos; |
|||
forward_kinematics(t_x, t_y, target_m1pos, target_m2pos); |
|||
|
|||
m_stepM1->setAcceleration(acc); |
|||
m_stepM2->setAcceleration(acc); |
|||
|
|||
m_stepM1->setDeceleration(dec); |
|||
m_stepM2->setDeceleration(dec); |
|||
// ZLOGI(TAG, "_motor_move_by target_m1pos:%d target_m2pos:%d", target_m1pos, target_m2pos); |
|||
m_stepM1->moveTo(target_m1pos, maxv); |
|||
m_stepM2->moveTo(target_m2pos, maxv); |
|||
} |
|||
void XYRobotCtrlModule::_motor_stop(int32_t dec) { |
|||
if (dec > 0) { |
|||
m_stepM1->setDeceleration(dec); |
|||
m_stepM2->setDeceleration(dec); |
|||
} |
|||
m_stepM1->stop(); |
|||
m_stepM2->stop(); |
|||
} |
|||
void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y) { |
|||
if (m_cfg.robot_type == kcorexy) { |
|||
x = (m1pos - m2pos); |
|||
y = (m1pos + m2pos); |
|||
} else { |
|||
x = m1pos + m2pos; |
|||
y = m1pos - m2pos; |
|||
} |
|||
|
|||
if (m_cfg.x_shaft) x = -x; |
|||
if (m_cfg.y_shaft) y = -y; |
|||
|
|||
x += m_cfg.shift_x; |
|||
y += m_cfg.shift_y; |
|||
} |
|||
void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) { |
|||
x -= m_cfg.shift_x; |
|||
y -= m_cfg.shift_y; |
|||
|
|||
if (m_cfg.x_shaft) x = -x; |
|||
if (m_cfg.y_shaft) y = -y; |
|||
|
|||
if (m_cfg.robot_type == kcorexy) { |
|||
m1pos = ((x + y) / 2); |
|||
m2pos = ((y - x) / 2); |
|||
} else { |
|||
m1pos = (x + y) / 2; |
|||
m2pos = (x - y) / 2; |
|||
} |
|||
} |
|||
|
|||
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); |
|||
} |
|||
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, "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, "look_zero_edge_max_d :%d", m_cfg.look_zero_edge_max_d); |
|||
ZLOGI(TAG, "look_zero_edge_speed :%d", m_cfg.look_zero_edge_speed); |
|||
ZLOGI(TAG, "look_zero_edge_dec :%d", m_cfg.look_zero_edge_dec); |
|||
} |
|||
|
|||
|
|||
int32_t XYRobotCtrlModule::getid(int32_t* id) { |
|||
*id = m_moduleId; |
|||
return 0; |
|||
} |
|||
int32_t XYRobotCtrlModule::module_stop() { |
|||
ZLOGI(TAG, "module_stop"); |
|||
m_thread.stop(); |
|||
_motor_stop(); |
|||
return 0; |
|||
} |
|||
int32_t XYRobotCtrlModule::module_break() { |
|||
ZLOGI(TAG, "module_break"); |
|||
m_thread.stop(); |
|||
_motor_stop(m_cfg.breakdec); |
|||
return 0; |
|||
} |
|||
int32_t XYRobotCtrlModule::module_get_last_exec_status(int32_t* status) { |
|||
*status = 0; |
|||
return 0; |
|||
} |
|||
int32_t XYRobotCtrlModule::module_get_status(int32_t* status) { |
|||
*status = m_thread.isworking() ? 0x01 : 0x00; |
|||
return 0; |
|||
} |
|||
int32_t XYRobotCtrlModule::module_get_error(int32_t* iserror) { |
|||
*iserror = 0; |
|||
return 0; |
|||
} |
|||
int32_t XYRobotCtrlModule::module_clear_error() { return 0; } |
|||
|
|||
int32_t XYRobotCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { |
|||
switch (param_id) { |
|||
MODULE_COMMON_PROCESS_REG_CB(); |
|||
|
|||
PROCESS_REG(kreg_xyrobot_default_velocity, REG_GET(m_cfg.maxspeed), REG_SET(m_cfg.maxspeed)); |
|||
PROCESS_REG(kreg_xyrobot_default_acc, REG_GET(m_cfg.acc), REG_SET(m_cfg.acc)); |
|||
PROCESS_REG(kreg_xyrobot_default_dec, REG_GET(m_cfg.dec), REG_SET(m_cfg.dec)); |
|||
PROCESS_REG(kreg_xyrobot_default_break_dec, REG_GET(m_cfg.breakdec), REG_SET(m_cfg.breakdec)); |
|||
PROCESS_REG(kreg_xyrobot_run_to_zero_speed, REG_GET(m_cfg.run_to_zero_speed), REG_SET(m_cfg.run_to_zero_speed)); |
|||
PROCESS_REG(kreg_xyrobot_run_to_zero_dec, REG_GET(m_cfg.run_to_zero_dec), REG_SET(m_cfg.run_to_zero_dec)); |
|||
PROCESS_REG(kreg_xyrobot_look_zero_edge_speed, REG_GET(m_cfg.look_zero_edge_speed), REG_SET(m_cfg.look_zero_edge_speed)); |
|||
PROCESS_REG(kreg_xyrobot_look_zero_edge_dec, REG_GET(m_cfg.look_zero_edge_dec), REG_SET(m_cfg.look_zero_edge_dec)); |
|||
PROCESS_REG(kreg_xyrobot_ihold, REG_GET(m_cfg.ihold), REG_SET(m_cfg.ihold)); |
|||
PROCESS_REG(kreg_xyrobot_irun, REG_GET(m_cfg.irun), REG_SET(m_cfg.irun)); |
|||
PROCESS_REG(kreg_xyrobot_iholddelay, REG_GET(m_cfg.iholddelay), REG_SET(m_cfg.iholddelay)); |
|||
|
|||
PROCESS_REG(kreg_xyrobot_x_shift, REG_GET(m_cfg.shift_x), REG_SET(m_cfg.shift_x)); |
|||
PROCESS_REG(kreg_xyrobot_y_shift, REG_GET(m_cfg.shift_y), REG_SET(m_cfg.shift_y)); |
|||
PROCESS_REG(kreg_xyrobot_x_shaft, REG_GET(m_cfg.x_shaft), REG_SET(m_cfg.x_shaft)); |
|||
PROCESS_REG(kreg_xyrobot_y_shaft, REG_GET(m_cfg.y_shaft), REG_SET(m_cfg.y_shaft)); |
|||
PROCESS_REG(kreg_xyrobot_x_one_circle_pulse, REG_GET(m_cfg.distance_scale), REG_SET(m_cfg.distance_scale)); |
|||
PROCESS_REG(kreg_xyrobot_y_one_circle_pulse, REG_GET(m_cfg.distance_scale), REG_SET(m_cfg.distance_scale)); |
|||
PROCESS_REG(kreg_xyrobot_run_to_zero_max_x_d, REG_GET(m_cfg.run_to_zero_max_d), REG_SET(m_cfg.run_to_zero_max_d)); |
|||
PROCESS_REG(kreg_xyrobot_run_to_zero_max_y_d, REG_GET(m_cfg.run_to_zero_max_d), REG_SET(m_cfg.run_to_zero_max_d)); |
|||
PROCESS_REG(kreg_xyrobot_look_zero_edge_max_x_d, REG_GET(m_cfg.look_zero_edge_max_d), REG_SET(m_cfg.look_zero_edge_max_d)); |
|||
PROCESS_REG(kreg_xyrobot_look_zero_edge_max_y_d, REG_GET(m_cfg.look_zero_edge_max_d), REG_SET(m_cfg.look_zero_edge_max_d)); |
|||
PROCESS_REG(kreg_xyrobot_robot_type, REG_GET(m_cfg.robot_type), REG_SET(m_cfg.robot_type)); |
|||
PROCESS_REG(kreg_xyrobot_io_state0, module_readio(&val), ACTION_NONE); |
|||
|
|||
default: |
|||
return err::kmodule_not_find_reg; |
|||
break; |
|||
} |
|||
return 0; |
|||
} |
|||
|
|||
int32_t XYRobotCtrlModule::module_readio(int32_t* io) { |
|||
*io = 0; |
|||
for (int i = 0; i < m_ngpio; i++) { |
|||
if (i > 32) break; |
|||
if (m_gpiotable[i].getState()) *io |= (1 << i); |
|||
} |
|||
return 0; |
|||
} |
|||
|
|||
int32_t XYRobotCtrlModule::module_read_adc(int32_t adcindex, int32_t* adc) { |
|||
*adc = 0; |
|||
return 0; |
|||
} |
|||
int32_t XYRobotCtrlModule::module_active_cfg() { |
|||
active_cfg(); |
|||
return 0; |
|||
} |
|||
|
|||
int32_t XYRobotCtrlModule::xymotor_enable(int32_t varenable) { |
|||
enable(varenable); |
|||
return 0; |
|||
} |
|||
int32_t XYRobotCtrlModule::xymotor_move_by(int32_t dx, int32_t dy, int32_t motor_velocity) { return move_by(dx, dy, motor_velocity, nullptr); } |
|||
int32_t XYRobotCtrlModule::xymotor_move_to(int32_t x, int32_t y, int32_t motor_velocity) { return move_to(x, y, motor_velocity, nullptr); } |
|||
int32_t XYRobotCtrlModule::xymotor_move_to_zero() { return move_to_zero(nullptr); } |
|||
|
|||
int32_t XYRobotCtrlModule::xymotor_move_to_zero_and_calculated_shift() { return move_to_zero_with_calibrate(0, 0, nullptr); } |
|||
int32_t XYRobotCtrlModule::xymotor_read_pos(int32_t* x, int32_t* y) { |
|||
getnowpos(*x, *y); |
|||
return 0; |
|||
} |
|||
|
|||
int32_t XYRobotCtrlModule::xymotor_calculated_pos_by_move_to_zero() { |
|||
zlock_guard lock(m_lock); |
|||
ZLOGI(TAG, "xymotor_calculated_pos_by_move_to_zero "); |
|||
m_thread.stop(); |
|||
|
|||
m_thread.start([this]() { |
|||
int32_t dx, dy; |
|||
int32_t erro = exec_move_to_zero_task(dx, dy); |
|||
if (erro != 0) { |
|||
m_dx = 0; |
|||
m_dy = 0; |
|||
_motor_stop(); |
|||
return; |
|||
} |
|||
m_dx = dx; |
|||
m_dy = dy; |
|||
|
|||
/** |
|||
* @brief |
|||
* TODO: add function to get dx and dy |
|||
*/ |
|||
|
|||
// creg.module_last_cmd_exec_status = 0; |
|||
// creg.module_last_cmd_exec_val0 = -m_dx; |
|||
// creg.module_last_cmd_exec_val1 = -m_dy; |
|||
m_stepM1->setXACTUAL(0); |
|||
m_stepM2->setXACTUAL(0); |
|||
ZLOGI(TAG, "xymotor_calculated_pos_by_move_to_zero dx:%d dy:%d", dx, dy); |
|||
return; |
|||
}); |
|||
return 0; |
|||
} |
|||
@ -1,136 +0,0 @@ |
|||
#pragma once |
|||
// |
|||
#include "a8000_protocol\protocol.hpp" |
|||
#include "sdk/os/zos.hpp" |
|||
#include "sdk\components\tmc\basic\tmc_ic_interface.hpp" |
|||
#include "sdk\components\zcancmder\zcanreceiver.hpp" |
|||
|
|||
namespace iflytop { |
|||
typedef int32_t s32; |
|||
typedef std::function<void(int32_t status)> action_cb_status_t; |
|||
|
|||
class XYRobotCtrlModule : public ZIXYMotor, public ZIModule { |
|||
ENABLE_MODULE(XYRobotCtrlModule, khbot_module, PC_VERSION); |
|||
|
|||
public: |
|||
typedef enum { khbot, kcorexy } robot_type_t; |
|||
|
|||
typedef struct { |
|||
bool configInited; |
|||
s32 robot_type; |
|||
s32 x_shaft; |
|||
s32 y_shaft; |
|||
s32 ihold; |
|||
s32 irun; |
|||
s32 iholddelay; |
|||
s32 distance_scale; // 0.001 |
|||
s32 shift_x; |
|||
s32 shift_y; |
|||
|
|||
// limit |
|||
s32 acc; |
|||
s32 dec; |
|||
s32 breakdec; |
|||
s32 maxspeed; |
|||
s32 min_x; |
|||
s32 max_x; |
|||
s32 min_y; |
|||
s32 max_y; |
|||
|
|||
s32 run_to_zero_max_d; |
|||
s32 run_to_zero_speed; |
|||
s32 run_to_zero_dec; |
|||
s32 look_zero_edge_max_d; |
|||
s32 look_zero_edge_speed; |
|||
s32 look_zero_edge_dec; |
|||
} flash_config_t; |
|||
|
|||
private: |
|||
IStepperMotor* m_stepM1; |
|||
IStepperMotor* m_stepM2; |
|||
|
|||
ZGPIO* m_Xgpio; |
|||
ZGPIO* m_Ygpio; |
|||
|
|||
ZGPIO* m_gpiotable; |
|||
int m_ngpio; |
|||
|
|||
ZThread m_thread; |
|||
|
|||
int32_t m_moduleId; |
|||
|
|||
int m_x = 0; |
|||
int m_y = 0; |
|||
|
|||
int32_t m_dx; |
|||
int32_t m_dy; |
|||
|
|||
flash_config_t m_cfg; |
|||
bool m_enable = false; |
|||
|
|||
zmutex m_lock; |
|||
// zmutex m_statu_lock; |
|||
|
|||
public: |
|||
void initialize(int32_t id, IStepperMotor* stepM1, IStepperMotor* stepM2, // |
|||
ZGPIO* Xgpio, int ngpio, // |
|||
flash_config_t& default_cfg, const char* flashmark); |
|||
|
|||
virtual int32_t move_to(int32_t x, int32_t y, int32_t speed, action_cb_status_t status_cb); |
|||
virtual int32_t move_by(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb); |
|||
virtual int32_t move_by_no_limit(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb); |
|||
|
|||
virtual int32_t move_to_zero(action_cb_status_t status_cb); |
|||
virtual int32_t move_to_zero_with_calibrate(int32_t x, int32_t y, action_cb_status_t status_cb); |
|||
virtual int32_t enable(bool venable); |
|||
virtual int32_t stop(uint8_t stopType); |
|||
virtual int32_t force_change_current_pos(int32_t x, int32_t y); |
|||
static void create_default_cfg(flash_config_t& cfg); |
|||
|
|||
|
|||
|
|||
void loop(); |
|||
void dumpcfg(); |
|||
virtual int32_t module_ping() { return 0; }; |
|||
|
|||
virtual int32_t getid(int32_t* id); |
|||
virtual int32_t module_stop(); |
|||
virtual int32_t module_break(); |
|||
virtual int32_t module_get_last_exec_status(int32_t* status); |
|||
virtual int32_t module_get_status(int32_t* status); |
|||
virtual int32_t module_get_error(int32_t* iserror); |
|||
virtual int32_t module_clear_error(); |
|||
virtual int32_t module_readio(int32_t* io); |
|||
virtual int32_t module_read_adc(int32_t adcindex, int32_t* adc); |
|||
virtual int32_t module_active_cfg(); |
|||
|
|||
virtual int32_t module_enable(int32_t enable) { return xymotor_enable(enable); } |
|||
|
|||
virtual int32_t xymotor_enable(int32_t enable); |
|||
virtual int32_t xymotor_move_by(int32_t dx, int32_t dy, int32_t motor_velocity); |
|||
virtual int32_t xymotor_move_to(int32_t x, int32_t y, int32_t motor_velocity); |
|||
virtual int32_t xymotor_move_to_zero(); |
|||
virtual int32_t xymotor_move_to_zero_and_calculated_shift(); |
|||
virtual int32_t xymotor_read_pos(int32_t* x, int32_t* y); |
|||
virtual int32_t xymotor_calculated_pos_by_move_to_zero(); |
|||
|
|||
private: |
|||
void active_cfg(); |
|||
void computeTargetMotorPos(); |
|||
void getnowpos(int32_t& x, int32_t& y); |
|||
|
|||
int32_t exec_move_to_zero_task(int32_t& dx, int32_t& dy); |
|||
int32_t exec_move_to_zero_task(); |
|||
|
|||
void inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y); |
|||
void forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos); |
|||
|
|||
void _motor_move_to(int32_t dx, int32_t dy, int32_t maxv, int32_t acc, int32_t dec); |
|||
void _motor_move_by(int32_t dx, int32_t dy, int32_t maxv, int32_t acc, int32_t dec); |
|||
void _motor_stop(int32_t dec = -1); |
|||
bool _motor_is_reach_target(); |
|||
|
|||
void call_status_cb(action_cb_status_t cb, int32_t status); |
|||
int32_t module_xxx_reg(int32_t param_id, bool read, int32_t& val); |
|||
}; |
|||
} // namespace iflytop |
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue