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
0cdfb470af
commit
61021e00a9
10 changed files with 600 additions and 229 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. 197
      components/flash/znvs.cpp
  2. 77
      components/flash/znvs.hpp
  3. 167
      components/flash/znvs_bak.cpp
  4. 83
      components/flash/znvs_bak.hpp
  5. 42
      components/scriptcmder_module/xy_robot_script_cmder_module.cpp
  6. 206
      components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
  7. 21
      components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
  8. 13
      components/zcancmder_master_module/zcan_xy_robot_master_module.cpp
  9. 21
      components/zcancmder_module/zcan_xy_robot_module.cpp
  10. 2
      components/zprotocols/zcancmder

197
components/flash/znvs.cpp
View File

@ -2,163 +2,86 @@
#include <string.h>
#include "sdk\components\flash\zsimple_flash.hpp"
#define TAG "config"
#ifdef IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM
#include "zsimple_flash.hpp"
using namespace iflytop;
using namespace std;
#define MARK_S 0x123456
#define MARK_E 0x87654321
#define GET_CFG(typename) \
cfg_t* cfg = get_cfg(key); \
if (cfg == nullptr || cfg->type != kcfg_type_##typename) { \
allocate_cfg(key, kcfg_type_##typename, (uint8_t*)&default_val, sizeof(default_val)); \
cfg = get_cfg(key); \
} \
return *(typename*)cfg->val;
#define SET_CFG(typename) \
cfg_t* cfg = get_cfg(key); \
if (cfg == nullptr || cfg->type != kcfg_type_##typename) { \
allocate_cfg(key, kcfg_type_##typename, (uint8_t*)&val, sizeof(val)); \
cfg = get_cfg(key); \
} else { \
*(typename*)cfg->val = val; \
flush(); \
}
#define TAG "ZNVS"
#define MARK_S 0x12345678
#define MARK_E 0x87654321
#ifdef IFLYTOP_NVS_CONFIG_FLASH_SECTOR
ZNVS& ZNVS::ins() {
static ZNVS instance;
return instance;
}
void ZNVS::initialize() {
zsimple_flash_init(IFLYTOP_NVS_CONFIG_FLASH_SECTOR);
zsimple_flash_read((uint8_t*)&m_cfg, sizeof(m_cfg));
if (m_cfg.config_start != MARK_S || m_cfg.config_end != MARK_E) {
ZLOGW(TAG, "config uninitialized, initialize it");
memset(&m_cfg, 0, sizeof(m_cfg));
m_cfg.config_start = MARK_S;
m_cfg.config_end = MARK_E;
zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg));
} else {
ZLOGI(TAG, "config initialized");
}
static ZNVS s_ins;
return s_ins;
}
void ZNVS::factory_reset() {
memset(&m_cfg, 0, sizeof(m_cfg));
m_cfg.config_start = MARK_S;
m_cfg.config_end = MARK_E;
zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg));
void ZNVS::initialize() { zsimple_flash_init(IFLYTOP_NVS_CONFIG_FLASH_SECTOR); }
bool ZNVS::alloc_config(const char* key, uint8_t* data, size_t datalen) {
ZASSERT(m_index_num < ZARRAY_SIZE(m_index));
m_index[m_index_num].name = key;
m_index[m_index_num].len = datalen;
m_index[m_index_num].defaultval = data;
m_index_num++;
return true;
}
ZNVS::cfg_t* ZNVS::get_cfg(const char* key) {
for (int i = 0; i < IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM; i++) {
if (strcmp(m_cfg.cfgs[i].key, key) == 0) {
return &m_cfg.cfgs[i];
}
bool ZNVS::init_config() {
int configtotallen = 0;
for (int i = 0; i < m_index_num; i++) {
configtotallen += m_index[i].len;
}
return nullptr;
}
configtotallen = configtotallen + sizeof(config_t) /*head*/ + 4 /*tail*/;
void ZNVS::allocate_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len) {
cfg_t* cfg = get_cfg(key);
for (int i = 0; i < IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM; i++) {
if (m_cfg.cfgs[i].is_initialed == false) {
cfg = &m_cfg.cfgs[i];
strcpy(cfg->key, key);
cfg->is_initialed = true;
cfg->type = (uint8_t)type;
memcpy(cfg->val, default_val, len);
#if 0
/**
* @brief 刷新flash
*
* TODO:优化这里的代码，不要每次都刷新flash
*/
zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg));
#endif
break;
}
m_cfgcache = (config_t*)malloc(configtotallen);
ZASSERT(m_cfgcache);
m_cfgcachesize = configtotallen;
zsimple_flash_read((uint8_t*)m_cfgcache, m_cfgcachesize);
if (m_cfgcache->config_start != MARK_S || m_cfgcache->data_size != (m_cfgcachesize - sizeof(config_t))) {
ZLOGW(TAG, "config uninitialized, initialize it");
memset(m_cfgcache, 0, m_cfgcachesize);
m_cfgcache->config_start = MARK_S;
m_cfgcache->data_size = m_cfgcachesize - sizeof(config_t);
flush_all_to_configcache();
zsimple_flash_write((uint8_t*)m_cfgcache, m_cfgcachesize);
} else {
ZLOGI(TAG, "config initialized successfully");
}
inited = true;
return true;
}
ZNVS::cfg_t* ZNVS::get_and_create_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len) {
cfg_t* cfg = get_cfg(key);
if (cfg == nullptr) {
allocate_cfg(key, type, default_val, len);
cfg = get_cfg(key);
void ZNVS::flush_all_to_configcache() {
for (int i = 0; i < m_index_num; i++) {
memcpy(m_cfgcache->data, m_index[i].defaultval, m_index[i].len);
}
return cfg;
}
void ZNVS::dumpcfg() {
ZLOGI(TAG, "=================dump nvs config ==================");
ZLOGI(TAG, "=");
for (int i = 0; i < IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM; i++) {
if (m_cfg.cfgs[i].is_initialed) {
dumpcfg(&m_cfg.cfgs[i]);
bool ZNVS::get_config(const char* key, uint8_t* data, size_t len) {
ZASSERT(inited);
int dataoffset = 0;
for (int i = 0; i < m_index_num; i++) {
if (strcmp(m_index[i].name, key) == 0) {
ZASSERT(len == m_index[i].len);
memcpy(data, m_cfgcache->data + dataoffset, m_index[i].len);
return true;
}
dataoffset += m_index[i].len;
}
ZLOGI(TAG, "=============");
return false;
}
void ZNVS::dumpcfg(cfg_t* cfg) {
if (!cfg->is_initialed) return;
switch (cfg->type) {
case kcfg_type_int8_t:
ZLOGI(TAG, "= %s val:%d", cfg->key, *(int8_t*)cfg->val);
break;
case kcfg_type_int16_t:
ZLOGI(TAG, "= %s val:%d", cfg->key, *(int16_t*)cfg->val);
break;
case kcfg_type_int32_t:
ZLOGI(TAG, "= %s val:%d", cfg->key, *(int32_t*)cfg->val);
break;
case kcfg_type_uint8_t:
ZLOGI(TAG, "= %s val:%u", cfg->key, *(int8_t*)cfg->val);
break;
case kcfg_type_uint16_t:
ZLOGI(TAG, "= %s val:%u", cfg->key, *(int16_t*)cfg->val);
break;
case kcfg_type_uint32_t:
ZLOGI(TAG, "= %s val:%u", cfg->key, *(int32_t*)cfg->val);
break;
case kcfg_type_float:
ZLOGI(TAG, "= %s val:%f", cfg->key, *(float*)cfg->val);
break;
case kcfg_type_bool:
ZLOGI(TAG, "= %s val:%d", cfg->key, *(bool*)cfg->val);
break;
default:
ZLOGI(TAG, "= %s val:unknow type", cfg->key);
break;
bool ZNVS::set_config(const char* key, uint8_t* data, size_t len) {
ZASSERT(inited);
int dataoffset = 0;
for (int i = 0; i < m_index_num; i++) {
if (strcmp(m_index[i].name, key) == 0) {
memcpy(m_cfgcache->data + dataoffset, data, len);
return true;
}
dataoffset += m_index[i].len;
}
return;
return false;
}
int8_t ZNVS::get_config_int8(const char* key, int8_t default_val) { GET_CFG(int8_t); }
void ZNVS::set_config_int8(const char* key, int8_t val) { SET_CFG(int8_t); }
uint8_t ZNVS::get_config_uint8(const char* key, uint8_t default_val) { GET_CFG(uint8_t); }
void ZNVS::set_config_uint8(const char* key, uint8_t val) { SET_CFG(uint8_t); }
int16_t ZNVS::get_config_int16(const char* key, int16_t default_val) { GET_CFG(int16_t); }
void ZNVS::set_config_int16(const char* key, int16_t val) { SET_CFG(int16_t); }
uint16_t ZNVS::get_config_uint16(const char* key, uint16_t default_val) { GET_CFG(uint16_t); }
void ZNVS::set_config_uint16(const char* key, uint16_t val) { SET_CFG(uint16_t); }
int32_t ZNVS::get_config_int32(const char* key, int32_t default_val) { GET_CFG(int32_t); }
void ZNVS::set_config_int32(const char* key, int32_t val) { SET_CFG(int32_t); }
uint32_t ZNVS::get_config_uint32(const char* key, uint32_t default_val) { GET_CFG(uint32_t); }
void ZNVS::set_config_uint32(const char* key, uint32_t val) { SET_CFG(uint32_t); }
float ZNVS::get_config_float(const char* key, float default_val) { GET_CFG(float); }
void ZNVS::set_config_float(const char* key, float val) { SET_CFG(float); }
bool ZNVS::get_config_bool(const char* key, bool default_val) { GET_CFG(bool); }
void ZNVS::set_config_bool(const char* key, bool val) { SET_CFG(bool); }
void ZNVS::flush() { zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg)); }
void ZNVS::flush() { zsimple_flash_write((uint8_t*)m_cfgcache, m_cfgcachesize); }
#endif

77
components/flash/znvs.hpp
View File

@ -2,80 +2,49 @@
#include "project_configs.h"
#include "sdk/os/zos.hpp"
#ifdef IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM
#ifdef IFLYTOP_NVS_CONFIG_FLASH_SECTOR
namespace iflytop {
using namespace std;
class ZNVS {
typedef enum {
kcfg_type_int8_t,
kcfg_type_uint8_t,
kcfg_type_int16_t,
kcfg_type_uint16_t,
kcfg_type_int32_t,
kcfg_type_uint32_t,
kcfg_type_float,
kcfg_type_bool,
} type_t;
#pragma pack(1)
typedef struct {
char key[64];
uint8_t val[8];
uint8_t type;
bool is_initialed;
} cfg_t;
const char* name;
uint8_t* defaultval;
size_t len;
} cfg_index_t;
#pragma pack(1)
typedef struct {
uint32_t config_start;
cfg_t cfgs[IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM];
uint32_t config_end;
uint32_t data_size;
uint8_t data[];
} config_t;
#pragma pack()
config_t m_cfg;
cfg_index_t m_index[20];
int m_index_num = 0;
bool inited = false;
config_t* m_cfgcache = nullptr;
size_t m_cfgcachesize = 0;
public:
static ZNVS& ins();
void initialize();
void factory_reset();
void dumpcfg();
void flush();
bool alloc_config(const char* key, uint8_t* data, size_t datalen);
bool init_config();
int8_t get_config_int8(const char* key, int8_t default_val);
void set_config_int8(const char* key, int8_t val);
bool get_config(const char* key, uint8_t* data, size_t len);
bool set_config(const char* key, uint8_t* data, size_t len);
uint8_t get_config_uint8(const char* key, uint8_t default_val);
void set_config_uint8(const char* key, uint8_t val);
int16_t get_config_int16(const char* key, int16_t default_val);
void set_config_int16(const char* key, int16_t val);
uint16_t get_config_uint16(const char* key, uint16_t default_val);
void set_config_uint16(const char* key, uint16_t val);
int32_t get_config_int32(const char* key, int32_t default_val);
void set_config_int32(const char* key, int32_t val);
uint32_t get_config_uint32(const char* key, uint32_t default_val);
void set_config_uint32(const char* key, uint32_t val);
float get_config_float(const char* key, float default_val);
void set_config_float(const char* key, float val);
bool get_config_bool(const char* key, bool default_val);
void set_config_bool(const char* key, bool val);
void flush();
private:
void dumpcfg(cfg_t* cfg);
cfg_t* get_cfg(const char* key);
void allocate_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len);
cfg_t* get_and_create_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len);
void flush_all_to_configcache();
};
#endif
} // namespace iflytop
#endif

167
components/flash/znvs_bak.cpp
View File

@ -0,0 +1,167 @@
#if 0
#include "znvs.hpp"
#include <string.h>
#include "sdk\components\flash\zsimple_flash.hpp"
#define TAG "config"
#ifdef IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM
using namespace iflytop;
using namespace std;
#define MARK_S 0x123456
#define MARK_E 0x87654321
#define GET_CFG(typename) \
cfg_t* cfg = get_cfg(key); \
if (cfg == nullptr || cfg->type != kcfg_type_##typename) { \
allocate_cfg(key, kcfg_type_##typename, (uint8_t*)&default_val, sizeof(default_val)); \
cfg = get_cfg(key); \
} \
return *(typename*)cfg->val;
#define SET_CFG(typename) \
cfg_t* cfg = get_cfg(key); \
if (cfg == nullptr || cfg->type != kcfg_type_##typename) { \
allocate_cfg(key, kcfg_type_##typename, (uint8_t*)&val, sizeof(val)); \
cfg = get_cfg(key); \
} else { \
*(typename*)cfg->val = val; \
flush(); \
}
ZNVS& ZNVS::ins() {
static ZNVS instance;
return instance;
}
void ZNVS::initialize() {
zsimple_flash_init(IFLYTOP_NVS_CONFIG_FLASH_SECTOR);
zsimple_flash_read((uint8_t*)&m_cfg, sizeof(m_cfg));
if (m_cfg.config_start != MARK_S || m_cfg.config_end != MARK_E) {
ZLOGW(TAG, "config uninitialized, initialize it");
memset(&m_cfg, 0, sizeof(m_cfg));
m_cfg.config_start = MARK_S;
m_cfg.config_end = MARK_E;
zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg));
} else {
ZLOGI(TAG, "config initialized");
}
}
void ZNVS::factory_reset() {
memset(&m_cfg, 0, sizeof(m_cfg));
m_cfg.config_start = MARK_S;
m_cfg.config_end = MARK_E;
zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg));
}
ZNVS::cfg_t* ZNVS::get_cfg(const char* key) {
for (int i = 0; i < IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM; i++) {
if (strcmp(m_cfg.cfgs[i].key, key) == 0) {
return &m_cfg.cfgs[i];
}
}
return nullptr;
}
void ZNVS::allocate_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len) {
cfg_t* cfg = get_cfg(key);
for (int i = 0; i < IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM; i++) {
if (m_cfg.cfgs[i].is_initialed == false) {
cfg = &m_cfg.cfgs[i];
strcpy(cfg->key, key);
cfg->is_initialed = true;
cfg->type = (uint8_t)type;
memcpy(cfg->val, default_val, len);
#if 0
/**
* @brief 刷新flash
*
* TODO:优化这里的代码，不要每次都刷新flash
*/
zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg));
#endif
break;
}
}
}
ZNVS::cfg_t* ZNVS::get_and_create_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len) {
cfg_t* cfg = get_cfg(key);
if (cfg == nullptr) {
allocate_cfg(key, type, default_val, len);
cfg = get_cfg(key);
}
return cfg;
}
void ZNVS::dumpcfg() {
ZLOGI(TAG, "=================dump nvs config ==================");
ZLOGI(TAG, "=");
for (int i = 0; i < IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM; i++) {
if (m_cfg.cfgs[i].is_initialed) {
dumpcfg(&m_cfg.cfgs[i]);
}
}
ZLOGI(TAG, "=============");
}
void ZNVS::dumpcfg(cfg_t* cfg) {
if (!cfg->is_initialed) return;
switch (cfg->type) {
case kcfg_type_int8_t:
ZLOGI(TAG, "= %s val:%d", cfg->key, *(int8_t*)cfg->val);
break;
case kcfg_type_int16_t:
ZLOGI(TAG, "= %s val:%d", cfg->key, *(int16_t*)cfg->val);
break;
case kcfg_type_int32_t:
ZLOGI(TAG, "= %s val:%d", cfg->key, *(int32_t*)cfg->val);
break;
case kcfg_type_uint8_t:
ZLOGI(TAG, "= %s val:%u", cfg->key, *(int8_t*)cfg->val);
break;
case kcfg_type_uint16_t:
ZLOGI(TAG, "= %s val:%u", cfg->key, *(int16_t*)cfg->val);
break;
case kcfg_type_uint32_t:
ZLOGI(TAG, "= %s val:%u", cfg->key, *(int32_t*)cfg->val);
break;
case kcfg_type_float:
ZLOGI(TAG, "= %s val:%f", cfg->key, *(float*)cfg->val);
break;
case kcfg_type_bool:
ZLOGI(TAG, "= %s val:%d", cfg->key, *(bool*)cfg->val);
break;
default:
ZLOGI(TAG, "= %s val:unknow type", cfg->key);
break;
}
return;
}
int8_t ZNVS::get_config_int8(const char* key, int8_t default_val) { GET_CFG(int8_t); }
void ZNVS::set_config_int8(const char* key, int8_t val) { SET_CFG(int8_t); }
uint8_t ZNVS::get_config_uint8(const char* key, uint8_t default_val) { GET_CFG(uint8_t); }
void ZNVS::set_config_uint8(const char* key, uint8_t val) { SET_CFG(uint8_t); }
int16_t ZNVS::get_config_int16(const char* key, int16_t default_val) { GET_CFG(int16_t); }
void ZNVS::set_config_int16(const char* key, int16_t val) { SET_CFG(int16_t); }
uint16_t ZNVS::get_config_uint16(const char* key, uint16_t default_val) { GET_CFG(uint16_t); }
void ZNVS::set_config_uint16(const char* key, uint16_t val) { SET_CFG(uint16_t); }
int32_t ZNVS::get_config_int32(const char* key, int32_t default_val) { GET_CFG(int32_t); }
void ZNVS::set_config_int32(const char* key, int32_t val) { SET_CFG(int32_t); }
uint32_t ZNVS::get_config_uint32(const char* key, uint32_t default_val) { GET_CFG(uint32_t); }
void ZNVS::set_config_uint32(const char* key, uint32_t val) { SET_CFG(uint32_t); }
float ZNVS::get_config_float(const char* key, float default_val) { GET_CFG(float); }
void ZNVS::set_config_float(const char* key, float val) { SET_CFG(float); }
bool ZNVS::get_config_bool(const char* key, bool default_val) { GET_CFG(bool); }
void ZNVS::set_config_bool(const char* key, bool val) { SET_CFG(bool); }
void ZNVS::flush() { zsimple_flash_write((uint8_t*)&m_cfg, sizeof(m_cfg)); }
#endif
#endif

83
components/flash/znvs_bak.hpp
View File

@ -0,0 +1,83 @@
#pragma once
#if 0
#include "project_configs.h"
#include "sdk/os/zos.hpp"
#ifdef IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM
namespace iflytop {
using namespace std;
class ZNVS {
typedef enum {
kcfg_type_int8_t,
kcfg_type_uint8_t,
kcfg_type_int16_t,
kcfg_type_uint16_t,
kcfg_type_int32_t,
kcfg_type_uint32_t,
kcfg_type_float,
kcfg_type_bool,
} type_t;
#pragma pack(1)
typedef struct {
char key[64];
uint8_t val[8];
uint8_t type;
bool is_initialed;
} cfg_t;
typedef struct {
uint32_t config_start;
cfg_t cfgs[IFLYTOP_NVS_CONFIG_MAX_ITEM_NUM];
uint32_t config_end;
} config_t;
#pragma pack()
config_t m_cfg;
public:
static ZNVS& ins();
void initialize();
void factory_reset();
void dumpcfg();
void flush();
int8_t get_config_int8(const char* key, int8_t default_val);
void set_config_int8(const char* key, int8_t val);
uint8_t get_config_uint8(const char* key, uint8_t default_val);
void set_config_uint8(const char* key, uint8_t val);
int16_t get_config_int16(const char* key, int16_t default_val);
void set_config_int16(const char* key, int16_t val);
uint16_t get_config_uint16(const char* key, uint16_t default_val);
void set_config_uint16(const char* key, uint16_t val);
int32_t get_config_int32(const char* key, int32_t default_val);
void set_config_int32(const char* key, int32_t val);
uint32_t get_config_uint32(const char* key, uint32_t default_val);
void set_config_uint32(const char* key, uint32_t val);
float get_config_float(const char* key, float default_val);
void set_config_float(const char* key, float val);
bool get_config_bool(const char* key, bool default_val);
void set_config_bool(const char* key, bool val);
private:
void dumpcfg(cfg_t* cfg);
cfg_t* get_cfg(const char* key);
void allocate_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len);
cfg_t* get_and_create_cfg(const char* key, type_t type, uint8_t* default_val, uint8_t len);
};
} // namespace iflytop
#endif
#endif

42
components/scriptcmder_module/xy_robot_script_cmder_module.cpp
View File

@ -112,16 +112,17 @@ void XYRobotScriptCmderModule::regcmd() { //
});
m_cmdScheduler->registerCmd("xy_robot_ctrl_move_by", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
SCRIPT_CMDER_CHECK(argc == 4);
SCRIPT_CMDER_CHECK(argc == 5);
int32_t id = atoi(argv[1]);
int32_t dx = atoi(argv[2]);
int32_t dy = atoi(argv[3]);
int32_t v = atoi(argv[4]);
I_XYRobotCtrlModule* module = findXYRobot(id);
SCRIPT_CMDER_CHECK(module != nullptr);
int32_t ack = module->move_by(dx, dy, [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); });
int32_t ack = module->move_by(dx, dy, v, [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); });
ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
});
@ -296,4 +297,41 @@ void XYRobotScriptCmderModule::regcmd() { //
ZLOGI(TAG, " run_to_zero_dec :%d", status.run_to_zero_dec);
}
});
m_cmdScheduler->registerCmd("xy_robot_ctrl_flsuh", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
SCRIPT_CMDER_CHECK(argc == 2);
int32_t id = atoi(argv[1]);
I_XYRobotCtrlModule* module = findXYRobot(id);
SCRIPT_CMDER_CHECK(module != nullptr);
int32_t ack = module->flush();
ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
});
m_cmdScheduler->registerCmd("xy_robot_ctrl_factory_reset", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
SCRIPT_CMDER_CHECK(argc == 2);
int32_t id = atoi(argv[1]);
I_XYRobotCtrlModule* module = findXYRobot(id);
SCRIPT_CMDER_CHECK(module != nullptr);
int32_t ack = module->factory_reset();
ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
});
m_cmdScheduler->registerCmd("xy_robot_ctrl_move_by_no_limit", [this](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
SCRIPT_CMDER_CHECK(argc == 5);
int32_t id = atoi(argv[1]);
int32_t dx = atoi(argv[2]);
int32_t dy = atoi(argv[3]);
int32_t v = atoi(argv[4]);
I_XYRobotCtrlModule* module = findXYRobot(id);
SCRIPT_CMDER_CHECK(module != nullptr);
int32_t ack = module->move_by_no_limit(dx, dy, v, [this](int32_t status) { ZLOGI(TAG, "move_by status:%d", status); });
ZLOGI(TAG, "ack:%d:%s", ack, err::error2str(ack));
});
}

206
components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
View File

@ -3,13 +3,14 @@
#include <string.h>
#include "sdk/components/errorcode/errorcode.hpp"
#include "sdk\components\flash\znvs.hpp"
using namespace iflytop;
using namespace std;
#define TAG "XYRobotCtrlModule"
void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, //
IStepperMotor* stepM2, //
ZGPIO* Xgpio, //
ZGPIO* Ygpio) {
ZGPIO* Ygpio, const char* flashmark) {
m_stepM1 = stepM1;
m_stepM2 = stepM2;
m_Xgpio = Xgpio;
@ -20,9 +21,15 @@ void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, //
ZASSERT(m_Ygpio);
m_lock.init();
create_default_cfg(m_cfg);
set_base_param(m_cfg);
// m_statu_lock.init();
m_flashmark = flashmark;
// base_param_t cfg;
if (m_flashmark) {
ZASSERT(ZNVS::ins().get_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg)));
} else {
create_default_cfg(m_cfg);
}
active_cfg();
m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal);
}
@ -37,12 +44,21 @@ int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int speed, action_cb_st
if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) {
runspeed = m_cfg.maxspeed;
}
if (m_cfg.min_x != 0 && x < m_cfg.min_x) {
x = m_cfg.min_x;
}
if (m_cfg.max_x != 0 && x > m_cfg.max_x) {
x = m_cfg.max_x;
}
if (m_cfg.min_y != 0 && y < m_cfg.min_y) {
y = m_cfg.min_y;
}
if (m_cfg.max_y != 0 && y > m_cfg.max_y) {
y = m_cfg.max_y;
}
// int32_t m1pos, m2pos;
m_thread.stop();
/**
* @TODO:Ìí¼ÓÏÞλ¼ì²é
*/
m_thread.start([this, x, y, runspeed, status_cb]() {
_motor_move_to(x, y, runspeed, m_cfg.acc, m_cfg.dec);
while (!_motor_is_reach_target()) {
@ -57,21 +73,73 @@ int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y, int speed, action_cb_st
return 0;
}
int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, action_cb_status_t status_cb) {
int32_t XYRobotCtrlModule::move_by_no_limit(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) {
zlock_guard lock(m_lock);
ZLOGI(TAG, "move_by_no_limit dx:%d dy:%d", dx, dy);
m_thread.stop();
_motor_stop();
if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) {
speed = m_cfg.maxspeed;
}
int32_t s_x, s_y, to_x, to_y;
getnowpos(s_x, s_y);
to_x = s_x + dx;
to_y = s_y + dy;
m_thread.start([this, to_x, to_y, speed, status_cb]() {
_motor_move_to(to_x, to_y, speed, m_cfg.acc, m_cfg.dec);
while (!_motor_is_reach_target()) {
if (m_thread.getExitFlag()) break;
vTaskDelay(10);
}
_motor_stop();
int32_t end_x, end_y;
getnowpos(end_x, end_y);
call_status_cb(status_cb, 0);
});
return 0;
};
int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) {
zlock_guard lock(m_lock);
ZLOGI(TAG, "move_by dx:%d dy:%d", dx, dy);
m_thread.stop();
_motor_stop();
/**
* @TODO:Ìí¼ÓÏÞλ¼ì²é
*/
m_thread.start([this, dx, dy, status_cb]() {
int32_t s_x, s_y, to_x, to_y;
getnowpos(s_x, s_y);
to_x = s_x + dx;
to_y = s_y + dy;
_motor_move_to(to_x, to_y, m_cfg.maxspeed, m_cfg.acc, m_cfg.dec);
if (m_cfg.maxspeed != 0 && speed > m_cfg.maxspeed) {
speed = m_cfg.maxspeed;
}
int32_t s_x, s_y, to_x, to_y;
getnowpos(s_x, s_y);
to_x = s_x + dx;
to_y = s_y + dy;
if (m_cfg.min_x != 0 && to_x < m_cfg.min_x) {
to_x = m_cfg.min_x;
}
if (m_cfg.max_x != 0 && to_x > m_cfg.max_x) {
to_x = m_cfg.max_x;
}
if (m_cfg.min_y != 0 && to_y < m_cfg.min_y) {
to_y = m_cfg.min_y;
}
if (m_cfg.max_y != 0 && to_y > m_cfg.max_y) {
to_y = m_cfg.max_y;
}
if (to_x == s_x && to_y == s_y) {
call_status_cb(status_cb, 0);
return 0;
}
m_thread.start([this, to_x, to_y, speed, status_cb]() {
_motor_move_to(to_x, to_y, speed, m_cfg.acc, m_cfg.dec);
while (!_motor_is_reach_target()) {
if (m_thread.getExitFlag()) break;
@ -208,7 +276,7 @@ int32_t XYRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info) {
getnowpos(debug_info.x, debug_info.y);
return 0;
}
void XYRobotCtrlModule::create_default_cfg(base_param_t& cfg) {
void XYRobotCtrlModule::create_default_cfg(flash_config_t& cfg) {
memset(&cfg, 0, sizeof(cfg));
cfg.robot_type = corexy;
cfg.x_shaft = 0;
@ -237,18 +305,64 @@ void XYRobotCtrlModule::create_default_cfg(base_param_t& cfg) {
return;
}
int32_t XYRobotCtrlModule::set_base_param(const base_param_t& param) {
int32_t XYRobotCtrlModule::set_base_param(const base_param_t& inparam) {
zlock_guard lock(m_lock);
m_cfg = param;
m_cfg.robot_type = inparam.robot_type;
m_cfg.x_shaft = inparam.x_shaft;
m_cfg.y_shaft = inparam.y_shaft;
m_cfg.ihold = inparam.ihold;
m_cfg.irun = inparam.irun;
m_cfg.iholddelay = inparam.iholddelay;
m_cfg.distance_scale = inparam.distance_scale;
m_cfg.shift_x = inparam.shift_x;
m_cfg.shift_y = inparam.shift_y;
m_cfg.acc = inparam.acc;
m_cfg.dec = inparam.dec;
m_cfg.breakdec = inparam.breakdec;
m_cfg.maxspeed = inparam.maxspeed;
m_cfg.min_x = inparam.min_x;
m_cfg.max_x = inparam.max_x;
m_cfg.min_y = inparam.min_y;
m_cfg.max_y = inparam.max_y;
m_cfg.run_to_zero_max_d = inparam.run_to_zero_max_d;
m_cfg.leave_from_zero_max_d = inparam.leave_from_zero_max_d;
m_cfg.run_to_zero_speed = inparam.run_to_zero_speed;
m_cfg.run_to_zero_dec = inparam.run_to_zero_dec;
active_cfg();
return 0;
};
void XYRobotCtrlModule::active_cfg() {
m_stepM1->setScale(m_cfg.distance_scale / 1000.0);
m_stepM2->setScale(m_cfg.distance_scale / 1000.0);
m_stepM1->setIHOLD_IRUN(m_cfg.ihold, m_cfg.irun, m_cfg.iholddelay);
m_stepM2->setIHOLD_IRUN(m_cfg.ihold, m_cfg.irun, m_cfg.iholddelay);
return 0;
};
}
int32_t XYRobotCtrlModule::get_base_param(base_param_t& ack) {
zlock_guard lock(m_lock);
ack = m_cfg;
ack.robot_type = m_cfg.robot_type;
ack.x_shaft = m_cfg.x_shaft;
ack.y_shaft = m_cfg.y_shaft;
ack.ihold = m_cfg.ihold;
ack.irun = m_cfg.irun;
ack.iholddelay = m_cfg.iholddelay;
ack.distance_scale = m_cfg.distance_scale;
ack.shift_x = m_cfg.shift_x;
ack.shift_y = m_cfg.shift_y;
ack.acc = m_cfg.acc;
ack.dec = m_cfg.dec;
ack.breakdec = m_cfg.breakdec;
ack.maxspeed = m_cfg.maxspeed;
ack.min_x = m_cfg.min_x;
ack.max_x = m_cfg.max_x;
ack.min_y = m_cfg.min_y;
ack.max_y = m_cfg.max_y;
ack.run_to_zero_max_d = m_cfg.run_to_zero_max_d;
ack.leave_from_zero_max_d = m_cfg.leave_from_zero_max_d;
ack.run_to_zero_speed = m_cfg.run_to_zero_speed;
ack.run_to_zero_dec = m_cfg.run_to_zero_dec;
return 0;
};
@ -456,7 +570,47 @@ void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos,
bool XYRobotCtrlModule::_motor_is_reach_target() { return m_stepM1->isReachTarget() && m_stepM2->isReachTarget(); }
void XYRobotCtrlModule::call_status_cb(action_cb_status_t cb, int32_t status) {
if (cb) {
cb(status);
}
if (cb) cb(status);
}
void XYRobotCtrlModule::dumpcfg() {
ZLOGI(TAG, "robot_type :%d", m_cfg.robot_type);
ZLOGI(TAG, "x_shaft :%d", m_cfg.x_shaft);
ZLOGI(TAG, "y_shaft :%d", m_cfg.y_shaft);
ZLOGI(TAG, "ihold :%d", m_cfg.ihold);
ZLOGI(TAG, "irun :%d", m_cfg.irun);
ZLOGI(TAG, "iholddelay :%d", m_cfg.iholddelay);
ZLOGI(TAG, "distance_scale :%d", m_cfg.distance_scale);
ZLOGI(TAG, "shift_x :%d", m_cfg.shift_x);
ZLOGI(TAG, "shift_y :%d", m_cfg.shift_y);
ZLOGI(TAG, "acc :%d", m_cfg.acc);
ZLOGI(TAG, "dec :%d", m_cfg.dec);
ZLOGI(TAG, "breakdec :%d", m_cfg.breakdec);
ZLOGI(TAG, "maxspeed :%d", m_cfg.maxspeed);
ZLOGI(TAG, "min_x :%d", m_cfg.min_x);
ZLOGI(TAG, "max_x :%d", m_cfg.max_x);
ZLOGI(TAG, "min_y :%d", m_cfg.min_y);
ZLOGI(TAG, "max_y :%d", m_cfg.max_y);
ZLOGI(TAG, "run_to_zero_max_d :%d", m_cfg.run_to_zero_max_d);
ZLOGI(TAG, "leave_from_zero_max_d :%d", m_cfg.leave_from_zero_max_d);
ZLOGI(TAG, "run_to_zero_speed :%d", m_cfg.run_to_zero_speed);
ZLOGI(TAG, "run_to_zero_dec :%d", m_cfg.run_to_zero_dec);
}
int32_t XYRobotCtrlModule::flush() { //
ZLOGI(TAG, "flush");
if (m_flashmark) {
ZNVS::ins().set_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg));
ZNVS::ins().flush();
}
return 0;
};
int32_t XYRobotCtrlModule::factory_reset() { //
ZLOGI(TAG, "factory_reset");
create_default_cfg(m_cfg);
if (m_flashmark) {
ZNVS::ins().set_config(m_flashmark, (uint8_t*)&m_cfg, sizeof(m_cfg));
ZNVS::ins().flush();
}
active_cfg();
return 0;
};

21
components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
View File

@ -22,22 +22,27 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule {
// uint8_t m_status = 0;
base_param_t m_cfg;
// base_param_t m_cfg;
int32_t m_zero_shift_x = 0;
int32_t m_zero_shift_y = 0;
const char* m_flashmark = nullptr;
flash_config_t m_cfg;
zmutex m_lock;
// zmutex m_statu_lock;
public:
void initialize(IStepperMotor* stepM1, //
IStepperMotor* stepM2, //
ZGPIO* Xgpio, //
ZGPIO* Ygpio);
IStepperMotor* stepM2, //
ZGPIO* Xgpio, //
ZGPIO* Ygpio, const char* flashmark);
virtual int32_t move_to(int32_t x, int32_t y, int speed, action_cb_status_t status_cb) override;
virtual int32_t move_by(int32_t dx, int32_t dy, action_cb_status_t status_cb) override;
virtual int32_t move_by(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override;
virtual int32_t move_by_no_limit(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) 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, int32_t y, action_cb_status_t status_cb) override;
virtual int32_t enable(bool venable) override;
@ -48,14 +53,18 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule {
virtual int32_t read_status(status_t& status) override;
virtual int32_t read_detailed_status(detailed_status_t& debug_info) override;
void create_default_cfg(base_param_t& ack);
static void create_default_cfg(flash_config_t& cfg);
virtual int32_t set_base_param(const base_param_t& param) override;
virtual int32_t get_base_param(base_param_t& ack) override;
virtual int32_t flush() override;
virtual int32_t factory_reset() override;
void loop();
void dumpcfg();
private:
void active_cfg();
void computeTargetMotorPos();
void getnowpos(int32_t& x, int32_t& y);

13
components/zcancmder_master_module/zcan_xy_robot_master_module.cpp
View File

@ -33,8 +33,11 @@ class ZCANXYRobotCtrlMasterModule : public I_XYRobotCtrlModule {
ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_to, OVERTIME, cmd->x = x; cmd->y = y; cmd->speed = speed;);
}
virtual int32_t move_by(int32_t dx, int32_t dy, action_cb_status_t status_cb) override { //
ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_by, OVERTIME, cmd->dx = dx; cmd->dy = dy;);
virtual int32_t move_by(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override { //
ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_by, OVERTIME, cmd->dx = dx; cmd->dy = dy; cmd->speed = speed;);
}
virtual int32_t move_by_no_limit(int32_t dx, int32_t dy, int speed, action_cb_status_t status_cb) override { //
ZCAN_SEND_CMD_WITH_STATUS_CB(kcmd_xy_robot_ctrl_move_by_no_limit, OVERTIME, cmd->dx = dx; cmd->dy = dy; cmd->speed = speed;);
}
virtual int32_t move_to_zero(action_cb_status_t status_cb) override {
@ -79,6 +82,12 @@ class ZCANXYRobotCtrlMasterModule : public I_XYRobotCtrlModule {
virtual int32_t get_base_param(base_param_t& ack) override { //
ZCAN_SEND_CMD(kcmd_xy_robot_ctrl_get_base_param, ack, OVERTIME);
}
virtual int32_t flush() override { //
ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_flush, OVERTIME);
}
virtual int32_t factory_reset() override { //
ZCAN_SEND_CMD_NO_ACK(kcmd_xy_robot_ctrl_factory_reset, OVERTIME);
}
};
I_XYRobotCtrlModule* create_xyrobot_ctrl_module(ZCanCommnaderMaster* cancmder, int id) {

21
components/zcancmder_module/zcan_xy_robot_module.cpp
View File

@ -54,7 +54,18 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
END_PP();
PROCESS_PACKET(kcmd_xy_robot_ctrl_move_by, m_id) { //
errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, [this, cmdheader](int32_t status) {
errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, cmd->speed, [this, cmdheader](int32_t status) {
osDelay(5); // 用来保证回执消息在前面
kcmd_xy_robot_ctrl_move_by_report_t report = {0};
ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_by exec_status:%d", status);
report.exec_status = status;
m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report));
});
}
END_PP();
PROCESS_PACKET(kcmd_xy_robot_ctrl_move_by_no_limit, m_id) { //
errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy, cmd->speed, [this, cmdheader](int32_t status) {
osDelay(5); // 用来保证回执消息在前面
kcmd_xy_robot_ctrl_move_by_report_t report = {0};
ZLOGI(TAG, "kcmd_xy_robot_ctrl_move_by exec_status:%d", status);
@ -93,6 +104,14 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
errorcode = m_xyRobotCtrlModule->get_base_param(ack->ack);
}
END_PP();
PROCESS_PACKET(kcmd_xy_robot_ctrl_flush, m_id) { //
errorcode = m_xyRobotCtrlModule->flush();
}
END_PP();
PROCESS_PACKET(kcmd_xy_robot_ctrl_factory_reset, m_id) { //
errorcode = m_xyRobotCtrlModule->factory_reset();
}
END_PP();
// PROCESS_PACKET(kcmd_xy_robot_ctrl_set_warning_limit_param, m_id) { //
// errorcode = m_xyRobotCtrlModule->set_warning_limit_param(cmd->opt_type, cmd->param, ack->ack);

2
components/zprotocols/zcancmder

@ -1 +1 @@
Subproject commit 59be09e6b8a14de2e702e376210957783bfbf42e
Subproject commit 9199515dfc8db3a0760429d918e96cf2d8d31f32
Write Preview
Loading…
Cancel
Save