From be32566b427d42bda9e59af79ea856c076830326 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Mon, 16 Oct 2023 15:11:36 +0800 Subject: [PATCH] update --- .../xy_robot_ctrl_module/xy_robot_ctrl_module.cpp | 365 ++++++++++----------- .../xy_robot_ctrl_module/xy_robot_ctrl_module.hpp | 5 +- .../xy_robot_script_cmder_module.cpp | 16 +- components/zprotocols/zcancmder | 2 +- 4 files changed, 181 insertions(+), 207 deletions(-) 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 5a6def2..cb1ba01 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -40,29 +40,29 @@ int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int speed, action_cb_st ZLOGI(TAG, "move_to x:%d y:%d", x, y); int runspeed = speed; if (speed == 0) { - runspeed = m_cfg.maxspeed; + runspeed = m_basecfg.maxspeed; } - if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) { - runspeed = m_cfg.maxspeed; + if (m_basecfg.maxspeed != 0 && speed > m_basecfg.maxspeed) { + runspeed = m_basecfg.maxspeed; } - if (m_cfg.min_x != 0 && x < m_cfg.min_x) { - x = m_cfg.min_x; + if (m_basecfg.min_x != 0 && x < m_basecfg.min_x) { + x = m_basecfg.min_x; } - if (m_cfg.max_x != 0 && x > m_cfg.max_x) { - x = m_cfg.max_x; + if (m_basecfg.max_x != 0 && x > m_basecfg.max_x) { + x = m_basecfg.max_x; } - if (m_cfg.min_y != 0 && y < m_cfg.min_y) { - y = m_cfg.min_y; + if (m_basecfg.min_y != 0 && y < m_basecfg.min_y) { + y = m_basecfg.min_y; } - if (m_cfg.max_y != 0 && y > m_cfg.max_y) { - y = m_cfg.max_y; + if (m_basecfg.max_y != 0 && y > m_basecfg.max_y) { + y = m_basecfg.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); + _motor_move_to(x, y, runspeed, m_basecfg.acc, m_basecfg.dec); while (!_motor_is_reach_target()) { if (m_thread.getExitFlag()) break; vTaskDelay(10); @@ -82,8 +82,8 @@ int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int speed, a m_thread.stop(); _motor_stop(); - if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) { - speed = m_cfg.maxspeed; + if (m_basecfg.maxspeed != 0 && speed > m_basecfg.maxspeed) { + speed = m_basecfg.maxspeed; } int32_t s_x, s_y, to_x, to_y; getnowpos(s_x, s_y); @@ -91,7 +91,7 @@ int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int speed, a 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); + _motor_move_to(to_x, to_y, speed, m_basecfg.acc, m_basecfg.dec); while (!_motor_is_reach_target()) { if (m_thread.getExitFlag()) break; @@ -115,28 +115,28 @@ int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, int speed, action_cb_ _motor_stop(); if (speed == 0) { - speed = m_cfg.maxspeed; + speed = m_basecfg.maxspeed; } - if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) { - speed = m_cfg.maxspeed; + if (m_basecfg.maxspeed != 0 && speed > m_basecfg.maxspeed) { + speed = m_basecfg.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_basecfg.min_x != 0 && to_x < m_basecfg.min_x) { + to_x = m_basecfg.min_x; } - if (m_cfg.max_x != 0 && to_x > m_cfg.max_x) { - to_x = m_cfg.max_x; + if (m_basecfg.max_x != 0 && to_x > m_basecfg.max_x) { + to_x = m_basecfg.max_x; } - if (m_cfg.min_y != 0 && to_y < m_cfg.min_y) { - to_y = m_cfg.min_y; + if (m_basecfg.min_y != 0 && to_y < m_basecfg.min_y) { + to_y = m_basecfg.min_y; } - if (m_cfg.max_y != 0 && to_y > m_cfg.max_y) { - to_y = m_cfg.max_y; + if (m_basecfg.max_y != 0 && to_y > m_basecfg.max_y) { + to_y = m_basecfg.max_y; } if (to_x == s_x && to_y == s_y) { @@ -145,7 +145,7 @@ int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, int speed, action_cb_ } 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); + _motor_move_to(to_x, to_y, speed, m_basecfg.acc, m_basecfg.dec); while (!_motor_is_reach_target()) { if (m_thread.getExitFlag()) break; @@ -212,8 +212,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_zero_shift_x = calibrate_x; - m_zero_shift_y = calibrate_y; + m_basecfg.shift_x = calibrate_x; + m_basecfg.shift_y = calibrate_y; m_stepM1->setXACTUAL(0); m_stepM2->setXACTUAL(0); @@ -245,7 +245,7 @@ int32_t XYRobotCtrlModule::stop(uint8_t stopType) { } else { ZLOGI(TAG, "breakStop"); m_thread.stop(); - _motor_stop(m_cfg.breakdec); + _motor_stop(m_basecfg.breakdec); return 0; } } @@ -293,91 +293,57 @@ int32_t XYRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info) { } void XYRobotCtrlModule::create_default_cfg(flash_config_t& cfg) { memset(&cfg, 0, sizeof(cfg)); - cfg.robot_type = corexy; - cfg.x_shaft = 0; - cfg.y_shaft = 0; - cfg.ihold = 1; - cfg.irun = 3; - cfg.iholddelay = 100; - cfg.distance_scale = 10000; - cfg.shift_x = 0; - cfg.shift_y = 0; + 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 = 10000; + cfg.basecfg.shift_x = 0; + cfg.basecfg.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_max_d = 10000 * 100; - cfg.leave_from_zero_max_d = 3 * 10000; - cfg.run_to_zero_speed = 300; - cfg.run_to_zero_dec = 1600; + 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; return; } int32_t XYRobotCtrlModule::set_base_param(const base_param_t& inparam) { zlock_guard lock(m_lock); - - m_cfg.robot_type = inparam.robot_type; - m_cfg.x_shaft = inparam.x_shaft; - m_cfg.y_shaft = inparam.y_shaft; - m_cfg.ihold = inparam.ihold; - m_cfg.irun = inparam.irun; - m_cfg.iholddelay = inparam.iholddelay; - m_cfg.distance_scale = inparam.distance_scale; - m_cfg.shift_x = inparam.shift_x; - m_cfg.shift_y = inparam.shift_y; - m_cfg.acc = inparam.acc; - m_cfg.dec = inparam.dec; - m_cfg.breakdec = inparam.breakdec; - m_cfg.maxspeed = inparam.maxspeed; - m_cfg.min_x = inparam.min_x; - m_cfg.max_x = inparam.max_x; - m_cfg.min_y = inparam.min_y; - m_cfg.max_y = inparam.max_y; - m_cfg.run_to_zero_max_d = inparam.run_to_zero_max_d; - m_cfg.leave_from_zero_max_d = inparam.leave_from_zero_max_d; - m_cfg.run_to_zero_speed = inparam.run_to_zero_speed; - m_cfg.run_to_zero_dec = inparam.run_to_zero_dec; + m_basecfg = inparam; active_cfg(); return 0; }; void XYRobotCtrlModule::active_cfg() { - m_stepM1->setScale(m_cfg.distance_scale); - m_stepM2->setScale(m_cfg.distance_scale); - m_stepM1->setIHOLD_IRUN(m_cfg.ihold, m_cfg.irun, m_cfg.iholddelay); - m_stepM2->setIHOLD_IRUN(m_cfg.ihold, m_cfg.irun, m_cfg.iholddelay); + m_stepM1->setScale(m_basecfg.distance_scale); + m_stepM2->setScale(m_basecfg.distance_scale); + 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); } int32_t XYRobotCtrlModule::get_base_param(base_param_t& ack) { zlock_guard lock(m_lock); - ack.robot_type = m_cfg.robot_type; - ack.x_shaft = m_cfg.x_shaft; - ack.y_shaft = m_cfg.y_shaft; - ack.ihold = m_cfg.ihold; - ack.irun = m_cfg.irun; - ack.iholddelay = m_cfg.iholddelay; - ack.distance_scale = m_cfg.distance_scale; - ack.shift_x = m_cfg.shift_x; - ack.shift_y = m_cfg.shift_y; - ack.acc = m_cfg.acc; - ack.dec = m_cfg.dec; - ack.breakdec = m_cfg.breakdec; - ack.maxspeed = m_cfg.maxspeed; - ack.min_x = m_cfg.min_x; - ack.max_x = m_cfg.max_x; - ack.min_y = m_cfg.min_y; - ack.max_y = m_cfg.max_y; - ack.run_to_zero_max_d = m_cfg.run_to_zero_max_d; - ack.leave_from_zero_max_d = m_cfg.leave_from_zero_max_d; - ack.run_to_zero_speed = m_cfg.run_to_zero_speed; - ack.run_to_zero_dec = m_cfg.run_to_zero_dec; + ack = m_basecfg; return 0; }; @@ -398,104 +364,103 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task(int32_t& dx, int32_t& dy) { int32_t XYRobotCtrlModule::exec_move_to_zero_task() { /******************************************************************************* - * 远离X零点 * + * 移动导零点 * *******************************************************************************/ - if (m_Xgpio->getState()) { - /** - * @brief 如果设备已经在零点，则反向移动一定距离远离零点 - */ - _motor_move_by(m_cfg.leave_from_zero_max_d, 0, m_cfg.run_to_zero_speed, m_cfg.acc, m_cfg.run_to_zero_dec); - while (!_motor_is_reach_target()) { - if (m_thread.getExitFlag()) break; + 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); + 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 0; + return err::kce_break_by_user; } - if (m_Xgpio->getState()) { - ZLOGE(TAG, "leave away zero failed"); + + if (!xreach_zero) { + ZLOGE(TAG, "find zero point fail"); return err::kce_x_leave_away_zero_point_fail; } } - /******************************************************************************* - * 移动X轴到零点 * - *******************************************************************************/ - _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; + + 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); + 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); } - vTaskDelay(1); - } - if (!xreach_zero) { - // 触发异常回调 - ZLOGE(TAG, "x reach zero failed"); - return err::kce_not_found_x_zero_point; - } + if (m_thread.getExitFlag()) { + ZLOGI(TAG, "break move to zero thread exit"); + return err::kce_break_by_user; + } - if (m_thread.getExitFlag()) { - ZLOGI(TAG, "break move to zero thread exit"); - return 0; + if (!xleave_zero) { + ZLOGI(TAG, "leave away zero failed"); + return err::kce_x_leave_away_zero_point_fail; + } } - ZLOGI(TAG, "x reach zero"); - /******************************************************************************* - * 远离Y零点 * + * 移动Y轴到零点 * *******************************************************************************/ - if (m_Ygpio->getState()) { - /** - * @brief 如果设备已经在零点，则反向移动一定距离远离零点 - */ - _motor_move_by(0, m_cfg.leave_from_zero_max_d, m_cfg.run_to_zero_speed, m_cfg.acc, m_cfg.dec); - while (!_motor_is_reach_target()) { - if (m_thread.getExitFlag()) break; + 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); + 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 0; + return err::kce_break_by_user; } - if (m_Ygpio->getState()) { - ZLOGE(TAG, "leave away zero failed"); + if (!yreach_zero) { + ZLOGE(TAG, "find zero point fail"); return err::kce_y_leave_away_zero_point_fail; } + } - /******************************************************************************* - * 移动Y轴到零点 * - *******************************************************************************/ - _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; + 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); + 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::kce_break_by_user; + } + if (!yleave_zero) { + ZLOGI(TAG, "leave away zero failed"); + return err::kce_y_leave_away_zero_point_fail; } - vTaskDelay(1); - } - if (!yreach_zero) { - // 触发异常回调 - ZLOGE(TAG, "y reach zero failed"); - return err::kce_not_found_y_zero_point; - } - if (m_thread.getExitFlag()) { - ZLOGI(TAG, "break move to zero thread exit"); - return 0; } - ZLOGI(TAG, "y reach zero"); - ZLOGI(TAG, "move_to_zero success"); - + return 0; } @@ -553,27 +518,27 @@ void XYRobotCtrlModule::_motor_stop(int32_t dec) { m_stepM2->stop(); } void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y) { - if (m_cfg.robot_type == corexy) { + if (m_basecfg.robot_type == corexy) { 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; + if (m_basecfg.x_shaft) x = -x; + if (m_basecfg.y_shaft) y = -y; - x += m_zero_shift_x; - y += m_zero_shift_y; + x += m_basecfg.shift_x; + y += m_basecfg.shift_y; } void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) { - if (m_cfg.x_shaft) x = -x; - if (m_cfg.y_shaft) y = -y; + if (m_basecfg.x_shaft) x = -x; + if (m_basecfg.y_shaft) y = -y; - x -= m_zero_shift_x; - y -= m_zero_shift_y; + x -= m_basecfg.shift_x; + y -= m_basecfg.shift_y; - if (m_cfg.robot_type == corexy) { + if (m_basecfg.robot_type == corexy) { m1pos = ((x + y) / 2); m2pos = ((y - x) / 2); } else { @@ -588,27 +553,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_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, "leave_from_zero_max_d :%d", m_cfg.leave_from_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, "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); } int32_t XYRobotCtrlModule::flush() { // 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 2b893ab..4974451 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp @@ -27,11 +27,12 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule { // base_param_t m_cfg; - int32_t m_zero_shift_x = 0; - int32_t m_zero_shift_y = 0; + // int32_t m_zero_shift_x = 0; + // int32_t m_zero_shift_y = 0; const char* m_flashmark = nullptr; flash_config_t m_cfg; + base_param_t& m_basecfg = m_cfg.basecfg; zmutex m_lock; // zmutex m_statu_lock; diff --git a/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp b/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp index 1a2e24e..7a463bf 100644 --- a/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp @@ -31,14 +31,14 @@ static void cmd_dump_ack(I_XYRobotCtrlModule::status_t& ack) { // ZLOGI(TAG, "status :%d", ack.status); ZLOGI(TAG, "x :%d", ack.x); ZLOGI(TAG, "y :%d", ack.y); - ZLOGI(TAG, "iostate :%d %d %d %d %d %d %d %d", BIT(ack.iostate,0), BIT(ack.iostate,1), BIT(ack.iostate,2), BIT(ack.iostate,3), BIT(ack.iostate,4), BIT(ack.iostate,5), BIT(ack.iostate,6), BIT(ack.iostate,7)); + ZLOGI(TAG, "iostate :%d %d %d %d %d %d %d %d", BIT(ack.iostate, 0), BIT(ack.iostate, 1), BIT(ack.iostate, 2), BIT(ack.iostate, 3), BIT(ack.iostate, 4), BIT(ack.iostate, 5), BIT(ack.iostate, 6), BIT(ack.iostate, 7)); } static void cmd_dump_ack(I_XYRobotCtrlModule::detailed_status_t& ack) { // ZLOGI(TAG, "status :%d", ack.status); ZLOGI(TAG, "x :%d", ack.x); ZLOGI(TAG, "y :%d", ack.y); - ZLOGI(TAG, "iostate :%d %d %d %d %d %d %d %d", BIT(ack.iostate,0), BIT(ack.iostate,1), BIT(ack.iostate,2), BIT(ack.iostate,3), BIT(ack.iostate,4), BIT(ack.iostate,5), BIT(ack.iostate,6), BIT(ack.iostate,7)); + ZLOGI(TAG, "iostate :%d %d %d %d %d %d %d %d", BIT(ack.iostate, 0), BIT(ack.iostate, 1), BIT(ack.iostate, 2), BIT(ack.iostate, 3), BIT(ack.iostate, 4), BIT(ack.iostate, 5), BIT(ack.iostate, 6), BIT(ack.iostate, 7)); } static void cmd_dump_ack(I_XYRobotCtrlModule::base_param_t& ack) { @@ -60,9 +60,11 @@ static void cmd_dump_ack(I_XYRobotCtrlModule::base_param_t& ack) { ZLOGI(TAG, " min_y :%d", ack.min_y); ZLOGI(TAG, " max_y :%d", ack.max_y); ZLOGI(TAG, " run_to_zero_max_d :%d", ack.run_to_zero_max_d); - ZLOGI(TAG, " leave_from_zero_max_d :%d", ack.leave_from_zero_max_d); ZLOGI(TAG, " run_to_zero_speed :%d", ack.run_to_zero_speed); ZLOGI(TAG, " run_to_zero_dec :%d", ack.run_to_zero_dec); + ZLOGI(TAG, " look_zero_edge_max_d :%d", ack.look_zero_edge_max_d); + ZLOGI(TAG, " look_zero_edge_speed :%d", ack.look_zero_edge_speed); + ZLOGI(TAG, " look_zero_edge_dec :%d", ack.look_zero_edge_dec); } void XYRobotScriptCmderModule::regmodule(int id, I_XYRobotCtrlModule* robot) { @@ -138,12 +140,16 @@ void XYRobotScriptCmderModule::regcmd() { // status.max_y = paramVal; } else if (streq(paramName, "run_to_zero_max_d")) { status.run_to_zero_max_d = paramVal; - } else if (streq(paramName, "leave_from_zero_max_d")) { - status.leave_from_zero_max_d = paramVal; } else if (streq(paramName, "run_to_zero_speed")) { status.run_to_zero_speed = paramVal; } else if (streq(paramName, "run_to_zero_dec")) { status.run_to_zero_dec = paramVal; + } else if (streq(paramName, "look_zero_edge_max_d")) { + status.look_zero_edge_max_d = paramVal; + } else if (streq(paramName, "look_zero_edge_speed")) { + status.look_zero_edge_speed = paramVal; + } else if (streq(paramName, "look_zero_edge_dec")) { + status.look_zero_edge_dec = paramVal; } else { ZLOGE(TAG, "invalid param name:%s", paramName); return (int32_t)err::kce_param_out_of_range; diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index 9f0c021..66f198b 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit 9f0c021c41d1d410ccea7cc8a3d3c80c767e854d +Subproject commit 66f198bd2f0f38fefbdba179b98317f3948e0c04