project_boditech_vidas_a8000_v3
/
a8000_subboard_sdk
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 1 year ago
parent
85d2a97eb1
commit
f312693388
3 changed files with 167 additions and 235 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 329
      components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
  2. 71
      components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
  3. 2
      components/zprotocols/zcancmder

329
components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
View File

@ -49,29 +49,29 @@ int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int32_t speed, action_c
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_basecfg.maxspeed;
runspeed = m_cfg.maxspeed;
} }
if (m_basecfg.maxspeed != 0 && speed > m_basecfg.maxspeed) {
runspeed = m_basecfg.maxspeed;
if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) {
runspeed = m_cfg.maxspeed;
} }
if (m_basecfg.min_x != 0 && x < m_basecfg.min_x) {
x = m_basecfg.min_x;
if (m_cfg.min_x != 0 && x < m_cfg.min_x) {
x = m_cfg.min_x;
} }
if (m_basecfg.max_x != 0 && x > m_basecfg.max_x) {
x = m_basecfg.max_x;
if (m_cfg.max_x != 0 && x > m_cfg.max_x) {
x = m_cfg.max_x;
} }
if (m_basecfg.min_y != 0 && y < m_basecfg.min_y) {
y = m_basecfg.min_y;
if (m_cfg.min_y != 0 && y < m_cfg.min_y) {
y = m_cfg.min_y;
} }
if (m_basecfg.max_y != 0 && y > m_basecfg.max_y) {
y = m_basecfg.max_y;
if (m_cfg.max_y != 0 && y > m_cfg.max_y) {
y = m_cfg.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_basecfg.acc, m_basecfg.dec);
_motor_move_to(x, y, runspeed, m_cfg.acc, m_cfg.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);
@ -92,8 +92,8 @@ int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int32_t spee
m_thread.stop(); m_thread.stop();
_motor_stop(); _motor_stop();
if (m_basecfg.maxspeed != 0 && speed > m_basecfg.maxspeed) {
speed = m_basecfg.maxspeed;
if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) {
speed = m_cfg.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);
@ -101,7 +101,7 @@ int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int32_t spee
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_basecfg.acc, m_basecfg.dec);
_motor_move_to(to_x, to_y, speed, m_cfg.acc, m_cfg.dec);
while (!_motor_is_reach_target()) { while (!_motor_is_reach_target()) {
if (m_thread.getExitFlag()) break; if (m_thread.getExitFlag()) break;
@ -127,28 +127,28 @@ int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, int32_t speed, action
_motor_stop(); _motor_stop();
if (speed == 0) { if (speed == 0) {
speed = m_basecfg.maxspeed;
speed = m_cfg.maxspeed;
} }
if (m_basecfg.maxspeed != 0 && speed > m_basecfg.maxspeed) {
speed = m_basecfg.maxspeed;
if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) {
speed = m_cfg.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_basecfg.min_x != 0 && to_x < m_basecfg.min_x) {
to_x = m_basecfg.min_x;
if (m_cfg.min_x != 0 && to_x < m_cfg.min_x) {
to_x = m_cfg.min_x;
} }
if (m_basecfg.max_x != 0 && to_x > m_basecfg.max_x) {
to_x = m_basecfg.max_x;
if (m_cfg.max_x != 0 && to_x > m_cfg.max_x) {
to_x = m_cfg.max_x;
} }
if (m_basecfg.min_y != 0 && to_y < m_basecfg.min_y) {
to_y = m_basecfg.min_y;
if (m_cfg.min_y != 0 && to_y < m_cfg.min_y) {
to_y = m_cfg.min_y;
} }
if (m_basecfg.max_y != 0 && to_y > m_basecfg.max_y) {
to_y = m_basecfg.max_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) { if (to_x == s_x && to_y == s_y) {
@ -157,7 +157,7 @@ int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, int32_t speed, action
} }
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_basecfg.acc, m_basecfg.dec);
_motor_move_to(to_x, to_y, speed, m_cfg.acc, m_cfg.dec);
while (!_motor_is_reach_target()) { while (!_motor_is_reach_target()) {
if (m_thread.getExitFlag()) break; if (m_thread.getExitFlag()) break;
@ -225,8 +225,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_basecfg.shift_x = calibrate_x;
m_basecfg.shift_y = calibrate_y;
m_cfg.shift_x = calibrate_x;
m_cfg.shift_y = calibrate_y;
m_stepM1->setXACTUAL(0); m_stepM1->setXACTUAL(0);
m_stepM2->setXACTUAL(0); m_stepM2->setXACTUAL(0);
@ -259,7 +259,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_basecfg.breakdec);
_motor_stop(m_cfg.breakdec);
return 0; return 0;
} }
} }
@ -272,97 +272,45 @@ int32_t XYRobotCtrlModule::force_change_current_pos(int32_t x, int32_t y) {
m_stepM2->setXACTUAL(m2pos); m_stepM2->setXACTUAL(m2pos);
return 0; return 0;
} }
int32_t XYRobotCtrlModule::read_version(version_t& version) {
zlock_guard lock(m_lock);
version = {0};
return 0;
}
int32_t XYRobotCtrlModule::read_status(status_t& status) {
zlock_guard lock(m_lock);
status = {0};
status.status = m_thread.isworking() ? 0x01 : 0x00;
status.iostate = 0xff;
for (int i = 0; i < m_ngpio; i++) {
if (i > 7) break;
if (!m_gpiotable[i].getState()) status.iostate &= ~(1 << i);
}
getnowpos(status.x, status.y);
return 0;
}
int32_t XYRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info) {
zlock_guard lock(m_lock);
debug_info = {0};
debug_info.iostate = 0xff;
for (int i = 0; i < m_ngpio; i++) {
if (i > 7) break;
// ZLOGI(TAG, "read_detailed_status i:%d %d", i,m_gpiotable[i].getState());
if (!m_gpiotable[i].getState()) debug_info.iostate &= ~(1 << i);
}
debug_info.status = m_thread.isworking() ? 0x01 : 0x00;
getnowpos(debug_info.x, debug_info.y);
return 0;
}
int32_t XYRobotCtrlModule::get_flash_config_size() { return sizeof(flash_config_t); }
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.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 = 7344;
cfg.basecfg.shift_x = 0;
cfg.basecfg.shift_y = 0;
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 // limit
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;
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.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; return;
} }
int32_t XYRobotCtrlModule::set_base_param(const base_param_t& inparam) {
zlock_guard lock(m_lock);
m_basecfg = inparam;
active_cfg();
return 0;
};
void XYRobotCtrlModule::active_cfg() { void XYRobotCtrlModule::active_cfg() {
m_stepM1->setScale(m_basecfg.distance_scale / 2);
m_stepM2->setScale(m_basecfg.distance_scale / 2);
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);
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::get_base_param(base_param_t& ack) {
zlock_guard lock(m_lock);
ack = m_basecfg;
return 0;
};
int32_t XYRobotCtrlModule::exec_move_to_zero_task(int32_t& dx, int32_t& dy) { int32_t XYRobotCtrlModule::exec_move_to_zero_task(int32_t& dx, int32_t& dy) {
int32_t xnow, ynow; int32_t xnow, ynow;
getnowpos(xnow, ynow); getnowpos(xnow, ynow);
@ -383,7 +331,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
* 移动导零点 * * 移动导零点 *
*******************************************************************************/ *******************************************************************************/
if (!m_Xgpio->getState()) { 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);
_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; bool xreach_zero = false;
while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
// ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget()); // ZLOGI(TAG, "xgpio %d %d %d", m_Xgpio->getState(), m_stepM1->isReachTarget(), m_stepM2->isReachTarget());
@ -408,7 +356,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
} }
if (m_Xgpio->getState()) { 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);
_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; bool xleave_zero = false;
while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
if (!m_Xgpio->getState()) { if (!m_Xgpio->getState()) {
@ -436,7 +384,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
*******************************************************************************/ *******************************************************************************/
if (!m_Ygpio->getState()) { 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);
_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; bool yreach_zero = false;
while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
// ZLOGI(TAG, "ygpio %d", m_Ygpio->getState()); // ZLOGI(TAG, "ygpio %d", m_Ygpio->getState());
@ -459,7 +407,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() {
} }
if (m_Ygpio->getState()) { 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);
_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; bool yleave_zero = false;
while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { while (!m_thread.getExitFlag() && !_motor_is_reach_target()) {
if (!m_Ygpio->getState()) { if (!m_Ygpio->getState()) {
@ -538,7 +486,7 @@ 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_basecfg.robot_type == corexy) {
if (m_cfg.robot_type == kcorexy) {
x = (m1pos - m2pos); x = (m1pos - m2pos);
y = (m1pos + m2pos); y = (m1pos + m2pos);
} else { } else {
@ -546,20 +494,20 @@ void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t
y = m1pos - m2pos; y = m1pos - m2pos;
} }
if (m_basecfg.x_shaft) x = -x;
if (m_basecfg.y_shaft) y = -y;
if (m_cfg.x_shaft) x = -x;
if (m_cfg.y_shaft) y = -y;
x += m_basecfg.shift_x;
y += m_basecfg.shift_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) { void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) {
x -= m_basecfg.shift_x;
y -= m_basecfg.shift_y;
x -= m_cfg.shift_x;
y -= m_cfg.shift_y;
if (m_basecfg.x_shaft) x = -x;
if (m_basecfg.y_shaft) y = -y;
if (m_cfg.x_shaft) x = -x;
if (m_cfg.y_shaft) y = -y;
if (m_basecfg.robot_type == corexy) {
if (m_cfg.robot_type == kcorexy) {
m1pos = ((x + y) / 2); m1pos = ((x + y) / 2);
m2pos = ((y - x) / 2); m2pos = ((y - x) / 2);
} else { } else {
@ -574,29 +522,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_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);
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::flush() { // int32_t XYRobotCtrlModule::flush() { //
@ -632,7 +580,7 @@ int32_t XYRobotCtrlModule::module_stop() {
int32_t XYRobotCtrlModule::module_break() { int32_t XYRobotCtrlModule::module_break() {
ZLOGI(TAG, "module_break"); ZLOGI(TAG, "module_break");
m_thread.stop(); m_thread.stop();
_motor_stop(m_basecfg.breakdec);
_motor_stop(m_cfg.breakdec);
return 0; return 0;
} }
int32_t XYRobotCtrlModule::module_get_last_exec_status(int32_t* status) { int32_t XYRobotCtrlModule::module_get_last_exec_status(int32_t* status) {
@ -649,35 +597,33 @@ int32_t XYRobotCtrlModule::module_get_error(int32_t* iserror) {
} }
int32_t XYRobotCtrlModule::module_clear_error() { return 0; } int32_t XYRobotCtrlModule::module_clear_error() { return 0; }
int32_t XYRobotCtrlModule::module_set_reg(int32_t param_id, int32_t param_value) { return module_xxx_reg(param_id, false, param_value); }
int32_t XYRobotCtrlModule::module_get_reg(int32_t param_id, int32_t* param_value) { return module_xxx_reg(param_id, true, *param_value); }
int32_t XYRobotCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { int32_t XYRobotCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) {
switch (param_id) { switch (param_id) {
MODULE_COMMON_PROCESS_REG_CB(); MODULE_COMMON_PROCESS_REG_CB();
PROCESS_REG(kreg_xyrobot_default_velocity, REG_GET(m_basecfg.maxspeed), REG_SET(m_basecfg.maxspeed));
PROCESS_REG(kreg_xyrobot_default_acc, REG_GET(m_basecfg.acc), REG_SET(m_basecfg.acc));
PROCESS_REG(kreg_xyrobot_default_dec, REG_GET(m_basecfg.dec), REG_SET(m_basecfg.dec));
PROCESS_REG(kreg_xyrobot_default_break_dec, REG_GET(m_basecfg.breakdec), REG_SET(m_basecfg.breakdec));
PROCESS_REG(kreg_xyrobot_run_to_zero_speed, REG_GET(m_basecfg.run_to_zero_speed), REG_SET(m_basecfg.run_to_zero_speed));
PROCESS_REG(kreg_xyrobot_run_to_zero_dec, REG_GET(m_basecfg.run_to_zero_dec), REG_SET(m_basecfg.run_to_zero_dec));
PROCESS_REG(kreg_xyrobot_look_zero_edge_speed, REG_GET(m_basecfg.look_zero_edge_speed), REG_SET(m_basecfg.look_zero_edge_speed));
PROCESS_REG(kreg_xyrobot_look_zero_edge_dec, REG_GET(m_basecfg.look_zero_edge_dec), REG_SET(m_basecfg.look_zero_edge_dec));
PROCESS_REG(kreg_xyrobot_ihold, REG_GET(m_basecfg.ihold), REG_SET(m_basecfg.ihold));
PROCESS_REG(kreg_xyrobot_irun, REG_GET(m_basecfg.irun), REG_SET(m_basecfg.irun));
PROCESS_REG(kreg_xyrobot_iholddelay, REG_GET(m_basecfg.iholddelay), REG_SET(m_basecfg.iholddelay));
PROCESS_REG(kreg_xyrobot_x_shift, REG_GET(m_basecfg.shift_x), REG_SET(m_basecfg.shift_x));
PROCESS_REG(kreg_xyrobot_y_shift, REG_GET(m_basecfg.shift_y), REG_SET(m_basecfg.shift_y));
PROCESS_REG(kreg_xyrobot_x_shaft, REG_GET(m_basecfg.x_shaft), REG_SET(m_basecfg.x_shaft));
PROCESS_REG(kreg_xyrobot_y_shaft, REG_GET(m_basecfg.y_shaft), REG_SET(m_basecfg.y_shaft));
PROCESS_REG(kreg_xyrobot_x_one_circle_pulse, REG_GET(m_basecfg.distance_scale), REG_SET(m_basecfg.distance_scale));
PROCESS_REG(kreg_xyrobot_y_one_circle_pulse, REG_GET(m_basecfg.distance_scale), REG_SET(m_basecfg.distance_scale));
PROCESS_REG(kreg_xyrobot_run_to_zero_max_x_d, REG_GET(m_basecfg.run_to_zero_max_d), REG_SET(m_basecfg.run_to_zero_max_d));
PROCESS_REG(kreg_xyrobot_run_to_zero_max_y_d, REG_GET(m_basecfg.run_to_zero_max_d), REG_SET(m_basecfg.run_to_zero_max_d));
PROCESS_REG(kreg_xyrobot_look_zero_edge_max_x_d, REG_GET(m_basecfg.look_zero_edge_max_d), REG_SET(m_basecfg.look_zero_edge_max_d));
PROCESS_REG(kreg_xyrobot_look_zero_edge_max_y_d, REG_GET(m_basecfg.look_zero_edge_max_d), REG_SET(m_basecfg.look_zero_edge_max_d));
PROCESS_REG(kreg_xyrobot_robot_type, REG_GET(m_basecfg.robot_type), REG_SET(m_basecfg.robot_type));
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); PROCESS_REG(kreg_xyrobot_io_state0, module_readio(&val), ACTION_NONE);
default: default:
@ -687,43 +633,6 @@ int32_t XYRobotCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t&
return 0; return 0;
} }
#if 0
int32_t XYRobotCtrlModule::module_set_state(int32_t state_id, int32_t state_value) { return err::kmodule_not_find_state_index; }
int32_t XYRobotCtrlModule::module_get_state(int32_t state_id, int32_t* state_value) {
#if 0
kstate_module_status
kstate_module_errorcode
// kstate_xyrobot_move
kstate_xyrobot_enable
kstate_xyrobot_x_pos
kstate_xyrobot_y_pos
kstate_xyrobot_x_dpos
kstate_xyrobot_y_dpos
#endif
// state_value
#define GET_STATE(param_id, configval) \
case param_id: \
*state_value = configval; \
return 0; \
break;
int32_t x, y;
getnowpos(x, y);
switch (state_id) {
GET_STATE(kstate_module_status, m_thread.isworking() ? 0x01 : 0x00);
GET_STATE(kstate_module_errorcode, 0);
GET_STATE(kstate_xyrobot_enable, m_enable);
GET_STATE(kstate_xyrobot_x_pos, x);
GET_STATE(kstate_xyrobot_y_pos, y);
GET_STATE(kstate_xyrobot_x_dpos, m_dx);
GET_STATE(kstate_xyrobot_y_dpos, m_dy);
default:
return err::kmodule_not_find_state_index;
}
}
#endif
int32_t XYRobotCtrlModule::module_readio(int32_t* io) { int32_t XYRobotCtrlModule::module_readio(int32_t* io) {
*io = 0; *io = 0;
for (int i = 0; i < m_ngpio; i++) { for (int i = 0; i < m_ngpio; i++) {

71
components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
View File

@ -1,16 +1,50 @@
#pragma once #pragma once
// //
#include "a8000_protocol\protocol.hpp"
#include "sdk/os/zos.hpp" #include "sdk/os/zos.hpp"
#include "sdk\components\tmc\basic\tmc_ic_interface.hpp" #include "sdk\components\tmc\basic\tmc_ic_interface.hpp"
#include "sdk\components\zcancmder\zcanreceiver.hpp" #include "sdk\components\zcancmder\zcanreceiver.hpp"
#include "a8000_protocol\protocol.hpp"
#include "sdk\components\zprotocols\zcancmder\api\i_xyrobot_ctrl_module.hpp"
namespace iflytop { namespace iflytop {
class XYRobotCtrlModule : public I_XYRobotCtrlModule, public ZIXYMotor, public ZIModule {
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); ENABLE_MODULE(XYRobotCtrlModule, khbot_module, PC_VERSION);
public: 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: private:
IStepperMotor* m_stepM1; IStepperMotor* m_stepM1;
IStepperMotor* m_stepM2; IStepperMotor* m_stepM2;
@ -34,7 +68,6 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule, public ZIXYMotor, public Z
const char* m_flashmark = nullptr; const char* m_flashmark = nullptr;
flash_config_t m_cfg; flash_config_t m_cfg;
flash_config_t m_default_cfg; flash_config_t m_default_cfg;
base_param_t& m_basecfg = m_cfg.basecfg;
bool m_enable = false; bool m_enable = false;
zmutex m_lock; zmutex m_lock;
@ -45,27 +78,19 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule, public ZIXYMotor, public Z
ZGPIO* Xgpio, int ngpio, // ZGPIO* Xgpio, int ngpio, //
flash_config_t& default_cfg, const char* flashmark); 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) override;
virtual int32_t move_by(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb) override;
virtual int32_t move_by_no_limit(int32_t dx, int32_t dy, int32_t speed, action_cb_status_t status_cb) override;
virtual int32_t move_to_zero(action_cb_status_t status_cb) override;
virtual int32_t move_to_zero_with_calibrate(int32_t x, int32_t y, action_cb_status_t status_cb) override;
virtual int32_t enable(bool venable) override;
virtual int32_t stop(uint8_t stopType) override;
virtual int32_t force_change_current_pos(int32_t x, int32_t y) override;
virtual int32_t read_version(version_t& version) override;
virtual int32_t read_status(status_t& status) override;
virtual int32_t read_detailed_status(detailed_status_t& debug_info) override;
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); static void create_default_cfg(flash_config_t& cfg);
static int32_t get_flash_config_size();
virtual int32_t set_base_param(const base_param_t& param) override;
virtual int32_t get_base_param(base_param_t& ack) override;
virtual int32_t flush() override;
virtual int32_t factory_reset() override;
virtual int32_t flush();
virtual int32_t factory_reset();
void loop(); void loop();
void dumpcfg(); void dumpcfg();
@ -78,8 +103,6 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule, public ZIXYMotor, public Z
virtual int32_t module_get_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_get_error(int32_t* iserror);
virtual int32_t module_clear_error(); virtual int32_t module_clear_error();
virtual int32_t module_set_reg(int32_t param_id, int32_t param_value);
virtual int32_t module_get_reg(int32_t param_id, int32_t* param_value);
virtual int32_t module_readio(int32_t* io); virtual int32_t module_readio(int32_t* io);
virtual int32_t module_read_adc(int32_t adcindex, int32_t* adc); virtual int32_t module_read_adc(int32_t adcindex, int32_t* adc);
virtual int32_t module_factory_reset(); virtual int32_t module_factory_reset();

2
components/zprotocols/zcancmder

@ -1 +1 @@
Subproject commit 002a60619ba163f1b271dff7d17363a2ebe3faca
Subproject commit 6b31abb3ebfee765f966007502e5c2108c207a51
Write Preview
Loading…
Cancel
Save