From a917ed12c676f599e2a6b0c3ceeb7026359a7123 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Tue, 26 Sep 2023 11:24:54 +0800 Subject: [PATCH] update some code --- components/tmc/basic/tmc_ic_interface.hpp | 2 + components/tmc/ic/ztmc4361A.hpp | 8 ++- components/tmc/ic/ztmc5130.hpp | 4 +- .../xy_robot_ctrl_module/xy_robot_ctrl_module.cpp | 74 ++++++++++++++++++++++ .../xy_robot_ctrl_module/xy_robot_ctrl_module.hpp | 54 ++++++++++++++++ components/zcancmder/cmd.hpp | 73 --------------------- components/zcancmder/cmd/basic_bean.hpp | 50 +++++++++++++++ components/zcancmder/cmd/basic_cmd.hpp | 49 ++++++++++++++ components/zcancmder/cmd/c1006_module_cmd.hpp | 61 ++++++++++++++++++ components/zcancmder/cmd/cmd.hpp | 8 +++ components/zcancmder/zcanreceiver.hpp | 4 +- .../zcancmder_module/zcan_basic_order_module.cpp | 28 +++++++- .../zcancmder_module/zcan_basic_order_module.hpp | 24 ++++--- .../zcancmder_module/zcan_xy_robot_module.cpp | 10 +++ .../zcancmder_module/zcan_xy_robot_module.hpp | 14 ++++ os/zos.hpp | 1 + os/zos_thread.hpp | 3 +- os/zthread.cpp | 70 ++++++++++++++++++++ os/zthread.hpp | 33 ++++++++++ 19 files changed, 478 insertions(+), 92 deletions(-) create mode 100644 components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp create mode 100644 components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp delete mode 100644 components/zcancmder/cmd.hpp create mode 100644 components/zcancmder/cmd/basic_bean.hpp create mode 100644 components/zcancmder/cmd/basic_cmd.hpp create mode 100644 components/zcancmder/cmd/c1006_module_cmd.hpp create mode 100644 components/zcancmder/cmd/cmd.hpp create mode 100644 components/zcancmder_module/zcan_xy_robot_module.cpp create mode 100644 components/zcancmder_module/zcan_xy_robot_module.hpp create mode 100644 os/zthread.cpp create mode 100644 os/zthread.hpp diff --git a/components/tmc/basic/tmc_ic_interface.hpp b/components/tmc/basic/tmc_ic_interface.hpp index f2fdf22..253d2fb 100644 --- a/components/tmc/basic/tmc_ic_interface.hpp +++ b/components/tmc/basic/tmc_ic_interface.hpp @@ -58,6 +58,8 @@ class IStepperMotor { virtual bool isStoped() = 0; virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) = 0; + + virtual void enable(bool enable) = 0; }; } // namespace iflytop \ No newline at end of file diff --git a/components/tmc/ic/ztmc4361A.hpp b/components/tmc/ic/ztmc4361A.hpp index a2344b6..6c4a2e0 100644 --- a/components/tmc/ic/ztmc4361A.hpp +++ b/components/tmc/ic/ztmc4361A.hpp @@ -90,8 +90,10 @@ class TMC4361A : public IStepperMotor { * @param config */ - void initialize(cfg_t *cfg); - void enableIC(bool enable); + void initialize(cfg_t *cfg); + void enableIC(bool enable); + virtual void enable(bool enable) { enableIC(enable); } + /******************************************************************************* * IStepperMotor impl * *******************************************************************************/ @@ -120,7 +122,7 @@ class TMC4361A : public IStepperMotor { * * @param channel SPI通道 * @param driver_ic_type 驱动芯片的类 - * + * * TMC4361A:0x2 DriverIC:0x30 */ int32_t readICVersion(); diff --git a/components/tmc/ic/ztmc5130.hpp b/components/tmc/ic/ztmc5130.hpp index 936363a..61fee3c 100644 --- a/components/tmc/ic/ztmc5130.hpp +++ b/components/tmc/ic/ztmc5130.hpp @@ -90,7 +90,9 @@ class TMC5130 : public IStepperMotor { // static void createDeafultTMC5130Config(TMC5130Config_t *config, TMC5130Port *m_port); void initialize(cfg_t *cfg); - void enableIC(bool enable); + void enableIC(bool enable); + virtual void enable(bool enable) { enableIC(enable); } + uint8_t reset(); /******************************************************************************* diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp new file mode 100644 index 0000000..863ea9b --- /dev/null +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -0,0 +1,74 @@ +#include "xy_robot_ctrl_module.hpp" +using namespace iflytop; +using namespace std; +void XYRobotCtrlModule::initialize(IStepperMotor* stepM1, // + IStepperMotor* stepM2, // + ZGPIO* Xgpio, // + ZGPIO* Ygpio, // + float distanceK) { + m_stepM1 = stepM1; + m_stepM2 = stepM2; + m_Xgpio = Xgpio; + m_Ygpio = Ygpio; + m_distanceK = distanceK; + m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); +} + +void XYRobotCtrlModule::enable(bool venable) { + m_stepM1->enable(venable); + m_stepM2->enable(venable); +} + +int32_t XYRobotCtrlModule::getMotor1Pos() { return m_stepM1->getXACTUAL() * m_distanceK; } +int32_t XYRobotCtrlModule::getMotor2Pos() { return m_stepM2->getXACTUAL() * m_distanceK; } + +void XYRobotCtrlModule::getNowPos(int32_t& x, int32_t& y) { + // y=M1+M2; + // X=m1-m2; + x = getMotor1Pos() - getMotor2Pos(); + y = getMotor1Pos() + getMotor2Pos(); +} + +void XYRobotCtrlModule::move_to(int32_t x, int32_t y) { + /** + * @brief + */ + m_thread.stop(); + m_thread.start([this]() { + // 计算dpos1,dpos2 + + /** + * @brief 等待电机运行到到位 + */ + }); +} +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() {} + +void XYRobotCtrlModule::set_acc(int32_t acc) {} +void XYRobotCtrlModule::set_dec(int32_t dec) {} + +void XYRobotCtrlModule::loop() {} + +void XYRobotCtrlModule::forward_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y) { +#if 1 + x = m1pos - m2pos; + y = m1pos + m2pos; +#else + x = m1pos + m2pos; + y = m1pos - m2pos; +#endif +} +void XYRobotCtrlModule::inverse_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) { +#if 1 + m1pos = (x + y) / 2; + m2pos = (y - x) / 2; +#else + m1pos = (x - y) / 2; + m2pos = (x + y) / 2; +#endif +} \ No newline at end of file diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp new file mode 100644 index 0000000..942f814 --- /dev/null +++ b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp @@ -0,0 +1,54 @@ +#pragma once +// +#include "sdk/os/zos.hpp" +#include "sdk\components\tmc\basic\tmc_ic_interface.hpp" +#include "sdk\components\zcancmder\zcanreceiver.hpp" + +namespace iflytop { +class XYRobotCtrlModule { + IStepperMotor* m_stepM1; + IStepperMotor* m_stepM2; + + ZGPIO* m_Xgpio; + ZGPIO* m_Ygpio; + + ZThread m_thread; + + float m_distanceK = 1.0f; + + int m_x = 0; + int m_y = 0; + + public: + void initialize(IStepperMotor* stepM1, // + IStepperMotor* stepM2, // + ZGPIO* Xgpio, // + ZGPIO* Ygpio, // + float distanceK); + + void 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(); + + uint8_t read_status(); + + void set_acc(int32_t acc); + void set_dec(int32_t dec); + + void loop(); + + private: + int32_t getMotor1Pos(); + int32_t getMotor2Pos(); + + void getNowPos(int32_t& x, int32_t& y); + void computeTargetMotorPos(); + + void forward_kinematics(int32_t m1pos, int32_t m2pos, int32_t& x, int32_t& y); + void inverse_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos); +}; +} // namespace iflytop \ No newline at end of file diff --git a/components/zcancmder/cmd.hpp b/components/zcancmder/cmd.hpp deleted file mode 100644 index 44d278f..0000000 --- a/components/zcancmder/cmd.hpp +++ /dev/null @@ -1,73 +0,0 @@ -#pragma once -#include "sdk/os/zos.hpp" - -namespace iflytop { -namespace zcr { - -typedef struct { - uint16_t packetindex; - uint16_t cmdid; - uint8_t subcmdid; - uint8_t packetType; - uint8_t data[]; -} Cmdheader_t; - -typedef enum { - kpt_cmd = 0, - kpt_ack = 1, - kpt_error_ack = 2, - kpt_status_report = 3, -} PacketType_t; - -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, // 华诚压力传感器 -} CmdID_t; -} // namespace zcr -} // namespace iflytop - -typedef struct { - uint8_t boardid; -} kcmd_ping_t; - -typedef struct { - uint8_t boardid; -} kcmd_ping_ack_t; - -typedef struct { - uint8_t ioid; -} kcmd_read_io_t; - -typedef struct { - uint8_t ioid; - uint8_t val; -} kcmd_read_io_ack_t; - -typedef struct { - uint8_t ioid; - uint8_t val; -} kcmd_set_io_t; - -typedef struct { - uint8_t ioid; - uint8_t val; -} kcmd_set_io_ack_t; - -typedef struct { - uint8_t sensorid; -} kcmd_readadc_raw_t; - -typedef struct { - uint8_t sensorid; - uint8_t __p; // padding - int32_t val; -} kcmd_readadc_raw_ack_t; diff --git a/components/zcancmder/cmd/basic_bean.hpp b/components/zcancmder/cmd/basic_bean.hpp new file mode 100644 index 0000000..4bb059a --- /dev/null +++ b/components/zcancmder/cmd/basic_bean.hpp @@ -0,0 +1,50 @@ +#pragma once +#include "sdk/os/zos.hpp" +/******************************************************************************* + * MARCO * + *******************************************************************************/ +#define SUBPACKET(name, ...) \ + struct { \ + __VA_ARGS__ \ + } name + +/******************************************************************************* + * STRUCT * + *******************************************************************************/ +namespace iflytop { +namespace zcr { +#pragma pack(push, 1) +typedef struct { + uint16_t packetindex; + uint16_t cmdid; + uint8_t subcmdid; + uint8_t packetType; + uint8_t data[]; +} Cmdheader_t; +#pragma pack(pop) + +typedef enum { + kpt_cmd = 0, + kpt_ack = 1, + kpt_error_ack = 2, + kpt_cmd_exec_status_report = 3, +} PacketType_t; + +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机器人控制 +} CmdID_t; + +} // namespace zcr +} // namespace iflytop diff --git a/components/zcancmder/cmd/basic_cmd.hpp b/components/zcancmder/cmd/basic_cmd.hpp new file mode 100644 index 0000000..5a4559d --- /dev/null +++ b/components/zcancmder/cmd/basic_cmd.hpp @@ -0,0 +1,49 @@ +#pragma once +#include "basic_bean.hpp" +#include "sdk/os/zos.hpp" + +namespace iflytop { +namespace zcr { + +#pragma pack(push, 1) +typedef struct { + uint8_t boardid; +} kcmd_ping_t; + +typedef struct { + uint8_t boardid; +} kcmd_ping_ack_t; + +typedef struct { + uint8_t ioid; +} kcmd_read_io_t; + +typedef struct { + uint8_t ioid; + uint8_t val; +} kcmd_read_io_ack_t; + +typedef struct { + uint8_t ioid; + uint8_t val; +} kcmd_set_io_t; + +typedef struct { + uint8_t ioid; + uint8_t val; +} kcmd_set_io_ack_t; + +typedef struct { + uint8_t sensorid; +} kcmd_readadc_raw_t; + +typedef struct { + uint8_t sensorid; + uint8_t __p; // padding + int32_t val; +} kcmd_readadc_raw_ack_t; + +#pragma pack(pop) + +} // namespace zcr +} // namespace iflytop diff --git a/components/zcancmder/cmd/c1006_module_cmd.hpp b/components/zcancmder/cmd/c1006_module_cmd.hpp new file mode 100644 index 0000000..de28c44 --- /dev/null +++ b/components/zcancmder/cmd/c1006_module_cmd.hpp @@ -0,0 +1,61 @@ +#pragma once +#include "basic_bean.hpp" +#include "sdk/os/zos.hpp" + +namespace iflytop { +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; + +#pragma pack(pop) + +} // namespace zcr +} // namespace iflytop diff --git a/components/zcancmder/cmd/cmd.hpp b/components/zcancmder/cmd/cmd.hpp new file mode 100644 index 0000000..3587d62 --- /dev/null +++ b/components/zcancmder/cmd/cmd.hpp @@ -0,0 +1,8 @@ +#pragma once +#include "sdk/os/zos.hpp" +// +#include "basic_bean.hpp" +// +#include "basic_cmd.hpp" +#include "c1006_module_cmd.hpp" + diff --git a/components/zcancmder/zcanreceiver.hpp b/components/zcancmder/zcanreceiver.hpp index 0460c3c..8c40aea 100644 --- a/components/zcancmder/zcanreceiver.hpp +++ b/components/zcancmder/zcanreceiver.hpp @@ -3,7 +3,7 @@ // #pragma once -#include "cmd.hpp" +#include "cmd/cmd.hpp" #include "sdk/os/zos.hpp" namespace iflytop { @@ -41,7 +41,7 @@ class CanPacketRxBuffer { uint8_t *get_data(); uint16_t get_datalen(); - Cmdheader_t* get_cmdheader(); + Cmdheader_t *get_cmdheader(); bool iscmd(CmdID_t id, uint8_t subcmdid); diff --git a/components/zcancmder_module/zcan_basic_order_module.cpp b/components/zcancmder_module/zcan_basic_order_module.cpp index 23ff117..f477bb6 100644 --- a/components/zcancmder_module/zcan_basic_order_module.cpp +++ b/components/zcancmder_module/zcan_basic_order_module.cpp @@ -11,10 +11,14 @@ void ZCanBasicOrderModule::initialize(ZCanCmder* zcanReceiver) { zcanReceiver->registerListener(this); m_zcanReceiver = zcanReceiver; } - +#if 0 void ZCanBasicOrderModule::regInputCtl(readfn_t fn) { m_readfn = fn; } void ZCanBasicOrderModule::regOutCtl(writefn_t fn) { m_writefn = fn; } void ZCanBasicOrderModule::regReadAdcVal(readadcval_t fn) { m_readadcval = fn; } +#endif +void ZCanBasicOrderModule::regInputCtl(uint8_t id, readfn_t fn) { m_inputCtlMap[id] = fn; } +void ZCanBasicOrderModule::regOutCtl(uint8_t id, writefn_t fn) { m_outCtlMap[id] = fn; } +void ZCanBasicOrderModule::regReadAdcVal(uint8_t id, readadcval_t fn) { m_readAdcValMap[id] = fn; } void ZCanBasicOrderModule::onRceivePacket(CanPacketRxBuffer* rxbuf) { if (rxbuf->iscmd(kcmd_ping, 0)) { @@ -31,34 +35,52 @@ void ZCanBasicOrderModule::onRceivePacket(CanPacketRxBuffer* rxbuf) { kcmd_read_io_ack_t* ack = (kcmd_read_io_ack_t*)txbuff; bool val = false; - if (m_readfn && m_readfn(cmd->ioid, val)) { + if (m_inputCtlMap.find(cmd->ioid) != m_inputCtlMap.end()) { + val = m_inputCtlMap[cmd->ioid](); ack->ioid = cmd->ioid; ack->val = val; m_zcanReceiver->sendAck(rxbuf->get_cmdheader(), txbuff, sizeof(kcmd_read_io_ack_t)); - return; } } else if (rxbuf->iscmd(kcmd_set_io, 0)) { kcmd_set_io_t* cmd = rxbuf->get_data_as(); kcmd_set_io_ack_t ack; bool val = cmd->val; +#if 0 if (m_writefn && m_writefn(cmd->ioid, val)) { ack.ioid = cmd->ioid; ack.val = val; m_zcanReceiver->sendAck(rxbuf->get_cmdheader(), (uint8_t*)&ack, sizeof(ack)); return; } +#endif + if (m_outCtlMap.find(cmd->ioid) != m_outCtlMap.end()) { + m_outCtlMap[cmd->ioid](val); + ack.ioid = cmd->ioid; + ack.val = val; + m_zcanReceiver->sendAck(rxbuf->get_cmdheader(), (uint8_t*)&ack, sizeof(ack)); + return; + } } else if (rxbuf->iscmd(kcmd_readadc_raw, 0)) { kcmd_readadc_raw_t* cmd = rxbuf->get_data_as(); kcmd_readadc_raw_ack_t ack; int32_t val = 0; +#if 0 if (m_readadcval && m_readadcval(cmd->sensorid, val)) { ack.sensorid = cmd->sensorid; ack.val = val; m_zcanReceiver->sendAck(rxbuf->get_cmdheader(), (uint8_t*)&ack, sizeof(ack)); return; } +#endif + if (m_readAdcValMap.find(cmd->sensorid) != m_readAdcValMap.end()) { + val = m_readAdcValMap[cmd->sensorid](); + ack.sensorid = cmd->sensorid; + ack.val = val; + m_zcanReceiver->sendAck(rxbuf->get_cmdheader(), (uint8_t*)&ack, sizeof(ack)); + return; + } } } diff --git a/components/zcancmder_module/zcan_basic_order_module.hpp b/components/zcancmder_module/zcan_basic_order_module.hpp index a8d2720..d3d8a5a 100644 --- a/components/zcancmder_module/zcan_basic_order_module.hpp +++ b/components/zcancmder_module/zcan_basic_order_module.hpp @@ -7,23 +7,29 @@ #include "sdk/os/zos.hpp" #include "sdk\components\zcancmder\zcanreceiver.hpp" #ifdef HAL_CAN_MODULE_ENABLED +#include namespace iflytop { +using namespace std; class ZCanBasicOrderModule : public ZCanCmderListener { public: private: ZCanCmder* m_zcanReceiver; - typedef function readfn_t; - typedef function writefn_t; - typedef function readadcval_t; + typedef function readfn_t; + typedef function writefn_t; + typedef function readadcval_t; - readfn_t m_readfn; - writefn_t m_writefn; - readadcval_t m_readadcval; + // readfn_t m_readfn; + // writefn_t m_writefn; + // readadcval_t m_readadcval; uint8_t txbuff[32]; + map m_inputCtlMap; + map m_outCtlMap; + map m_readAdcValMap; + public: ZCanBasicOrderModule(/* args */) {} ~ZCanBasicOrderModule() {} @@ -31,9 +37,9 @@ class ZCanBasicOrderModule : public ZCanCmderListener { public: void initialize(ZCanCmder* zcanReceiver); - void regInputCtl(readfn_t fn); - void regOutCtl(writefn_t fn); - void regReadAdcVal(readadcval_t fn); + void regInputCtl(uint8_t id, readfn_t fn); + void regOutCtl(uint8_t id, writefn_t fn); + void regReadAdcVal(uint8_t id, readadcval_t fn); public: virtual void onRceivePacket(CanPacketRxBuffer* rxbuf); diff --git a/components/zcancmder_module/zcan_xy_robot_module.cpp b/components/zcancmder_module/zcan_xy_robot_module.cpp new file mode 100644 index 0000000..79ee433 --- /dev/null +++ b/components/zcancmder_module/zcan_xy_robot_module.cpp @@ -0,0 +1,10 @@ +#include "zcan_xy_robot_module.hpp" +using namespace iflytop; + +void ZCANXYRobotCtrlModule::initialize(ZCanCmder* cancmder, int id) { + m_cancmder = cancmder; + m_id = id; +} +void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { + +} \ No newline at end of file diff --git a/components/zcancmder_module/zcan_xy_robot_module.hpp b/components/zcancmder_module/zcan_xy_robot_module.hpp new file mode 100644 index 0000000..41f197f --- /dev/null +++ b/components/zcancmder_module/zcan_xy_robot_module.hpp @@ -0,0 +1,14 @@ +#pragma once +#include "sdk\components\xy_robot_ctrl_module\xy_robot_ctrl_module.hpp" +#include "sdk\components\zcancmder\zcanreceiver.hpp" + +namespace iflytop { +class ZCANXYRobotCtrlModule : public ZCanCmderListener { + ZCanCmder* m_cancmder = nullptr; + int m_id = 0; + + public: + void initialize(ZCanCmder* cancmder, int id); + virtual void onRceivePacket(CanPacketRxBuffer* rxcmd); +}; +} // namespace iflytop \ No newline at end of file diff --git a/os/zos.hpp b/os/zos.hpp index 72d8932..c40d673 100644 --- a/os/zos.hpp +++ b/os/zos.hpp @@ -15,6 +15,7 @@ #include "os_default_schduler.hpp" // #include "zos_thread.hpp" +#include "zthread.hpp" // #include "zos_schduler.hpp" // diff --git a/os/zos_thread.hpp b/os/zos_thread.hpp index 37617ed..d84e767 100644 --- a/os/zos_thread.hpp +++ b/os/zos_thread.hpp @@ -10,8 +10,9 @@ class ZOSThread { const char* _threadname; public: - void init(const char* threadname, int stack_size = 1024,osPriority priority = osPriorityNormal); + void init(const char* threadname, int stack_size = 1024, osPriority priority = osPriorityNormal); void run(function func); + void waitingForStop(); public: void __callfunc(); diff --git a/os/zthread.cpp b/os/zthread.cpp new file mode 100644 index 0000000..9911690 --- /dev/null +++ b/os/zthread.cpp @@ -0,0 +1,70 @@ +#include "zthread.hpp" + +#include "zoslogger.hpp" +using namespace iflytop; +using namespace std; + +static void zosthread_default_task(void const *argument) { + ZThread *thread = (ZThread *)argument; + ZASSERT(thread); + + while (true) { + thread->m_threadisworking = false; + + // 等待taskworking + xEventGroupWaitBits(thread->m_zthreadstartworkevent, 0x01, pdTRUE, pdTRUE, portMAX_DELAY); + thread->m_threadisworking = true; + thread->m_taskfunction(); + } +}; + +void ZThread::init(const char *threadname, int stack_size, osPriority priority) { + int r_task_create = 0; + ZASSERT(threadname); + + m_lock = xSemaphoreCreateMutex(); + ZASSERT(m_lock); + + m_stacksize = stack_size; + m_uxPriority = osPriorityNormal; + m_taskfunction = nullptr; + m_zthreadstartworkevent = xEventGroupCreate(); + m_name = threadname; + + osThreadDef(zosthread_default_task, zosthread_default_task, m_uxPriority, 0, m_stacksize); + m_defaultTaskHandle = osThreadCreate(osThread(zosthread_default_task), this); + ZASSERT(m_defaultTaskHandle != NULL); +} +void ZThread::start(zosthread_cb_t cb) { + m_taskfunction = cb; + + ZASSERT(m_taskfunction); + xSemaphoreTake(m_lock, portMAX_DELAY); + xEventGroupSetBits(m_zthreadstartworkevent, 0x01); + while (m_threadisworking != true) { + vTaskDelay(1); + } + xSemaphoreGive(m_lock); +} + +void ZThread::stop() { + xSemaphoreTake(m_lock, portMAX_DELAY); + m_expect_stop = true; + while (m_threadisworking) { + xTaskNotifyGive(m_defaultTaskHandle); + vTaskDelay(1); + } + m_expect_stop = false; + xSemaphoreGive(m_lock); +} + +bool ZThread::isExpectStop() { return m_expect_stop; } +void ZThread::sleep(uint32_t ms) { ulTaskNotifyTake(pdFALSE, pdMS_TO_TICKS(ms)); } +void ZThread::wake() { + BaseType_t state; + if (xPortIsInsideInterrupt()) { + vTaskNotifyGiveFromISR(m_defaultTaskHandle, &state); + } else { + xTaskNotifyGive(m_defaultTaskHandle); + } +} \ No newline at end of file diff --git a/os/zthread.hpp b/os/zthread.hpp new file mode 100644 index 0000000..d3a3f3c --- /dev/null +++ b/os/zthread.hpp @@ -0,0 +1,33 @@ +#pragma once +#include "osbasic_h.hpp" +namespace iflytop { + +typedef function zosthread_cb_t; + +class ZThread { + public: + const char* m_name; + size_t m_stacksize; + osPriority m_uxPriority; + + bool m_threadisworking; + bool m_expect_stop; + + zosthread_cb_t m_taskfunction; + EventGroupHandle_t m_zthreadstartworkevent; + + osThreadId m_defaultTaskHandle; + + SemaphoreHandle_t m_lock; + + public: + void init(const char* threadname, int stack_size = 1024, osPriority priority = osPriorityNormal); + void start(zosthread_cb_t cb); + void stop(); + + bool isExpectStop(); + void sleep(uint32_t ms); + void wake(); +}; + +} // namespace iflytop