From ec9aaef27a8c4c88db168b48b214592068758ac7 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 5 Oct 2023 17:10:59 +0800 Subject: [PATCH] update --- .../xy_robot_ctrl_module/xy_robot_ctrl_module.cpp | 236 ++++++++------------- .../xy_robot_ctrl_module/xy_robot_ctrl_module.hpp | 56 ++--- .../zcancmder_module/zcan_xy_robot_module.cpp | 53 ++--- components/zprotocols/zcancmder | 2 +- 4 files changed, 131 insertions(+), 216 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 15fab0f..c1fc84b 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -1,5 +1,7 @@ #include "xy_robot_ctrl_module.hpp" +#include + #include "sdk/components/errorcode/errorcode.hpp" using namespace iflytop; using namespace std; @@ -20,11 +22,11 @@ void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, // m_stepM1->setScale(scale); m_stepM2->setScale(scale); m_lock.init(); - m_statu_lock.init(); + // m_statu_lock.init(); m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); } -int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, function status_cb) { +int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, action_cb_status_t status_cb) { zlock_guard lock(m_lock); ZLOGI(TAG, "move_to x:%d y:%d", x, y); int32_t m1pos, m2pos; @@ -32,9 +34,8 @@ int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, function status_cb) { +int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, action_cb_status_t status_cb) { zlock_guard lock(m_lock); ZLOGI(TAG, "move_by dx:%d dy:%d", dx, dy); @@ -62,13 +57,12 @@ int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, function status_cb) { +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(); - updateStatus(1); m_thread.start([this, status_cb]() { - int32_t dx, dy; - move_to_zero_cb_status_t status = {0}; + 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(); - status.exec_status = erro; - if (status_cb) status_cb(status); - updateStatus(0); + call_status_cb(status_cb, erro); return; } m_stepM1->setXACTUAL(0); m_stepM2->setXACTUAL(0); - status.exec_status = 0; - if (status_cb) status_cb(status); - updateStatus(0); + call_status_cb(status_cb, 0); + return; }); return 0; } -int32_t XYRobotCtrlModule::move_to_zero_with_calibrate(int32_t x, int32_t y, function status_cb) { +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(); - updateStatus(1); m_thread.start([this, x, y, status_cb]() { - move_to_zero_with_calibrate_cb_status_t cb_status = {0}; - int32_t dx, dy; - int32_t erro = exec_move_to_zero_task(dx, dy); + 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); - updateStatus(0); + // cb_status.exec_status = erro; + // if (status_cb) status_cb(cb_status); + call_status_cb(status_cb, erro); return; } @@ -144,12 +134,12 @@ int32_t XYRobotCtrlModule::move_to_zero_with_calibrate(int32_t x, int32_t y, fun m_stepM1->setXACTUAL(0); m_stepM2->setXACTUAL(0); - cb_status.exec_status = 0; - cb_status.zero_shift_x = m_zero_shift_x; - cb_status.zero_shift_y = m_zero_shift_y; + 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); - updateStatus(0); + // if (status_cb) status_cb(cb_status); return; }); @@ -172,7 +162,7 @@ int32_t XYRobotCtrlModule::stop(uint8_t stopType) { } else { ZLOGI(TAG, "breakStop"); m_thread.stop(); - _motor_stop(m_cfg_breakdec); + _motor_stop(m_cfg.breakdec); return 0; } } @@ -191,9 +181,9 @@ int32_t XYRobotCtrlModule::read_version(version_t& version) { return 0; } int32_t XYRobotCtrlModule::read_status(status_t& status) { - zlock_guard lock(m_statu_lock); + zlock_guard lock(m_lock); status = {0}; - status.status = m_status; + status.status = m_thread.isworking() ? 0x01 : 0x00; if (m_Xgpio) status.iostate |= m_Xgpio->getState() ? 0x01 : 0x00; if (m_Ygpio) status.iostate |= m_Ygpio->getState() ? 0x02 : 0x00; getnowpos(status.x, status.y); @@ -205,106 +195,54 @@ int32_t XYRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info) { if (m_Xgpio) debug_info.iostate |= m_Xgpio->getState() ? 0x01 : 0x00; if (m_Ygpio) debug_info.iostate |= m_Ygpio->getState() ? 0x02 : 0x00; - debug_info.status = m_status; + debug_info.status = m_thread.isworking() ? 0x01 : 0x00; getnowpos(debug_info.x, debug_info.y); return 0; } - -int32_t XYRobotCtrlModule::read_run_param(run_param_t& param) { return set_run_param(kset_cmd_type_read, param, param); } -int32_t XYRobotCtrlModule::read_run_to_zero_param(run_to_zero_param_t& param) { return set_run_to_zero_param(kset_cmd_type_read, param, param); } -int32_t XYRobotCtrlModule::read_warning_limit_param(warning_limit_param_t& param) { return set_warning_limit_param(kset_cmd_type_read, param, param); } - -int32_t XYRobotCtrlModule::set_run_param(run_param_t& param) { return set_run_param(kset_cmd_type_set, param, param); } -int32_t XYRobotCtrlModule::set_run_to_zero_param(run_to_zero_param_t& param) { return set_run_to_zero_param(kset_cmd_type_set, param, param); } -int32_t XYRobotCtrlModule::set_warning_limit_param(warning_limit_param_t& param) { return set_warning_limit_param(kset_cmd_type_set, param, param); } - -int32_t XYRobotCtrlModule::set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) { - zlock_guard lock(m_lock); - if (operation == kset_cmd_type_set) { - m_robot_type = (RobotType_t)param.robot_type; - m_x_shaft = param.x_shaft; - m_y_shaft = param.y_shaft; - m_cfg_ihold = param.ihold; - m_cfg_irun = param.irun; - m_cfg_iholddelay = param.iholddelay; - m_cfg_distance_scale = param.distance_scale / 1000.0; - m_zero_shift_x = param.shift_x; - m_zero_shift_y = param.shift_y; - m_cfg_acc = param.acc; - m_cfg_dec = param.dec; - m_cfg_velocity = param.maxspeed; - m_min_x = param.min_x; - m_max_x = param.max_x; - m_min_y = param.min_y; - m_max_y = param.max_y; - ZLOGI(TAG, "================set_run_param===================="); - ZLOGI(TAG, "= robot_type :%d", m_robot_type); - ZLOGI(TAG, "= x_shaft :%d", m_x_shaft); - ZLOGI(TAG, "= y_shaft :%d", m_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 :%f", m_cfg_distance_scale); - ZLOGI(TAG, "= shift_x :%d", m_zero_shift_x); - ZLOGI(TAG, "= shift_y :%d", m_zero_shift_y); - ZLOGI(TAG, "= acc :%d", m_cfg_acc); - ZLOGI(TAG, "= dec :%d", m_cfg_dec); - ZLOGI(TAG, "= maxspeed :%d", m_cfg_velocity); - ZLOGI(TAG, "= min_x :%d", m_min_x); - ZLOGI(TAG, "= max_x :%d", m_max_x); - ZLOGI(TAG, "= min_y :%d", m_min_y); - ZLOGI(TAG, "= max_y :%d", m_max_y); - ZLOGI(TAG, "==================================================="); - - 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); - } - - ack.robot_type = (u8)m_robot_type; - ack.x_shaft = m_x_shaft; - ack.y_shaft = m_y_shaft; - ack.ihold = m_cfg_ihold; - ack.irun = m_cfg_irun; - ack.iholddelay = m_cfg_iholddelay; - ack.distance_scale = m_cfg_distance_scale * 1000; - ack.shift_x = m_zero_shift_x; - ack.shift_y = m_zero_shift_y; - ack.acc = m_cfg_acc; - ack.dec = m_cfg_dec; - ack.maxspeed = m_cfg_velocity; - ack.min_x = m_min_x; - ack.max_x = m_max_x; - ack.min_y = m_min_y; - ack.max_y = m_max_y; - return 0; +void XYRobotCtrlModule::create_default_cfg(base_param_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 = 1000; + cfg.shift_x = 0; + cfg.shift_y = 0; + + // limit + cfg.acc = 300000; + cfg.dec = 300000; + cfg.breakdec = 300000; + cfg.maxspeed = 300000; + cfg.min_x = 0; + cfg.max_x = 0; + cfg.min_y = 0; + cfg.max_y = 0; + + cfg.run_to_zero_max_d = 5120000; + cfg.leave_from_zero_max_d = 51200; + cfg.run_to_zero_speed = 30000; + cfg.run_to_zero_dec = 600000; + return; } -int32_t XYRobotCtrlModule::set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) { - if (operation == kset_cmd_type_set) { - m_cfg_runtozero_maxX = param.move_to_zero_max_d; - m_cfg_runtozero_maxY = param.move_to_zero_max_d; - m_cfg_runtozero_leave_away_zero_maxXY = param.leave_from_zero_max_d; - m_cfg_runtozero_speed = param.speed; - m_cfg_runtozero_dec = param.dec; - ZLOGI(TAG, "================set_run_to_zero_param===================="); - ZLOGI(TAG, "= move_to_zero_max_d :%d", m_cfg_runtozero_maxX); - ZLOGI(TAG, "= leave_from_zero_max_d :%d", m_cfg_runtozero_leave_away_zero_maxXY); - ZLOGI(TAG, "= speed :%d", m_cfg_runtozero_speed); - ZLOGI(TAG, "= dec :%d", m_cfg_runtozero_dec); - ZLOGI(TAG, "==========================================================="); - } - ack.move_to_zero_max_d = m_cfg_runtozero_maxX; - ack.leave_from_zero_max_d = m_cfg_runtozero_leave_away_zero_maxXY; - ack.speed = m_cfg_runtozero_speed; - ack.dec = m_cfg_runtozero_dec; +int32_t XYRobotCtrlModule::set_base_param(const base_param_t& param) { + zlock_guard lock(m_lock); + m_cfg = param; + m_stepM1->setScale(m_cfg.distance_scale / 1000.0); + m_stepM2->setScale(m_cfg.distance_scale / 1000.0); + 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); return 0; -} -int32_t XYRobotCtrlModule::set_warning_limit_param(uint8_t operation, const warning_limit_param_t& param, warning_limit_param_t& ack) { // - ZLOGI(TAG, "set_warning_limit_param %d", operation); +}; +int32_t XYRobotCtrlModule::get_base_param(base_param_t& ack) { + zlock_guard lock(m_lock); + ack = m_cfg; return 0; -} +}; + int32_t XYRobotCtrlModule::exec_move_to_zero_task(int32_t& dx, int32_t& dy) { int32_t xnow, ynow; getnowpos(xnow, ynow); @@ -328,7 +266,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { /** * @brief 如果设备已经在零点，则反向移动一定距离远离零点 */ - _motor_move_by(m_cfg_runtozero_leave_away_zero_maxXY, 0, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_dec); + _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; vTaskDelay(1); @@ -346,7 +284,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { /******************************************************************************* * 移动X轴到零点 * *******************************************************************************/ - _motor_move_by(-m_cfg_runtozero_maxX, 0, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_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()); @@ -379,7 +317,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { /** * @brief 如果设备已经在零点，则反向移动一定距离远离零点 */ - _motor_move_by(0, m_cfg_runtozero_leave_away_zero_maxXY, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_dec); + _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; vTaskDelay(1); @@ -397,7 +335,7 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { /******************************************************************************* * 移动Y轴到零点 * *******************************************************************************/ - _motor_move_by(0, -m_cfg_runtozero_maxY, m_cfg_runtozero_speed, m_cfg_acc, m_cfg_runtozero_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()); @@ -423,10 +361,6 @@ int32_t XYRobotCtrlModule::exec_move_to_zero_task() { return 0; } -void XYRobotCtrlModule::updateStatus(uint8_t status) { - zlock_guard lock(m_statu_lock); - m_status = status; -} void XYRobotCtrlModule::getnowpos(int32_t& x, int32_t& y) { int32_t m1pos, m2pos; m1pos = m_stepM1->getXACTUAL(); @@ -481,27 +415,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_robot_type == corexy) { + if (m_cfg.robot_type == corexy) { x = (m1pos - m2pos); y = (m1pos + m2pos); } else { x = m1pos + m2pos; y = m1pos - m2pos; } - if (m_x_shaft) x = -x; - if (m_y_shaft) y = -y; + if (m_cfg.x_shaft) x = -x; + if (m_cfg.y_shaft) y = -y; x += m_zero_shift_x; y += m_zero_shift_y; } void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) { - if (m_x_shaft) x = -x; - if (m_y_shaft) y = -y; + if (m_cfg.x_shaft) x = -x; + if (m_cfg.y_shaft) y = -y; x -= m_zero_shift_x; y -= m_zero_shift_y; - if (m_robot_type == corexy) { + if (m_cfg.robot_type == corexy) { m1pos = ((x + y) / 2); m2pos = ((y - x) / 2); } else { @@ -511,3 +445,9 @@ void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, } 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); + } +} 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 8988009..09fdb54 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp @@ -20,38 +20,15 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule { int m_x = 0; int m_y = 0; - uint8_t m_status = 0; + // uint8_t m_status = 0; + + base_param_t m_cfg; int32_t m_zero_shift_x = 0; int32_t m_zero_shift_y = 0; zmutex m_lock; - zmutex m_statu_lock; - - int32_t m_cfg_acc = 300000; - int32_t m_cfg_dec = 300000; - int32_t m_cfg_breakdec = 300000; - int32_t m_cfg_velocity = 300000; - - u8 m_cfg_ihold = 1; - u8 m_cfg_irun = 3; - u16 m_cfg_iholddelay = 10; - - int32_t m_cfg_runtozero_maxX = 5120000; - int32_t m_cfg_runtozero_maxY = 5120000; - int32_t m_cfg_runtozero_speed = 30000; - int32_t m_cfg_runtozero_dec = 600000; - int32_t m_cfg_runtozero_leave_away_zero_maxXY = 51200; - float m_cfg_distance_scale = 1.0f; - - RobotType_t m_robot_type = corexy; - bool m_x_shaft = false; - bool m_y_shaft = false; - - int32_t m_max_x = 0; - int32_t m_max_y = 0; - int32_t m_min_x = 0; - int32_t m_min_y = 0; + // zmutex m_statu_lock; public: void initialize(IStepperMotor* stepM1, // @@ -60,10 +37,10 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule { ZGPIO* Ygpio, // float scale); - virtual int32_t move_to(int32_t x, int32_t y, function status_cb) override; - virtual int32_t move_by(int32_t dx, int32_t dy, function status_cb) override; - virtual int32_t move_to_zero(function status_cb) override; - virtual int32_t move_to_zero_with_calibrate(int32_t x, int32_t y, function status_cb) override; + virtual int32_t move_to(int32_t x, int32_t y, action_cb_status_t status_cb) override; + virtual int32_t move_by(int32_t dx, int32_t dy, 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; @@ -72,23 +49,14 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule { virtual int32_t read_status(status_t& status) override; virtual int32_t read_detailed_status(detailed_status_t& debug_info) override; - virtual int32_t set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) override; - virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override; - virtual int32_t set_warning_limit_param(uint8_t operation, const warning_limit_param_t& param, warning_limit_param_t& ack) override; - - int32_t read_run_param(run_param_t& param); - int32_t read_run_to_zero_param(run_to_zero_param_t& param); - int32_t read_warning_limit_param(warning_limit_param_t& param); + void create_default_cfg(base_param_t& ack); + virtual int32_t set_base_param(const base_param_t& param) override; + virtual int32_t get_base_param(base_param_t& ack) override; - int32_t set_run_param( run_param_t& param); - int32_t set_run_to_zero_param( run_to_zero_param_t& param); - int32_t set_warning_limit_param( warning_limit_param_t& param); void loop(); private: - void updateStatus(uint8_t status); - void computeTargetMotorPos(); void getnowpos(int32_t& x, int32_t& y); @@ -102,5 +70,7 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule { 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); }; } // namespace iflytop \ No newline at end of file diff --git a/components/zcancmder_module/zcan_xy_robot_module.cpp b/components/zcancmder_module/zcan_xy_robot_module.cpp index 9b8be46..ac96a50 100644 --- a/components/zcancmder_module/zcan_xy_robot_module.cpp +++ b/components/zcancmder_module/zcan_xy_robot_module.cpp @@ -21,51 +21,51 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { } END_PP(); PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_zero, m_id) { // - errorcode = m_xyRobotCtrlModule->move_to_zero([this, cmdheader](I_XYRobotCtrlModule::move_to_zero_cb_status_t& status) { + errorcode = m_xyRobotCtrlModule->move_to_zero([this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_xy_robot_ctrl_move_to_zero_report_t report = {0}; - ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to_zero exec_status:%d", status.exec_status); + ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to_zero exec_status:%d", status); - report.id = m_id; - report.status = status; + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } END_PP(); PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, m_id) { // - errorcode = m_xyRobotCtrlModule->move_to_zero_with_calibrate(cmd->nowx, cmd->nowy, [this, cmdheader](I_XYRobotCtrlModule::move_to_zero_with_calibrate_cb_status_t& status) { + errorcode = m_xyRobotCtrlModule->move_to_zero_with_calibrate(cmd->nowx, cmd->nowy, [this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_xy_robot_ctrl_move_to_zero_with_calibrate_report_t report = {0}; - ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to_zero_with_calibrate exec_status:%d %d %d", status.exec_status, status.zero_shift_x, status.zero_shift_y); + ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to_zero_with_calibrate exec_status:%d", status); - report.id = m_id; - report.status = status; + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } END_PP(); PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to, m_id) { // - errorcode = m_xyRobotCtrlModule->move_to(cmd->x, cmd->y, [this, cmdheader](I_XYRobotCtrlModule::move_to_cb_status_t& status) { + errorcode = m_xyRobotCtrlModule->move_to(cmd->x, cmd->y, [this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_xy_robot_ctrl_move_to_report_t report = {0}; - ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to exec_status:%d %d %d", status.exec_status, status.tox, status.toy); + ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_to exec_status:%d", status); - report.id = m_id; - report.status = status; + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } END_PP(); PROCESS_PACKET(kcmd_xy_robot_ctrl_move_by, m_id) { // - errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, [this, cmdheader](I_XYRobotCtrlModule::move_by_cb_status_t& status) { + errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, [this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_xy_robot_ctrl_move_by_report_t report = {0}; - ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_by exec_status:%d %d %d", status.exec_status, status.dx, status.dy); + ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_by exec_status:%d", status); - report.id = m_id; - report.status = status; + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } @@ -91,19 +91,24 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { } END_PP(); - PROCESS_PACKET(kcmd_xy_robot_ctrl_set_run_param, m_id) { // - errorcode = m_xyRobotCtrlModule->set_run_param(cmd->opt_type, cmd->param, ack->ack); + PROCESS_PACKET(kcmd_xy_robot_ctrl_set_base_param, m_id) { // + errorcode = m_xyRobotCtrlModule->set_base_param(cmd->param); } END_PP(); - PROCESS_PACKET(kcmd_xy_robot_ctrl_set_warning_limit_param, m_id) { // - errorcode = m_xyRobotCtrlModule->set_warning_limit_param(cmd->opt_type, cmd->param, ack->ack); + PROCESS_PACKET(kcmd_xy_robot_ctrl_get_base_param, m_id) { // + errorcode = m_xyRobotCtrlModule->get_base_param(ack->ack); } END_PP(); - PROCESS_PACKET(kcmd_xy_robot_ctrl_set_run_to_zero_param, m_id) { // - errorcode = m_xyRobotCtrlModule->set_run_to_zero_param(cmd->opt_type, cmd->param, ack->ack); - } - END_PP(); + // PROCESS_PACKET(kcmd_xy_robot_ctrl_set_warning_limit_param, m_id) { // + // errorcode = m_xyRobotCtrlModule->set_warning_limit_param(cmd->opt_type, cmd->param, ack->ack); + // } + // END_PP(); + + // PROCESS_PACKET(kcmd_xy_robot_ctrl_set_run_to_zero_param, m_id) { // + // errorcode = m_xyRobotCtrlModule->set_run_to_zero_param(cmd->opt_type, cmd->param, ack->ack); + // } + // END_PP(); } #endif \ No newline at end of file diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index 25c741a..fc43406 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit 25c741a63cac28ee61d0658350dc82946e26e9cc +Subproject commit fc4340666e8afc748a8adb759439c561b62afa5d