From dab1b34caa78d6451fcad1e84df2746c47f67f74 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 26 Jun 2024 15:16:10 +0800 Subject: [PATCH] update --- .../xy_robot_ctrl_module/xy_robot_ctrl_module.cpp | 218 ++++--- .../xy_robot_ctrl_module.cpp.bak | 668 --------------------- .../xy_robot_ctrl_module/xy_robot_ctrl_module.hpp | 42 +- .../xy_robot_ctrl_module.hpp.bak | 136 ----- 4 files changed, 168 insertions(+), 896 deletions(-) delete mode 100644 components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp.bak delete mode 100644 components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp.bak diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index 6ff2146..c859e8b 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -7,7 +7,7 @@ using namespace iflytop; using namespace std; #define TAG "XYRobotCtrlModule" -void XYRobotCtrlModule::initialize(int32_t id, IStepperMotor* stepM1, IStepperMotor* stepM2, ZGPIO* Xgpio, int ngpio, config_t& default_cfg) { +void XYRobotCtrlModule::initialize(int32_t id, TMC51X0* stepM1, TMC51X0* stepM2, ZGPIO* Xgpio, int ngpio, config_t& default_cfg) { m_stepM1 = stepM1; m_stepM2 = stepM2; m_gpiotable = Xgpio; @@ -21,32 +21,29 @@ void XYRobotCtrlModule::initialize(int32_t id, IStepperMotor* stepM1, IStepperMo m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); module_active_cfg(); xymotor_enable(1); - m_stepM1->setScaleDenominator(10); // TODO:修改这个参数为配置参数 - m_stepM2->setScaleDenominator(10); // TODO:修改这个参数为配置参数 } void XYRobotCtrlModule::create_default_cfg(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.min_x = 0; - cfg.max_x = 0; - cfg.min_y = 0; - cfg.max_y = 0; - - cfg.run_to_zero_speed = 200; - cfg.look_zero_edge_speed = 10; + // 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.min_x = 0; + // cfg.max_x = 0; + // cfg.min_y = 0; + // cfg.max_y = 0; + + // cfg.run_to_zero_speed = 200; + // cfg.look_zero_edge_speed = 10; return; } @@ -64,10 +61,40 @@ int32_t XYRobotCtrlModule::do_public_check() { return 0; } int32_t XYRobotCtrlModule::module_active_cfg() { + m_stepM1->enable(false); + m_stepM2->enable(false); + m_stepM1->setIHOLD_IRUN(m_cfg.ihold, m_cfg.irun, m_cfg.iholddelay); + m_stepM1->setScale(m_cfg.one_circle_pulse / 2); + m_stepM1->setScaleDenominator(m_cfg.one_circle_pulse_denominator); + m_stepM1->set_vstart(m_cfg.vstart); + m_stepM1->set_a1(m_cfg.a1); + m_stepM1->set_amax(m_cfg.amax); + m_stepM1->set_v1(m_cfg.v1); + m_stepM1->set_dmax(m_cfg.dmax); + m_stepM1->set_d1(m_cfg.d1); + m_stepM1->set_vstop(m_cfg.vstop); + m_stepM1->set_tzerowait(m_cfg.tzerowait); + m_stepM1->set_enc_resolution(m_cfg.enc_resolution); + m_stepM2->setIHOLD_IRUN(m_cfg.ihold, m_cfg.irun, m_cfg.iholddelay); - m_stepM1->setScale(m_cfg.distance_scale / 2); - m_stepM2->setScale(m_cfg.distance_scale / 2); + m_stepM2->setScale(m_cfg.one_circle_pulse / 2); + m_stepM2->setScaleDenominator(m_cfg.one_circle_pulse_denominator); + m_stepM2->set_vstart(m_cfg.vstart); + m_stepM2->set_a1(m_cfg.a1); + m_stepM2->set_amax(m_cfg.amax); + m_stepM2->set_v1(m_cfg.v1); + m_stepM2->set_dmax(m_cfg.dmax); + m_stepM2->set_d1(m_cfg.d1); + m_stepM2->set_vstop(m_cfg.vstop); + m_stepM2->set_tzerowait(m_cfg.tzerowait); + m_stepM2->set_enc_resolution(m_cfg.enc_resolution); + + if (m_state.enable) { + m_stepM1->enable(true); + m_stepM2->enable(true); + } + return 0; } @@ -121,31 +148,38 @@ int32_t XYRobotCtrlModule::xymotor_read_pos(int32_t* x, int32_t* y) { return 0; } +#define UPDATE_CFG(key) PROCESS_REG(kreg_xyrobot_##key, REG_GET(m_cfg.key), REG_SET(m_cfg.key)) + 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.default_velocity), REG_SET(m_cfg.default_velocity)); - 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_run_to_zero_speed, REG_GET(m_cfg.run_to_zero_speed), REG_SET(m_cfg.run_to_zero_speed)); - 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_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, _readio(&val), ACTION_NONE); + UPDATE_CFG(robot_type); + UPDATE_CFG(one_circle_pulse); + UPDATE_CFG(one_circle_pulse_denominator); + UPDATE_CFG(ihold); + UPDATE_CFG(irun); + UPDATE_CFG(iholddelay); + UPDATE_CFG(iglobalscaler); + UPDATE_CFG(vstart); + UPDATE_CFG(a1); + UPDATE_CFG(amax); + UPDATE_CFG(v1); + UPDATE_CFG(dmax); + UPDATE_CFG(d1); + UPDATE_CFG(vstop); + UPDATE_CFG(tzerowait); + UPDATE_CFG(enc_resolution); + UPDATE_CFG(enable_enc); + UPDATE_CFG(x_shaft); + UPDATE_CFG(y_shaft); + UPDATE_CFG(default_velocity); + UPDATE_CFG(shift_x); + UPDATE_CFG(shift_y); + UPDATE_CFG(min_x); + UPDATE_CFG(min_y); + UPDATE_CFG(max_x); + UPDATE_CFG(max_y); + UPDATE_CFG(run_to_zero_speed); + UPDATE_CFG(look_zero_edge_speed); default: return err::kmodule_not_find_reg; @@ -166,18 +200,7 @@ int32_t XYRobotCtrlModule::do_xymotor_move_to(int32_t x, int32_t y) { befor_motor_move(); { - int32_t target_m1pos, target_m2pos; - forward_kinematics(x, y, target_m1pos, target_m2pos); - - m_stepM1->setAcceleration(m_cfg.acc); - m_stepM1->setDeceleration(m_cfg.dec); - - m_stepM2->setAcceleration(m_cfg.acc); - m_stepM2->setDeceleration(m_cfg.dec); - - ZLOGI(TAG, "_motor_move_to target_m1pos:%d target_m2pos:%d", target_m1pos, target_m2pos); - m_stepM1->moveTo(target_m1pos, m_cfg.default_velocity); - m_stepM2->moveTo(target_m2pos, m_cfg.default_velocity); + moveTo(x, y, m_cfg.default_velocity); while (true) { if (m_thread.getExitFlag()) break; @@ -205,10 +228,8 @@ int32_t XYRobotCtrlModule::do_xymotor_move_to_zero() { int32_t ecode = exec_move_to_zero_task(); after_motor_move(); if (ecode == 0) { - m_stepM1->setXACTUAL(0); - m_stepM2->setXACTUAL(0); + setnowpos(0, 0); } - return; }); return 0; @@ -347,10 +368,7 @@ void XYRobotCtrlModule::_motor_move_to_end(int32_t xdirection, int32_t ydirectio 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->moveTo(target_m1pos, maxv); - m_stepM2->moveTo(target_m2pos, maxv); + moveTo(t_x, t_y, maxv); } bool XYRobotCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget() && m_stepM2->isReachTarget(); } void XYRobotCtrlModule::_motor_stop() { @@ -399,19 +417,10 @@ void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, } } -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); -} - void XYRobotCtrlModule::befor_motor_move() { // - getnowpos(m_state.before_x, m_state.before_y); - m_state.before_y = m_stepM1->getXACTUAL(); - creg.module_status = 1; + creg.module_status = 1; creg.module_errorcode = 0; module_active_cfg(); @@ -456,4 +465,59 @@ bool XYRobotCtrlModule::check_when_run() { // } return true; +} + +void XYRobotCtrlModule::getnowpos(int32_t& x, int32_t& y) { + int32_t m1pos, m2pos; + + if (m_cfg.enable_enc != 0) { + m1pos = m_stepM1->read_enc_val(); + m2pos = m_stepM2->read_enc_val(); + } else { + m1pos = m_stepM1->getXACTUAL(); + m2pos = m_stepM2->getXACTUAL(); + } + + inverse_kinematics(m1pos, m2pos, x, y); +} + +void XYRobotCtrlModule::trysyncpos() { // + int32_t m1pos, m2pos; + int32_t m1pos_enc, m2pos_enc; + + if (m_cfg.enable_enc == 0) { + return; + } + + m1pos_enc = m_stepM1->read_enc_val(); + m2pos_enc = m_stepM2->read_enc_val(); + + m1pos = m_stepM1->getXACTUAL(); + m2pos = m_stepM2->getXACTUAL(); + + if (m1pos != m1pos_enc || m2pos != m2pos_enc) { + m_stepM1->setXACTUAL(m1pos_enc); + m_stepM2->setXACTUAL(m2pos_enc); + } +} +void XYRobotCtrlModule::setnowpos(int32_t x, int32_t y) { + int32_t m1pos, m2pos; + forward_kinematics(x, y, m1pos, m2pos); + m_stepM1->setXACTUAL(m1pos); + m_stepM2->setXACTUAL(m2pos); + m_stepM1->set_enc_val(m1pos); + m_stepM2->set_enc_val(m2pos); +} + +void XYRobotCtrlModule::moveTo(int32_t t_x, int32_t t_y, int32_t v) { + trysyncpos(); + int32_t target_m1pos, target_m2pos; + forward_kinematics(t_x, t_y, target_m1pos, target_m2pos); + m_stepM1->moveTo(target_m1pos, v); + m_stepM2->moveTo(target_m2pos, v); +} +void XYRobotCtrlModule::moveBy(int32_t dx, int32_t dy, int32_t v) { + int32_t x, y; + getnowpos(x, y); + moveTo(x + dx, y + dy, v); } \ No newline at end of file diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp.bak b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp.bak deleted file mode 100644 index 4dde171..0000000 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp.bak +++ /dev/null @@ -1,668 +0,0 @@ -#include "xy_robot_ctrl_module.hpp" - -#include - -#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; -} diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp index a2a2aaa..901af12 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp @@ -3,6 +3,7 @@ #include "a8000_protocol\protocol.hpp" #include "sdk/os/zos.hpp" #include "sdk\components\tmc\basic\tmc_ic_interface.hpp" +#include "sdk\components\tmc\ic\ztmc5130.hpp" #include "sdk\components\zcancmder\zcanreceiver.hpp" namespace iflytop { @@ -14,28 +15,35 @@ class XYRobotCtrlModule : public ZIXYMotor, public ZIModule { typedef enum { khbot, kcorexy } robot_type_t; typedef struct { - bool configInited; int32_t robot_type; - int32_t x_shaft; - int32_t y_shaft; + int32_t one_circle_pulse; + int32_t one_circle_pulse_denominator; int32_t ihold; int32_t irun; int32_t iholddelay; - int32_t distance_scale; // 0.001 + int32_t iglobalscaler; + int32_t vstart; + int32_t a1; + int32_t amax; + int32_t v1; + int32_t dmax; + int32_t d1; + int32_t vstop; + int32_t tzerowait; + int32_t enc_resolution; + int32_t enable_enc; + int32_t x_shaft; + int32_t y_shaft; + int32_t default_velocity; int32_t shift_x; int32_t shift_y; - - // limit - int32_t acc; - int32_t dec; - int32_t default_velocity; int32_t min_x; - int32_t max_x; int32_t min_y; + int32_t max_x; int32_t max_y; - int32_t run_to_zero_speed; int32_t look_zero_edge_speed; + } config_t; typedef struct { @@ -43,16 +51,16 @@ class XYRobotCtrlModule : public ZIXYMotor, public ZIModule { int32_t y; int32_t dx; int32_t dy; - bool enable; int32_t before_x; int32_t before_y; + bool enable; } state_t; private: - IStepperMotor* m_stepM1 = nullptr; - IStepperMotor* m_stepM2 = nullptr; + TMC51X0* m_stepM1 = nullptr; + TMC51X0* m_stepM2 = nullptr; ZGPIO* m_gpiotable = nullptr; int32_t m_ngpio = 0; @@ -64,7 +72,7 @@ class XYRobotCtrlModule : public ZIXYMotor, public ZIModule { state_t m_state = {0}; public: - void initialize(int32_t id, IStepperMotor* stepM1, IStepperMotor* stepM2, ZGPIO* Xgpio, int ngpio, config_t& default_cfg); + void initialize(int32_t id, TMC51X0* stepM1, TMC51X0* stepM2, ZGPIO* Xgpio, int ngpio, config_t& default_cfg); public: static void create_default_cfg(config_t& cfg); @@ -107,5 +115,9 @@ class XYRobotCtrlModule : public ZIXYMotor, public ZIModule { 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 getnowpos(int32_t& x, int32_t& y); + void trysyncpos(); + void setnowpos(int32_t x, int32_t y); + void moveTo(int32_t x, int32_t y, int32_t v); + void moveBy(int32_t dx, int32_t dy, int32_t v); }; } // namespace iflytop diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp.bak b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp.bak deleted file mode 100644 index ac4be5d..0000000 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp.bak +++ /dev/null @@ -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 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 \ No newline at end of file