9 changed files with 864 additions and 68 deletions
-
14components/pipette_module/pipette_ctrl_module.cpp
-
463components/scriptcmder_module/step_motor_ctrl_script_cmder_module.cpp
-
22components/scriptcmder_module/step_motor_ctrl_script_cmder_module.hpp
-
192components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
-
42components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
-
164components/zcancmder_master_module/zcan_master_step_motor_ctrl_module.cpp
-
10components/zcancmder_master_module/zcan_master_step_motor_ctrl_module.hpp
-
23components/zcancmder_module/zcan_step_motor_ctrl_module.cpp
-
2components/zprotocols/zcancmder
@ -0,0 +1,463 @@ |
|||
#include "step_motor_ctrl_script_cmder_module.hpp"
|
|||
|
|||
#include <string.h>
|
|||
|
|||
#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); |
|||
}); |
|||
} |
|||
@ -0,0 +1,22 @@ |
|||
#pragma once
|
|||
#include <map>
|
|||
|
|||
#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<uint8_t, I_StepMotorCtrlModule*> moduler; |
|||
|
|||
public: |
|||
void initialize(CmdScheduler* cmdScheduler); |
|||
void regmodule(int id, I_StepMotorCtrlModule* robot); |
|||
void regcmd(); |
|||
|
|||
private: |
|||
I_StepMotorCtrlModule* findXYRobot(int id); |
|||
}; |
|||
|
|||
} // namespace iflytop
|
|||
@ -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<int, uint16_t> 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
|
|||
@ -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
|
|||
@ -1 +1 @@ |
|||
Subproject commit 9199515dfc8db3a0760429d918e96cf2d8d31f32 |
|||
Subproject commit 183dd634eed673741826f01cfe1147379543cf29 |
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue