diff --git a/components/pipette_module/pipette_ctrl_module.cpp b/components/pipette_module/pipette_ctrl_module.cpp index 7173380..0e77a57 100644 --- a/components/pipette_module/pipette_ctrl_module.cpp +++ b/components/pipette_module/pipette_ctrl_module.cpp @@ -99,13 +99,13 @@ int32_t PipetteModule::take_tip(s16 vel, s16 height_mm, s16 tip_hight_mm, action m_thread.start([this, vel, height_mm, tip_hight_mm, status_cb]() { // 移动移液枪到取Tip位置 ZLOGI(TAG, "move to take_tip_height_mm %d", height_mm); - DO("move_to", m_stepMotor->move_to_block(vel, hight_trans(height_mm))); + DO("move_to", m_stepMotor->move_to_block(hight_trans(height_mm), vel)); // 取tip头 ZLOGI(TAG, "take tip"); osDelay(1000); // 移液枪返回零点 ZLOGI(TAG, "move to transfer_height_mm %d", m_transfer_height_mm); - DO("move_to_block", m_stepMotor->move_to_block(vel, hight_trans(m_transfer_height_mm))); + DO("move_to_block", m_stepMotor->move_to_block(hight_trans(m_transfer_height_mm), vel)); bool hastip = false; DO("m_smtp2->read_tip_state(hastip)", m_smtp2->read_tip_state(hastip)); @@ -129,7 +129,7 @@ int32_t PipetteModule::remove_tip(s16 vel, s16 height_mm, action_cb_status_t sta m_thread.start([this, vel, height_mm, status_cb]() { // 移动移液枪到丢Tip位置 ZLOGI(TAG, "move to remove_tip_height_mm %d", height_mm); - DO("move_to_block", m_stepMotor->move_to_block(vel, hight_trans(height_mm))); + DO("move_to_block", m_stepMotor->move_to_block(hight_trans(height_mm), vel)); // 丢弃tip头 ZLOGI(TAG, "put_tip"); @@ -137,7 +137,7 @@ int32_t PipetteModule::remove_tip(s16 vel, s16 height_mm, action_cb_status_t sta // 移液枪返回零点 ZLOGI(TAG, "move to transfer_height_mm %d", m_transfer_height_mm); - DO("move_to_block", m_stepMotor->move_to_block(vel, hight_trans(m_transfer_height_mm))); + DO("move_to_block", m_stepMotor->move_to_block(hight_trans(m_transfer_height_mm), vel)); call_status_cb(status_cb, 0); }); @@ -149,7 +149,7 @@ int32_t PipetteModule::move_to(s16 vel, s16 height_mm, action_cb_status_t status m_thread.stop(); m_thread.start([this, vel, height_mm, status_cb]() { ZLOGI(TAG, "move to transfer_height_mm %d", height_mm); - DO("m_stepMotor->move_to_block(targethith)", m_stepMotor->move_to_block(vel, hight_trans(height_mm))); + DO("m_stepMotor->move_to_block(targethith)", m_stepMotor->move_to_block(hight_trans(height_mm), vel)); call_status_cb(status_cb, 0); }); @@ -179,7 +179,6 @@ int32_t PipetteModule::exec_move_to_with_lld(s16 vel, s16 lld_cap_thr, s16 lld_m } return int32_t(0); }); - } int32_t PipetteModule::move_to_with_lld( // @@ -196,7 +195,7 @@ int32_t PipetteModule::move_to_with_lld( // if (targethith > lld_max_hight_mm) { targethith = lld_max_hight_mm; } - DO("m_stepMotor->move_to_block(targethith)", m_stepMotor->move_to_block(vel, hight_trans(targethith))); + DO("m_stepMotor->move_to_block(targethith)", m_stepMotor->move_to_block(hight_trans(targethith), vel)); call_status_cb(status_cb, 0); }); @@ -227,7 +226,6 @@ int32_t PipetteModule::pipette_move_to_ul(s16 take_volume_mm, action_cb_status_t return 0; } - int32_t PipetteModule::hight_trans(s16 targethith, s16 &motormovehight) { motormovehight = targethith; return 0; diff --git a/components/scriptcmder_module/step_motor_ctrl_script_cmder_module.cpp b/components/scriptcmder_module/step_motor_ctrl_script_cmder_module.cpp new file mode 100644 index 0000000..9395da8 --- /dev/null +++ b/components/scriptcmder_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/scriptcmder_module/step_motor_ctrl_script_cmder_module.hpp b/components/scriptcmder_module/step_motor_ctrl_script_cmder_module.hpp new file mode 100644 index 0000000..a48d835 --- /dev/null +++ b/components/scriptcmder_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/step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp index 28d64a2..0960f05 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -1,19 +1,32 @@ #include "step_motor_ctrl_module.hpp" -#include "sdk/components/errorcode/errorcode.hpp" - #include +#include + +#include "sdk/components/errorcode/errorcode.hpp" +#include "sdk\components\flash\znvs.hpp" using namespace iflytop; #define TAG "StepMotorCtrlModule" -void StepMotorCtrlModule::initialize(int id, IStepperMotor* stepM, ZGPIO* zero_gpio, ZGPIO* end_gpio) { - m_id = id; - m_stepM1 = stepM; - m_Xgpio = zero_gpio; - m_end_gpio = end_gpio; +void StepMotorCtrlModule::initialize(int id, IStepperMotor* stepM, ZGPIO iotable[], int nio, const char* flashmark) { + m_id = id; + m_stepM1 = stepM; + m_iotable = iotable; + m_nio = nio; + if (m_nio >= 8) m_nio = 8; + if (m_nio >= 1) m_Xgpio = &m_iotable[0]; + if (m_nio >= 2) m_end_gpio = &m_iotable[1]; m_lock.init(); m_statu_lock.init(); m_thread.init(TAG, 1024, osPriorityNormal); + m_flashmark = flashmark; + + if (m_flashmark) { + ZASSERT(ZNVS::ins().get_config(m_flashmark, (uint8_t*)&m_flash_config, sizeof(m_flash_config))); + } else { + create_default_cfg(m_flash_config); + } + active_cfg(); #if 0 run_param_t run_param; @@ -32,6 +45,30 @@ void StepMotorCtrlModule::initialize(int id, IStepperMotor* stepM, ZGPIO* zero_g bool StepMotorCtrlModule::isbusy() { return m_thread.isworking(); } +int32_t StepMotorCtrlModule::move_to_logic_point(int32_t logic_point_num, action_cb_status_t status_cb) { + if (logic_point_num >= ZARRAY_SIZE(m_flash_config.logic_point)) { + return err::kce_param_out_of_range; + } + logic_point_t logic_point = m_flash_config.logic_point[logic_point_num]; + return move_to(logic_point.velocity, logic_point.x, status_cb); +} +int32_t StepMotorCtrlModule::set_logic_point(int logic_point_num, int32_t x, int32_t vel, s32 acc, s32 dec) { + if (logic_point_num >= ZARRAY_SIZE(m_flash_config.logic_point)) return err::kce_param_out_of_range; + // memset(&m_flash_config, 0, sizeof(m_flash_config)); + m_flash_config.logic_point[logic_point_num].x = x; + m_flash_config.logic_point[logic_point_num].velocity = vel; + m_flash_config.logic_point[logic_point_num].acc = acc; + m_flash_config.logic_point[logic_point_num].dec = dec; + return 0; +} +int32_t StepMotorCtrlModule::get_logic_point(int logic_point_num, logic_point_t& logic_point) { + if (logic_point_num >= ZARRAY_SIZE(m_flash_config.logic_point)) { + return err::kce_param_out_of_range; + } + logic_point = m_flash_config.logic_point[logic_point_num]; + return 0; +} + int32_t StepMotorCtrlModule::_move_to(int32_t tox, action_cb_status_t status_cb) { // zlock_guard lock(m_lock); @@ -52,14 +89,9 @@ int32_t StepMotorCtrlModule::_move_to(int32_t tox, action_cb_status_t status_cb) if (m_thread.getExitFlag()) break; vTaskDelay(10); } - m_lastexecstatus = 0; + call_exec_status_cb(0, status_cb); }, - [this, tox, status_cb]() { - _motor_stop(); - int32_t nowx = 0; - getnowpos(nowx); - if (status_cb) status_cb(m_lastexecstatus); - }); + [this, tox, status_cb]() { _motor_stop(); }); return 0; } int32_t StepMotorCtrlModule::_move_by(int32_t dx, action_cb_status_t status_cb) { @@ -76,20 +108,20 @@ int32_t StepMotorCtrlModule::_move_by(int32_t dx, action_cb_status_t status_cb) }); } -int32_t StepMotorCtrlModule::move_to(int32_t velocity, int32_t x, action_cb_status_t status_cb) { +int32_t StepMotorCtrlModule::move_to(int32_t x, int32_t velocity, action_cb_status_t status_cb) { if (m_default_speed > m_param.maxspeed) { ZLOGW(TAG, "m%d default speed:%d > m_cfg_max_speed:%d", m_id, m_default_speed, m_param.maxspeed); m_default_speed = m_param.maxspeed; } - m_default_speed = velocity; + if (velocity != 0) m_default_speed = velocity; return move_to(x, status_cb); } -int32_t StepMotorCtrlModule::move_by(int32_t velocity, int32_t dx, action_cb_status_t status_cb) { +int32_t StepMotorCtrlModule::move_by(int32_t dx, int32_t velocity, action_cb_status_t status_cb) { if (m_default_speed > m_param.maxspeed) { ZLOGW(TAG, "m%d default speed:%d > m_cfg_max_speed:%d", m_id, m_default_speed, m_param.maxspeed); m_default_speed = m_param.maxspeed; } - m_default_speed = velocity; + if (velocity != 0) m_default_speed = velocity; return move_by(dx, status_cb); } @@ -102,6 +134,27 @@ int32_t StepMotorCtrlModule::move_by(int32_t dx, action_cb_status_t status_cb) { return _move_by(dx, status_cb); } +int32_t StepMotorCtrlModule::calculate_curpos(action_cb_status_t status_cb) { + zlock_guard lock(m_lock); + ZLOGI(TAG, "read_curpos_by_move2zero"); + m_thread.stop(); + m_thread.start( + [this, status_cb]() { + // + int32_t dx; + int32_t erro = exec_move_to_zero_task(dx); + int xstart = dx + m_param.shift_x; + m_calculate_curpos_result = xstart; + call_exec_status_cb(erro, status_cb); + }, + [this, status_cb]() { _motor_stop(); }); + return 0; +} +int32_t StepMotorCtrlModule::read_calculate_curpos_action_result(int32_t& pos) { + pos = m_calculate_curpos_result; + return 0; +} + int32_t StepMotorCtrlModule::move_to_zero(action_cb_status_t status_cb) { zlock_guard lock(m_lock); ZLOGI(TAG, "move_to_zero"); @@ -110,17 +163,13 @@ int32_t StepMotorCtrlModule::move_to_zero(action_cb_status_t status_cb) { m_thread.start( [this, status_cb]() { int32_t dx; - int32_t erro = exec_move_to_zero_task(dx); - m_lastexecstatus = erro; - }, - [this, status_cb]() { - _motor_stop(); - if (m_lastexecstatus == 0) { + int32_t erro = exec_move_to_zero_task(dx); + if (erro == 0) { m_stepM1->setXACTUAL(0); } - if (status_cb) status_cb(m_lastexecstatus); - return; - }); + call_exec_status_cb(erro, status_cb); + }, + [this, status_cb]() { _motor_stop(); }); return 0; } @@ -136,8 +185,8 @@ int32_t StepMotorCtrlModule::move_to_zero_with_calibrate(int32_t nowx, action_cb if (erro != 0) { ZLOGI(TAG, "exec_move_to_zero_task failed:%d", erro); _motor_stop(); - m_lastexecstatus = erro; - if (status_cb) status_cb(erro); + call_exec_status_cb(erro, status_cb); + return; } @@ -146,8 +195,7 @@ int32_t StepMotorCtrlModule::move_to_zero_with_calibrate(int32_t nowx, action_cb m_param.shift_x = calibrate_x; m_stepM1->setXACTUAL(0); - m_lastexecstatus = 0; - if (status_cb) status_cb(m_lastexecstatus); + call_exec_status_cb(erro, status_cb); return; }); @@ -204,8 +252,7 @@ int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, action_cb_ } osDelay(1); } - m_lastexecstatus = 0; - if (status_cb) status_cb(m_lastexecstatus); + call_exec_status_cb(0, status_cb); m_stepM1->stop(); return; }); @@ -216,8 +263,13 @@ int32_t StepMotorCtrlModule::read_version(version_t& version) { return 0; } int32_t StepMotorCtrlModule::read_status(status_t& status) { zlock_guard l(m_statu_lock); status.status = m_thread.isworking() ? 1 : 0; - if (m_Xgpio) status.io_state |= m_Xgpio->getState() ? 0x01 : 0x00; - if (m_end_gpio) status.io_state |= m_end_gpio->getState() ? 0x02 : 0x00; + + for (size_t i = 0; i < m_nio; i++) { + if (m_iotable[i].getState()) { + status.io_state |= (0x01 << i); + } + } + getnowpos(status.x); return 0; } @@ -225,15 +277,14 @@ int32_t StepMotorCtrlModule::read_detailed_status(detailed_status_t& debug_info) debug_info.status = m_thread.isworking() ? 1 : 0; if (m_Xgpio) debug_info.io_state |= m_Xgpio->getState() ? 0x01 : 0x00; if (m_end_gpio) debug_info.io_state |= m_end_gpio->getState() ? 0x02 : 0x00; + debug_info.last_exec_status = m_lastexecstatus; getnowpos(debug_info.x); return 0; } int32_t StepMotorCtrlModule::set_base_param(const base_param_t& param) { m_param = param; - m_stepM1->setIHOLD_IRUN(m_param.ihold, m_param.irun, m_param.iholddelay); - m_stepM1->setScale(m_param.distance_scale / 1000.0); - + active_cfg(); ZLOGI(TAG, "=========== base param ============"); ZLOGI(TAG, "= x_shaft :%d", m_param.x_shaft); ZLOGI(TAG, "= distance_scale :%f", m_param.distance_scale); @@ -318,6 +369,12 @@ int32_t StepMotorCtrlModule::exec_move_to_zero_task() { if (m_Xgpio->getState()) { xreach_zero = true; _motor_stop(-1); + + while (!m_stepM1->isStoped()) { // 等待电机停止 + _motor_stop(-1); + vTaskDelay(10); + } + break; } vTaskDelay(1); @@ -431,21 +488,64 @@ int32_t StepMotorCtrlModule::do_motor_action_block_2(function action, int32_t StepMotorCtrlModule::do_motor_action_block(function action) { return do_motor_action_block_2(action, nullptr); } -int32_t StepMotorCtrlModule::move_to_block(int32_t tox) { +int32_t StepMotorCtrlModule::move_to_block(int32_t tox, int overtime) { return do_motor_action_block([this, tox]() { return move_to(tox, nullptr); }); } -int32_t StepMotorCtrlModule::move_by_block(int32_t dx) { +int32_t StepMotorCtrlModule::move_by_block(int32_t dx, int overtime) { return do_motor_action_block([this, dx]() { return move_by(dx, nullptr); }); } -int32_t StepMotorCtrlModule::move_to_zero_block() { +int32_t StepMotorCtrlModule::move_to_zero_block(int overtime) { return do_motor_action_block([this]() { return move_to_zero(nullptr); }); } -int32_t StepMotorCtrlModule::move_to_zero_with_calibrate_block(int32_t x) { +int32_t StepMotorCtrlModule::move_to_zero_with_calibrate_block(int32_t x, int overtime) { return do_motor_action_block([this, x]() { return move_to_zero_with_calibrate(x, nullptr); }); } -int32_t StepMotorCtrlModule::move_to_block(int32_t velocity, int32_t tox) { - return do_motor_action_block([this, velocity, tox]() { return move_to(velocity, tox, nullptr); }); +int32_t StepMotorCtrlModule::move_to_block(int32_t tox, int32_t velocity, int overtime) { + return do_motor_action_block([this, velocity, tox]() { return move_to(tox, velocity, nullptr); }); +} +int32_t StepMotorCtrlModule::move_by_block(int32_t dx, int32_t velocity, int overtime) { + return do_motor_action_block([this, velocity, dx]() { return move_by(dx, velocity, nullptr); }); } -int32_t StepMotorCtrlModule::move_by_block(int32_t velocity, int32_t dx) { - return do_motor_action_block([this, velocity, dx]() { return move_by(velocity, dx, nullptr); }); + +void StepMotorCtrlModule::call_exec_status_cb(int32_t status, action_cb_status_t status_cb) { + m_lastexecstatus = status; + if (status_cb) status_cb(status); +} +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.run_to_zero_move_to_zero_max_d = INT32_MAX; + cfg.base_param.run_to_zero_leave_from_zero_max_d = 51200 * 3; + cfg.base_param.run_to_zero_speed = 300000; + cfg.base_param.run_to_zero_dec = 3000000; +} + +int32_t StepMotorCtrlModule::flush() { + ZLOGI(TAG, "flush"); + if (m_flashmark) { + ZNVS::ins().set_config(m_flashmark, (uint8_t*)&m_flash_config, sizeof(m_flash_config)); + ZNVS::ins().flush(); + } + return 0; +} +int32_t StepMotorCtrlModule::factory_reset() { + // + ZLOGI(TAG, "factory_reset"); + create_default_cfg(m_flash_config); + if (m_flashmark) { + ZNVS::ins().set_config(m_flashmark, (uint8_t*)&m_flash_config, sizeof(m_flash_config)); + ZNVS::ins().flush(); + } + active_cfg(); + return 0; +} +void StepMotorCtrlModule::active_cfg() { + m_stepM1->setIHOLD_IRUN(m_param.ihold, m_param.irun, m_param.iholddelay); + m_stepM1->setScale(m_param.distance_scale / 1000.0); } diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index c33b801..e1ca4bd 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -19,33 +19,48 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { zmutex m_lock; zmutex m_statu_lock; - base_param_t m_param; + flash_config_t m_flash_config; + base_param_t& m_param = m_flash_config.base_param; // bool m_x_shaft = false; int32_t m_lastexecstatus = 0; int32_t m_default_speed = 0; + ZGPIO* m_iotable; + int m_nio; + + int32_t m_calculate_curpos_result = 0; + const char* m_flashmark = nullptr; + public: - void initialize(int id, IStepperMotor* stepM, ZGPIO* zero_gpio, ZGPIO* end_gpio); + void initialize(int id, IStepperMotor* stepM, ZGPIO iotable[], int nio, const char* flashmark); + static void create_default_cfg(flash_config_t& cfg); virtual bool isbusy() override; virtual int32_t get_last_exec_status() override { return m_lastexecstatus; }; virtual int32_t move_to(int32_t tox, action_cb_status_t status_cb) override; virtual int32_t move_by(int32_t dx, action_cb_status_t status_cb) override; - virtual int32_t move_to(int32_t velocity, int32_t x, action_cb_status_t status_cb) override; - virtual int32_t move_by(int32_t velocity, int32_t dx, action_cb_status_t status_cb) override; + virtual int32_t move_to(int32_t x, int32_t velocity, action_cb_status_t status_cb) override; + virtual int32_t move_by(int32_t dx, int32_t velocity, action_cb_status_t status_cb) override; + + virtual int32_t calculate_curpos(action_cb_status_t status_cb); + virtual int32_t read_calculate_curpos_action_result(int32_t& pos); + + virtual int32_t move_to_logic_point(int32_t logic_point_num, action_cb_status_t status_cb) override; + virtual int32_t set_logic_point(int logic_point_num, int32_t x, int32_t vel, s32 acc, s32 dec) override; + virtual int32_t get_logic_point(int logic_point_num, logic_point_t& logic_point) override; virtual int32_t move_to_zero(action_cb_status_t status_cb) override; virtual int32_t move_to_zero_with_calibrate(int32_t x, action_cb_status_t status_cb) override; - virtual int32_t move_to_block(int32_t tox) override; - virtual int32_t move_by_block(int32_t dx) override; + virtual int32_t move_to_block(int32_t tox, int overtime = 0) override; + virtual int32_t move_by_block(int32_t dx, int overtime = 0) override; - virtual int32_t move_to_block(int32_t velocity, int32_t tox) override; - virtual int32_t move_by_block(int32_t velocity, int32_t dx) override; + virtual int32_t move_to_block(int32_t tox, int32_t velocity, int overtime = 0) override; + virtual int32_t move_by_block(int32_t dx, int32_t velocity, int overtime = 0) override; - virtual int32_t move_to_zero_block() override; - virtual int32_t move_to_zero_with_calibrate_block(int32_t x) override; + virtual int32_t move_to_zero_block(int overtime = 0) override; + virtual int32_t move_to_zero_with_calibrate_block(int32_t x, int overtime = 0) override; virtual int32_t rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb); @@ -64,6 +79,9 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { virtual int32_t read_pos() override; virtual bool read_zero_io_state() override; + virtual int32_t flush() override; + virtual int32_t factory_reset() override; + #if 0 virtual int32_t set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) override; // virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override; @@ -81,6 +99,8 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { int32_t do_motor_action_block_2(function action, function break_condition); private: + void active_cfg(); + int32_t do_motor_action_block(function action); // void updateStatus(uint8_t status); @@ -99,5 +119,7 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { int32_t _move_to(int32_t tox, action_cb_status_t status_cb); int32_t _move_by(int32_t dx, action_cb_status_t status_cb); + + void call_exec_status_cb(int32_t status, action_cb_status_t status_cb); }; } // namespace iflytop \ No newline at end of file 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 new file mode 100644 index 0000000..af46241 --- /dev/null +++ b/components/zcancmder_master_module/zcan_master_step_motor_ctrl_module.cpp @@ -0,0 +1,164 @@ +#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 new file mode 100644 index 0000000..6ecda3b --- /dev/null +++ b/components/zcancmder_master_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/zcancmder_module/zcan_step_motor_ctrl_module.cpp b/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp index af2ca1e..3622587 100644 --- a/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp +++ b/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp @@ -24,17 +24,17 @@ void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { } END_PP(); PROCESS_PACKET(kcmd_step_motor_ctrl_move_to_zero_with_calibrate, m_id) { - errorcode = m_module->move_to_zero_with_calibrate(cmd->nowx, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_to_zero_with_calibrate); }); + 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, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_to); }); + 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, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_by); }); + errorcode = m_module->move_by(cmd->dx, cmd->speed, [this, cmdheader](int32_t status) { PROCESS_REPORT(kcmd_step_motor_ctrl_move_by); }); } END_PP(); @@ -70,5 +70,22 @@ void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { 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/zprotocols/zcancmder b/components/zprotocols/zcancmder index 9199515..183dd63 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit 9199515dfc8db3a0760429d918e96cf2d8d31f32 +Subproject commit 183dd634eed673741826f01cfe1147379543cf29