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

udpate code

master
zhaohe 2 years ago
parent
66ce8c1962
commit
967d39b1f8
9 changed files with 200 additions and 108 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. 59
      components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
  2. 19
      components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
  3. 66
      components/zcancmder/cmd/basic_bean.hpp
  4. 78
      components/zcancmder/cmd/c1006_module_cmd.hpp
  5. 13
      components/zcancmder/zcanreceiver.cpp
  6. 5
      components/zcancmder/zcanreceiver.hpp
  7. 8
      components/zcancmder_module/zcan_basic_order_module.cpp
  8. 51
      components/zcancmder_module/zcan_xy_robot_module.cpp
  9. 9
      components/zcancmder_module/zcan_xy_robot_module.hpp

59
components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
View File

@ -1,6 +1,7 @@
#include "xy_robot_ctrl_module.hpp"
using namespace iflytop;
using namespace std;
#define TAG "XYRobotCtrlModule"
void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, //
IStepperMotor* stepM2, //
ZGPIO* Xgpio, //
@ -14,9 +15,11 @@ void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, //
m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal);
}
void XYRobotCtrlModule::enable(bool venable) {
int32_t XYRobotCtrlModule::enable(bool venable) {
ZLOGI(TAG, "enable:%d", venable);
m_stepM1->enable(venable);
m_stepM2->enable(venable);
return 0;
}
int32_t XYRobotCtrlModule::getMotor1Pos() { return m_stepM1->getXACTUAL() * m_distanceK; }
@ -29,28 +32,48 @@ void XYRobotCtrlModule::getNowPos(int32_t& x, int32_t& y) {
y = getMotor1Pos() + getMotor2Pos();
}
void XYRobotCtrlModule::move_to(int32_t x, int32_t y) {
/**
* @brief
*/
m_thread.stop();
m_thread.start([this]() {
// 计算dpos1,dpos2
int32_t XYRobotCtrlModule::move_to(int32_t x, int32_t y) {
ZLOGI(TAG, "move_to x:%d y:%d", x, y);
int32_t m1pos, m2pos;
inverse_kinematics(x, y, m1pos, m2pos);
/**
* @brief 等待电机运行到到位
*/
m_thread.stop();
m_thread.start([this,m1pos,m2pos]() {
ZLOGI("XYRobotCtrlModule", "m1pos:%d m2pos:%d", m1pos, m2pos);
});
return 0;
}
int32_t XYRobotCtrlModule::move_by(int32_t dx, int32_t dy) {
ZLOGI(TAG, "move_by dx:%d dy:%d", dx, dy);
return 0;
}
int32_t XYRobotCtrlModule::move_to_zero() {
ZLOGI(TAG, "move_to_zero");
return 0;
}
int32_t XYRobotCtrlModule::move_to_with_calibrate(int32_t x, int32_t y) {
ZLOGI(TAG, "move_to_with_calibrate x:%d y:%d", x, y);
return 0;
}
int32_t XYRobotCtrlModule::stop() {
ZLOGI(TAG, "stop");
return 0;
}
void XYRobotCtrlModule::move_by(int32_t dx, int32_t dy) {}
void XYRobotCtrlModule::move_to_zero() {}
void XYRobotCtrlModule::move_to_with_calibrate(int32_t x, int32_t y) {}
void XYRobotCtrlModule::stop() {}
uint8_t XYRobotCtrlModule::read_status() {}
uint8_t XYRobotCtrlModule::read_status(uint8_t* module_statu, int32_t* x, int32_t* y) { return 0; }
void XYRobotCtrlModule::set_acc(int32_t acc) {}
void XYRobotCtrlModule::set_dec(int32_t dec) {}
int32_t XYRobotCtrlModule::set_acc(int32_t acc) {
ZLOGI(TAG, "set_acc acc:%d", acc);
return 0;
}
int32_t XYRobotCtrlModule::set_dec(int32_t dec) {
ZLOGI(TAG, "set_dec dec:%d", dec);
return 0;
}
int32_t XYRobotCtrlModule::set_speed(int32_t speed) {
ZLOGI(TAG, "set_speed speed:%d", speed);
return 0;
}
void XYRobotCtrlModule::loop() {}

19
components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp
View File

@ -26,18 +26,19 @@ class XYRobotCtrlModule {
ZGPIO* Ygpio, //
float distanceK);
void enable(bool venable);
int32_t enable(bool venable);
void move_to(int32_t x, int32_t y);
void move_by(int32_t dx, int32_t dy);
void move_to_zero();
void move_to_with_calibrate(int32_t x, int32_t y);
void stop();
int32_t move_to(int32_t x, int32_t y);
int32_t move_by(int32_t dx, int32_t dy);
int32_t move_to_zero();
int32_t move_to_with_calibrate(int32_t x, int32_t y);
int32_t stop();
uint8_t read_status();
uint8_t read_status(uint8_t* module_statu, int32_t* x, int32_t* y);
void set_acc(int32_t acc);
void set_dec(int32_t dec);
int32_t set_acc(int32_t acc);
int32_t set_dec(int32_t dec);
int32_t set_speed(int32_t speed);
void loop();

66
components/zcancmder/cmd/basic_bean.hpp
View File

@ -3,10 +3,28 @@
/*******************************************************************************
* MARCO *
*******************************************************************************/
#define SUBPACKET(name, ...) \
struct { \
__VA_ARGS__ \
} name
#define ZPACKET_STRUCT(ordername, type, ...) \
typedef struct { \
__VA_ARGS__ \
} ordername##_##type##_t
#define PROCESS_PACKET(ordername, varid) \
if (rxcmd->iscmd(ordername)) { \
auto* cmd = rxcmd->get_data_as<ordername##_##cmd##_t>(); \
auto* ack = (ordername##_##ack##_t*)m_txbuf; \
uint32_t errorcode = 0; \
if (cmd->id == varid) { \
ack->id = cmd->id;
#define END_PROCESS_PACKET() \
if (errorcode == 0) { \
m_cancmder->sendAck(rxcmd->get_cmdheader(), m_txbuf, sizeof(*ack)); \
} else { \
m_cancmder->sendErrorAck(rxcmd->get_cmdheader(), errorcode); \
} \
} \
return; \
}
/*******************************************************************************
* STRUCT *
@ -30,20 +48,34 @@ typedef enum {
kpt_cmd_exec_status_report = 3,
} PacketType_t;
#define CMDID(cmdid, subcmdid) ((cmdid << 8) + subcmdid)
typedef enum {
kcmd_ping = 0,
kcmd_read_io = 1,
kcmd_set_io = 2,
kcmd_readadc_raw = 3,
kcmd_m211887_operation = 1000, // 维萨拉臭氧传感器
kcmd_read_presure_sensor = 1001, // 压力传感器
kcmd_triple_warning_light_ctl = 1002, // 三色警示灯控制
kcmd_high_power_electrical_ctl = 1003, // 大功率电源控制
kcmd_peristaltic_pump_ctl = 1004, // 液泵控制
kcmd_read_huacheng_pressure_sensor = 1005, // 华诚压力传感器
kc1006_xy_robot_ctrl = 1006, // XY机器人控制
kcmd_ping = CMDID(0, 0),
kcmd_read_io = CMDID(1, 0),
kcmd_set_io = CMDID(2, 0),
kcmd_readadc_raw = CMDID(3, 0),
kcmd_m211887_operation = CMDID(1000, 0), // 维萨拉臭氧传感器
kcmd_read_presure_sensor = CMDID(1001, 0), // 压力传感器
kcmd_triple_warning_light_ctl = CMDID(1002, 0), // 三色警示灯控制
kcmd_high_power_electrical_ctl = CMDID(1003, 0), // 大功率电源控制
kcmd_peristaltic_pump_ctl = CMDID(1004, 0), // 液泵控制
kcmd_read_huacheng_pressure_sensor = CMDID(1005, 0), // 华诚压力传感器
/*******************************************************************************
* 1006 *
*******************************************************************************/
kcmd_xy_robot_ctrl_enable = CMDID(1006, 0),
kcmd_xy_robot_ctrl_stop = CMDID(1006, 1),
kcmd_xy_robot_ctrl_move_to_zero = CMDID(1006, 2),
kcmd_xy_robot_ctrl_move_to_with_calibrate = CMDID(1006, 3),
kcmd_xy_robot_ctrl_move_to = CMDID(1006, 4),
kcmd_xy_robot_ctrl_move_by = CMDID(1006, 5),
kcmd_xy_robot_ctrl_read_status = CMDID(1006, 50),
kcmd_xy_robot_ctrl_set_acc = CMDID(1006, 100),
kcmd_xy_robot_ctrl_set_dec = CMDID(1006, 101),
kcmd_xy_robot_ctrl_set_speed = CMDID(1006, 102),
} CmdID_t;
} // namespace zcr

78
components/zcancmder/cmd/c1006_module_cmd.hpp
View File

@ -8,52 +8,40 @@ namespace zcr {
/*******************************************************************************
* 1006 *
*******************************************************************************/
typedef enum {
kc000_enable = 0,
kc001_stop = 1,
kc002_move_to_zero = 2,
kc003_move_to_with_calibrate = 3,
kc004_move_to = 4,
kc005_move_by = 5,
kc050_read_status = 50,
kc100_set_acc = 100,
kc101_set_dec = 101,
kc102_set_speed = 102,
} kc1006_xy_robot_ctrl_subcmd_t;
#pragma pack(push, 1)
typedef struct {
uint8_t sensorid;
union {
SUBPACKET(cxxx, uint8_t _pad;);
SUBPACKET(c000_enable, uint8_t enable;);
SUBPACKET(c003_move_to_with_calibrate, uint8_t _pad; int32_t x; int32_t y;);
SUBPACKET(c004_move_to, uint8_t _pad; int32_t x; int32_t y;);
SUBPACKET(c005_move_by, uint8_t _pad; int32_t dx; int32_t dy;);
SUBPACKET(c100_set_acc, uint8_t _pad; int32_t acc;);
SUBPACKET(c101_set_dec, uint8_t _pad; int32_t dec;);
SUBPACKET(c102_set_speed, uint8_t _pad; int32_t speed;);
} cmd;
} kc1006_xy_robot_ctrl_t;
typedef struct {
uint8_t sensorid;
union {
SUBPACKET(cxxx, uint8_t _pad;);
SUBPACKET(c050_read_pos, uint8_t module_statu; int32_t x; int32_t y;);
} ack;
} kc1006_xy_robot_ctrl_ack_t;
typedef struct {
uint8_t sensorid;
union {
SUBPACKET(c002_move_to_zero, uint8_t _pad;);
SUBPACKET(c003_move_to_with_calibrate, uint8_t _pad;);
SUBPACKET(c004_move_to, uint8_t _pad;);
SUBPACKET(c005_move_by, uint8_t _pad;);
} report;
} kc1006_xy_robot_ctrl_exec_status_report_t;
/*******************************************************************************
* CMD *
*******************************************************************************/
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_enable, cmd, uint8_t id; uint8_t enable;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_stop, cmd, uint8_t id; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero, cmd, uint8_t id; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_with_calibrate, cmd, uint8_t id; uint8_t _pad; int32_t x; int32_t y;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to, cmd, uint8_t id; uint8_t _pad; int32_t x; int32_t y;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_by, cmd, uint8_t id; uint8_t _pad; int32_t dx; int32_t dy;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_status, cmd, uint8_t id; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_acc, cmd, uint8_t id; uint8_t _pad; int32_t acc;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_dec, cmd, uint8_t id; uint8_t _pad; int32_t dec;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_speed, cmd, uint8_t id; uint8_t _pad; int32_t speed;);
/*******************************************************************************
* ACK *
*******************************************************************************/
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_enable, ack, uint8_t id; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_stop, ack, uint8_t id; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero, ack, uint8_t id; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_with_calibrate, ack, uint8_t id; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to, ack, uint8_t id; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_by, ack, uint8_t id; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_read_status, ack, uint8_t id; uint8_t module_statu; int32_t x; int32_t y;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_acc, ack, uint8_t id; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_dec, ack, uint8_t id; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_set_speed, ack, uint8_t id; uint8_t _pad;);
/*******************************************************************************
* status_report *
*******************************************************************************/
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_zero, status_report, uint8_t sensorid; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to_with_calibrate, status_report, uint8_t sensorid; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_to, status_report, uint8_t sensorid; uint8_t _pad;);
ZPACKET_STRUCT(kcmd_xy_robot_ctrl_move_by, status_report, uint8_t sensorid; uint8_t _pad;);
#pragma pack(pop)

13
components/zcancmder/zcanreceiver.cpp
View File

@ -35,12 +35,12 @@ uint16_t CanPacketRxBuffer::get_datalen() {
Cmdheader_t *cmdheader = (Cmdheader_t *)rxdata;
return rxdataSize - sizeof(Cmdheader_t);
}
bool CanPacketRxBuffer::iscmd(CmdID_t id, uint8_t subcmdid) {
bool CanPacketRxBuffer::iscmd(CmdID_t id) {
Cmdheader_t *cmdheader = (Cmdheader_t *)rxdata;
return cmdheader->cmdid == (uint16_t)id && cmdheader->subcmdid == subcmdid;
uint16_t maincmdid = ((uint32_t)id >> 8) & 0xFFFF;
uint8_t subcmdId = ((uint32_t)id) & 0xFF;
return cmdheader->cmdid == maincmdid && cmdheader->subcmdid == subcmdId;
}
Cmdheader_t *CanPacketRxBuffer::get_cmdheader() {
Cmdheader_t *cmdheader = (Cmdheader_t *)rxdata;
return cmdheader;
@ -177,11 +177,12 @@ void ZCanCmder::sendAck(Cmdheader_t *cmdheader, uint8_t *data, size_t len) {
sendPacket(txbuff, sizeof(Cmdheader_t) + len);
}
void ZCanCmder::sendErrorAck(Cmdheader_t *cmdheader, int16_t errcode) {
void ZCanCmder::sendErrorAck(Cmdheader_t *cmdheader, uint32_t errcode) {
Cmdheader_t *txheader = (Cmdheader_t *)txbuff;
memcpy(txheader, cmdheader, sizeof(Cmdheader_t));
txheader->packetType = kpt_error_ack;
sendPacket(txbuff, sizeof(Cmdheader_t) + 2);
memcpy(txheader->data, &errcode, sizeof(errcode));
sendPacket(txbuff, sizeof(Cmdheader_t) + sizeof(errcode));
}
bool ZCanCmder::sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems) {

5
components/zcancmder/zcanreceiver.hpp
View File

@ -43,7 +43,8 @@ class CanPacketRxBuffer {
Cmdheader_t *get_cmdheader();
bool iscmd(CmdID_t id, uint8_t subcmdid);
bool iscmd(CmdID_t id);
template <typename T>
T *get_data_as() {
@ -104,7 +105,7 @@ class ZCanCmder : public ZCanIRQListener {
void sendPacket(uint8_t *packet, size_t len);
void sendAck(Cmdheader_t *rxcmdheader, uint8_t *data, size_t len);
void sendErrorAck(Cmdheader_t *rxcmdheader, int16_t errcode);
void sendErrorAck(Cmdheader_t *cmdheader, uint32_t errcode);
bool sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems);
uint8_t getDeviceId() { return m_config->deviceId; }
void setTxPacketInterval(int interval_ms) { txPacketInterval_ms = interval_ms; }

8
components/zcancmder_module/zcan_basic_order_module.cpp
View File

@ -21,7 +21,7 @@ void ZCanBasicOrderModule::reg_set_io(uint8_t id, writefn_t fn) { m_outCtlMap[id
void ZCanBasicOrderModule::reg_read_adc(uint8_t id, readadcval_t fn) { m_readAdcValMap[id] = fn; }
void ZCanBasicOrderModule::onRceivePacket(CanPacketRxBuffer* rxbuf) {
if (rxbuf->iscmd(kcmd_ping, 0)) {
if (rxbuf->iscmd(kcmd_ping)) {
kcmd_ping_t* pingcmd = rxbuf->get_data_as<kcmd_ping_t>();
kcmd_ping_ack_t* ack = (kcmd_ping_ack_t*)txbuff;
@ -30,7 +30,7 @@ void ZCanBasicOrderModule::onRceivePacket(CanPacketRxBuffer* rxbuf) {
m_zcanReceiver->sendAck(rxbuf->get_cmdheader(), txbuff, sizeof(kcmd_ping_ack_t));
return;
}
} else if (rxbuf->iscmd(kcmd_read_io, 0)) {
} else if (rxbuf->iscmd(kcmd_read_io)) {
kcmd_read_io_t* cmd = rxbuf->get_data_as<kcmd_read_io_t>();
kcmd_read_io_ack_t* ack = (kcmd_read_io_ack_t*)txbuff;
bool val = false;
@ -42,7 +42,7 @@ void ZCanBasicOrderModule::onRceivePacket(CanPacketRxBuffer* rxbuf) {
m_zcanReceiver->sendAck(rxbuf->get_cmdheader(), txbuff, sizeof(kcmd_read_io_ack_t));
}
} else if (rxbuf->iscmd(kcmd_set_io, 0)) {
} else if (rxbuf->iscmd(kcmd_set_io)) {
kcmd_set_io_t* cmd = rxbuf->get_data_as<kcmd_set_io_t>();
kcmd_set_io_ack_t ack;
bool val = cmd->val;
@ -55,7 +55,7 @@ void ZCanBasicOrderModule::onRceivePacket(CanPacketRxBuffer* rxbuf) {
return;
}
} else if (rxbuf->iscmd(kcmd_readadc_raw, 0)) {
} else if (rxbuf->iscmd(kcmd_readadc_raw)) {
kcmd_readadc_raw_t* cmd = rxbuf->get_data_as<kcmd_readadc_raw_t>();
kcmd_readadc_raw_ack_t ack;
int32_t val = 0;

51
components/zcancmder_module/zcan_xy_robot_module.cpp
View File

@ -1,10 +1,53 @@
#include "zcan_xy_robot_module.hpp"
using namespace iflytop;
void ZCANXYRobotCtrlModule::initialize(ZCanCmder* cancmder, int id) {
m_cancmder = cancmder;
m_id = id;
void ZCANXYRobotCtrlModule::initialize(ZCanCmder* cancmder, int id, XYRobotCtrlModule* xyRobotCtrlModule) {
m_cancmder = cancmder;
m_id = id;
m_xyRobotCtrlModule = xyRobotCtrlModule;
cancmder->registerListener(this);
}
void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
printf("ZCANXYRobotCtrlModule::onRceivePacket %d %d\n", rxcmd->get_cmdheader()->cmdid, rxcmd->get_cmdheader()->subcmdid);
PROCESS_PACKET(kcmd_xy_robot_ctrl_enable, m_id) { //
errorcode = m_xyRobotCtrlModule->enable(cmd->enable);
}
END_PROCESS_PACKET();
PROCESS_PACKET(kcmd_xy_robot_ctrl_stop, m_id) { //
errorcode = m_xyRobotCtrlModule->stop();
}
END_PROCESS_PACKET();
PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_zero, m_id) { //
errorcode = m_xyRobotCtrlModule->move_to_zero();
}
END_PROCESS_PACKET();
PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to_with_calibrate, m_id) { //
errorcode = m_xyRobotCtrlModule->move_to_with_calibrate(cmd->x, cmd->y);
}
END_PROCESS_PACKET();
PROCESS_PACKET(kcmd_xy_robot_ctrl_move_to, m_id) { //
errorcode = m_xyRobotCtrlModule->move_to(cmd->x, cmd->y);
}
END_PROCESS_PACKET();
PROCESS_PACKET(kcmd_xy_robot_ctrl_move_by, m_id) { //
errorcode = m_xyRobotCtrlModule->move_by(cmd->dx, cmd->dy);
}
END_PROCESS_PACKET();
PROCESS_PACKET(kcmd_xy_robot_ctrl_read_status, m_id) { //
errorcode = m_xyRobotCtrlModule->read_status(&ack->module_statu, &ack->x, &ack->y);
}
END_PROCESS_PACKET();
PROCESS_PACKET(kcmd_xy_robot_ctrl_set_acc, m_id) { //
errorcode = m_xyRobotCtrlModule->set_acc(cmd->acc);
}
END_PROCESS_PACKET();
PROCESS_PACKET(kcmd_xy_robot_ctrl_set_dec, m_id) { //
errorcode = m_xyRobotCtrlModule->set_dec(cmd->dec);
}
END_PROCESS_PACKET();
PROCESS_PACKET(kcmd_xy_robot_ctrl_set_speed, m_id) { //
errorcode = m_xyRobotCtrlModule->set_speed(cmd->speed);
}
END_PROCESS_PACKET();
}

9
components/zcancmder_module/zcan_xy_robot_module.hpp
View File

@ -4,11 +4,14 @@
namespace iflytop {
class ZCANXYRobotCtrlModule : public ZCanCmderListener {
ZCanCmder* m_cancmder = nullptr;
int m_id = 0;
ZCanCmder* m_cancmder = nullptr;
int m_id = 0;
XYRobotCtrlModule* m_xyRobotCtrlModule;
uint8_t m_txbuf[128] = {0};
public:
void initialize(ZCanCmder* cancmder, int id);
void initialize(ZCanCmder* cancmder, int id, XYRobotCtrlModule* xyRobotCtrlModule);
virtual void onRceivePacket(CanPacketRxBuffer* rxcmd);
};
} // namespace iflytop
Write Preview
Loading…
Cancel
Save