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 cf6bcab..a4b8e9a 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -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); int runspeed = speed; 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; m_thread.stop(); 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()) { if (m_thread.getExitFlag()) break; 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(); _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; 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; 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()) { 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(); 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; getnowpos(s_x, s_y); to_x = s_x + dx; 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) { @@ -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]() { - _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()) { 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_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_stepM2->setXACTUAL(0); @@ -259,7 +259,7 @@ int32_t XYRobotCtrlModule::stop(uint8_t stopType) { } else { ZLOGI(TAG, "breakStop"); m_thread.stop(); - _motor_stop(m_basecfg.breakdec); + _motor_stop(m_cfg.breakdec); return 0; } } @@ -272,97 +272,45 @@ int32_t XYRobotCtrlModule::force_change_current_pos(int32_t x, int32_t y) { m_stepM2->setXACTUAL(m2pos); 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) { 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 - 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; } -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() { - 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 xnow, ynow; getnowpos(xnow, ynow); @@ -383,7 +331,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { * 移动导零点 * *******************************************************************************/ 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; while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { // 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()) { - _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; while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { if (!m_Xgpio->getState()) { @@ -436,7 +384,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { *******************************************************************************/ 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; while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { // ZLOGI(TAG, "ygpio %d", m_Ygpio->getState()); @@ -459,7 +407,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { } 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; while (!m_thread.getExitFlag() && !_motor_is_reach_target()) { if (!m_Ygpio->getState()) { @@ -538,7 +486,7 @@ void XYRobotCtrlModule::_motor_stop(int32_t dec) { m_stepM2->stop(); } void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y) { - if (m_basecfg.robot_type == corexy) { + if (m_cfg.robot_type == kcorexy) { x = (m1pos - m2pos); y = (m1pos + m2pos); } else { @@ -546,20 +494,20 @@ void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t 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) { - 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); m2pos = ((y - x) / 2); } else { @@ -574,29 +522,29 @@ 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_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() { // @@ -632,7 +580,7 @@ int32_t XYRobotCtrlModule::module_stop() { int32_t XYRobotCtrlModule::module_break() { ZLOGI(TAG, "module_break"); m_thread.stop(); - _motor_stop(m_basecfg.breakdec); + _motor_stop(m_cfg.breakdec); return 0; } 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_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) { switch (param_id) { 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); default: @@ -687,43 +633,6 @@ int32_t XYRobotCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& 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) { *io = 0; for (int i = 0; i < m_ngpio; i++) { 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 8e06614..fc78246 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp @@ -1,16 +1,50 @@ #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" -#include "a8000_protocol\protocol.hpp" -#include "sdk\components\zprotocols\zcancmder\api\i_xyrobot_ctrl_module.hpp" namespace iflytop { -class XYRobotCtrlModule : public I_XYRobotCtrlModule, public ZIXYMotor, public ZIModule { +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; @@ -34,7 +68,6 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule, public ZIXYMotor, public Z const char* m_flashmark = nullptr; flash_config_t m_cfg; flash_config_t m_default_cfg; - base_param_t& m_basecfg = m_cfg.basecfg; bool m_enable = false; zmutex m_lock; @@ -45,27 +78,19 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule, public ZIXYMotor, public Z 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) 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 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 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_error(int32_t* iserror); 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_read_adc(int32_t adcindex, int32_t* adc); virtual int32_t module_factory_reset(); diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index 002a606..6b31abb 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit 002a60619ba163f1b271dff7d17363a2ebe3faca +Subproject commit 6b31abb3ebfee765f966007502e5c2108c207a51