From b3dbb159e8a0f0593bfc9088c96ae7c24edb975a Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 12 Oct 2023 21:06:50 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E6=AD=BB=E9=94=81=E7=9A=84BU?= =?UTF-8?q?G?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../step_motor_ctrl_script_cmder_module.cpp | 463 --------------------- .../step_motor_ctrl_script_cmder_module.hpp | 22 - .../xy_robot_script_cmder_module.cpp | 338 --------------- .../xy_robot_script_cmder_module.hpp | 22 - .../step_motor_ctrl_module.cpp | 22 +- .../step_motor_ctrl_script_cmder_module.cpp | 463 +++++++++++++++++++++ .../step_motor_ctrl_script_cmder_module.hpp | 22 + .../zcan_master_step_motor_ctrl_module.cpp | 164 ++++++++ .../zcan_master_step_motor_ctrl_module.hpp | 10 + .../zcan_step_motor_ctrl_module.cpp | 92 ++++ .../zcan_step_motor_ctrl_module.hpp | 17 + .../xy_robot_script_cmder_module.cpp | 338 +++++++++++++++ .../xy_robot_script_cmder_module.hpp | 22 + .../zcan_xy_robot_master_module.cpp | 100 +++++ .../zcan_xy_robot_master_module.hpp | 10 + .../xy_robot_ctrl_module/zcan_xy_robot_module.cpp | 126 ++++++ .../xy_robot_ctrl_module/zcan_xy_robot_module.hpp | 19 + components/zcancmder/zcanreceiver.cpp | 11 +- components/zcancmder/zcanreceiver.hpp | 5 +- .../zcan_master_step_motor_ctrl_module.cpp | 164 -------- .../zcan_master_step_motor_ctrl_module.hpp | 10 - .../zcan_xy_robot_master_module.cpp | 100 ----- .../zcan_xy_robot_master_module.hpp | 10 - components/zcancmder_module/zcan_eeprom_module.cpp | 2 - .../zcan_mini_servo_ctrl_module.cpp | 1 - .../zcan_motor_laser_code_scanner_scan_module.cpp | 1 - .../zcancmder_module/zcan_pipette_module.cpp | 1 - .../zcan_step_motor_ctrl_module.cpp | 91 ---- .../zcan_step_motor_ctrl_module.hpp | 17 - .../zcancmder_module/zcan_xy_robot_module.cpp | 126 ------ .../zcancmder_module/zcan_xy_robot_module.hpp | 19 - components/zprotocols/zcancmder | 2 +- 32 files changed, 1410 insertions(+), 1400 deletions(-) delete mode 100644 components/scriptcmder_module/step_motor_ctrl_script_cmder_module.cpp delete mode 100644 components/scriptcmder_module/step_motor_ctrl_script_cmder_module.hpp delete mode 100644 components/scriptcmder_module/xy_robot_script_cmder_module.cpp delete mode 100644 components/scriptcmder_module/xy_robot_script_cmder_module.hpp create mode 100644 components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.cpp create mode 100644 components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.hpp create mode 100644 components/step_motor_ctrl_module/zcan_master_step_motor_ctrl_module.cpp create mode 100644 components/step_motor_ctrl_module/zcan_master_step_motor_ctrl_module.hpp create mode 100644 components/step_motor_ctrl_module/zcan_step_motor_ctrl_module.cpp create mode 100644 components/step_motor_ctrl_module/zcan_step_motor_ctrl_module.hpp create mode 100644 components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp create mode 100644 components/xy_robot_ctrl_module/xy_robot_script_cmder_module.hpp create mode 100644 components/xy_robot_ctrl_module/zcan_xy_robot_master_module.cpp create mode 100644 components/xy_robot_ctrl_module/zcan_xy_robot_master_module.hpp create mode 100644 components/xy_robot_ctrl_module/zcan_xy_robot_module.cpp create mode 100644 components/xy_robot_ctrl_module/zcan_xy_robot_module.hpp delete mode 100644 components/zcancmder_master_module/zcan_master_step_motor_ctrl_module.cpp delete mode 100644 components/zcancmder_master_module/zcan_master_step_motor_ctrl_module.hpp delete mode 100644 components/zcancmder_master_module/zcan_xy_robot_master_module.cpp delete mode 100644 components/zcancmder_master_module/zcan_xy_robot_master_module.hpp delete mode 100644 components/zcancmder_module/zcan_step_motor_ctrl_module.cpp delete mode 100644 components/zcancmder_module/zcan_step_motor_ctrl_module.hpp delete mode 100644 components/zcancmder_module/zcan_xy_robot_module.cpp delete mode 100644 components/zcancmder_module/zcan_xy_robot_module.hpp diff --git a/components/scriptcmder_module/step_motor_ctrl_script_cmder_module.cpp b/components/scriptcmder_module/step_motor_ctrl_script_cmder_module.cpp deleted file mode 100644 index 9395da8..0000000 --- a/components/scriptcmder_module/step_motor_ctrl_script_cmder_module.cpp +++ /dev/null @@ -1,463 +0,0 @@ -#include "step_motor_ctrl_script_cmder_module.hpp" - -#include - -#include "sdk\components\zprotocols\zcancmder\zcancmder_protocol.hpp" -using namespace iflytop; -#define TAG "StepMotorCtrlScriptCmderModule" - -#define CHECK_ARGC(n) \ - if (argc != (n + 1)) { \ - ZLOGE(TAG, "argc != %d", n); \ - context->breakflag = true; \ - return; \ - } -#define CHECK_MIN_ARGC(n) \ - if (argc < (n + 1)) { \ - ZLOGE(TAG, "argc <= %d", n + 1); \ - context->breakflag = true; \ - return; \ - } - -#define SCRIPT_CMDER_CHECK(exptr) \ - if (!(exptr)) { \ - ZLOGE(TAG, "check %s failed", #exptr); \ - context->breakflag = true; \ - return; \ - } - -void StepMotorCtrlScriptCmderModule::initialize(CmdScheduler* cmdScheduler) { - m_cmdScheduler = cmdScheduler; - ZASSERT(m_cmdScheduler != nullptr); - regcmd(); -} - -I_StepMotorCtrlModule* StepMotorCtrlScriptCmderModule::findXYRobot(int id) { - auto it = moduler.find(id); - if (it == moduler.end()) { - return nullptr; - } - return it->second; -} - -static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; } - -void StepMotorCtrlScriptCmderModule::regmodule(int id, I_StepMotorCtrlModule* robot) { - ZASSERT(moduler.find(id) == moduler.end()); - ZASSERT(robot != nullptr); - moduler[id] = robot; -} -void StepMotorCtrlScriptCmderModule::regcmd() { // - ZASSERT(m_cmdScheduler != nullptr); - -#if 0 - virtual bool isbusy() = 0; - virtual int32_t get_last_exec_status() = 0; - - virtual int32_t move_to(int32_t x, action_cb_status_t status_cb) = 0; - virtual int32_t move_by(int32_t dx, action_cb_status_t status_cb) = 0; - virtual int32_t move_to(int32_t velocity, int32_t x, action_cb_status_t status_cb) = 0; - virtual int32_t move_by(int32_t velocity, int32_t dx, action_cb_status_t status_cb) = 0; - - virtual int32_t move_to_zero(action_cb_status_t status_cb) = 0; - virtual int32_t move_to_zero_with_calibrate(int32_t x, action_cb_status_t status_cb) = 0; - virtual int32_t rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb) = 0; - virtual int32_t enable(bool venable) = 0; - virtual int32_t stop(uint8_t stopType) = 0; - virtual int32_t force_change_current_pos(int32_t x) = 0; - - virtual int32_t move_to_logic_point(int32_t logic_point_num, action_cb_status_t status_cb) = 0; - virtual int32_t set_logic_point(int logic_point_num, int32_t x, int32_t vel, s32 acc, s32 dec) = 0; - virtual int32_t get_logic_point(int logic_point_num, logic_point_t& logic_point) = 0; - - virtual int32_t flush() = 0; - virtual int32_t factory_reset() = 0; - - virtual int32_t move_to_block(int32_t tox, int overtime) = 0; - virtual int32_t move_by_block(int32_t dx, int overtime) = 0; - virtual int32_t move_to_block(int32_t velocity, int32_t tox, int overtime) = 0; - virtual int32_t move_by_block(int32_t velocity, int32_t dx, int overtime) = 0; - virtual int32_t move_to_zero_block(int overtime) = 0; - virtual int32_t move_to_zero_with_calibrate_block(int32_t x, int overtime) = 0; - - virtual int32_t read_version(version_t& version) = 0; - virtual int32_t read_status(status_t& status) = 0; - virtual int32_t read_detailed_status(detailed_status_t& debug_info) = 0; - - virtual int32_t read_pos(int32_t& pos) = 0; - virtual int32_t read_pos() = 0; - - virtual bool read_zero_io_state() = 0; - - virtual int32_t set_base_param(const base_param_t& param) = 0; - virtual int32_t get_base_param(base_param_t& param) = 0; -#endif - -#if 0 - m_cmdScheduler->registerCmd("xy_robot_ctrl_enable", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 3); - - int id = atoi(argv[1]); - bool enable = atoi(argv[2]); - - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->enable(enable); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - }); -#endif - - m_cmdScheduler->registerCmd("step_motor_ctrl_is_busy", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - - int id = atoi(argv[1]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - bool ack = module->isbusy(); - ZLOGI(TAG, "is busy:%d", ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_get_last_exec_status", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - - int id = atoi(argv[1]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->get_last_exec_status(); - ZLOGI(TAG, "last exec status:%s(%d)", err::error2str(ack), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_move_to", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_MIN_ARGC(2); - - int id = atoi(argv[1]); - int32_t x = atoi(argv[2]); - int32_t speed = 0; - if (argc >= 4) { - int32_t speed = atoi(argv[3]); - } - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->move_to(x, speed, [](int32_t status) { ZLOGI(TAG, "move to exec end status:%s(%d)", err::error2str(status), status); }); - ZLOGI(TAG, "move to:%s(%d)", err::error2str(ack), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_move_by", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_MIN_ARGC(2); - - int id = atoi(argv[1]); - int32_t dx = atoi(argv[2]); - int32_t speed = 0; - if (argc >= 4) { - int32_t speed = atoi(argv[3]); - } - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->move_by(dx, speed, [](int32_t status) { ZLOGI(TAG, "move by exec end status:%s(%d)", err::error2str(status), status); }); - ZLOGI(TAG, "move by:%s(%d)", err::error2str(ack), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_zero", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - - int id = atoi(argv[1]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->move_to_zero([](int32_t status) { ZLOGI(TAG, "move to zero exec end status:%s(%d)", err::error2str(status), status); }); - ZLOGI(TAG, "move to zero:%s(%d)", err::error2str(ack), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_zero_with_calibrate", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_MIN_ARGC(2); - - int id = atoi(argv[1]); - int32_t x = atoi(argv[2]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->move_to_zero_with_calibrate(x, [](int32_t status) { ZLOGI(TAG, "move to zero with calibrate exec end status:%s(%d)", err::error2str(status), status); }); - ZLOGI(TAG, "move to zero with calibrate:%s(%d)", err::error2str(ack), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_rotate", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_MIN_ARGC(2); - - int id = atoi(argv[1]); - int32_t speed = atoi(argv[2]); - int32_t lastforms = 0; - if (argc >= 4) { - int32_t lastforms = atoi(argv[3]); - } - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->rotate(speed, lastforms, [](int32_t status) { ZLOGI(TAG, "rotate exec end status:%s(%d)", err::error2str(status), status); }); - ZLOGI(TAG, "rotate:%s(%d)", err::error2str(ack), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_stop", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_MIN_ARGC(2); - - int id = atoi(argv[1]); - uint8_t stopType = atoi(argv[2]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->stop(stopType); - ZLOGI(TAG, "stop:%s(%d)", err::error2str(ack), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_force_change_current_pos", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_MIN_ARGC(2); - - int id = atoi(argv[1]); - int32_t x = atoi(argv[2]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->force_change_current_pos(x); - ZLOGI(TAG, "force change current pos:%s(%d)", err::error2str(ack), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_logic_point", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_MIN_ARGC(2); - - int id = atoi(argv[1]); - int32_t logic_point_id = atoi(argv[2]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->move_to_logic_point(logic_point_id, [](int32_t status) { ZLOGI(TAG, "move to logic point exec end status:%s(%d)", err::error2str(status), status); }); - ZLOGI(TAG, "move to logic point:%s(%d)", err::error2str(ack), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_set_logic_point", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_MIN_ARGC(6); - - int id = atoi(argv[1]); - int32_t logic_point_id = atoi(argv[2]); - int32_t x = atoi(argv[3]); - int32_t vel = atoi(argv[4]); - int32_t acc = atoi(argv[5]); - int32_t dec = atoi(argv[6]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->set_logic_point(logic_point_id, x, vel, acc, dec); - ZLOGI(TAG, "set logic point:%s(%d)", err::error2str(ack), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_set_logic_point_simplify", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_MIN_ARGC(2); - - int id = atoi(argv[1]); - int32_t logic_point_id = atoi(argv[2]); - int32_t x = 0; - int32_t vel = 0; - int32_t acc = 0; - int32_t dec = 0; - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - x = module->read_pos(); - - int32_t ack = module->set_logic_point(logic_point_id, x, vel, acc, dec); - ZLOGI(TAG, "set logic point:%s(%d)", err::error2str(ack), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_get_logic_point", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_MIN_ARGC(2); - - int id = atoi(argv[1]); - int32_t logic_point_id = atoi(argv[2]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - I_StepMotorCtrlModule::logic_point_t logic_point; - int32_t ack = module->get_logic_point(logic_point_id, logic_point); - if (ack != 0) { - ZLOGI(TAG, "get logic point:%s(%d)", err::error2str(ack), ack); - return; - } - - ZLOGI(TAG, "logic_point_id :%d", logic_point.index); - ZLOGI(TAG, "logic_point.x :%d", logic_point.x); - ZLOGI(TAG, "logic_point.vel :%d", logic_point.velocity); - ZLOGI(TAG, "logic_point.acc :%d", logic_point.acc); - ZLOGI(TAG, "logic_point.dec :%d", logic_point.dec); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_flush", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - - int id = atoi(argv[1]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->flush(); - ZLOGI(TAG, "flush:%s(%d)", err::error2str(ack), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_factory_reset", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - - int id = atoi(argv[1]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->factory_reset(); - ZLOGI(TAG, "factory reset:%s(%d)", err::error2str(ack), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_read_version", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - - int id = atoi(argv[1]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - I_StepMotorCtrlModule::version_t version; - int32_t ack = module->read_version(version); - if (ack != 0) { - ZLOGI(TAG, "read version:%s(%d)", err::error2str(ack), ack); - return; - } - - ZLOGI(TAG, "version.version:%d", version.version); - }); - -#define GET_BIT(x, n) ((x >> n) & 0x01) - - m_cmdScheduler->registerCmd("step_motor_ctrl_read_status", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - - int id = atoi(argv[1]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - I_StepMotorCtrlModule::status_t status; - int32_t ack = module->read_status(status); - if (ack != 0) { - ZLOGI(TAG, "read status:%s(%d)", err::error2str(ack), ack); - return; - } - - ZLOGI(TAG, "status.status :%d", status.status); - ZLOGI(TAG, "status.io_state :%d,%d,%d,%d,%d,%d,%d,%d", GET_BIT(status.io_state, 0), GET_BIT(status.io_state, 1), GET_BIT(status.io_state, 2), GET_BIT(status.io_state, 3), GET_BIT(status.io_state, 4), GET_BIT(status.io_state, 5), GET_BIT(status.io_state, 6), GET_BIT(status.io_state, 7)); - ZLOGI(TAG, "status.x :%d", status.x); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_set_base_param", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_MIN_ARGC(4); - - int id = atoi(argv[1]); - const char* paramName = argv[2]; - int32_t value = atoi(argv[3]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - I_StepMotorCtrlModule::base_param_t param; - int32_t ack = module->get_base_param(param); - if (ack != 0) { - ZLOGE(TAG, "get_base_param failed:%d", ack); - return; - } - if (streq(paramName, "x_shaft")) { - param.x_shaft = value; - } else if (streq(paramName, "distance_scale")) { - param.distance_scale = value; - } else if (streq(paramName, "ihold")) { - param.ihold = value; - } else if (streq(paramName, "irun")) { - param.irun = value; - } else if (streq(paramName, "iholddelay")) { - param.iholddelay = value; - } else if (streq(paramName, "acc")) { - param.acc = value; - } else if (streq(paramName, "dec")) { - param.dec = value; - } else if (streq(paramName, "maxspeed")) { - param.maxspeed = value; - } else if (streq(paramName, "min_x")) { - param.min_x = value; - } else if (streq(paramName, "max_x")) { - param.max_x = value; - } else if (streq(paramName, "shift_x")) { - param.shift_x = value; - } else if (streq(paramName, "run_to_zero_move_to_zero_max_d")) { - param.run_to_zero_move_to_zero_max_d = value; - } else if (streq(paramName, "run_to_zero_leave_from_zero_max_d")) { - param.run_to_zero_leave_from_zero_max_d = value; - } else if (streq(paramName, "run_to_zero_speed")) { - param.run_to_zero_speed = value; - } else if (streq(paramName, "run_to_zero_dec")) { - param.run_to_zero_dec = value; - } else { - ZLOGE(TAG, "invalid param name:%s", paramName); - return; - } - ack = module->set_base_param(param); - if (ack != 0) { - ZLOGE(TAG, "set_base_param failed:%s(%d)", err::error2str(ack), ack); - return; - } - ZLOGI(TAG, "set_base_param ok"); - return; - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_get_base_param", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - CHECK_ARGC(1); - - int id = atoi(argv[1]); - - I_StepMotorCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - I_StepMotorCtrlModule::base_param_t param; - int32_t ack = module->get_base_param(param); - if (ack != 0) { - ZLOGE(TAG, "get_base_param failed:%d", ack); - return; - } - - ZLOGI(TAG, "param.x_shaft :%d", param.x_shaft); - ZLOGI(TAG, "param.distance_scale :%d", param.distance_scale); - ZLOGI(TAG, "param.ihold :%d", param.ihold); - ZLOGI(TAG, "param.irun :%d", param.irun); - ZLOGI(TAG, "param.iholddelay :%d", param.iholddelay); - ZLOGI(TAG, "param.acc :%d", param.acc); - ZLOGI(TAG, "param.dec :%d", param.dec); - ZLOGI(TAG, "param.maxspeed :%d", param.maxspeed); - ZLOGI(TAG, "param.min_x :%d", param.min_x); - ZLOGI(TAG, "param.max_x :%d", param.max_x); - ZLOGI(TAG, "param.shift_x :%d", param.shift_x); - ZLOGI(TAG, "param.run_to_zero_move_to_zero_max_d:%d", param.run_to_zero_move_to_zero_max_d); - ZLOGI(TAG, "param.run_to_zero_leave_from_zero_max_d:%d", param.run_to_zero_leave_from_zero_max_d); - ZLOGI(TAG, "param.run_to_zero_speed :%d", param.run_to_zero_speed); - ZLOGI(TAG, "param.run_to_zero_dec :%d", param.run_to_zero_dec); - }); -} \ No newline at end of file diff --git a/components/scriptcmder_module/step_motor_ctrl_script_cmder_module.hpp b/components/scriptcmder_module/step_motor_ctrl_script_cmder_module.hpp deleted file mode 100644 index a48d835..0000000 --- a/components/scriptcmder_module/step_motor_ctrl_script_cmder_module.hpp +++ /dev/null @@ -1,22 +0,0 @@ -#pragma once -#include - -#include "sdk/os/zos.hpp" -#include "sdk\components\cmdscheduler\cmd_scheduler.hpp" -#include "sdk\components\zprotocols\zcancmder\api\i_step_motor_ctrl_module.hpp" -namespace iflytop { - -class StepMotorCtrlScriptCmderModule { - CmdScheduler* m_cmdScheduler; - map moduler; - - public: - void initialize(CmdScheduler* cmdScheduler); - void regmodule(int id, I_StepMotorCtrlModule* robot); - void regcmd(); - - private: - I_StepMotorCtrlModule* findXYRobot(int id); -}; - -} // namespace iflytop \ No newline at end of file diff --git a/components/scriptcmder_module/xy_robot_script_cmder_module.cpp b/components/scriptcmder_module/xy_robot_script_cmder_module.cpp deleted file mode 100644 index 5ed0bb1..0000000 --- a/components/scriptcmder_module/xy_robot_script_cmder_module.cpp +++ /dev/null @@ -1,338 +0,0 @@ -#include "xy_robot_script_cmder_module.hpp" - -#include - -#include "sdk\components\zprotocols\zcancmder\zcancmder_protocol.hpp" -using namespace iflytop; -#define TAG "XYRobotScriptCmderModule" - -#define CHECK_ARGC(n) \ - if (argc != (n + 1)) { \ - ZLOGE(TAG, "argc != %d", n); \ - context->breakflag = true; \ - return; \ - } - -#define SCRIPT_CMDER_CHECK(exptr) \ - if (!(exptr)) { \ - ZLOGE(TAG, "check %s failed", #exptr); \ - context->breakflag = true; \ - return; \ - } - -void XYRobotScriptCmderModule::initialize(CmdScheduler* cmdScheduler) { - m_cmdScheduler = cmdScheduler; - ZASSERT(m_cmdScheduler != nullptr); - regcmd(); -} - -I_XYRobotCtrlModule* XYRobotScriptCmderModule::findXYRobot(int id) { - auto it = moduler.find(id); - if (it == moduler.end()) { - return nullptr; - } - return it->second; -} - -static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; } - -void XYRobotScriptCmderModule::regXYRobot(int id, I_XYRobotCtrlModule* robot) { - ZASSERT(moduler.find(id) == moduler.end()); - ZASSERT(robot != nullptr); - moduler[id] = robot; -} -void XYRobotScriptCmderModule::regcmd() { // - ZASSERT(m_cmdScheduler != nullptr); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_enable", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 3); - - int id = atoi(argv[1]); - bool enable = atoi(argv[2]); - - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->enable(enable); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_stop", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 3); - - int id = atoi(argv[1]); - uint8_t stopType = atoi(argv[2]); - - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->stop(stopType); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_move_to_zero", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 2); - - int id = atoi(argv[1]); - - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->move_to_zero([this](int32_t status) { ZLOGI(TAG, "move_to_zero status:%d", status); }); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_move_to_zero_with_calibrate", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 4); - - int32_t id = atoi(argv[1]); - int32_t nowx = atoi(argv[2]); - int32_t nowy = atoi(argv[3]); - - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->move_to_zero_with_calibrate(nowx, nowy, [this](int32_t status) { ZLOGI(TAG, "move_to_zero_with_calibrate status:%d", status); }); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_move_to", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 5); - - int32_t id = atoi(argv[1]); - int32_t x = atoi(argv[2]); - int32_t y = atoi(argv[3]); - int32_t speed = atoi(argv[4]); - - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->move_to(x, y, speed, [this](int32_t status) { ZLOGI(TAG, "move_to status:%d", status); }); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_move_by", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 5); - - int32_t id = atoi(argv[1]); - int32_t dx = atoi(argv[2]); - int32_t dy = atoi(argv[3]); - int32_t v = atoi(argv[4]); - - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->move_by(dx, dy, v, [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); }); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_force_change_current_pos", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 4); - - int32_t id = atoi(argv[1]); - int32_t x = atoi(argv[2]); - int32_t y = atoi(argv[3]); - - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->force_change_current_pos(x, y); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_read_version", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 2); - - int32_t id = atoi(argv[1]); - - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - I_XYRobotCtrlModule::version_t version; - int32_t ack = module->read_version(version); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - if (ack == 0) { - ZLOGI(TAG, "version:%d", version.version); - } - }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_read_status", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 2); - - int32_t id = atoi(argv[1]); - - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - I_XYRobotCtrlModule::status_t status; - int32_t ack = module->read_status(status); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - if (ack == 0) { - ZLOGI(TAG, " status:%d", status.status); - ZLOGI(TAG, " x:%d", status.x); - ZLOGI(TAG, " y:%d", status.y); - ZLOGI(TAG, " iostate:%d", status.iostate); - } - }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_read_detailed_status", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 2); - - int32_t id = atoi(argv[1]); - - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - I_XYRobotCtrlModule::detailed_status_t status; - int32_t ack = module->read_detailed_status(status); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - if (ack == 0) { - ZLOGI(TAG, " status:%d", status.status); - ZLOGI(TAG, " x :%d", status.x); - ZLOGI(TAG, " y :%d", status.y); - ZLOGI(TAG, " iostate:%d", status.iostate); - } - }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_set_base_param", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 4); - int32_t id = atoi(argv[1]); - const char* paramName = argv[2]; - int32_t paramVal = atoi(argv[3]); - - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - I_XYRobotCtrlModule::base_param_t status; - int32_t ack = module->get_base_param(status); - if (ack != 0) { - ZLOGE(TAG, "get_base_param failed:%d", ack); - return; - } - - if (streq(paramName, "robot_type")) { - status.robot_type = paramVal; - } else if (streq(paramName, "x_shaft")) { - status.x_shaft = paramVal; - } else if (streq(paramName, "y_shaft")) { - status.y_shaft = paramVal; - } else if (streq(paramName, "ihold")) { - status.ihold = paramVal; - } else if (streq(paramName, "irun")) { - status.irun = paramVal; - } else if (streq(paramName, "iholddelay")) { - status.iholddelay = paramVal; - } else if (streq(paramName, "distance_scale")) { - status.distance_scale = paramVal; - } else if (streq(paramName, "shift_x")) { - status.shift_x = paramVal; - } else if (streq(paramName, "shift_y")) { - status.shift_y = paramVal; - } else if (streq(paramName, "acc")) { - status.acc = paramVal; - } else if (streq(paramName, "dec")) { - status.dec = paramVal; - } else if (streq(paramName, "breakdec")) { - status.breakdec = paramVal; - } else if (streq(paramName, "maxspeed")) { - status.maxspeed = paramVal; - } else if (streq(paramName, "min_x")) { - status.min_x = paramVal; - } else if (streq(paramName, "max_x")) { - status.max_x = paramVal; - } else if (streq(paramName, "min_y")) { - status.min_y = paramVal; - } else if (streq(paramName, "max_y")) { - 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 { - ZLOGE(TAG, "invalid param name:%s", paramName); - return; - } - ack = module->set_base_param(status); - if (ack != 0) { - ZLOGE(TAG, "set_base_param failed:%d,%s", ack, err::error2str(ack)); - return; - } - ZLOGI(TAG, "set_base_param ok"); - return; - }); - m_cmdScheduler->registerCmd("xy_robot_ctrl_get_base_param", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 2); - - int32_t id = atoi(argv[1]); - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - I_XYRobotCtrlModule::base_param_t status; - int32_t ack = module->get_base_param(status); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - if (ack == 0) { - ZLOGI(TAG, " robot_type :%d", status.robot_type); - ZLOGI(TAG, " x_shaft :%d", status.x_shaft); - ZLOGI(TAG, " y_shaft :%d", status.y_shaft); - ZLOGI(TAG, " ihold :%d", status.ihold); - ZLOGI(TAG, " irun :%d", status.irun); - ZLOGI(TAG, " iholddelay :%d", status.iholddelay); - ZLOGI(TAG, " distance_scale :%d", status.distance_scale); - ZLOGI(TAG, " shift_x :%d", status.shift_x); - ZLOGI(TAG, " shift_y :%d", status.shift_y); - ZLOGI(TAG, " acc :%d", status.acc); - ZLOGI(TAG, " dec :%d", status.dec); - ZLOGI(TAG, " breakdec :%d", status.breakdec); - ZLOGI(TAG, " maxspeed :%d", status.maxspeed); - ZLOGI(TAG, " min_x :%d", status.min_x); - ZLOGI(TAG, " max_x :%d", status.max_x); - ZLOGI(TAG, " min_y :%d", status.min_y); - ZLOGI(TAG, " max_y :%d", status.max_y); - ZLOGI(TAG, " run_to_zero_max_d :%d", status.run_to_zero_max_d); - ZLOGI(TAG, " leave_from_zero_max_d :%d", status.leave_from_zero_max_d); - ZLOGI(TAG, " run_to_zero_speed :%d", status.run_to_zero_speed); - ZLOGI(TAG, " run_to_zero_dec :%d", status.run_to_zero_dec); - } - }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_flush", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 2); - - int32_t id = atoi(argv[1]); - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->flush(); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_factory_reset", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 2); - - int32_t id = atoi(argv[1]); - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->factory_reset(); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_move_by_no_limit", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { - SCRIPT_CMDER_CHECK(argc == 5); - - int32_t id = atoi(argv[1]); - int32_t dx = atoi(argv[2]); - int32_t dy = atoi(argv[3]); - int32_t v = atoi(argv[4]); - - I_XYRobotCtrlModule* module = findXYRobot(id); - SCRIPT_CMDER_CHECK(module != nullptr); - - int32_t ack = module->move_by_no_limit(dx, dy, v, [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); }); - ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); - }); -} \ No newline at end of file diff --git a/components/scriptcmder_module/xy_robot_script_cmder_module.hpp b/components/scriptcmder_module/xy_robot_script_cmder_module.hpp deleted file mode 100644 index fbac794..0000000 --- a/components/scriptcmder_module/xy_robot_script_cmder_module.hpp +++ /dev/null @@ -1,22 +0,0 @@ -#pragma once -#include - -#include "sdk/os/zos.hpp" -#include "sdk\components\cmdscheduler\cmd_scheduler.hpp" -#include "sdk\components\zprotocols\zcancmder\api\i_xyrobot_ctrl_module.hpp" -namespace iflytop { - -class XYRobotScriptCmderModule { - CmdScheduler* m_cmdScheduler; - map moduler; - - public: - void initialize(CmdScheduler* cmdScheduler); - void regXYRobot(int id, I_XYRobotCtrlModule* robot); - void regcmd(); - - private: - I_XYRobotCtrlModule* findXYRobot(int id); -}; - -} // namespace iflytop \ No newline at end of file diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp index 0960f05..1aa0737 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -231,12 +231,14 @@ int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, action_cb_ return err::kce_param_out_of_range; } - if (abs(speed) > m_param.maxspeed) { + if (m_param.maxspeed != 0 && abs(speed) > m_param.maxspeed) { ZLOGW(TAG, "speed:%d > m_cfg_max_speed:%d", speed, m_param.maxspeed); speed = m_param.maxspeed; } m_thread.stop(); m_thread.start([this, lastforms, speed, status_cb]() { + ZLOGI(TAG, "(in work thread)rotate speed:%d lastforms:%d acc:%d dec:%d", speed, lastforms, m_param.acc, m_param.dec); + m_stepM1->setAcceleration(m_param.acc); m_stepM1->setDeceleration(m_param.dec); m_stepM1->rotate(speed); @@ -245,12 +247,13 @@ int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, action_cb_ bool reachtime = false; while (!m_thread.getExitFlag()) { - if (zos_haspassedms(startticket) > lastforms || lastforms == 0) { + if (zos_haspassedms(startticket) > lastforms && lastforms != 0) { reachtime = true; m_stepM1->stop(); break; } - osDelay(1); + // ZLOGI(TAG,"..... state %d",m_thread.getExitFlag()); + osDelay(100); } call_exec_status_cb(0, status_cb); m_stepM1->stop(); @@ -513,12 +516,13 @@ void StepMotorCtrlModule::call_exec_status_cb(int32_t status, action_cb_status_t } void StepMotorCtrlModule::create_default_cfg(flash_config_t& cfg) { memset(&cfg, 0, sizeof(cfg)); - cfg.base_param.ihold = 0; - cfg.base_param.irun = 8; - cfg.base_param.iholddelay = 100; - cfg.base_param.acc = 3000000; - cfg.base_param.dec = 3000000; - cfg.base_param.maxspeed = 300000; + cfg.base_param.distance_scale = 1000; + cfg.base_param.ihold = 0; + cfg.base_param.irun = 8; + cfg.base_param.iholddelay = 100; + cfg.base_param.acc = 3000000; + cfg.base_param.dec = 3000000; + cfg.base_param.maxspeed = 300000; cfg.base_param.run_to_zero_move_to_zero_max_d = INT32_MAX; cfg.base_param.run_to_zero_leave_from_zero_max_d = 51200 * 3; diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.cpp new file mode 100644 index 0000000..9395da8 --- /dev/null +++ b/components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.cpp @@ -0,0 +1,463 @@ +#include "step_motor_ctrl_script_cmder_module.hpp" + +#include + +#include "sdk\components\zprotocols\zcancmder\zcancmder_protocol.hpp" +using namespace iflytop; +#define TAG "StepMotorCtrlScriptCmderModule" + +#define CHECK_ARGC(n) \ + if (argc != (n + 1)) { \ + ZLOGE(TAG, "argc != %d", n); \ + context->breakflag = true; \ + return; \ + } +#define CHECK_MIN_ARGC(n) \ + if (argc < (n + 1)) { \ + ZLOGE(TAG, "argc <= %d", n + 1); \ + context->breakflag = true; \ + return; \ + } + +#define SCRIPT_CMDER_CHECK(exptr) \ + if (!(exptr)) { \ + ZLOGE(TAG, "check %s failed", #exptr); \ + context->breakflag = true; \ + return; \ + } + +void StepMotorCtrlScriptCmderModule::initialize(CmdScheduler* cmdScheduler) { + m_cmdScheduler = cmdScheduler; + ZASSERT(m_cmdScheduler != nullptr); + regcmd(); +} + +I_StepMotorCtrlModule* StepMotorCtrlScriptCmderModule::findXYRobot(int id) { + auto it = moduler.find(id); + if (it == moduler.end()) { + return nullptr; + } + return it->second; +} + +static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; } + +void StepMotorCtrlScriptCmderModule::regmodule(int id, I_StepMotorCtrlModule* robot) { + ZASSERT(moduler.find(id) == moduler.end()); + ZASSERT(robot != nullptr); + moduler[id] = robot; +} +void StepMotorCtrlScriptCmderModule::regcmd() { // + ZASSERT(m_cmdScheduler != nullptr); + +#if 0 + virtual bool isbusy() = 0; + virtual int32_t get_last_exec_status() = 0; + + virtual int32_t move_to(int32_t x, action_cb_status_t status_cb) = 0; + virtual int32_t move_by(int32_t dx, action_cb_status_t status_cb) = 0; + virtual int32_t move_to(int32_t velocity, int32_t x, action_cb_status_t status_cb) = 0; + virtual int32_t move_by(int32_t velocity, int32_t dx, action_cb_status_t status_cb) = 0; + + virtual int32_t move_to_zero(action_cb_status_t status_cb) = 0; + virtual int32_t move_to_zero_with_calibrate(int32_t x, action_cb_status_t status_cb) = 0; + virtual int32_t rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb) = 0; + virtual int32_t enable(bool venable) = 0; + virtual int32_t stop(uint8_t stopType) = 0; + virtual int32_t force_change_current_pos(int32_t x) = 0; + + virtual int32_t move_to_logic_point(int32_t logic_point_num, action_cb_status_t status_cb) = 0; + virtual int32_t set_logic_point(int logic_point_num, int32_t x, int32_t vel, s32 acc, s32 dec) = 0; + virtual int32_t get_logic_point(int logic_point_num, logic_point_t& logic_point) = 0; + + virtual int32_t flush() = 0; + virtual int32_t factory_reset() = 0; + + virtual int32_t move_to_block(int32_t tox, int overtime) = 0; + virtual int32_t move_by_block(int32_t dx, int overtime) = 0; + virtual int32_t move_to_block(int32_t velocity, int32_t tox, int overtime) = 0; + virtual int32_t move_by_block(int32_t velocity, int32_t dx, int overtime) = 0; + virtual int32_t move_to_zero_block(int overtime) = 0; + virtual int32_t move_to_zero_with_calibrate_block(int32_t x, int overtime) = 0; + + virtual int32_t read_version(version_t& version) = 0; + virtual int32_t read_status(status_t& status) = 0; + virtual int32_t read_detailed_status(detailed_status_t& debug_info) = 0; + + virtual int32_t read_pos(int32_t& pos) = 0; + virtual int32_t read_pos() = 0; + + virtual bool read_zero_io_state() = 0; + + virtual int32_t set_base_param(const base_param_t& param) = 0; + virtual int32_t get_base_param(base_param_t& param) = 0; +#endif + +#if 0 + m_cmdScheduler->registerCmd("xy_robot_ctrl_enable", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 3); + + int id = atoi(argv[1]); + bool enable = atoi(argv[2]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->enable(enable); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); +#endif + + m_cmdScheduler->registerCmd("step_motor_ctrl_is_busy", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + + int id = atoi(argv[1]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + bool ack = module->isbusy(); + ZLOGI(TAG, "is busy:%d", ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_get_last_exec_status", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + + int id = atoi(argv[1]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->get_last_exec_status(); + ZLOGI(TAG, "last exec status:%s(%d)", err::error2str(ack), ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_move_to", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_MIN_ARGC(2); + + int id = atoi(argv[1]); + int32_t x = atoi(argv[2]); + int32_t speed = 0; + if (argc >= 4) { + int32_t speed = atoi(argv[3]); + } + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->move_to(x, speed, [](int32_t status) { ZLOGI(TAG, "move to exec end status:%s(%d)", err::error2str(status), status); }); + ZLOGI(TAG, "move to:%s(%d)", err::error2str(ack), ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_move_by", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_MIN_ARGC(2); + + int id = atoi(argv[1]); + int32_t dx = atoi(argv[2]); + int32_t speed = 0; + if (argc >= 4) { + int32_t speed = atoi(argv[3]); + } + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->move_by(dx, speed, [](int32_t status) { ZLOGI(TAG, "move by exec end status:%s(%d)", err::error2str(status), status); }); + ZLOGI(TAG, "move by:%s(%d)", err::error2str(ack), ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_zero", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + + int id = atoi(argv[1]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->move_to_zero([](int32_t status) { ZLOGI(TAG, "move to zero exec end status:%s(%d)", err::error2str(status), status); }); + ZLOGI(TAG, "move to zero:%s(%d)", err::error2str(ack), ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_zero_with_calibrate", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_MIN_ARGC(2); + + int id = atoi(argv[1]); + int32_t x = atoi(argv[2]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->move_to_zero_with_calibrate(x, [](int32_t status) { ZLOGI(TAG, "move to zero with calibrate exec end status:%s(%d)", err::error2str(status), status); }); + ZLOGI(TAG, "move to zero with calibrate:%s(%d)", err::error2str(ack), ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_rotate", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_MIN_ARGC(2); + + int id = atoi(argv[1]); + int32_t speed = atoi(argv[2]); + int32_t lastforms = 0; + if (argc >= 4) { + int32_t lastforms = atoi(argv[3]); + } + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->rotate(speed, lastforms, [](int32_t status) { ZLOGI(TAG, "rotate exec end status:%s(%d)", err::error2str(status), status); }); + ZLOGI(TAG, "rotate:%s(%d)", err::error2str(ack), ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_stop", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_MIN_ARGC(2); + + int id = atoi(argv[1]); + uint8_t stopType = atoi(argv[2]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->stop(stopType); + ZLOGI(TAG, "stop:%s(%d)", err::error2str(ack), ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_force_change_current_pos", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_MIN_ARGC(2); + + int id = atoi(argv[1]); + int32_t x = atoi(argv[2]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->force_change_current_pos(x); + ZLOGI(TAG, "force change current pos:%s(%d)", err::error2str(ack), ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_logic_point", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_MIN_ARGC(2); + + int id = atoi(argv[1]); + int32_t logic_point_id = atoi(argv[2]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->move_to_logic_point(logic_point_id, [](int32_t status) { ZLOGI(TAG, "move to logic point exec end status:%s(%d)", err::error2str(status), status); }); + ZLOGI(TAG, "move to logic point:%s(%d)", err::error2str(ack), ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_set_logic_point", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_MIN_ARGC(6); + + int id = atoi(argv[1]); + int32_t logic_point_id = atoi(argv[2]); + int32_t x = atoi(argv[3]); + int32_t vel = atoi(argv[4]); + int32_t acc = atoi(argv[5]); + int32_t dec = atoi(argv[6]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->set_logic_point(logic_point_id, x, vel, acc, dec); + ZLOGI(TAG, "set logic point:%s(%d)", err::error2str(ack), ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_set_logic_point_simplify", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_MIN_ARGC(2); + + int id = atoi(argv[1]); + int32_t logic_point_id = atoi(argv[2]); + int32_t x = 0; + int32_t vel = 0; + int32_t acc = 0; + int32_t dec = 0; + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + x = module->read_pos(); + + int32_t ack = module->set_logic_point(logic_point_id, x, vel, acc, dec); + ZLOGI(TAG, "set logic point:%s(%d)", err::error2str(ack), ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_get_logic_point", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_MIN_ARGC(2); + + int id = atoi(argv[1]); + int32_t logic_point_id = atoi(argv[2]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + I_StepMotorCtrlModule::logic_point_t logic_point; + int32_t ack = module->get_logic_point(logic_point_id, logic_point); + if (ack != 0) { + ZLOGI(TAG, "get logic point:%s(%d)", err::error2str(ack), ack); + return; + } + + ZLOGI(TAG, "logic_point_id :%d", logic_point.index); + ZLOGI(TAG, "logic_point.x :%d", logic_point.x); + ZLOGI(TAG, "logic_point.vel :%d", logic_point.velocity); + ZLOGI(TAG, "logic_point.acc :%d", logic_point.acc); + ZLOGI(TAG, "logic_point.dec :%d", logic_point.dec); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_flush", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + + int id = atoi(argv[1]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->flush(); + ZLOGI(TAG, "flush:%s(%d)", err::error2str(ack), ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_factory_reset", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + + int id = atoi(argv[1]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->factory_reset(); + ZLOGI(TAG, "factory reset:%s(%d)", err::error2str(ack), ack); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_read_version", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + + int id = atoi(argv[1]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + I_StepMotorCtrlModule::version_t version; + int32_t ack = module->read_version(version); + if (ack != 0) { + ZLOGI(TAG, "read version:%s(%d)", err::error2str(ack), ack); + return; + } + + ZLOGI(TAG, "version.version:%d", version.version); + }); + +#define GET_BIT(x, n) ((x >> n) & 0x01) + + m_cmdScheduler->registerCmd("step_motor_ctrl_read_status", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + + int id = atoi(argv[1]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + I_StepMotorCtrlModule::status_t status; + int32_t ack = module->read_status(status); + if (ack != 0) { + ZLOGI(TAG, "read status:%s(%d)", err::error2str(ack), ack); + return; + } + + ZLOGI(TAG, "status.status :%d", status.status); + ZLOGI(TAG, "status.io_state :%d,%d,%d,%d,%d,%d,%d,%d", GET_BIT(status.io_state, 0), GET_BIT(status.io_state, 1), GET_BIT(status.io_state, 2), GET_BIT(status.io_state, 3), GET_BIT(status.io_state, 4), GET_BIT(status.io_state, 5), GET_BIT(status.io_state, 6), GET_BIT(status.io_state, 7)); + ZLOGI(TAG, "status.x :%d", status.x); + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_set_base_param", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_MIN_ARGC(4); + + int id = atoi(argv[1]); + const char* paramName = argv[2]; + int32_t value = atoi(argv[3]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + I_StepMotorCtrlModule::base_param_t param; + int32_t ack = module->get_base_param(param); + if (ack != 0) { + ZLOGE(TAG, "get_base_param failed:%d", ack); + return; + } + if (streq(paramName, "x_shaft")) { + param.x_shaft = value; + } else if (streq(paramName, "distance_scale")) { + param.distance_scale = value; + } else if (streq(paramName, "ihold")) { + param.ihold = value; + } else if (streq(paramName, "irun")) { + param.irun = value; + } else if (streq(paramName, "iholddelay")) { + param.iholddelay = value; + } else if (streq(paramName, "acc")) { + param.acc = value; + } else if (streq(paramName, "dec")) { + param.dec = value; + } else if (streq(paramName, "maxspeed")) { + param.maxspeed = value; + } else if (streq(paramName, "min_x")) { + param.min_x = value; + } else if (streq(paramName, "max_x")) { + param.max_x = value; + } else if (streq(paramName, "shift_x")) { + param.shift_x = value; + } else if (streq(paramName, "run_to_zero_move_to_zero_max_d")) { + param.run_to_zero_move_to_zero_max_d = value; + } else if (streq(paramName, "run_to_zero_leave_from_zero_max_d")) { + param.run_to_zero_leave_from_zero_max_d = value; + } else if (streq(paramName, "run_to_zero_speed")) { + param.run_to_zero_speed = value; + } else if (streq(paramName, "run_to_zero_dec")) { + param.run_to_zero_dec = value; + } else { + ZLOGE(TAG, "invalid param name:%s", paramName); + return; + } + ack = module->set_base_param(param); + if (ack != 0) { + ZLOGE(TAG, "set_base_param failed:%s(%d)", err::error2str(ack), ack); + return; + } + ZLOGI(TAG, "set_base_param ok"); + return; + }); + + m_cmdScheduler->registerCmd("step_motor_ctrl_get_base_param", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + CHECK_ARGC(1); + + int id = atoi(argv[1]); + + I_StepMotorCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + I_StepMotorCtrlModule::base_param_t param; + int32_t ack = module->get_base_param(param); + if (ack != 0) { + ZLOGE(TAG, "get_base_param failed:%d", ack); + return; + } + + ZLOGI(TAG, "param.x_shaft :%d", param.x_shaft); + ZLOGI(TAG, "param.distance_scale :%d", param.distance_scale); + ZLOGI(TAG, "param.ihold :%d", param.ihold); + ZLOGI(TAG, "param.irun :%d", param.irun); + ZLOGI(TAG, "param.iholddelay :%d", param.iholddelay); + ZLOGI(TAG, "param.acc :%d", param.acc); + ZLOGI(TAG, "param.dec :%d", param.dec); + ZLOGI(TAG, "param.maxspeed :%d", param.maxspeed); + ZLOGI(TAG, "param.min_x :%d", param.min_x); + ZLOGI(TAG, "param.max_x :%d", param.max_x); + ZLOGI(TAG, "param.shift_x :%d", param.shift_x); + ZLOGI(TAG, "param.run_to_zero_move_to_zero_max_d:%d", param.run_to_zero_move_to_zero_max_d); + ZLOGI(TAG, "param.run_to_zero_leave_from_zero_max_d:%d", param.run_to_zero_leave_from_zero_max_d); + ZLOGI(TAG, "param.run_to_zero_speed :%d", param.run_to_zero_speed); + ZLOGI(TAG, "param.run_to_zero_dec :%d", param.run_to_zero_dec); + }); +} \ No newline at end of file diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.hpp b/components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.hpp new file mode 100644 index 0000000..a48d835 --- /dev/null +++ b/components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.hpp @@ -0,0 +1,22 @@ +#pragma once +#include + +#include "sdk/os/zos.hpp" +#include "sdk\components\cmdscheduler\cmd_scheduler.hpp" +#include "sdk\components\zprotocols\zcancmder\api\i_step_motor_ctrl_module.hpp" +namespace iflytop { + +class StepMotorCtrlScriptCmderModule { + CmdScheduler* m_cmdScheduler; + map moduler; + + public: + void initialize(CmdScheduler* cmdScheduler); + void regmodule(int id, I_StepMotorCtrlModule* robot); + void regcmd(); + + private: + I_StepMotorCtrlModule* findXYRobot(int id); +}; + +} // namespace iflytop \ No newline at end of file diff --git a/components/step_motor_ctrl_module/zcan_master_step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/zcan_master_step_motor_ctrl_module.cpp new file mode 100644 index 0000000..ff1a0ee --- /dev/null +++ b/components/step_motor_ctrl_module/zcan_master_step_motor_ctrl_module.cpp @@ -0,0 +1,164 @@ +#include "sdk\components\zprotocols\errorcode\errorcode.hpp" +#include "zcan_master_step_motor_ctrl_module.hpp" + +using namespace iflytop; + +#define TAG "StepMotorCtrlModule" + +#define OVERTIME 30 + +namespace iflytop { +namespace zcancmder_master { + +class StepMotorCtrlModule : public I_StepMotorCtrlModule { + int m_id = 0; + uint8_t m_txbuf[128] = {0}; + ZCanCommnaderMaster* m_cancmder; + map indexcache; + + public: + void initialize(ZCanCommnaderMaster* cancmder, int id) { + m_cancmder = cancmder; + m_id = id; + } + uint16_t& getindexcache(int cmdid) { + auto it = indexcache.find(cmdid); + if (it == indexcache.end()) { + indexcache[cmdid] = 0; + return indexcache[cmdid]; + } else { + return it->second; + } + } + virtual int32_t move_to(int32_t x, action_cb_status_t status_cb) { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_to, OVERTIME, cmd->x = x;); + } + virtual int32_t move_by(int32_t dx, action_cb_status_t status_cb) { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_by, OVERTIME, cmd->dx = dx;); + } + virtual int32_t move_to(int32_t x, int32_t velocity, action_cb_status_t status_cb) { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_to, OVERTIME, cmd->x = x; cmd->speed = velocity;); + } + virtual int32_t move_by(int32_t dx, int32_t velocity, action_cb_status_t status_cb) { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_by, OVERTIME, cmd->dx = dx; cmd->speed = velocity;); + } + virtual int32_t move_to_zero(action_cb_status_t status_cb) { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_to_zero, OVERTIME); + } + virtual int32_t move_to_zero_with_calibrate(int32_t x, action_cb_status_t status_cb) { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_to_zero_with_calibrate, OVERTIME, cmd->x = x;); + } + virtual int32_t rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb) { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_rotate, OVERTIME, cmd->speed = speed; cmd->run_time = lastforms;); + } + virtual int32_t enable(bool venable) { // + ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_enable, OVERTIME, cmd->enable = venable;); + } + virtual int32_t stop(uint8_t stopType) { // + ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_stop, OVERTIME, cmd->stop_type = stopType;); + } + virtual int32_t force_change_current_pos(int32_t x) { // + ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_force_change_current_pos, OVERTIME, cmd->x = x;); + } + + virtual int32_t move_to_logic_point(int32_t logic_point_num, action_cb_status_t status_cb) { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_to_logic_point, OVERTIME, cmd->logic_point_num = logic_point_num;); + } + virtual int32_t set_logic_point(int logic_point_num, int32_t x, int32_t vel, s32 acc, s32 dec) { // + ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_set_logic_point, OVERTIME, cmd->index = logic_point_num; cmd->x = x; cmd->velocity = vel; cmd->acc = acc; cmd->dec = dec;); + } + virtual int32_t get_logic_point(int logic_point_num, logic_point_t& logic_point) { // + ZCAN_SEND_CMD(kcmd_step_motor_ctrl_get_logic_point, logic_point, OVERTIME, cmd->logic_point_num = logic_point_num;); + } + + virtual int32_t flush() { // + ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_flush, OVERTIME); + } + virtual int32_t factory_reset() { // + ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_factory_reset, OVERTIME); + } + + virtual int32_t read_version(version_t& version) { // + ZCAN_SEND_CMD(kcmd_step_motor_ctrl_read_version, version, OVERTIME); + } + virtual int32_t read_status(status_t& status) { // + ZCAN_SEND_CMD(kcmd_step_motor_ctrl_read_status, status, OVERTIME); + } + virtual int32_t read_detailed_status(detailed_status_t& debug_info) { // + ZCAN_SEND_CMD(kcmd_step_motor_ctrl_read_detailed_status, debug_info, OVERTIME); + } + virtual int32_t set_base_param(const base_param_t& param) { // + ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_set_base_param, OVERTIME, cmd->param = param;); + } + virtual int32_t get_base_param(base_param_t& param) { // + ZCAN_SEND_CMD(kcmd_step_motor_ctrl_get_base_param, param, OVERTIME); + } + + virtual bool isbusy() { // + status_t status; + int32_t err = read_status(status); + if (err != 0) { + return status.status != 0; + } + return false; + } + virtual int32_t get_last_exec_status() { // + detailed_status_t status; + int32_t err = read_detailed_status(status); + if (err != 0) { + return status.last_exec_status; + } + return err; + } + + virtual int32_t read_pos(int32_t& pos) { // + status_t status; + int32_t err = read_status(status); + if (err != 0) { + pos = status.x; + } + return err; + } + virtual int32_t read_pos() { // + int32_t pos = 0; + read_pos(pos); + return pos; + } + + virtual bool read_zero_io_state() { // + status_t status; + int32_t err = read_status(status); + if (err != 0) { + return status.io_state & 0x01; + } + return 0; + } + + virtual int32_t move_to_block(int32_t tox, int overtime) { // + return err::kce_overtime; + } + virtual int32_t move_by_block(int32_t dx, int overtime) { // + return err::kce_overtime; + } + virtual int32_t move_to_block(int32_t tox, int32_t velocity, int overtime) { // + return err::kce_overtime; + } + virtual int32_t move_by_block(int32_t dx, int32_t velocity, int overtime) { // + return err::kce_overtime; + } + virtual int32_t move_to_zero_block(int overtime) { // + return err::kce_overtime; + } + virtual int32_t move_to_zero_with_calibrate_block(int32_t x, int overtime) { // + return err::kce_overtime; + } +}; + +I_StepMotorCtrlModule* create_zcan_step_motor_ctrl_module(ZCanCommnaderMaster* cancmder, int id) { + StepMotorCtrlModule* module = new StepMotorCtrlModule(); + ZASSERT(module); + module->initialize(cancmder, id); + return module; +} +} // namespace zcancmder_master +} // namespace iflytop diff --git a/components/step_motor_ctrl_module/zcan_master_step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/zcan_master_step_motor_ctrl_module.hpp new file mode 100644 index 0000000..6ecda3b --- /dev/null +++ b/components/step_motor_ctrl_module/zcan_master_step_motor_ctrl_module.hpp @@ -0,0 +1,10 @@ +#pragma once +#include "sdk\components\zcancmder\zcanreceiver_master.hpp" +#include "sdk\components\zprotocols\zcancmder\api\i_step_motor_ctrl_module.hpp" +#if 1 +namespace iflytop { +namespace zcancmder_master { +I_StepMotorCtrlModule* create_zcan_master_step_motor_ctrl_module(ZCanCommnaderMaster* cancmder, int id); +} // namespace zcancmder_master +} // namespace iflytop +#endif \ No newline at end of file diff --git a/components/step_motor_ctrl_module/zcan_step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/zcan_step_motor_ctrl_module.cpp new file mode 100644 index 0000000..50b49b6 --- /dev/null +++ b/components/step_motor_ctrl_module/zcan_step_motor_ctrl_module.cpp @@ -0,0 +1,92 @@ +#include "zcan_step_motor_ctrl_module.hpp" +using namespace iflytop; + +#define TAG "ZCanStepMotorCtrlModule" +#if 1 +void ZCanStepMotorCtrlModule::initialize(ZCanCmder* cancmder, int id, I_StepMotorCtrlModule* module) { + m_cancmder = cancmder; + m_id = id; + m_module = module; + // m_lock.init(); + cancmder->registerListener(this); +} +void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { + // zlock_guard l(m_lock); + ZLOGI(TAG, "onRceivePacket %d %d", rxcmd->get_cmdheader()->cmdid, rxcmd->get_cmdheader()->subcmdid); + + PROCESS_PACKET(kcmd_step_motor_ctrl_enable, m_id) { errorcode = m_module->enable(cmd->enable); } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_stop, m_id) { errorcode = m_module->stop(cmd->stop_type); } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_move_to_zero, m_id) { + errorcode = m_module->move_to_zero([this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_to_zero); }); + } + END_PP(); + PROCESS_PACKET(kcmd_step_motor_ctrl_move_to_zero_with_calibrate, m_id) { + errorcode = m_module->move_to_zero_with_calibrate(cmd->x, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_to_zero_with_calibrate); }); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_move_to, m_id) { + errorcode = m_module->move_to(cmd->x, cmd->speed, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_to); }); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_move_by, m_id) { + errorcode = m_module->move_by(cmd->dx, cmd->speed, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_by); }); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_force_change_current_pos, m_id) { errorcode = m_module->force_change_current_pos(cmd->x); } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_rotate, m_id) { + errorcode = m_module->rotate(cmd->speed, cmd->run_time, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_rotate); }); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_read_version, m_id) { // + errorcode = m_module->read_version(ack->ack); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_read_status, m_id) { // + errorcode = m_module->read_status(ack->ack); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_read_detailed_status, m_id) { // + errorcode = m_module->read_detailed_status(ack->ack); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_set_base_param, m_id) { // + errorcode = m_module->set_base_param(cmd->param); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_get_base_param, m_id) { // + errorcode = m_module->get_base_param(ack->ack); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_move_to_logic_point, m_id) { + errorcode = m_module->move_to_logic_point(cmd->logic_point_num, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_to_logic_point); }); + } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_set_logic_point, m_id) { errorcode = m_module->set_logic_point(cmd->index, cmd->x, cmd->velocity, cmd->acc, cmd->dec); } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_get_logic_point, m_id) { errorcode = m_module->get_logic_point(cmd->logic_point_num, ack->ack); } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_flush, m_id) { errorcode = m_module->flush(); } + END_PP(); + + PROCESS_PACKET(kcmd_step_motor_ctrl_factory_reset, m_id) { errorcode = m_module->factory_reset(); } + END_PP(); +} +#endif \ No newline at end of file diff --git a/components/step_motor_ctrl_module/zcan_step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/zcan_step_motor_ctrl_module.hpp new file mode 100644 index 0000000..09a39f3 --- /dev/null +++ b/components/step_motor_ctrl_module/zcan_step_motor_ctrl_module.hpp @@ -0,0 +1,17 @@ +#pragma once +#include "sdk/components/zprotocols/zcancmder/api/i_step_motor_ctrl_module.hpp" +#include "sdk\components\zcancmder\zcanreceiver.hpp" +namespace iflytop { +class ZCanStepMotorCtrlModule : public ZCanCmderListener { + ZCanCmder* m_cancmder = nullptr; + int m_id = 0; + I_StepMotorCtrlModule* m_module; + + uint8_t m_txbuf[128] = {0}; + // zmutex m_lock; + + public: + void initialize(ZCanCmder* cancmder, int id, I_StepMotorCtrlModule* m_module); + virtual void onRceivePacket(CanPacketRxBuffer* rxcmd); +}; +} // namespace iflytop 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 new file mode 100644 index 0000000..5ed0bb1 --- /dev/null +++ b/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp @@ -0,0 +1,338 @@ +#include "xy_robot_script_cmder_module.hpp" + +#include + +#include "sdk\components\zprotocols\zcancmder\zcancmder_protocol.hpp" +using namespace iflytop; +#define TAG "XYRobotScriptCmderModule" + +#define CHECK_ARGC(n) \ + if (argc != (n + 1)) { \ + ZLOGE(TAG, "argc != %d", n); \ + context->breakflag = true; \ + return; \ + } + +#define SCRIPT_CMDER_CHECK(exptr) \ + if (!(exptr)) { \ + ZLOGE(TAG, "check %s failed", #exptr); \ + context->breakflag = true; \ + return; \ + } + +void XYRobotScriptCmderModule::initialize(CmdScheduler* cmdScheduler) { + m_cmdScheduler = cmdScheduler; + ZASSERT(m_cmdScheduler != nullptr); + regcmd(); +} + +I_XYRobotCtrlModule* XYRobotScriptCmderModule::findXYRobot(int id) { + auto it = moduler.find(id); + if (it == moduler.end()) { + return nullptr; + } + return it->second; +} + +static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; } + +void XYRobotScriptCmderModule::regXYRobot(int id, I_XYRobotCtrlModule* robot) { + ZASSERT(moduler.find(id) == moduler.end()); + ZASSERT(robot != nullptr); + moduler[id] = robot; +} +void XYRobotScriptCmderModule::regcmd() { // + ZASSERT(m_cmdScheduler != nullptr); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_enable", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 3); + + int id = atoi(argv[1]); + bool enable = atoi(argv[2]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->enable(enable); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_stop", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 3); + + int id = atoi(argv[1]); + uint8_t stopType = atoi(argv[2]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->stop(stopType); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_move_to_zero", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 2); + + int id = atoi(argv[1]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->move_to_zero([this](int32_t status) { ZLOGI(TAG, "move_to_zero status:%d", status); }); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_move_to_zero_with_calibrate", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 4); + + int32_t id = atoi(argv[1]); + int32_t nowx = atoi(argv[2]); + int32_t nowy = atoi(argv[3]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->move_to_zero_with_calibrate(nowx, nowy, [this](int32_t status) { ZLOGI(TAG, "move_to_zero_with_calibrate status:%d", status); }); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_move_to", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 5); + + int32_t id = atoi(argv[1]); + int32_t x = atoi(argv[2]); + int32_t y = atoi(argv[3]); + int32_t speed = atoi(argv[4]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->move_to(x, y, speed, [this](int32_t status) { ZLOGI(TAG, "move_to status:%d", status); }); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_move_by", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 5); + + int32_t id = atoi(argv[1]); + int32_t dx = atoi(argv[2]); + int32_t dy = atoi(argv[3]); + int32_t v = atoi(argv[4]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->move_by(dx, dy, v, [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); }); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_force_change_current_pos", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 4); + + int32_t id = atoi(argv[1]); + int32_t x = atoi(argv[2]); + int32_t y = atoi(argv[3]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->force_change_current_pos(x, y); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_read_version", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 2); + + int32_t id = atoi(argv[1]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + I_XYRobotCtrlModule::version_t version; + int32_t ack = module->read_version(version); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + if (ack == 0) { + ZLOGI(TAG, "version:%d", version.version); + } + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_read_status", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 2); + + int32_t id = atoi(argv[1]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + I_XYRobotCtrlModule::status_t status; + int32_t ack = module->read_status(status); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + if (ack == 0) { + ZLOGI(TAG, " status:%d", status.status); + ZLOGI(TAG, " x:%d", status.x); + ZLOGI(TAG, " y:%d", status.y); + ZLOGI(TAG, " iostate:%d", status.iostate); + } + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_read_detailed_status", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 2); + + int32_t id = atoi(argv[1]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + I_XYRobotCtrlModule::detailed_status_t status; + int32_t ack = module->read_detailed_status(status); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + if (ack == 0) { + ZLOGI(TAG, " status:%d", status.status); + ZLOGI(TAG, " x :%d", status.x); + ZLOGI(TAG, " y :%d", status.y); + ZLOGI(TAG, " iostate:%d", status.iostate); + } + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_set_base_param", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 4); + int32_t id = atoi(argv[1]); + const char* paramName = argv[2]; + int32_t paramVal = atoi(argv[3]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + I_XYRobotCtrlModule::base_param_t status; + int32_t ack = module->get_base_param(status); + if (ack != 0) { + ZLOGE(TAG, "get_base_param failed:%d", ack); + return; + } + + if (streq(paramName, "robot_type")) { + status.robot_type = paramVal; + } else if (streq(paramName, "x_shaft")) { + status.x_shaft = paramVal; + } else if (streq(paramName, "y_shaft")) { + status.y_shaft = paramVal; + } else if (streq(paramName, "ihold")) { + status.ihold = paramVal; + } else if (streq(paramName, "irun")) { + status.irun = paramVal; + } else if (streq(paramName, "iholddelay")) { + status.iholddelay = paramVal; + } else if (streq(paramName, "distance_scale")) { + status.distance_scale = paramVal; + } else if (streq(paramName, "shift_x")) { + status.shift_x = paramVal; + } else if (streq(paramName, "shift_y")) { + status.shift_y = paramVal; + } else if (streq(paramName, "acc")) { + status.acc = paramVal; + } else if (streq(paramName, "dec")) { + status.dec = paramVal; + } else if (streq(paramName, "breakdec")) { + status.breakdec = paramVal; + } else if (streq(paramName, "maxspeed")) { + status.maxspeed = paramVal; + } else if (streq(paramName, "min_x")) { + status.min_x = paramVal; + } else if (streq(paramName, "max_x")) { + status.max_x = paramVal; + } else if (streq(paramName, "min_y")) { + status.min_y = paramVal; + } else if (streq(paramName, "max_y")) { + 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 { + ZLOGE(TAG, "invalid param name:%s", paramName); + return; + } + ack = module->set_base_param(status); + if (ack != 0) { + ZLOGE(TAG, "set_base_param failed:%d,%s", ack, err::error2str(ack)); + return; + } + ZLOGI(TAG, "set_base_param ok"); + return; + }); + m_cmdScheduler->registerCmd("xy_robot_ctrl_get_base_param", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 2); + + int32_t id = atoi(argv[1]); + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + I_XYRobotCtrlModule::base_param_t status; + int32_t ack = module->get_base_param(status); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + if (ack == 0) { + ZLOGI(TAG, " robot_type :%d", status.robot_type); + ZLOGI(TAG, " x_shaft :%d", status.x_shaft); + ZLOGI(TAG, " y_shaft :%d", status.y_shaft); + ZLOGI(TAG, " ihold :%d", status.ihold); + ZLOGI(TAG, " irun :%d", status.irun); + ZLOGI(TAG, " iholddelay :%d", status.iholddelay); + ZLOGI(TAG, " distance_scale :%d", status.distance_scale); + ZLOGI(TAG, " shift_x :%d", status.shift_x); + ZLOGI(TAG, " shift_y :%d", status.shift_y); + ZLOGI(TAG, " acc :%d", status.acc); + ZLOGI(TAG, " dec :%d", status.dec); + ZLOGI(TAG, " breakdec :%d", status.breakdec); + ZLOGI(TAG, " maxspeed :%d", status.maxspeed); + ZLOGI(TAG, " min_x :%d", status.min_x); + ZLOGI(TAG, " max_x :%d", status.max_x); + ZLOGI(TAG, " min_y :%d", status.min_y); + ZLOGI(TAG, " max_y :%d", status.max_y); + ZLOGI(TAG, " run_to_zero_max_d :%d", status.run_to_zero_max_d); + ZLOGI(TAG, " leave_from_zero_max_d :%d", status.leave_from_zero_max_d); + ZLOGI(TAG, " run_to_zero_speed :%d", status.run_to_zero_speed); + ZLOGI(TAG, " run_to_zero_dec :%d", status.run_to_zero_dec); + } + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_flush", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 2); + + int32_t id = atoi(argv[1]); + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->flush(); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_factory_reset", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 2); + + int32_t id = atoi(argv[1]); + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->factory_reset(); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); + + m_cmdScheduler->registerCmd("xy_robot_ctrl_move_by_no_limit", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { + SCRIPT_CMDER_CHECK(argc == 5); + + int32_t id = atoi(argv[1]); + int32_t dx = atoi(argv[2]); + int32_t dy = atoi(argv[3]); + int32_t v = atoi(argv[4]); + + I_XYRobotCtrlModule* module = findXYRobot(id); + SCRIPT_CMDER_CHECK(module != nullptr); + + int32_t ack = module->move_by_no_limit(dx, dy, v, [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); }); + ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack)); + }); +} \ No newline at end of file diff --git a/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.hpp b/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.hpp new file mode 100644 index 0000000..fbac794 --- /dev/null +++ b/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.hpp @@ -0,0 +1,22 @@ +#pragma once +#include + +#include "sdk/os/zos.hpp" +#include "sdk\components\cmdscheduler\cmd_scheduler.hpp" +#include "sdk\components\zprotocols\zcancmder\api\i_xyrobot_ctrl_module.hpp" +namespace iflytop { + +class XYRobotScriptCmderModule { + CmdScheduler* m_cmdScheduler; + map moduler; + + public: + void initialize(CmdScheduler* cmdScheduler); + void regXYRobot(int id, I_XYRobotCtrlModule* robot); + void regcmd(); + + private: + I_XYRobotCtrlModule* findXYRobot(int id); +}; + +} // namespace iflytop \ No newline at end of file diff --git a/components/xy_robot_ctrl_module/zcan_xy_robot_master_module.cpp b/components/xy_robot_ctrl_module/zcan_xy_robot_master_module.cpp new file mode 100644 index 0000000..3e0d268 --- /dev/null +++ b/components/xy_robot_ctrl_module/zcan_xy_robot_master_module.cpp @@ -0,0 +1,100 @@ +#include "zcan_xy_robot_master_module.hpp" +using namespace iflytop; + +#define TAG "ZCANXYRobotCtrlMasterModule" + +#define OVERTIME 30 + +namespace iflytop { +namespace zcancmder_master { + +class ZCANXYRobotCtrlMasterModule : public I_XYRobotCtrlModule { + int m_id = 0; + uint8_t m_txbuf[128] = {0}; + ZCanCommnaderMaster* m_cancmder; + map indexcache; + + public: + void initialize(ZCanCommnaderMaster* cancmder, int id) { + m_cancmder = cancmder; + m_id = id; + } + uint16_t& getindexcache(int cmdid) { + auto it = indexcache.find(cmdid); + if (it == indexcache.end()) { + indexcache[cmdid] = 0; + return indexcache[cmdid]; + } else { + return it->second; + } + } + + virtual int32_t move_to(int32_t x, int32_t y, int speed, action_cb_status_t status_cb) override { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_to, OVERTIME, cmd->x = x; cmd->y = y; cmd->speed = speed;); + } + + virtual int32_t move_by(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_by, OVERTIME, cmd->dx = dx; cmd->dy = dy; cmd->speed = speed;); + } + virtual int32_t move_by_no_limit(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_by_no_limit, OVERTIME, cmd->dx = dx; cmd->dy = dy; cmd->speed = speed;); + } + + virtual int32_t move_to_zero(action_cb_status_t status_cb) override { + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_to_zero, OVERTIME); // + } + + virtual int32_t move_to_zero_with_calibrate(int32_t nowx, int32_t nowxy, action_cb_status_t status_cb) override { // + ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, OVERTIME, cmd->nowx = nowx; cmd->nowy = nowxy;); + } + + virtual int32_t enable(bool venable) override { // + ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_enable, OVERTIME, cmd->enable = venable;); + } + virtual int32_t stop(uint8_t stop_type) override { // + ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_stop, OVERTIME, cmd->stop_type = stop_type;); + } + virtual int32_t force_change_current_pos(int32_t x, int32_t y) override { // + ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_force_change_current_pos, OVERTIME, cmd->x = x; cmd->y = y;); + } + virtual int32_t read_version(version_t& version) override { // + ZCAN_SEND_CMD(kcmd_xy_robot_ctrl_read_version, version, OVERTIME); + } + virtual int32_t read_status(status_t& status) override { // + // ZCAN_SEND_CMD(kcmd_xy_robot_ctrl_read_status, status, OVERTIME); + + auto* cmdheader = (Cmdheader_t*)m_txbuf; + auto* cmd = (kcmd_xy_robot_ctrl_read_status_cmd_t*)cmdheader->data; + cmdheader->cmdid = (kcmd_xy_robot_ctrl_read_status >> 8); + cmdheader->subcmdid = (kcmd_xy_robot_ctrl_read_status & 0xff); + cmdheader->packetType = zcr::kpt_cmd; + static_assert(sizeof(kcmd_xy_robot_ctrl_read_status_cmd_t) <= sizeof(m_txbuf), "m_txbuf is too small"); + cmd->id = m_id; + int32_t errorcode = m_cancmder->sendCmd((uint8_t*)m_txbuf, sizeof(*cmdheader) + sizeof(*cmd), (uint8_t*)&status, sizeof(status), 30); + return errorcode; + } + virtual int32_t read_detailed_status(detailed_status_t& debug_info) override { // + ZCAN_SEND_CMD(kcmd_xy_robot_ctrl_read_detailed_status, debug_info, OVERTIME); + } + virtual int32_t set_base_param(const base_param_t& param) override { // + ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_set_base_param, OVERTIME, cmd->param = param;); + } + virtual int32_t get_base_param(base_param_t& ack) override { // + ZCAN_SEND_CMD(kcmd_xy_robot_ctrl_get_base_param, ack, OVERTIME); + } + virtual int32_t flush() override { // + ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_flush, 3000); + } + virtual int32_t factory_reset() override { // + ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_factory_reset, 3000); + } +}; + +I_XYRobotCtrlModule* create_xyrobot_ctrl_module(ZCanCommnaderMaster* cancmder, int id) { + ZCANXYRobotCtrlMasterModule* module = new ZCANXYRobotCtrlMasterModule(); + ZASSERT(module); + module->initialize(cancmder, id); + return module; +} +} // namespace zcancmder_master +} // namespace iflytop \ No newline at end of file diff --git a/components/xy_robot_ctrl_module/zcan_xy_robot_master_module.hpp b/components/xy_robot_ctrl_module/zcan_xy_robot_master_module.hpp new file mode 100644 index 0000000..028119f --- /dev/null +++ b/components/xy_robot_ctrl_module/zcan_xy_robot_master_module.hpp @@ -0,0 +1,10 @@ +#pragma once +#include "sdk\components\zcancmder\zcanreceiver_master.hpp" +#include "sdk\components\zprotocols\zcancmder\api\i_xyrobot_ctrl_module.hpp" +#if 1 +namespace iflytop { +namespace zcancmder_master { +I_XYRobotCtrlModule* create_xyrobot_ctrl_module(ZCanCommnaderMaster* cancmder, int id); +} // namespace zcancmder_master +} // namespace iflytop +#endif \ No newline at end of file diff --git a/components/xy_robot_ctrl_module/zcan_xy_robot_module.cpp b/components/xy_robot_ctrl_module/zcan_xy_robot_module.cpp new file mode 100644 index 0000000..7ec3e58 --- /dev/null +++ b/components/xy_robot_ctrl_module/zcan_xy_robot_module.cpp @@ -0,0 +1,126 @@ +#include "zcan_xy_robot_module.hpp" +using namespace iflytop; + +#define TAG "ZCANXYRobotCtrlModule" +#if 1 +void ZCANXYRobotCtrlModule::initialize(ZCanCmder* cancmder, int id, I_XYRobotCtrlModule* xyRobotCtrlModule) { + m_cancmder = cancmder; + m_id = id; + m_xyRobotCtrlModule = xyRobotCtrlModule; + m_lock.init(); + cancmder->registerListener(this); +} +void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { + zlock_guard l(m_lock); + + PROCESS_PACKET(kcmd_xy_robot_ctrl_enable, m_id) { // + errorcode = m_xyRobotCtrlModule->enable(cmd->enable); + } + END_PP(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_stop, m_id) { // + errorcode = m_xyRobotCtrlModule->stop(cmd->stop_type); + } + END_PP(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_zero, m_id) { // + 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); + 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](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", status); + 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, cmd->speed, [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", status); + 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, cmd->speed, [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", status); + report.exec_status = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); + + PROCESS_PACKET(kcmd_xy_robot_ctrl_move_by_no_limit, m_id) { // + errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, cmd->speed, [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", status); + report.exec_status = status; + m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); + }); + } + END_PP(); + + PROCESS_PACKET(kcmd_xy_robot_ctrl_force_change_current_pos, m_id) { // + errorcode = m_xyRobotCtrlModule->force_change_current_pos(cmd->x, cmd->y); + } + END_PP(); + + PROCESS_PACKET(kcmd_xy_robot_ctrl_read_version, m_id) { // + errorcode = m_xyRobotCtrlModule->read_version(ack->ack); + } + END_PP(); + + PROCESS_PACKET(kcmd_xy_robot_ctrl_read_status, m_id) { // + errorcode = m_xyRobotCtrlModule->read_status(ack->ack); + } + END_PP(); + + PROCESS_PACKET(kcmd_xy_robot_ctrl_read_detailed_status, m_id) { // + errorcode = m_xyRobotCtrlModule->read_detailed_status(ack->ack); + } + END_PP(); + + 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_get_base_param, m_id) { // + errorcode = m_xyRobotCtrlModule->get_base_param(ack->ack); + } + END_PP(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_flush, m_id) { // + errorcode = m_xyRobotCtrlModule->flush(); + } + END_PP(); + PROCESS_PACKET(kcmd_xy_robot_ctrl_factory_reset, m_id) { // + errorcode = m_xyRobotCtrlModule->factory_reset(); + } + 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/xy_robot_ctrl_module/zcan_xy_robot_module.hpp b/components/xy_robot_ctrl_module/zcan_xy_robot_module.hpp new file mode 100644 index 0000000..c01a26f --- /dev/null +++ b/components/xy_robot_ctrl_module/zcan_xy_robot_module.hpp @@ -0,0 +1,19 @@ +#pragma once +#include "sdk\components\xy_robot_ctrl_module\xy_robot_ctrl_module.hpp" +#include "sdk\components\zcancmder\zcanreceiver.hpp" +#if 1 +namespace iflytop { +class ZCANXYRobotCtrlModule : public ZCanCmderListener { + ZCanCmder* m_cancmder = nullptr; + int m_id = 0; + I_XYRobotCtrlModule* m_xyRobotCtrlModule; + + uint8_t m_txbuf[128] = {0}; + zmutex m_lock; + + public: + void initialize(ZCanCmder* cancmder, int id, I_XYRobotCtrlModule* xyRobotCtrlModule); + virtual void onRceivePacket(CanPacketRxBuffer* rxcmd); +}; +} // namespace iflytop +#endif \ No newline at end of file diff --git a/components/zcancmder/zcanreceiver.cpp b/components/zcancmder/zcanreceiver.cpp index 1856651..771671d 100644 --- a/components/zcancmder/zcanreceiver.cpp +++ b/components/zcancmder/zcanreceiver.cpp @@ -11,7 +11,6 @@ using namespace zcr; #define OVER_TIME_MS 5 - ZCanCmder::CFG *ZCanCmder::createCFG(uint8_t deviceId) { CFG *cfg = new CFG(); ZASSERT(cfg != NULL); @@ -29,6 +28,7 @@ ZCanCmder::CFG *ZCanCmder::createCFG(uint8_t deviceId) { void ZCanCmder::init(CFG *cfg) { HAL_StatusTypeDef hal_status; m_config = cfg; + m_lock.init(); /** * @brief 初始化CAN @@ -136,6 +136,7 @@ void ZCanCmder::sendPacket(uint8_t *packet, size_t len) { } void ZCanCmder::sendAck(Cmdheader_t *cmdheader, uint8_t *data, size_t len) { + zlock_guard l(m_lock); Cmdheader_t *txheader = (Cmdheader_t *)txbuff; memcpy(txheader, cmdheader, sizeof(Cmdheader_t)); txheader->packetType = kpt_ack; @@ -143,6 +144,8 @@ void ZCanCmder::sendAck(Cmdheader_t *cmdheader, uint8_t *data, size_t len) { sendPacket(txbuff, sizeof(Cmdheader_t) + len); } void ZCanCmder::sendExecStatusReport(Cmdheader_t *rxcmdheader, uint8_t *data, size_t len) { + zlock_guard l(m_lock); + Cmdheader_t *txheader = (Cmdheader_t *)txbuff; memcpy(txheader, rxcmdheader, sizeof(Cmdheader_t)); txheader->packetType = kpt_cmd_exec_status_report; @@ -150,6 +153,8 @@ void ZCanCmder::sendExecStatusReport(Cmdheader_t *rxcmdheader, uint8_t *data, si sendPacket(txbuff, sizeof(Cmdheader_t) + len); } void ZCanCmder::sendStatusReport(Cmdheader_t *rxcmdheader, uint8_t *data, size_t len) { + zlock_guard l(m_lock); + Cmdheader_t *txheader = (Cmdheader_t *)txbuff; memcpy(txheader, rxcmdheader, sizeof(Cmdheader_t)); txheader->packetType = kpt_report; @@ -158,6 +163,8 @@ void ZCanCmder::sendStatusReport(Cmdheader_t *rxcmdheader, uint8_t *data, size_t } void ZCanCmder::sendErrorAck(Cmdheader_t *cmdheader, uint16_t id, uint32_t errcode) { + zlock_guard l(m_lock); + Cmdheader_t *txheader = (Cmdheader_t *)txbuff; memcpy(txheader, cmdheader, sizeof(Cmdheader_t)); txheader->packetType = kpt_error_ack; @@ -168,6 +175,8 @@ void ZCanCmder::sendErrorAck(Cmdheader_t *cmdheader, uint16_t id, uint32_t errco } bool ZCanCmder::sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems) { + zlock_guard l(m_lock); + // ZLOGI(TAG, "sendPacketSub(%d:%d)", npacket, packetIndex); CAN_TxHeaderTypeDef pHeader; uint8_t aData[8] /*8byte table*/; diff --git a/components/zcancmder/zcanreceiver.hpp b/components/zcancmder/zcanreceiver.hpp index e5f1be7..28bd6c5 100644 --- a/components/zcancmder/zcanreceiver.hpp +++ b/components/zcancmder/zcanreceiver.hpp @@ -3,9 +3,9 @@ // #pragma once +#include "basic.hpp" #include "sdk/os/zos.hpp" #include "sdk\components\zprotocols\zcancmder\zcancmder_protocol.hpp" -#include "basic.hpp" namespace iflytop { using namespace zcr; @@ -50,7 +50,8 @@ class ZCanCmder : public ZCanIRQListener { list m_listenerList2; CanPacketRxBuffer m_canPacketRxBuffer[1]; - int txPacketInterval_ms = 0; + int txPacketInterval_ms = 0; + zmutex m_lock; public: ZCanCmder() {} diff --git a/components/zcancmder_master_module/zcan_master_step_motor_ctrl_module.cpp b/components/zcancmder_master_module/zcan_master_step_motor_ctrl_module.cpp deleted file mode 100644 index af46241..0000000 --- a/components/zcancmder_master_module/zcan_master_step_motor_ctrl_module.cpp +++ /dev/null @@ -1,164 +0,0 @@ -#include "sdk\components\zprotocols\errorcode\errorcode.hpp" -#include "zcan_xy_robot_master_module.hpp" - -using namespace iflytop; - -#define TAG "StepMotorCtrlModule" - -#define OVERTIME 30 - -namespace iflytop { -namespace zcancmder_master { - -class StepMotorCtrlModule : public I_StepMotorCtrlModule { - int m_id = 0; - uint8_t m_txbuf[128] = {0}; - ZCanCommnaderMaster* m_cancmder; - map indexcache; - - public: - void initialize(ZCanCommnaderMaster* cancmder, int id) { - m_cancmder = cancmder; - m_id = id; - } - uint16_t& getindexcache(int cmdid) { - auto it = indexcache.find(cmdid); - if (it == indexcache.end()) { - indexcache[cmdid] = 0; - return indexcache[cmdid]; - } else { - return it->second; - } - } - virtual int32_t move_to(int32_t x, action_cb_status_t status_cb) { // - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_to, OVERTIME, cmd->x = x;); - } - virtual int32_t move_by(int32_t dx, action_cb_status_t status_cb) { // - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_by, OVERTIME, cmd->dx = dx;); - } - virtual int32_t move_to(int32_t x, int32_t velocity, action_cb_status_t status_cb) { // - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_to, OVERTIME, cmd->x = x; cmd->speed = velocity;); - } - virtual int32_t move_by(int32_t dx, int32_t velocity, action_cb_status_t status_cb) { // - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_by, OVERTIME, cmd->dx = dx; cmd->speed = velocity;); - } - virtual int32_t move_to_zero(action_cb_status_t status_cb) { // - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_to_zero, OVERTIME); - } - virtual int32_t move_to_zero_with_calibrate(int32_t x, action_cb_status_t status_cb) { // - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_to_zero_with_calibrate, OVERTIME, cmd->x = x;); - } - virtual int32_t rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb) { // - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_rotate, OVERTIME, cmd->speed = speed; cmd->run_time = lastforms;); - } - virtual int32_t enable(bool venable) { // - ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_enable, OVERTIME, cmd->enable = venable;); - } - virtual int32_t stop(uint8_t stopType) { // - ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_stop, OVERTIME, cmd->stop_type = stopType;); - } - virtual int32_t force_change_current_pos(int32_t x) { // - ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_force_change_current_pos, OVERTIME, cmd->x = x;); - } - - virtual int32_t move_to_logic_point(int32_t logic_point_num, action_cb_status_t status_cb) { // - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_step_motor_ctrl_move_to_logic_point, OVERTIME, cmd->logic_point_num = logic_point_num;); - } - virtual int32_t set_logic_point(int logic_point_num, int32_t x, int32_t vel, s32 acc, s32 dec) { // - ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_set_logic_point, OVERTIME, cmd->index = logic_point_num; cmd->x = x; cmd->velocity = vel; cmd->acc = acc; cmd->dec = dec;); - } - virtual int32_t get_logic_point(int logic_point_num, logic_point_t& logic_point) { // - ZCAN_SEND_CMD(kcmd_step_motor_ctrl_get_logic_point, logic_point, OVERTIME, cmd->logic_point_num = logic_point_num;); - } - - virtual int32_t flush() { // - ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_flush, OVERTIME); - } - virtual int32_t factory_reset() { // - ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_factory_reset, OVERTIME); - } - - virtual int32_t read_version(version_t& version) { // - ZCAN_SEND_CMD(kcmd_step_motor_ctrl_read_version, version, OVERTIME); - } - virtual int32_t read_status(status_t& status) { // - ZCAN_SEND_CMD(kcmd_step_motor_ctrl_read_status, status, OVERTIME); - } - virtual int32_t read_detailed_status(detailed_status_t& debug_info) { // - ZCAN_SEND_CMD(kcmd_step_motor_ctrl_read_detailed_status, debug_info, OVERTIME); - } - virtual int32_t set_base_param(const base_param_t& param) { // - ZCAN_SEND_CMD_NO_ACK(kcmd_step_motor_ctrl_set_base_param, OVERTIME, cmd->param = param;); - } - virtual int32_t get_base_param(base_param_t& param) { // - ZCAN_SEND_CMD(kcmd_step_motor_ctrl_get_base_param, param, OVERTIME); - } - - virtual bool isbusy() { // - status_t status; - int32_t err = read_status(status); - if (err != 0) { - return status.status != 0; - } - return false; - } - virtual int32_t get_last_exec_status() { // - detailed_status_t status; - int32_t err = read_detailed_status(status); - if (err != 0) { - return status.last_exec_status; - } - return err; - } - - virtual int32_t read_pos(int32_t& pos) { // - status_t status; - int32_t err = read_status(status); - if (err != 0) { - pos = status.x; - } - return err; - } - virtual int32_t read_pos() { // - int32_t pos = 0; - read_pos(pos); - return pos; - } - - virtual bool read_zero_io_state() { // - status_t status; - int32_t err = read_status(status); - if (err != 0) { - return status.io_state & 0x01; - } - return 0; - } - - virtual int32_t move_to_block(int32_t tox, int overtime) { // - return err::kce_overtime; - } - virtual int32_t move_by_block(int32_t dx, int overtime) { // - return err::kce_overtime; - } - virtual int32_t move_to_block(int32_t tox, int32_t velocity, int overtime) { // - return err::kce_overtime; - } - virtual int32_t move_by_block(int32_t dx, int32_t velocity, int overtime) { // - return err::kce_overtime; - } - virtual int32_t move_to_zero_block(int overtime) { // - return err::kce_overtime; - } - virtual int32_t move_to_zero_with_calibrate_block(int32_t x, int overtime) { // - return err::kce_overtime; - } -}; - -I_StepMotorCtrlModule* create_zcan_step_motor_ctrl_module(ZCanCommnaderMaster* cancmder, int id) { - StepMotorCtrlModule* module = new StepMotorCtrlModule(); - ZASSERT(module); - module->initialize(cancmder, id); - return module; -} -} // namespace zcancmder_master -} // namespace iflytop diff --git a/components/zcancmder_master_module/zcan_master_step_motor_ctrl_module.hpp b/components/zcancmder_master_module/zcan_master_step_motor_ctrl_module.hpp deleted file mode 100644 index 6ecda3b..0000000 --- a/components/zcancmder_master_module/zcan_master_step_motor_ctrl_module.hpp +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once -#include "sdk\components\zcancmder\zcanreceiver_master.hpp" -#include "sdk\components\zprotocols\zcancmder\api\i_step_motor_ctrl_module.hpp" -#if 1 -namespace iflytop { -namespace zcancmder_master { -I_StepMotorCtrlModule* create_zcan_master_step_motor_ctrl_module(ZCanCommnaderMaster* cancmder, int id); -} // namespace zcancmder_master -} // namespace iflytop -#endif \ No newline at end of file diff --git a/components/zcancmder_master_module/zcan_xy_robot_master_module.cpp b/components/zcancmder_master_module/zcan_xy_robot_master_module.cpp deleted file mode 100644 index 3e0d268..0000000 --- a/components/zcancmder_master_module/zcan_xy_robot_master_module.cpp +++ /dev/null @@ -1,100 +0,0 @@ -#include "zcan_xy_robot_master_module.hpp" -using namespace iflytop; - -#define TAG "ZCANXYRobotCtrlMasterModule" - -#define OVERTIME 30 - -namespace iflytop { -namespace zcancmder_master { - -class ZCANXYRobotCtrlMasterModule : public I_XYRobotCtrlModule { - int m_id = 0; - uint8_t m_txbuf[128] = {0}; - ZCanCommnaderMaster* m_cancmder; - map indexcache; - - public: - void initialize(ZCanCommnaderMaster* cancmder, int id) { - m_cancmder = cancmder; - m_id = id; - } - uint16_t& getindexcache(int cmdid) { - auto it = indexcache.find(cmdid); - if (it == indexcache.end()) { - indexcache[cmdid] = 0; - return indexcache[cmdid]; - } else { - return it->second; - } - } - - virtual int32_t move_to(int32_t x, int32_t y, int speed, action_cb_status_t status_cb) override { // - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_to, OVERTIME, cmd->x = x; cmd->y = y; cmd->speed = speed;); - } - - virtual int32_t move_by(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override { // - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_by, OVERTIME, cmd->dx = dx; cmd->dy = dy; cmd->speed = speed;); - } - virtual int32_t move_by_no_limit(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override { // - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_by_no_limit, OVERTIME, cmd->dx = dx; cmd->dy = dy; cmd->speed = speed;); - } - - virtual int32_t move_to_zero(action_cb_status_t status_cb) override { - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_to_zero, OVERTIME); // - } - - virtual int32_t move_to_zero_with_calibrate(int32_t nowx, int32_t nowxy, action_cb_status_t status_cb) override { // - ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_to_zero_with_calibrate, OVERTIME, cmd->nowx = nowx; cmd->nowy = nowxy;); - } - - virtual int32_t enable(bool venable) override { // - ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_enable, OVERTIME, cmd->enable = venable;); - } - virtual int32_t stop(uint8_t stop_type) override { // - ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_stop, OVERTIME, cmd->stop_type = stop_type;); - } - virtual int32_t force_change_current_pos(int32_t x, int32_t y) override { // - ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_force_change_current_pos, OVERTIME, cmd->x = x; cmd->y = y;); - } - virtual int32_t read_version(version_t& version) override { // - ZCAN_SEND_CMD(kcmd_xy_robot_ctrl_read_version, version, OVERTIME); - } - virtual int32_t read_status(status_t& status) override { // - // ZCAN_SEND_CMD(kcmd_xy_robot_ctrl_read_status, status, OVERTIME); - - auto* cmdheader = (Cmdheader_t*)m_txbuf; - auto* cmd = (kcmd_xy_robot_ctrl_read_status_cmd_t*)cmdheader->data; - cmdheader->cmdid = (kcmd_xy_robot_ctrl_read_status >> 8); - cmdheader->subcmdid = (kcmd_xy_robot_ctrl_read_status & 0xff); - cmdheader->packetType = zcr::kpt_cmd; - static_assert(sizeof(kcmd_xy_robot_ctrl_read_status_cmd_t) <= sizeof(m_txbuf), "m_txbuf is too small"); - cmd->id = m_id; - int32_t errorcode = m_cancmder->sendCmd((uint8_t*)m_txbuf, sizeof(*cmdheader) + sizeof(*cmd), (uint8_t*)&status, sizeof(status), 30); - return errorcode; - } - virtual int32_t read_detailed_status(detailed_status_t& debug_info) override { // - ZCAN_SEND_CMD(kcmd_xy_robot_ctrl_read_detailed_status, debug_info, OVERTIME); - } - virtual int32_t set_base_param(const base_param_t& param) override { // - ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_set_base_param, OVERTIME, cmd->param = param;); - } - virtual int32_t get_base_param(base_param_t& ack) override { // - ZCAN_SEND_CMD(kcmd_xy_robot_ctrl_get_base_param, ack, OVERTIME); - } - virtual int32_t flush() override { // - ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_flush, 3000); - } - virtual int32_t factory_reset() override { // - ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_factory_reset, 3000); - } -}; - -I_XYRobotCtrlModule* create_xyrobot_ctrl_module(ZCanCommnaderMaster* cancmder, int id) { - ZCANXYRobotCtrlMasterModule* module = new ZCANXYRobotCtrlMasterModule(); - ZASSERT(module); - module->initialize(cancmder, id); - return module; -} -} // namespace zcancmder_master -} // namespace iflytop \ No newline at end of file diff --git a/components/zcancmder_master_module/zcan_xy_robot_master_module.hpp b/components/zcancmder_master_module/zcan_xy_robot_master_module.hpp deleted file mode 100644 index 028119f..0000000 --- a/components/zcancmder_master_module/zcan_xy_robot_master_module.hpp +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once -#include "sdk\components\zcancmder\zcanreceiver_master.hpp" -#include "sdk\components\zprotocols\zcancmder\api\i_xyrobot_ctrl_module.hpp" -#if 1 -namespace iflytop { -namespace zcancmder_master { -I_XYRobotCtrlModule* create_xyrobot_ctrl_module(ZCanCommnaderMaster* cancmder, int id); -} // namespace zcancmder_master -} // namespace iflytop -#endif \ No newline at end of file diff --git a/components/zcancmder_module/zcan_eeprom_module.cpp b/components/zcancmder_module/zcan_eeprom_module.cpp index 58b8043..6243da6 100644 --- a/components/zcancmder_module/zcan_eeprom_module.cpp +++ b/components/zcancmder_module/zcan_eeprom_module.cpp @@ -11,11 +11,9 @@ void ZCanEepromModule::initialize(ZCanCmder* cancmder, int id, I_EEPROMModule* m cancmder->registerListener(this); } void ZCanEepromModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { // - zlock_guard l(m_lock); PROCESS_PACKET(kcmd_eeprom_start_monitor_status, m_id) { errorcode = m_module->start_monitor_status([this, cmdheader](I_EEPROMModule::eeprom_status_t& status) { // - zlock_guard l(m_lock); auto* report = (kcmd_eeprom_start_monitor_status_report_t*)m_txbuf; static_assert(sizeof(*report) < sizeof(m_txbuf), "report size too large"); diff --git a/components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp b/components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp index 6e9fb69..e546270 100644 --- a/components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp +++ b/components/zcancmder_module/zcan_mini_servo_ctrl_module.cpp @@ -11,7 +11,6 @@ void ZCanMiniServoCtrlModule::initialize(ZCanCmder* cancmder, int id, I_MiniServ } void ZCanMiniServoCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { - zlock_guard l(m_lock); PROCESS_PACKET(kcmd_mini_servo_ctrl_enable, m_id) { errorcode = m_module->enable(cmd->enable); } END_PP(); diff --git a/components/zcancmder_module/zcan_motor_laser_code_scanner_scan_module.cpp b/components/zcancmder_module/zcan_motor_laser_code_scanner_scan_module.cpp index 7135719..a8dc959 100644 --- a/components/zcancmder_module/zcan_motor_laser_code_scanner_scan_module.cpp +++ b/components/zcancmder_module/zcan_motor_laser_code_scanner_scan_module.cpp @@ -13,7 +13,6 @@ void ZcanMotorLaserCodeScannerScanModule::initialize(ZCanCmder* cancmder, int id cancmder->registerListener(this); } void ZcanMotorLaserCodeScannerScanModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { - zlock_guard l(m_lock); PROCESS_PACKET(kcmd_motor_laser_code_scanner_scan, m_id) { errorcode = m_module->start_scan( // diff --git a/components/zcancmder_module/zcan_pipette_module.cpp b/components/zcancmder_module/zcan_pipette_module.cpp index 9677f5d..92b818e 100644 --- a/components/zcancmder_module/zcan_pipette_module.cpp +++ b/components/zcancmder_module/zcan_pipette_module.cpp @@ -11,7 +11,6 @@ void ZcanPipetteModule::initialize(ZCanCmder* cancmder, int id, I_PipetteModule* cancmder->registerListener(this); } void ZcanPipetteModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { - zlock_guard l(m_lock); PROCESS_PACKET(kcmd_pipette_module_enable, m_id) { errorcode = m_module->enable(cmd->enable); } END_PP(); diff --git a/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp b/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp deleted file mode 100644 index 3622587..0000000 --- a/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include "zcan_step_motor_ctrl_module.hpp" -using namespace iflytop; - -#define TAG "ZCanStepMotorCtrlModule" -#if 1 -void ZCanStepMotorCtrlModule::initialize(ZCanCmder* cancmder, int id, I_StepMotorCtrlModule* module) { - m_cancmder = cancmder; - m_id = id; - m_module = module; - m_lock.init(); - cancmder->registerListener(this); -} -void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { - zlock_guard l(m_lock); - - PROCESS_PACKET(kcmd_step_motor_ctrl_enable, m_id) { errorcode = m_module->enable(cmd->enable); } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_stop, m_id) { errorcode = m_module->stop(cmd->stop_type); } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_move_to_zero, m_id) { - errorcode = m_module->move_to_zero([this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_to_zero); }); - } - END_PP(); - PROCESS_PACKET(kcmd_step_motor_ctrl_move_to_zero_with_calibrate, m_id) { - errorcode = m_module->move_to_zero_with_calibrate(cmd->x, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_to_zero_with_calibrate); }); - } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_move_to, m_id) { - errorcode = m_module->move_to(cmd->x, cmd->speed, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_to); }); - } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_move_by, m_id) { - errorcode = m_module->move_by(cmd->dx, cmd->speed, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_by); }); - } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_force_change_current_pos, m_id) { errorcode = m_module->force_change_current_pos(cmd->x); } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_rotate, m_id) { - errorcode = m_module->rotate(cmd->speed, cmd->run_time, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_rotate); }); - } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_read_version, m_id) { // - errorcode = m_module->read_version(ack->ack); - } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_read_status, m_id) { // - errorcode = m_module->read_status(ack->ack); - } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_read_detailed_status, m_id) { // - errorcode = m_module->read_detailed_status(ack->ack); - } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_set_base_param, m_id) { // - errorcode = m_module->set_base_param(cmd->param); - } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_get_base_param, m_id) { // - errorcode = m_module->get_base_param(ack->ack); - } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_move_to_logic_point, m_id) { - errorcode = m_module->move_to_logic_point(cmd->logic_point_num, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_to_logic_point); }); - } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_set_logic_point, m_id) { errorcode = m_module->set_logic_point(cmd->index, cmd->x, cmd->velocity, cmd->acc, cmd->dec); } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_get_logic_point, m_id) { errorcode = m_module->get_logic_point(cmd->logic_point_num, ack->ack); } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_flush, m_id) { errorcode = m_module->flush(); } - END_PP(); - - PROCESS_PACKET(kcmd_step_motor_ctrl_factory_reset, m_id) { errorcode = m_module->factory_reset(); } - END_PP(); -} -#endif \ No newline at end of file diff --git a/components/zcancmder_module/zcan_step_motor_ctrl_module.hpp b/components/zcancmder_module/zcan_step_motor_ctrl_module.hpp deleted file mode 100644 index f85882d..0000000 --- a/components/zcancmder_module/zcan_step_motor_ctrl_module.hpp +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once -#include "sdk/components/zprotocols/zcancmder/api/i_step_motor_ctrl_module.hpp" -#include "sdk\components\zcancmder\zcanreceiver.hpp" -namespace iflytop { -class ZCanStepMotorCtrlModule : public ZCanCmderListener { - ZCanCmder* m_cancmder = nullptr; - int m_id = 0; - I_StepMotorCtrlModule* m_module; - - uint8_t m_txbuf[128] = {0}; - zmutex m_lock; - - public: - void initialize(ZCanCmder* cancmder, int id, I_StepMotorCtrlModule* m_module); - virtual void onRceivePacket(CanPacketRxBuffer* rxcmd); -}; -} // namespace iflytop diff --git a/components/zcancmder_module/zcan_xy_robot_module.cpp b/components/zcancmder_module/zcan_xy_robot_module.cpp deleted file mode 100644 index 7ec3e58..0000000 --- a/components/zcancmder_module/zcan_xy_robot_module.cpp +++ /dev/null @@ -1,126 +0,0 @@ -#include "zcan_xy_robot_module.hpp" -using namespace iflytop; - -#define TAG "ZCANXYRobotCtrlModule" -#if 1 -void ZCANXYRobotCtrlModule::initialize(ZCanCmder* cancmder, int id, I_XYRobotCtrlModule* xyRobotCtrlModule) { - m_cancmder = cancmder; - m_id = id; - m_xyRobotCtrlModule = xyRobotCtrlModule; - m_lock.init(); - cancmder->registerListener(this); -} -void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { - zlock_guard l(m_lock); - - PROCESS_PACKET(kcmd_xy_robot_ctrl_enable, m_id) { // - errorcode = m_xyRobotCtrlModule->enable(cmd->enable); - } - END_PP(); - PROCESS_PACKET(kcmd_xy_robot_ctrl_stop, m_id) { // - errorcode = m_xyRobotCtrlModule->stop(cmd->stop_type); - } - END_PP(); - PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_zero, m_id) { // - 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); - 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](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", status); - 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, cmd->speed, [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", status); - 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, cmd->speed, [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", status); - report.exec_status = status; - m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); - }); - } - END_PP(); - - PROCESS_PACKET(kcmd_xy_robot_ctrl_move_by_no_limit, m_id) { // - errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, cmd->speed, [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", status); - report.exec_status = status; - m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); - }); - } - END_PP(); - - PROCESS_PACKET(kcmd_xy_robot_ctrl_force_change_current_pos, m_id) { // - errorcode = m_xyRobotCtrlModule->force_change_current_pos(cmd->x, cmd->y); - } - END_PP(); - - PROCESS_PACKET(kcmd_xy_robot_ctrl_read_version, m_id) { // - errorcode = m_xyRobotCtrlModule->read_version(ack->ack); - } - END_PP(); - - PROCESS_PACKET(kcmd_xy_robot_ctrl_read_status, m_id) { // - errorcode = m_xyRobotCtrlModule->read_status(ack->ack); - } - END_PP(); - - PROCESS_PACKET(kcmd_xy_robot_ctrl_read_detailed_status, m_id) { // - errorcode = m_xyRobotCtrlModule->read_detailed_status(ack->ack); - } - END_PP(); - - 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_get_base_param, m_id) { // - errorcode = m_xyRobotCtrlModule->get_base_param(ack->ack); - } - END_PP(); - PROCESS_PACKET(kcmd_xy_robot_ctrl_flush, m_id) { // - errorcode = m_xyRobotCtrlModule->flush(); - } - END_PP(); - PROCESS_PACKET(kcmd_xy_robot_ctrl_factory_reset, m_id) { // - errorcode = m_xyRobotCtrlModule->factory_reset(); - } - 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/zcancmder_module/zcan_xy_robot_module.hpp b/components/zcancmder_module/zcan_xy_robot_module.hpp deleted file mode 100644 index c01a26f..0000000 --- a/components/zcancmder_module/zcan_xy_robot_module.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once -#include "sdk\components\xy_robot_ctrl_module\xy_robot_ctrl_module.hpp" -#include "sdk\components\zcancmder\zcanreceiver.hpp" -#if 1 -namespace iflytop { -class ZCANXYRobotCtrlModule : public ZCanCmderListener { - ZCanCmder* m_cancmder = nullptr; - int m_id = 0; - I_XYRobotCtrlModule* m_xyRobotCtrlModule; - - uint8_t m_txbuf[128] = {0}; - zmutex m_lock; - - public: - void initialize(ZCanCmder* cancmder, int id, I_XYRobotCtrlModule* xyRobotCtrlModule); - virtual void onRceivePacket(CanPacketRxBuffer* rxcmd); -}; -} // namespace iflytop -#endif \ No newline at end of file diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index 1a22209..a160247 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit 1a22209b189847bee8761a2f773c3dea084c2e96 +Subproject commit a160247f4b19096115532301b61a81f133ab9b67