project_boditech_vidas_a8000_v3
/
a8000_subboard_sdk
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 2 years ago
parent
78ca69f445
commit
c664738f2d
7 changed files with 6 additions and 764 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 166
      components/cmdscheduler/cmd_scheduler.cpp
  2. 112
      components/cmdscheduler/cmd_scheduler.hpp
  3. 12
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  4. 273
      components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.cpp
  5. 23
      components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.hpp
  6. 161
      components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp
  7. 23
      components/xy_robot_ctrl_module/xy_robot_script_cmder_module.hpp

166
components/cmdscheduler/cmd_scheduler.cpp
View File

@ -1,166 +0,0 @@
#include "cmd_scheduler.hpp"
#include <stdlib.h>
#include <string.h>
#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;
}
}
}

112
components/cmdscheduler/cmd_scheduler.hpp
View File

@ -1,112 +0,0 @@
#pragma once
#include <map>
#include <string>
#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<int32_t(Context* context)> call_cmd_t;
class CMD {
public:
call_cmd_t call_cmd;
string help_info;
int npara;
};
private:
map<string, CMD> 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

12
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -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;
}
}

273
components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.cpp
View File

@ -1,273 +0,0 @@
#include "step_motor_ctrl_script_cmder_module.hpp"
#include <string.h>
// #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;
});
}

23
components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.hpp
View File

@ -1,23 +0,0 @@
#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;
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

161
components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp
View File

@ -1,161 +0,0 @@
#include "xy_robot_script_cmder_module.hpp"
#include <string.h>
#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;
});
}

23
components/xy_robot_ctrl_module/xy_robot_script_cmder_module.hpp
View File

@ -1,23 +0,0 @@
#pragma once
#include <map>
#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<uint8_t, I_XYRobotCtrlModule*> 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
Write Preview
Loading…
Cancel
Save