diff --git a/components/cmdscheduler/cmd_scheduler.cpp b/components/cmdscheduler/cmd_scheduler.cpp deleted file mode 100644 index 2d5e3ca..0000000 --- a/components/cmdscheduler/cmd_scheduler.cpp +++ /dev/null @@ -1,166 +0,0 @@ - -#include "cmd_scheduler.hpp" - -#include -#include - -#include "sdk\components\zprotocols\errorcode\errorcode.hpp" -using namespace iflytop; - -#define TAG "CMD" - -void CmdScheduler::registerCmd(std::string cmd, const char* helpinfo, int npara, call_cmd_t call_cmd) { - CMD cmdinfo; - cmdinfo.call_cmd = call_cmd; - cmdinfo.help_info = helpinfo; - cmdinfo.npara = npara; - m_cmdMap[cmd] = cmdinfo; -} - -void CmdScheduler::regbasiccmd() { - this->registerCmd("help", "", 0, [this](Context* context) { - ZLOGI(TAG, "cmdlist:"); - for (auto it = m_cmdMap.begin(); it != m_cmdMap.end(); it++) { - ZLOGI(TAG, " %s %s", it->first.c_str(), it->second.help_info.c_str()); - } - return (int32_t)0; - }); - this->registerCmd("sleep_ms", "(ms)", 1, [this](Context* context) { - int ms = atoi(context->argv[1]); - osDelay(ms); - return 0; - }); -} - -void CmdScheduler::initialize(UART_HandleTypeDef* huart, uint32_t rxbufsize) { - m_rxbufsize = rxbufsize; - - m_uart = new ZUART(); - ZASSERT(m_uart != NULL); - - ZUART::cfg_t cfg; - cfg.huart = huart; - cfg.rxbuffersize = rxbufsize; - cfg.rxovertime_ms = 3; - cfg.name = "CmdSchedulerUart"; - - rxbuf = new char[rxbufsize + 1]; - ZASSERT(rxbuf != NULL); - - m_uart->initialize(&cfg); - ZASSERT(m_uart->startRxIt()); - m_uart->setrxcb([this](uint8_t* data, size_t len) { - if (m_dataisready) { - return; - } - memcpy(rxbuf, data, len); - rxbuf[len] = '\0'; - m_rxsize = len; - m_dataisready = true; - // on data ,in irq context - }); - regbasiccmd(); -} - -void CmdScheduler::schedule() { - if (!m_dataisready) { - return; - } - for (int i = 0; i < m_rxsize; i++) { - if (rxbuf[i] == '\r' || rxbuf[i] == '\n') { - rxbuf[i] = '\0'; - } - } - for (int i = 0; i < m_rxsize; i++) { - if (rxbuf[i] != '\0') { - ZLOGI(TAG, "docmd: %s", &rxbuf[i]); - int inext = strlen(&rxbuf[i]) + i; - int32_t ecode = callcmd(&rxbuf[i]); - i = inext; - - if (ecode == 0) { - ZLOGI(TAG, "\tsuccess"); - } else { - ZLOGE(TAG, "\tfailed:%s(%d)", err::error2str(ecode), ecode); - } - ZLOGI(TAG, "\r\n"); - if (ecode != 0) { - break; - } - } - } - // if (m_rxsize != 1) { - // ZLOGI(TAG, "doscript:end"); - // } - m_dataisready = false; -} - -void CmdScheduler::tx(const char* data, int len) { m_uart->tx((uint8_t*)data, len); } -int32_t CmdScheduler::callcmd(const char* cmd) { - int argc = 0; - char* argv[10] = {0}; - memset(cmdcache, 0, sizeof(cmdcache)); - argc = 0; - memset(argv, 0, sizeof(argv)); - strcpy(cmdcache, cmd); - remove_note(cmdcache, strlen(cmdcache)); - prase_cmd(cmdcache, strlen(cmdcache), argc, argv); - if (argc == 0) { - return (int32_t)0; - } - - // printf("argc:%d\n", argc); - // for (size_t i = 0; i < argc; i++) { - // printf("argv[%d]:%s\n", i, argv[i]); - // } - /** - * @brief 在这里处理指令 - */ - auto cmder = m_cmdMap.find(string(argv[0])); - if (cmder != m_cmdMap.end()) { - Context context; - context.argc = argc; - context.argv = argv; - // ZLOGI(TAG, "callcmd:argc %d %d", argc, cmder->second.npara); - if (cmder->second.npara != context.argc - 1) return err::kcmd_param_num_error; - int32_t ret = cmder->second.call_cmd(&context); - return ret; - - } else { - return err::kcmd_not_found; - } -} - -void CmdScheduler::remove_note(char* input, int inputlen) { - bool detect_note = false; - for (int i = 0; i < inputlen; i++) { - if (!detect_note && input[i] == '#') { - detect_note = true; - } - if (detect_note) { - input[i] = 0; - } - } -} - -void CmdScheduler::prase_cmd(char* input, int inputlen, int& argc, char* argv[]) { - for (int i = 0; input[i] == 0 || i < inputlen; i++) { - if (input[i] == ' ' || input[i] == '\r' || input[i] == '\n') { - input[i] = 0; - } - } - - int j = 0; - for (int i = 0; input[i] == 0 || i < inputlen; i++) { - if (input[i] != 0 && j == 0) { - argv[argc++] = &input[i]; - j = 1; - continue; - } - - if (input[i] == 0 && j == 1) { - j = 0; - continue; - } - } -} diff --git a/components/cmdscheduler/cmd_scheduler.hpp b/components/cmdscheduler/cmd_scheduler.hpp deleted file mode 100644 index 4d467c5..0000000 --- a/components/cmdscheduler/cmd_scheduler.hpp +++ /dev/null @@ -1,112 +0,0 @@ -#pragma once -#include -#include - -#include "sdk/os/zos.hpp" -#include "sdk\components\zprotocols\errorcode\errorcode.hpp" -#include "sdk\components\zprotocols\zcancmder_v2\api\api.hpp" - -namespace iflytop { -using namespace std; - -class CmdScheduler { - public: - class Context { - public: - int argc; - char** argv; - - int getInt(int index, int defaultvalue = 0) { - if (index >= argc) { - return defaultvalue; - } - return atoi(argv[index]); - } - - bool getBool(int index, bool defaultvalue = false) { - if (index >= argc) { - return defaultvalue; - } - return atoi(argv[index]) != 0; - } - - float getFloat(int index, float defaultvalue = 0) { - if (index >= argc) { - return defaultvalue; - } - return atof(argv[index]); - } - const char* getString(int index, const char* defaultvalue = "") { - if (index >= argc) { - return defaultvalue; - } - return argv[index]; - } - }; - - typedef function call_cmd_t; - - class CMD { - public: - call_cmd_t call_cmd; - string help_info; - int npara; - }; - - private: - map m_cmdMap; - - ZUART* m_uart; - char* rxbuf; - int32_t m_rxsize = 0; - - uint32_t m_rxbufsize; - - bool m_dataisready = false; - - char cmdcache[1024] = {0}; - - public: - void initialize(UART_HandleTypeDef* huart, uint32_t rxbufsize); - void registerCmd(std::string cmd, const char* helpinfo, int npara, call_cmd_t call_cmd); - void tx(const char* data, int len); - - void schedule(); - - private: - void regbasiccmd(); - int32_t callcmd(const char* cmd); - void prase_cmd(char* input, int inputlen, int& argc, char* argv[]); - void remove_note(char* input, int inputlen); -}; - -#define DO_CMD(cond) \ - { \ - int32_t ret = cond; \ - if (ret != 0) { \ - return ret; \ - } \ - } -#define IMPL_CMD(cmd, ...) \ - DO_CMD(findmodule(con->getInt(1), &module)); \ - DO_CMD(module->cmd(__VA_ARGS__)); \ - return (int32_t)0; - -#define IMPL_READ_STATE(cmd, ...) \ - DO_CMD(findmodule(con->getInt(1), &module)); \ - DO_CMD(module->cmd(__VA_ARGS__)); \ - cmd_dump_ack(ack); \ - return (int32_t)0; - -#define REG_CMD___NO_ACK(prefix, cmd, para, npara, ...) /**/ \ - m_cmdScheduler->registerCmd(prefix #cmd, para, npara, [this](CmdScheduler::Context* con) { /**/ \ - IMPL_CMD(cmd, __VA_ARGS__); /**/ \ - }); - -#define REG_CMD_WITH_ACK(prefix, cmd, para, npara, acktype, ...) /**/ \ - m_cmdScheduler->registerCmd(prefix #cmd, para, npara, [this](CmdScheduler::Context* con) { /**/ \ - acktype ack; /**/ \ - IMPL_READ_STATE(cmd, __VA_ARGS__); /**/ \ - }); - -} // 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 b2b3534..a083744 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -255,11 +255,11 @@ int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, action_cb_ _rotate(speed, m_param.acc, m_param.dec); int32_t startticket = zos_get_tick(); - bool reachtime = false; + //bool reachtime = false; while (!m_thread.getExitFlag()) { if ((int32_t)zos_haspassedms(startticket) > lastforms && lastforms != 0) { - reachtime = true; + //reachtime = true; m_stepM1->stop(); break; } @@ -278,7 +278,7 @@ int32_t StepMotorCtrlModule::read_status(status_t& status) { zlock_guard l(m_statu_lock); status.status = m_thread.isworking() ? 1 : 0; - for (size_t i = 0; i < m_nio; i++) { + for (int32_t i = 0; i < m_nio; i++) { if (m_iotable[i].getState()) { status.io_state |= (0x01 << i); } @@ -289,7 +289,7 @@ int32_t StepMotorCtrlModule::read_status(status_t& status) { } int32_t StepMotorCtrlModule::read_detailed_status(detailed_status_t& debug_info) { debug_info.status = m_thread.isworking() ? 1 : 0; - for (size_t i = 0; i < m_nio; i++) { + for (int32_t i = 0; i < m_nio; i++) { if (m_iotable[i].getState()) { debug_info.io_state |= (0x01 << i); } @@ -694,7 +694,7 @@ int32_t StepMotorCtrlModule::module_clear_error() { return 0; } int32_t StepMotorCtrlModule::module_readio(int32_t* io) { *io = 0; - for (size_t i = 0; i < m_nio; i++) { + for (int32_t i = 0; i < m_nio; i++) { if (m_iotable[i].getState()) { *io |= (0x01 << i); } @@ -804,4 +804,4 @@ int32_t StepMotorCtrlModule::motor_read_pos(int32_t* pos) { getnowpos(xnow); *pos = xnow; return 0; -} \ No newline at end of file +} 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 deleted file mode 100644 index d105818..0000000 --- a/components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.cpp +++ /dev/null @@ -1,273 +0,0 @@ -#include "step_motor_ctrl_script_cmder_module.hpp" - -#include - -// #include "sdk\components\zprotocols\zcancmder\zcancmder_protocol.hpp" -using namespace iflytop; -#define TAG "CMD" - -#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(); -} -int32_t StepMotorCtrlScriptCmderModule::findmodule(int id, I_StepMotorCtrlModule** module) { - auto it = moduler.find(id); - if (it == moduler.end()) { - return err::kmodule_not_found; - } - *module = it->second; - - return 0; -} -#define GET_BIT(x, n) ((x >> n) & 0x01) - -static void cmd_dump_ack(I_StepMotorCtrlModule::base_param_t& ack) { - ZLOGI(TAG, "base_param:"); - ZLOGI(TAG, " x_shaft: %d", ack.x_shaft); - ZLOGI(TAG, " distance_scale: %d", ack.distance_scale); - ZLOGI(TAG, " ihold: %d", ack.ihold); - ZLOGI(TAG, " irun: %d", ack.irun); - ZLOGI(TAG, " iholddelay: %d", ack.iholddelay); - ZLOGI(TAG, " acc: %d", ack.acc); - ZLOGI(TAG, " dec: %d", ack.dec); - ZLOGI(TAG, " maxspeed: %d", ack.maxspeed); - ZLOGI(TAG, " min_x: %d", ack.min_x); - ZLOGI(TAG, " max_x: %d", ack.max_x); - ZLOGI(TAG, " shift_x: %d", ack.shift_x); - ZLOGI(TAG, "= run_to_zero_max_d :%d", ack.run_to_zero_max_d); - ZLOGI(TAG, "= run_to_zero_speed :%d", ack.run_to_zero_speed); - ZLOGI(TAG, "= run_to_zero_dec :%d", ack.run_to_zero_dec); - ZLOGI(TAG, "= look_zero_edge_max_d :%d", ack.look_zero_edge_max_d); - ZLOGI(TAG, "= look_zero_edge_speed :%d", ack.look_zero_edge_speed); - ZLOGI(TAG, "= look_zero_edge_dec :%d", ack.look_zero_edge_dec); - - // ZLOGI(TAG, " run_to_zero_move_to_zero_max_d: %d", ack.run_to_zero_move_to_zero_max_d); - // ZLOGI(TAG, " run_to_zero_leave_from_zero_max_d: %d", ack.run_to_zero_leave_from_zero_max_d); - // ZLOGI(TAG, " run_to_zero_speed: %d", ack.run_to_zero_speed); - // ZLOGI(TAG, " run_to_zero_dec: %d", ack.run_to_zero_dec); -} - -static void cmd_dump_ack(I_StepMotorCtrlModule::logic_point_t& ack) { - ZLOGI(TAG, "logic_point:"); - ZLOGI(TAG, " index: %d", ack.index); - ZLOGI(TAG, " acc: %d", ack.acc); - ZLOGI(TAG, " dec: %d", ack.dec); - ZLOGI(TAG, " velocity: %d", ack.velocity); - ZLOGI(TAG, " x: %d", ack.x); -} - -static void cmd_dump_ack(I_StepMotorCtrlModule::status_t& ack) { - ZLOGI(TAG, "status:"); - ZLOGI(TAG, " status: :%d", ack.status); - ZLOGI(TAG, " io_state :%d,%d,%d,%d,%d,%d,%d,%d", GET_BIT(ack.io_state, 0), GET_BIT(ack.io_state, 1), GET_BIT(ack.io_state, 2), GET_BIT(ack.io_state, 3), GET_BIT(ack.io_state, 4), GET_BIT(ack.io_state, 5), GET_BIT(ack.io_state, 6), GET_BIT(ack.io_state, 7)); - ZLOGI(TAG, " x: :%d", ack.x); -} - -static void cmd_dump_ack(I_StepMotorCtrlModule::detailed_status_t& ack) { - ZLOGI(TAG, "detailed_status:"); - ZLOGI(TAG, " status: %d", ack.status); - ZLOGI(TAG, " io_state :%d,%d,%d,%d,%d,%d,%d,%d", GET_BIT(ack.io_state, 0), GET_BIT(ack.io_state, 1), GET_BIT(ack.io_state, 2), GET_BIT(ack.io_state, 3), GET_BIT(ack.io_state, 4), GET_BIT(ack.io_state, 5), GET_BIT(ack.io_state, 6), GET_BIT(ack.io_state, 7)); - ZLOGI(TAG, " x: %d", ack.x); - ZLOGI(TAG, " last_exec_status: %d", ack.last_exec_status); -} - -static void cmd_dump_ack(I_StepMotorCtrlModule::version_t& ack) { - ZLOGI(TAG, "version:"); - ZLOGI(TAG, " version: %d", ack.version); -} - -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 1 - m_cmdScheduler->registerCmd("step_motor_ctrl_is_busy", "(id)", 1, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - ZLOGI(TAG, "is busy:%d", module->isbusy()); - return (int32_t)0; - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_get_last_exec_status", "(id)", 1, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - ZLOGI(TAG, "last exec status:%s(%d)", err::error2str(module->get_last_exec_status()), module->get_last_exec_status()); - return (int32_t)0; - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_move_to", "(id,x,speed)", 3, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - IMPL_CMD(move_to, con->getInt(2), con->getInt(3), // - [this](int32_t status) { ZLOGI(TAG, "move to exec end status:%s(%d)", err::error2str(status), status); }); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_move_by", "(id,dx,speed)", 3, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - IMPL_CMD(move_by, con->getInt(2), con->getInt(3), // - [this](int32_t status) { ZLOGI(TAG, "move by exec end status:%s(%d)", err::error2str(status), status); }); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_zero", "(id)", 1, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - IMPL_CMD(move_to_zero, [this](int32_t status) { ZLOGI(TAG, "move to zero exec end status:%s(%d)", err::error2str(status), status); }); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_zero_with_calibrate", "(id,x)", 2, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - IMPL_CMD(move_to_zero_with_calibrate, con->getInt(2), // - [this](int32_t status) { ZLOGI(TAG, "move to zero with calibrate exec end status:%s(%d)", err::error2str(status), status); }); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_rotate", "(id,speed,lastforms)", 3, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - IMPL_CMD(rotate, con->getInt(2), con->getInt(3), // - [this](int32_t status) { ZLOGI(TAG, "rotate exec end status:%s(%d)", err::error2str(status), status); }); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_stop", "(id,stop_type)", 2, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - IMPL_CMD(stop, con->getInt(2)); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_force_change_current_pos", "(id,x)", 2, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - IMPL_CMD(force_change_current_pos, con->getInt(2)); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_move_to_logic_point", "(id,logic_point_id)", 2, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - IMPL_CMD(move_to_logic_point, con->getInt(2), // - [this](int32_t status) { ZLOGI(TAG, "move to logic point exec end status:%s(%d)", err::error2str(status), status); }); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_set_logic_point", "(id,logic_point_id,x,vel,acc,dec)", 6, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - IMPL_CMD(set_logic_point, con->getInt(2), con->getInt(3), con->getInt(4), con->getInt(5), con->getInt(6)); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_set_logic_point_simplify", "(id,logic_point_id)", 2, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - IMPL_CMD(set_logic_point, con->getInt(2), module->read_pos(), 0, 0, 0); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_get_logic_point", "(id,logic_point_id)", 2, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - I_StepMotorCtrlModule::logic_point_t ack; - IMPL_READ_STATE(get_logic_point, con->getInt(2), ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_get_base_param", "(id)", 1, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - I_StepMotorCtrlModule::base_param_t ack; - IMPL_READ_STATE(get_base_param, ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_flush", "(id)", 1, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - IMPL_CMD(flush); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_enable", "(id,en)", 2, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - IMPL_CMD(enable, con->getBool(2)); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_factory_reset", "(id)", 1, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - IMPL_CMD(factory_reset); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_read_version", "(id)", 1, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - I_StepMotorCtrlModule::version_t ack; - IMPL_READ_STATE(read_version, ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_read_status", "(id)", 1, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - I_StepMotorCtrlModule::status_t ack; - IMPL_READ_STATE(read_status, ack); - }); - - m_cmdScheduler->registerCmd("step_motor_ctrl_read_detailed_status", "(id)", 1, [this](CmdScheduler::Context* con) { - DO_CMD(findmodule(con->getInt(1), &module)); - I_StepMotorCtrlModule::detailed_status_t ack; - IMPL_READ_STATE(read_detailed_status, ack); - }); -#endif - - m_cmdScheduler->registerCmd("step_motor_ctrl_set_base_param", "(id,paramName,val)", 3, [this](CmdScheduler::Context* con) { - const char* paramName = con->getString(2); - int32_t value = con->getInt(3); - - DO_CMD(findmodule(con->getInt(1), &module)); - - I_StepMotorCtrlModule::base_param_t param; - DO_CMD(module->get_base_param(param)); - - 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_max_d")) { - param.run_to_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 if (streq(paramName, "look_zero_edge_max_d")) { - param.look_zero_edge_max_d = value; - } else if (streq(paramName, "look_zero_edge_speed")) { - param.look_zero_edge_speed = value; - } else if (streq(paramName, "look_zero_edge_dec")) { - param.look_zero_edge_dec = value; - } else { - ZLOGE(TAG, "invalid param name:%s", paramName); - return (int32_t)err::kparam_out_of_range; - } - DO_CMD(module->set_base_param(param)); - return (int32_t)0; - }); -} \ 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 deleted file mode 100644 index 94db864..0000000 --- a/components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.hpp +++ /dev/null @@ -1,23 +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; - I_StepMotorCtrlModule* module; - - public: - void initialize(CmdScheduler* cmdScheduler); - void regmodule(int id, I_StepMotorCtrlModule* robot); - void regcmd(); - - private: - int32_t findmodule(int id, I_StepMotorCtrlModule** module); -}; - -} // namespace iflytop \ No newline at end of file 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 deleted file mode 100644 index f74835d..0000000 --- a/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp +++ /dev/null @@ -1,161 +0,0 @@ -#include "xy_robot_script_cmder_module.hpp" - -#include - -#include "sdk\components\zprotocols\zcancmder\zcancmder_protocol.hpp" -using namespace iflytop; -#define TAG "XYRobotScriptCmderModule" - -void XYRobotScriptCmderModule::initialize(CmdScheduler* cmdScheduler) { - m_cmdScheduler = cmdScheduler; - ZASSERT(m_cmdScheduler != nullptr); - regcmd(); -} -int32_t XYRobotScriptCmderModule::findmodule(int id, I_XYRobotCtrlModule** module) { - auto it = moduler.find(id); - if (it == moduler.end()) { - return err::kmodule_not_found; - } - *module = it->second; - return 0; -} -static bool streq(const char* a, const char* b) { return strcmp(a, b) == 0; } - -static void cmd_dump_ack(I_XYRobotCtrlModule::version_t& ack) { // - ZLOGI(TAG, "version:%d", ack.version); -} - -#define BIT(x, n) (((x) >> (n)) & 1) - -static void cmd_dump_ack(I_XYRobotCtrlModule::status_t& ack) { // - ZLOGI(TAG, "status :%d", ack.status); - ZLOGI(TAG, "x :%d", ack.x); - ZLOGI(TAG, "y :%d", ack.y); - ZLOGI(TAG, "iostate :%d %d %d %d %d %d %d %d", BIT(ack.iostate, 0), BIT(ack.iostate, 1), BIT(ack.iostate, 2), BIT(ack.iostate, 3), BIT(ack.iostate, 4), BIT(ack.iostate, 5), BIT(ack.iostate, 6), BIT(ack.iostate, 7)); -} - -static void cmd_dump_ack(I_XYRobotCtrlModule::detailed_status_t& ack) { // - ZLOGI(TAG, "status :%d", ack.status); - ZLOGI(TAG, "x :%d", ack.x); - ZLOGI(TAG, "y :%d", ack.y); - ZLOGI(TAG, "iostate :%d %d %d %d %d %d %d %d", BIT(ack.iostate, 0), BIT(ack.iostate, 1), BIT(ack.iostate, 2), BIT(ack.iostate, 3), BIT(ack.iostate, 4), BIT(ack.iostate, 5), BIT(ack.iostate, 6), BIT(ack.iostate, 7)); -} - -static void cmd_dump_ack(I_XYRobotCtrlModule::base_param_t& ack) { - ZLOGI(TAG, " robot_type :%d", ack.robot_type); - ZLOGI(TAG, " x_shaft :%d", ack.x_shaft); - ZLOGI(TAG, " y_shaft :%d", ack.y_shaft); - ZLOGI(TAG, " ihold :%d", ack.ihold); - ZLOGI(TAG, " irun :%d", ack.irun); - ZLOGI(TAG, " iholddelay :%d", ack.iholddelay); - ZLOGI(TAG, " distance_scale :%d", ack.distance_scale); - ZLOGI(TAG, " shift_x :%d", ack.shift_x); - ZLOGI(TAG, " shift_y :%d", ack.shift_y); - ZLOGI(TAG, " acc :%d", ack.acc); - ZLOGI(TAG, " dec :%d", ack.dec); - ZLOGI(TAG, " breakdec :%d", ack.breakdec); - ZLOGI(TAG, " maxspeed :%d", ack.maxspeed); - ZLOGI(TAG, " min_x :%d", ack.min_x); - ZLOGI(TAG, " max_x :%d", ack.max_x); - ZLOGI(TAG, " min_y :%d", ack.min_y); - ZLOGI(TAG, " max_y :%d", ack.max_y); - ZLOGI(TAG, " run_to_zero_max_d :%d", ack.run_to_zero_max_d); - ZLOGI(TAG, " run_to_zero_speed :%d", ack.run_to_zero_speed); - ZLOGI(TAG, " run_to_zero_dec :%d", ack.run_to_zero_dec); - ZLOGI(TAG, " look_zero_edge_max_d :%d", ack.look_zero_edge_max_d); - ZLOGI(TAG, " look_zero_edge_speed :%d", ack.look_zero_edge_speed); - ZLOGI(TAG, " look_zero_edge_dec :%d", ack.look_zero_edge_dec); -} - -void XYRobotScriptCmderModule::regmodule(int id, I_XYRobotCtrlModule* robot) { - ZASSERT(moduler.find(id) == moduler.end()); - ZASSERT(robot != nullptr); - moduler[id] = robot; -} -void XYRobotScriptCmderModule::regcmd() { // - ZASSERT(m_cmdScheduler != nullptr); - -#define PREIX "xy_robot_ctrl_" - - REG_CMD___NO_ACK(PREIX, enable, "(id,en)", 2, con->getBool(2)); - REG_CMD___NO_ACK(PREIX, stop, "(id,stop_type)", 2, con->getInt(2)); - REG_CMD___NO_ACK(PREIX, move_to_zero, "(id)", 1, [this](int32_t status) { ZLOGI(TAG, "move_to_zero status:%d", status); }); - REG_CMD___NO_ACK(PREIX, move_to_zero_with_calibrate, "(id,nowx,nowy)", 3, con->getInt(2), con->getInt(3), [this](int32_t status) { ZLOGI(TAG, "move_to_zero_with_calibrate status:%d", status); }); - REG_CMD___NO_ACK(PREIX, move_to, "(id,x,y,speed)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_to status:%d", status); }); - REG_CMD___NO_ACK(PREIX, move_by, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); }); - REG_CMD___NO_ACK(PREIX, force_change_current_pos, "(id,x,y)", 3, con->getInt(2), con->getInt(3)); - REG_CMD_WITH_ACK(PREIX, read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack); - REG_CMD_WITH_ACK(PREIX, read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack); - REG_CMD_WITH_ACK(PREIX, read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack); - REG_CMD_WITH_ACK(PREIX, get_base_param, "(id)", 1, I_XYRobotCtrlModule::base_param_t, ack); - REG_CMD_WITH_ACK(PREIX, read_detailed_status, "(id)", 1, I_XYRobotCtrlModule::detailed_status_t, ack); - REG_CMD_WITH_ACK(PREIX, read_status, "(id)", 1, I_XYRobotCtrlModule::status_t, ack); - REG_CMD_WITH_ACK(PREIX, read_version, "(id)", 1, I_XYRobotCtrlModule::version_t, ack); - REG_CMD___NO_ACK(PREIX, flush, "(id)", 1); - REG_CMD___NO_ACK(PREIX, factory_reset, "(id)", 1); - REG_CMD___NO_ACK(PREIX, move_by_no_limit, "(id,dx,dy,v)", 4, con->getInt(2), con->getInt(3), con->getInt(4), [this](int32_t status) { ZLOGI(TAG, "move_by_no_limit status:%d", status); }); - - m_cmdScheduler->registerCmd("xy_robot_ctrl_set_base_param", "(id,paramName,val)", 3, [this](CmdScheduler::Context* con) { - const char* paramName = con->getString(2); - int32_t paramVal = con->getInt(3); - - DO_CMD(findmodule(con->getInt(1), &module)); - - I_XYRobotCtrlModule::base_param_t status; - DO_CMD(module->get_base_param(status)); - - 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, "run_to_zero_speed")) { - status.run_to_zero_speed = paramVal; - } else if (streq(paramName, "run_to_zero_dec")) { - status.run_to_zero_dec = paramVal; - } else if (streq(paramName, "look_zero_edge_max_d")) { - status.look_zero_edge_max_d = paramVal; - } else if (streq(paramName, "look_zero_edge_speed")) { - status.look_zero_edge_speed = paramVal; - } else if (streq(paramName, "look_zero_edge_dec")) { - status.look_zero_edge_dec = paramVal; - } else { - ZLOGE(TAG, "invalid param name:%s", paramName); - return (int32_t)err::kparam_out_of_range; - } - - DO_CMD(module->set_base_param(status)); - return (int32_t)0; - }); -} 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 deleted file mode 100644 index 34cfeae..0000000 --- a/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.hpp +++ /dev/null @@ -1,23 +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; - I_XYRobotCtrlModule* module; - - public: - void initialize(CmdScheduler* cmdScheduler); - void regmodule(int id, I_XYRobotCtrlModule* robot); - void regcmd(); - - private: - int32_t findmodule(int id, I_XYRobotCtrlModule** module); -}; - -} // namespace iflytop \ No newline at end of file