From c6b5f769257d0fdf5cf145f7828bc82749b1e9d3 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 30 Jul 2023 21:14:18 +0800 Subject: [PATCH] update --- .../device_base_control_service.cpp | 20 +- .../device_base_control_service.hpp | 6 +- .../idcard_reader_service.cpp | 21 +- .../idcard_reader_service.hpp | 6 +- .../iflytop_can_slave_v1/iflytop_can_slave.cpp | 97 +++++-- .../iflytop_can_slave_v1/iflytop_can_slave.hpp | 45 ++-- .../single_axis_motor_control.cpp | 290 +++++++++++++++++++++ .../single_axis_motor_control.hpp | 141 ++++++++++ .../i_iflytop_can_slave_module.cpp | 25 +- .../i_iflytop_can_slave_module.hpp | 8 +- .../single_axis_motor_control_v2.cpp | 2 +- .../step_action_scheduler/step_scheduler.cpp | 117 +++++++++ .../step_action_scheduler/step_scheduler.hpp | 57 ++++ hal/zhal_core.hpp | 1 + 14 files changed, 739 insertions(+), 97 deletions(-) create mode 100644 components/single_axis_motor_control/single_axis_motor_control.cpp create mode 100644 components/single_axis_motor_control/single_axis_motor_control.hpp create mode 100644 components/step_action_scheduler/step_scheduler.cpp create mode 100644 components/step_action_scheduler/step_scheduler.hpp diff --git a/components/iflytop_can_slave_modules/device_base_control_service.cpp b/components/iflytop_can_slave_modules/device_base_control_service.cpp index 3ff25e2..63c5ebe 100644 --- a/components/iflytop_can_slave_modules/device_base_control_service.cpp +++ b/components/iflytop_can_slave_modules/device_base_control_service.cpp @@ -7,24 +7,12 @@ void DeviceBaseControlService::initialize(IflytopCanProtocolStackProcesser* prot m_deviceId = deviceId; ZASSERT(m_protocol_processer != NULL); activeRegs(); - m_protocol_processer->registerListener(this); + m_slave = m_protocol_processer->createICPSSlaveModule("DeviceBaseControlService", this, 0); } void DeviceBaseControlService::activeRegs() { - m_protocol_processer->activeReg(REG_DEVICE_ID, icps::kr, m_deviceId); - m_protocol_processer->activeReg(REG_DEVICE_REBOOT, icps::kwr, 1); - m_protocol_processer->activeReg(REG_DEVICE_ENGINEER_MODE_MODE, icps::kwr, 0); -} - -bool DeviceBaseControlService::isThisRegOwnToMe(IflytopCanProtocolStackProcesser* processer, icps::Reg_t* reg) { - switch (reg->add) { - case REG_DEVICE_ID: - case REG_DEVICE_REBOOT: - case REG_DEVICE_ENGINEER_MODE_MODE: - return true; - default: - break; - } - return false; + m_protocol_processer->activeReg(m_slave, REG_DEVICE_ID, icps::kr, m_deviceId); + m_protocol_processer->activeReg(m_slave, REG_DEVICE_REBOOT, icps::kwr, 1); + m_protocol_processer->activeReg(m_slave, REG_DEVICE_ENGINEER_MODE_MODE, icps::kwr, 0); } icps::error_t DeviceBaseControlService::onHostRegisterWriteEvent(IflytopCanProtocolStackProcesser* processer, IflytopCanProtocolStackWriteEvent* writeEvent) { diff --git a/components/iflytop_can_slave_modules/device_base_control_service.hpp b/components/iflytop_can_slave_modules/device_base_control_service.hpp index 5e66e30..8f69a58 100644 --- a/components/iflytop_can_slave_modules/device_base_control_service.hpp +++ b/components/iflytop_can_slave_modules/device_base_control_service.hpp @@ -15,7 +15,7 @@ class DeviceBaseControlService; // public: // virtual icps::error_t onSetEngineerMode(DeviceBaseControlService* service, int32_t engineer_mode) = 0; // }; -class DeviceBaseControlService : public IflytopCanProtocolStackProcesserListener { +class DeviceBaseControlService : public ICPSListener { public: typedef function SetEngineerModeCallback_t; @@ -24,6 +24,8 @@ class DeviceBaseControlService : public IflytopCanProtocolStackProcesserListener SetEngineerModeCallback_t m_listener; uint8_t m_deviceId; + ICPSSlaveModule* m_slave; + public: DeviceBaseControlService() {} virtual ~DeviceBaseControlService() {} @@ -31,8 +33,6 @@ class DeviceBaseControlService : public IflytopCanProtocolStackProcesserListener void initialize(IflytopCanProtocolStackProcesser* protocol_processer, uint8_t deviceId); void setListener(SetEngineerModeCallback_t listener) { m_listener = listener; } - virtual bool isThisRegOwnToMe(IflytopCanProtocolStackProcesser* processer, icps::Reg_t* reg); - virtual icps::error_t onHostRegisterWriteEvent(IflytopCanProtocolStackProcesser* processer, IflytopCanProtocolStackWriteEvent* writeEvent); private: diff --git a/components/iflytop_can_slave_modules/idcard_reader_service.cpp b/components/iflytop_can_slave_modules/idcard_reader_service.cpp index bb55869..b435830 100644 --- a/components/iflytop_can_slave_modules/idcard_reader_service.cpp +++ b/components/iflytop_can_slave_modules/idcard_reader_service.cpp @@ -32,12 +32,13 @@ void IDCardReaderService::initialize(const char* name, IflytopCanProtocolStackPr *******************************************************************************/ m_codeScanner = codeScanner; - m_protocolProcesser->activeReg(m_regStartOff + REG_CODE_SCANER_ACT_CTRL, icps::kwr, 0); - m_protocolProcesser->activeReg(m_regStartOff + REG_CODE_SCANER_ACT_CLEAR_EXCEPTION, icps::kwr, 0); - m_protocolProcesser->activeReg(m_regStartOff + REG_CODE_SCANER_STAT_STATUS, icps::kr, 0); - m_protocolProcesser->activeReg(m_regStartOff + REG_CODE_SCANER_STAT_EXCEPTION, icps::kr, 0); - m_codeReg = m_protocolProcesser->activeReg(m_regStartOff + REG_CODE_SCANER_CODE, icps::kr | icps::kextreg, 0); - m_protocolProcesser->registerListener(this); + m_slave = m_protocolProcesser->createICPSSlaveModule("IDCardReaderService", this, regStartOff); + + m_protocolProcesser->activeReg(m_slave, +REG_CODE_SCANER_ACT_CTRL, icps::kwr, 0); + m_protocolProcesser->activeReg(m_slave, +REG_CODE_SCANER_ACT_CLEAR_EXCEPTION, icps::kwr, 0); + m_protocolProcesser->activeReg(m_slave, +REG_CODE_SCANER_STAT_STATUS, icps::kr, 0); + m_protocolProcesser->activeReg(m_slave, +REG_CODE_SCANER_STAT_EXCEPTION, icps::kr, 0); + m_codeReg = m_protocolProcesser->activeReg(m_slave, +REG_CODE_SCANER_CODE, icps::kr | icps::kextreg, 0); } icps::error_t IDCardReaderService::onHostRegisterWriteEvent(IflytopCanProtocolStackProcesser* processer, IflytopCanProtocolStackWriteEvent* event) { @@ -184,13 +185,5 @@ void IDCardReaderService::onProcessState(void* module, StateMachine::StateProces * 通用方法 * *******************************************************************************/ void IDCardReaderService::writeRegVal(uint16_t regoff, uint16_t regval) { m_protocolProcesser->writeRegValue(uint16_t(m_regStartOff + regoff), regval, false); } -bool IDCardReaderService::isThisRegOwnToMe(IflytopCanProtocolStackProcesser* processer, icps::Reg_t* reg) { - int regoff = reg->add; - if (regoff >= m_regStartOff && regoff < m_regStartOff + REG_CODE_SCANER_SIZE) { - return true; - } - return false; -} - void IDCardReaderService::periodicJob() { m_stateMachine.periodicCall(); } #endif diff --git a/components/iflytop_can_slave_modules/idcard_reader_service.hpp b/components/iflytop_can_slave_modules/idcard_reader_service.hpp index ee8cef3..1b4c4f9 100644 --- a/components/iflytop_can_slave_modules/idcard_reader_service.hpp +++ b/components/iflytop_can_slave_modules/idcard_reader_service.hpp @@ -28,7 +28,7 @@ using namespace iflytop_can_slave_modules_pri; * https://iflytop1.feishu.cn/wiki/OYZFwSNf2i0Hyek1zouc8eSsnsg * */ -class IDCardReaderService : public IflytopCanProtocolStackProcesserListener { +class IDCardReaderService : public ICPSListener { public: typedef enum { kidle = 0, @@ -60,12 +60,14 @@ class IDCardReaderService : public IflytopCanProtocolStackProcesserListener { M3078CodeScanner* m_codeScanner; icps::Reg_t* m_codeReg; + ICPSSlaveModule* m_slave; + bool m_oneShutScan; public: void initialize(const char* name, IflytopCanProtocolStackProcesser* protocolProcesser, int32_t regStartOff, M3078CodeScanner* codeScanner); - virtual bool isThisRegOwnToMe(IflytopCanProtocolStackProcesser* processer, icps::Reg_t* reg); + // virtual bool isThisRegOwnToMe(IflytopCanProtocolStackProcesser* processer, icps::Reg_t* reg); virtual icps::error_t onHostRegisterWriteEvent(IflytopCanProtocolStackProcesser* processer, IflytopCanProtocolStackWriteEvent* writeEvent); virtual void onHostRegisterReportEvent(IflytopCanProtocolStackProcesser* processer, icps::ReportEvent* event); diff --git a/components/iflytop_can_slave_v1/iflytop_can_slave.cpp b/components/iflytop_can_slave_v1/iflytop_can_slave.cpp index b40ddda..dbdaaa9 100644 --- a/components/iflytop_can_slave_v1/iflytop_can_slave.cpp +++ b/components/iflytop_can_slave_v1/iflytop_can_slave.cpp @@ -41,7 +41,7 @@ IflytopCanProtocolStackProcesser::cfg_t *IflytopCanProtocolStackProcesser::creat config->m_regListSize = regListSize; return config; } -icps::Reg_t *IflytopCanProtocolStackProcesser::activeReg(uint16_t mappingAdd, uint16_t mask, int32_t defaultValue) { +icps::Reg_t *IflytopCanProtocolStackProcesser::activeReg(ICPSSlaveModule *container, uint16_t regoff, uint16_t mask, int32_t defaultValue) { int off = -1; for (int i = 0; i < m_config->m_regListSize; i++) { if (m_config->m_regList[i].mask == 0) { @@ -50,20 +50,20 @@ icps::Reg_t *IflytopCanProtocolStackProcesser::activeReg(uint16_t mappingAdd, ui } } if (off == -1) { - ZLOGE(TAG, "active reg fail, no reg"); - ZASSERT(false); - return NULL; - } - if (off > m_config->m_regListSize) { - ZLOGE(TAG, "active reg %d fail", off); + ZLOGE(TAG, "active %s[%d] reg fail", container->modulename, regoff); ZASSERT(false); return NULL; } + memset(&m_config->m_regList[off], 0, sizeof(icps::Reg_t)); m_config->m_regList[off].mask = mask | icps::kactived; - m_config->m_regList[off].add = mappingAdd; + m_config->m_regList[off].add = container->moduleRegAddoff + regoff; m_config->m_regList[off].setValue(defaultValue); - ZLOGI(TAG, "active regoff %03d, add %05d, mask %02x, value %d", off, mappingAdd, mask, defaultValue); + ZLOGI(TAG, "active regoff%03d, add:%s:%05d, mask %02x, value %d", off, container->modulename, container->moduleRegAddoff + regoff, mask, defaultValue); + + if (regoff > container->maxregadd) { + container->maxregadd = regoff; + } return &m_config->m_regList[off]; } void IflytopCanProtocolStackProcesser::disablePermission(int regadd, uint32_t mask) { @@ -112,11 +112,15 @@ void IflytopCanProtocolStackProcesser::initialize(cfg_t *config) { ZHALCORE::getInstance()->regPeriodJob([this](ZHALCORE::Context &context) { periodicJob(); }, 0); } -void IflytopCanProtocolStackProcesser::registerListener( // - IflytopCanProtocolStackProcesserListener *listener) { // +ICPSSlaveModule *IflytopCanProtocolStackProcesser::createICPSSlaveModule(const char *modulename, ICPSListener *listener, int regaddoff) { // ZASSERT(m_listenerSize < ZARRAY_SIZE(m_listeners)); - m_listeners[m_listenerSize] = listener; + ICPSSlaveModule *container = new ICPSSlaveModule(); + container->modulename = modulename; + container->listener = listener; + container->moduleRegAddoff = regaddoff; + m_listeners[m_listenerSize] = container; m_listenerSize++; + return container; } void IflytopCanProtocolStackProcesser::periodicJob() { LoopJobContext loopContext = {0}; @@ -691,9 +695,17 @@ icps::error_t IflytopCanProtocolStackProcesser::callOnHostRegisterWriteEvent(icp writeEvent.oldvalue = oldvalue; writeEvent.reg = reg; // if (m_listener) return m_listener->onHostRegisterWriteEvent(this, &writeEvent); - for (size_t i = 0; i < m_listenerSize; i++) { - if (m_listeners[i]->isThisRegOwnToMe(this, reg)) { - return m_listeners[i]->onHostRegisterWriteEvent(this, &writeEvent); + + for (int i = 0; i < m_listenerSize; i++) { + ICPSSlaveModule *mod = m_listeners[i]; + + int moduleregstart = mod->moduleRegAddoff; + int moduleregend = mod->moduleRegAddoff + mod->maxregadd; + + if (reg->add >= moduleregstart && reg->add < moduleregend) { + icps::error_t result = mod->listener->onHostRegisterWriteEvent(&writeEvent); + newvalue = writeEvent.newvalue; + return result; } } @@ -705,11 +717,25 @@ icps::error_t IflytopCanProtocolStackProcesser::callOnHostRegisterReadEvent(icps eve.reg = reg; eve.value = value; - for (size_t i = 0; i < m_listenerSize; i++) { - if (m_listeners[i]->isThisRegOwnToMe(this, reg)) { - return m_listeners[i]->onHostRegisterReadEvent(this, &eve); + // for (int i = 0; i < m_listenerSize; i++) { + // if (m_listeners[i]->isThisRegOwnToMe(this, reg)) { + // return m_listeners[i]->onHostRegisterReadEvent(this, &eve); + // } + // } + + for (int i = 0; i < m_listenerSize; i++) { + ICPSSlaveModule *mod = m_listeners[i]; + + int moduleregstart = mod->moduleRegAddoff; + int moduleregend = mod->moduleRegAddoff + mod->maxregadd; + + if (reg->add >= moduleregstart && reg->add < moduleregend) { + icps::error_t result = mod->listener->onHostRegisterReadEvent(&eve); + value = eve.value; + return result; } } + value = eve.value; return icps::kSuccess; } @@ -719,9 +745,20 @@ void IflytopCanProtocolStackProcesser::callOnRegisterValueAutoReportEvent(icps:: eve.value = value; // if (m_listener) m_listener->onHostRegisterReportEvent(this, &eve); - for (size_t i = 0; i < m_listenerSize; i++) { - if (m_listeners[i]->isThisRegOwnToMe(this, reg)) { - m_listeners[i]->onHostRegisterReportEvent(this, &eve); + // for (int i = 0; i < m_listenerSize; i++) { + // if (m_listeners[i]->isThisRegOwnToMe(this, reg)) { + // m_listeners[i]->onHostRegisterReportEvent(this, &eve); + // } + // } + + for (int i = 0; i < m_listenerSize; i++) { + ICPSSlaveModule *mod = m_listeners[i]; + + int moduleregstart = mod->moduleRegAddoff; + int moduleregend = mod->moduleRegAddoff + mod->maxregadd; + + if (reg->add >= moduleregstart && reg->add < moduleregend) { + mod->listener->onHostRegisterReportEvent(&eve); } } @@ -735,11 +772,23 @@ void IflytopCanProtocolStackProcesser::callOnRegisterExtEvent(icps::Reg_t *reg, eve.extRegSubOff = subregindex; // if (m_listener) m_listener->onHostRegisterReportEvent(this, &eve); - for (size_t i = 0; i < m_listenerSize; i++) { - if (m_listeners[i]->isThisRegOwnToMe(this, reg)) { - m_listeners[i]->onHostRegisterReportEvent(this, &eve); + // for (int i = 0; i < m_listenerSize; i++) { + // if (m_listeners[i]->isThisRegOwnToMe(this, reg)) { + // m_listeners[i]->onHostRegisterReportEvent(this, &eve); + // } + // } + + for (int i = 0; i < m_listenerSize; i++) { + ICPSSlaveModule *mod = m_listeners[i]; + + int moduleregstart = mod->moduleRegAddoff; + int moduleregend = mod->moduleRegAddoff + mod->maxregadd; + + if (reg->add >= moduleregstart && reg->add < moduleregend) { + mod->listener->onHostRegisterReportEvent(&eve); } } + value = eve.value; } #endif diff --git a/components/iflytop_can_slave_v1/iflytop_can_slave.hpp b/components/iflytop_can_slave_v1/iflytop_can_slave.hpp index 06ba440..65a851a 100644 --- a/components/iflytop_can_slave_v1/iflytop_can_slave.hpp +++ b/components/iflytop_can_slave_v1/iflytop_can_slave.hpp @@ -25,18 +25,9 @@ using namespace std; class IflytopCanProtocolStackProcesser; typedef icps::WriteEvent IflytopCanProtocolStackWriteEvent; -class IflytopCanProtocolStackProcesserListener { +class ICPSListener { public: - virtual bool isThisRegOwnToMe(IflytopCanProtocolStackProcesser *processer, icps::Reg_t *reg) { return false; } - /** - * @brief onHostRegisterWriteEvent - * - * @param reg 被写入的寄存器 - * @param oldvalue 原先数值 - * @param newvalue 新的数值 - * @return icps::error_t 该方法返回kSuccess，寄存器的数值才会被赋值 - */ - virtual icps::error_t onHostRegisterWriteEvent(IflytopCanProtocolStackProcesser *processer, icps::WriteEvent *event) { return icps::kSuccess; }; + virtual icps::error_t onHostRegisterWriteEvent(icps::WriteEvent *event) { return icps::kSuccess; }; /** * @brief * @@ -44,12 +35,21 @@ class IflytopCanProtocolStackProcesserListener { * @param value 读取到的数值，该数值可以被修改,如果不修改怎么保持和寄存器的数值一致 * @return icps::error_t 该方法返回kSuccess，数值才会被读走 */ - virtual icps::error_t onHostRegisterReadEvent(IflytopCanProtocolStackProcesser *processer, icps::ReadEvent *event) { return icps::kSuccess; }; + virtual icps::error_t onHostRegisterReadEvent(icps::ReadEvent *event) { return icps::kSuccess; }; /** * @brief onRegisterValuePeriodicReportEvent * 当寄存器数值发生变化时，如果设置了自动上报，该方法就会被回调，用来通知应用层产生了一次消息上报事件。 */ - virtual void onHostRegisterReportEvent(IflytopCanProtocolStackProcesser *processer, icps::ReportEvent *event){}; + virtual void onHostRegisterReportEvent(icps::ReportEvent *event){}; +}; + +class ICPSSlaveModule { + public: + const char *modulename = "null"; + ICPSListener *listener = NULL; + + int moduleRegAddoff = 0; + int maxregadd = 0; }; class IflytopCanProtocolStackProcesser : public ZCanIRQListener { @@ -85,7 +85,7 @@ class IflytopCanProtocolStackProcesser : public ZCanIRQListener { private: cfg_t *m_config; // 配置 // IflytopMicroOS *m_os; // 操作系统相关方法 - // IflytopCanProtocolStackProcesserListener *m_listener; // 监听者 + // ICSP *m_listener; // 监听者 icps::Reg_t *m_regList; // 寄存器列表 uint16_t m_regListSize; // 寄存器列表大小 @@ -97,8 +97,8 @@ class IflytopCanProtocolStackProcesser : public ZCanIRQListener { uint8_t m_reportSeq; // 上报序列号 ReportTask m_reportTask; - IflytopCanProtocolStackProcesserListener *m_listeners[30]; - int m_listenerSize = 0; + ICPSSlaveModule *m_listeners[10] = {NULL}; + int m_listenerSize = 0; public: IflytopCanProtocolStackProcesser(); @@ -109,7 +109,7 @@ class IflytopCanProtocolStackProcesser : public ZCanIRQListener { * @param deviceId 7位设备ID */ void initialize(cfg_t *config); - icps::Reg_t *activeReg(uint16_t mappingAdd, uint16_t mask, int32_t defaultValue); + icps::Reg_t *activeReg(ICPSSlaveModule *container, uint16_t regoff, uint16_t mask, int32_t defaultValue); void disablePermission(int regadd, uint32_t mask); /** @@ -117,11 +117,8 @@ class IflytopCanProtocolStackProcesser : public ZCanIRQListener { * * @param listener */ - void registerListener(IflytopCanProtocolStackProcesserListener *listener); - /** - * @brief 在Main的Loop中尽可能频繁的的调用该方法 - */ - void periodicJob(); + ICPSSlaveModule *createICPSSlaveModule(const char *modulename, ICPSListener *listener, int regaddoff); + /** * @brief 读取寄存器数值 * @@ -219,6 +216,10 @@ class IflytopCanProtocolStackProcesser : public ZCanIRQListener { private: HAL_StatusTypeDef send_ext_report_packet(icps::packet_type_t subregindex, uint16_t regAdd, int32_t regValue, uint16_t overtimems); + /** + * @brief 在Main的Loop中尽可能频繁的的调用该方法 + */ + void periodicJob(); }; class ICPSUtils { diff --git a/components/single_axis_motor_control/single_axis_motor_control.cpp b/components/single_axis_motor_control/single_axis_motor_control.cpp new file mode 100644 index 0000000..b5dc08c --- /dev/null +++ b/components/single_axis_motor_control/single_axis_motor_control.cpp @@ -0,0 +1,290 @@ +#include "single_axis_motor_control.hpp" +#ifdef HAL_CAN_MODULE_ENABLED +using namespace std; +using namespace iflytop; + +void SingleAxisMotorControler::initialize(const char* name, IflytopCanProtocolStackProcesser* processer, int regaddoff, // + ZGPIO* homegpio, // + ZGPIO* mingpio, // + ZGPIO* rightgpio, // + IStepperMotor* motor) { + m_homegpio = homegpio; + m_minlimitgpio = mingpio; + m_maxlimitgpio = rightgpio; + m_motor = motor; + m_regaddoff = regaddoff; + m_name = name; + m_processer = processer; + + ZASSERT(motor); + if (m_homegpio) { + ZASSERT(m_homegpio->isItRisingAndItFallingEXITGPIO()); + } + + m_slave = m_processer->createICPSSlaveModule(name, this, m_regaddoff); + + m_processer->activeReg(m_slave, SAMC_REG_ACT_ROTATE, icps::kw, 0); + m_processer->activeReg(m_slave, SAMC_REG_ACT_MOVE_TO, icps::kw, 0); + m_processer->activeReg(m_slave, SAMC_REG_ACT_MOVE_BY, icps::kw, 0); + m_processer->activeReg(m_slave, SAMC_REG_ACT_RUN_TO_HOME, icps::kw, 0); + m_processer->activeReg(m_slave, SAMC_REG_ACT_DO_NOTHING, icps::kw, 0); + m_processer->activeReg(m_slave, SAMC_REG_ACT_STOP, icps::kw, 0); + m_processer->activeReg(m_slave, SAMC_REG_ACT_CLEAR_EXCEPTION, icps::kw, 0); + + m_statusReg = m_processer->activeReg(m_slave, SAMC_REG_STAT_STATUS, icps::kwr, 0); + m_currentVelocityReg = m_processer->activeReg(m_slave, SAMC_REG_STAT_CURRENT_VELOCITY, icps::kwr, 0); + m_currentPosReg = m_processer->activeReg(m_slave, SAMC_REG_STAT_CURRENT_POS, icps::kwr, 0); + m_exceptionReg = m_processer->activeReg(m_slave, SAMC_REG_STAT_EXCEPTION, icps::kwr, 0); + + cfg_acc_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_ACC, icps::kwr, 0); + cfg_dec_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_DEC, icps::kwr, 0); + cfg_velocity_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_VELOCITY, icps::kwr, 0); + cfg_zero_shift_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_ZERO_SHIFT, icps::kwr, 0); // 零点偏移 + cfg_runhome_velocity_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_RUNHOME_VELOCITY, icps::kwr, 0); // 归零速度 + cfg_runtohome_dec_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_RUNTOHOME_DEC, icps::kwr, 0); // 归零减速度 + cfg_runtohome_max_distance_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_RUNTOHOME_MAX_DISTANCE, icps::kwr, 0); // 归零最大距离 + cfg_runtohome_leave_zero_point_distance_reg = + m_processer->activeReg(m_slave, SAMC_REG_CFG_RUNTOHOME_LEAVE_ZERO_POINT_DISTANCE, icps::kwr, 0); // 离开零点距离 + cfg_min_pos_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_MIN_POS, icps::kwr, 0); // 最小位置 + cfg_max_pos_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_MAX_POS, icps::kwr, 0); // 最大位置 + cfg_runtohome_keep_move_distance_reg = m_processer->activeReg(m_slave, SAMC_REG_CFG_RUNTOHOME_KEEP_MOVE_DISTANCE, icps::kwr, 0); // 归零阶段1移动距离 +} + +icps::error_t SingleAxisMotorControler::onHostRegisterWriteEvent(icps::WriteEvent* event) { + int regoff = event->reg->add - m_regaddoff; + int val = event->newvalue; + switch (regoff) { + case SAMC_REG_ACT_ROTATE: + return rotate(val); + case SAMC_REG_ACT_MOVE_TO: + return moveTo(val); + case SAMC_REG_ACT_MOVE_BY: + return moveBy(val); + case SAMC_REG_ACT_RUN_TO_HOME: + return runToHome(); + case SAMC_REG_ACT_DO_NOTHING: + return icps::kSuccess; + case SAMC_REG_ACT_STOP: + return stop(); + case SAMC_REG_ACT_CLEAR_EXCEPTION: + return clearException(); + default: + break; + }; + return icps::kSuccess; +} + +icps::error_t SingleAxisMotorControler::rotate(int32_t velocity) { + if (!(m_state == kstate_rotate || m_state == kstate_idle)) { + return icps::kDeviceBusy; + } + changeToState(kstate_rotate); + + if (velocity == 0 && m_state == kstate_rotate) { + m_motor->stop(); + m_step_scheduler.stop(); + return icps::kSuccess; + } + + m_step_scheduler.reset(); + m_step_scheduler.pushActionAction([this, velocity](StepActionContext* context) { // + motor_rotate(velocity, cfg_acc_reg->getValue(), cfg_dec_reg->getValue()); + }); + m_step_scheduler.pushActionIsDone([](StepActionContext* context) { return false; }); + m_step_scheduler.regDoneCallback([this]() { changeToState(kstate_idle); }); + m_step_scheduler.regBreakCallback([this]() { changeToState(kstate_idle); }); + m_step_scheduler.startSchedule(); + return icps::kSuccess; +} +icps::error_t SingleAxisMotorControler::moveTo(int32_t pos) { + if (!(m_state == kstate_moveTo || m_state == kstate_idle)) { + return icps::kDeviceBusy; + } + changeToState(kstate_moveTo); + + m_step_scheduler.reset(); + m_step_scheduler.pushActionAction([this, pos](StepActionContext* context) { // + motor_moveTo(pos, cfg_velocity_reg->getValue(), cfg_acc_reg->getValue(), cfg_dec_reg->getValue()); + }); + m_step_scheduler.pushActionIsDone(bind(&SingleAxisMotorControler::motorIsStop, this, std::placeholders::_1)); + m_step_scheduler.regDoneCallback([this]() { changeToState(kstate_idle); }); + m_step_scheduler.regBreakCallback([this]() { changeToState(kstate_idle); }); + m_step_scheduler.startSchedule(); + + return icps::kSuccess; +} +icps::error_t SingleAxisMotorControler::moveBy(int32_t pos) { + if (!(m_state == kstate_moveBy || m_state == kstate_idle)) { + return icps::kDeviceBusy; + } + changeToState(kstate_moveBy); + + m_step_scheduler.reset(); + m_step_scheduler.pushActionAction([this, pos](StepActionContext* context) { // + motor_moveBy(pos, cfg_velocity_reg->getValue(), cfg_acc_reg->getValue(), cfg_dec_reg->getValue()); + }); + m_step_scheduler.pushActionIsDone(bind(&SingleAxisMotorControler::motorIsStop, this, std::placeholders::_1)); + m_step_scheduler.regDoneCallback([this]() { changeToState(kstate_idle); }); + m_step_scheduler.regBreakCallback([this]() { changeToState(kstate_idle); }); + m_step_scheduler.startSchedule(); + + return icps::kSuccess; +} +icps::error_t SingleAxisMotorControler::runToHome() { + if (!(m_state == kstate_runToHome || m_state == kstate_idle)) { + return icps::kDeviceBusy; + } + changeToState(kstate_runToHome); + + m_step_scheduler.reset(); + + /** + * @brief 步骤1 如果当前不在零点的位置，则缓慢移动到零点 + */ + if (!m_homegpio->getState()) { + m_step_scheduler.pushActionAction([this](StepActionContext* context) { // + regGpioIrqProcesser([this](ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) { + if (GPIO_Pin != m_homegpio) return; + m_runToHomeContext.stopByIrq = true; + m_motor->stop(); + }); + + motor_moveBy(-cfg_runtohome_max_distance_reg->getValue(), // + cfg_runhome_velocity_reg->getValue() * 2, // + cfg_acc_reg->getValue(), // + cfg_runtohome_dec_reg->getValue()); + }); + m_step_scheduler.pushActionIsDone([this](StepActionContext* context) { + if (motorIsStop(context)) { + if (m_runToHomeContext.stopByIrq) { + return true; + } else { + context->breakflag = true; + ZLOGE(m_name, "runToHome fail, not reach home point"); + return false; + } + } + return false; + }); + } + /** + * @brief STEP 2 离开原点一定的距离 + * + */ + m_step_scheduler.pushActionAction([this](StepActionContext* context) { // + regGpioIrqProcesser(nullptr); + motor_moveBy(cfg_runtohome_leave_zero_point_distance_reg->getValue(), // + cfg_runhome_velocity_reg->getValue(), // + cfg_acc_reg->getValue(), // + cfg_dec_reg->getValue()); // 此时处于零点位置，先离开原点一定位置 + }); + m_step_scheduler.pushActionIsDone(bind(&SingleAxisMotorControler::motorIsStop, this, std::placeholders::_1)); + + /** + * @brief STEP 3 返回原点 + */ + m_step_scheduler.pushActionAction([this](StepActionContext* context) { // + regGpioIrqProcesser([this](ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) { + if (GPIO_Pin != m_homegpio) return; + m_runToHomeContext.stopByIrq = true; + m_runToHomeContext.zeroPointPos = m_motor->getXACTUAL(); + m_motor->stop(); + }); + motor_moveBy(-cfg_runtohome_leave_zero_point_distance_reg->getValue() * 1.5, // + cfg_runhome_velocity_reg->getValue(), // + cfg_acc_reg->getValue(), // + cfg_runtohome_dec_reg->getValue()); // 此时处于零点位置，先离开原点一定位置 + }); + m_step_scheduler.pushActionIsDone([this](StepActionContext* context) { + if (motorIsStop(context)) { + if (m_runToHomeContext.stopByIrq) { + return true; + } else { + context->breakflag = true; + ZLOGE(m_name, "runToHome fail, not reach home point"); + return false; + } + } + return false; + }); + m_step_scheduler.pushActionDone([this](StepActionContext* context) { + int32_t nowposition = m_motor->getXACTUAL(); + int32_t nowposition_real = nowposition - m_runToHomeContext.zeroPointPos; + ZLOGI(m_name, "nowposition:%d,zeroPointPosition:%d", nowposition, m_runToHomeContext.zeroPointPos); + m_motor->setXACTUAL(nowposition_real - cfg_zero_shift_reg->getValue()); + }); + + /** + * @brief STEP 4 移动到零点 + */ + m_step_scheduler.pushActionAction([this](StepActionContext* context) { // + regGpioIrqProcesser(nullptr); + motor_moveTo(0, // + cfg_runhome_velocity_reg->getValue(), // + cfg_acc_reg->getValue(), // + cfg_dec_reg->getValue()); // 此时处于零点位置，先离开原点一定位置 + }); + m_step_scheduler.pushActionIsDone(bind(&SingleAxisMotorControler::motorIsStop, this, std::placeholders::_1)); + + /** + * @brief 异常处理 + */ + m_step_scheduler.regDoneCallback([this]() { changeToState(kstate_idle); }); + m_step_scheduler.regBreakCallback([this]() { + m_exceptionReg->setValue(kexcep_loseZeroPoint); + changeToState(kstate_exception); + }); + m_step_scheduler.startSchedule(); + + return icps::kSuccess; +} + +icps::error_t SingleAxisMotorControler::stop() { + changeToState(kstate_idle); + m_motor->stop(); + m_step_scheduler.stop(); + return icps::kSuccess; +} +icps::error_t SingleAxisMotorControler::clearException() { + m_exceptionReg->setValue(0); + changeToState(kstate_idle); + return icps::kSuccess; +} + +void SingleAxisMotorControler::changeToState(state_t state) { + m_state = state; + m_statusReg->setValue(m_state); +} + +void SingleAxisMotorControler::periodJob(ZHALCORE::Context* context) {} +void SingleAxisMotorControler::motor_moveTo(int32_t pos, int32_t velocity, int32_t acc, int32_t dec) { + ZLOGI(m_name, "motor_moveTo %d %d %d %d", pos, velocity, acc, dec); + int32_t position = m_motor->getXACTUAL(); + // position >= pos ? m_direction = 1 : m_direction = -1; + m_motor->setAcceleration(acc); + m_motor->setDeceleration(dec); + m_motor->moveTo(pos, velocity); +} +void SingleAxisMotorControler::motor_moveBy(int32_t pos, int32_t velocity, int32_t acc, int32_t dec) { + ZLOGI(m_name, "motor_moveBy %d %d %d %d", pos, velocity, acc, dec); + // pos >= 0 ? m_direction = 1 : m_direction = -1; + m_motor->setAcceleration(acc); + m_motor->setDeceleration(dec); + m_motor->moveBy(pos, velocity); +} +void SingleAxisMotorControler::motor_rotate(int32_t velocity, int32_t acc, int32_t dec) { + ZLOGI(m_name, "motor_rotate %d %d %d", velocity, acc, dec); + // velocity >= 0 ? m_direction = 1 : m_direction = -1; + m_motor->setAcceleration(acc); + m_motor->setDeceleration(dec); + m_motor->rotate(velocity); +} +bool SingleAxisMotorControler::motorIsStop(StepActionContext* context) { + if (!context) { + return m_motor->isReachTarget(); + } + if (context->coreContext->getScheduleTimes() % 33 == 0 /*33 ms*/) return m_motor->isReachTarget(); + return false; +} + +#endif diff --git a/components/single_axis_motor_control/single_axis_motor_control.hpp b/components/single_axis_motor_control/single_axis_motor_control.hpp new file mode 100644 index 0000000..ad28c7f --- /dev/null +++ b/components/single_axis_motor_control/single_axis_motor_control.hpp @@ -0,0 +1,141 @@ +#pragma once +#include "sdk/hal/zhal.hpp" +#ifdef HAL_CAN_MODULE_ENABLED +#include + +#include "sdk/components/step_action_scheduler/step_scheduler.hpp" +#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp" +#include "sdk\components\tmc\basic\tmc_ic_interface.hpp" + +/** + * @brief + * + * service: SingleAxisMotorControler + * + */ +#define SINGLE_AXIS_MOTOR_CTRL_REG_NUM 99 + +#define SAMC_REG_ACT_ROTATE (0) // 速度模式控制 +#define SAMC_REG_ACT_MOVE_TO (1) // 位置模式控制 +#define SAMC_REG_ACT_MOVE_BY (2) // 相对位置模式控制 +#define SAMC_REG_ACT_RUN_TO_HOME (3) // 归零 +#define SAMC_REG_ACT_DO_NOTHING (4) // __ +#define SAMC_REG_ACT_STOP (8) // 停止 +#define SAMC_REG_ACT_CLEAR_EXCEPTION (9) // 清空异常 + +#define SAMC_REG_STAT_STATUS (10) // 设备状态 +#define SAMC_REG_STAT_CURRENT_VELOCITY (12) // 当前转速 +#define SAMC_REG_STAT_CURRENT_POS (13) // 当前位置 +#define SAMC_REG_STAT_EXCEPTION (14) // 异常状态 + +#define SAMC_REG_CFG_ACC (20) // 加速度pps^2 +#define SAMC_REG_CFG_DEC (21) // 减速度pps^2 +#define SAMC_REG_CFG_VELOCITY (22) // 速度 +#define SAMC_REG_CFG_ZERO_SHIFT (23) // 零点偏移 +#define SAMC_REG_CFG_RUNHOME_VELOCITY (24) // 归零速度 +#define SAMC_REG_CFG_RUNTOHOME_DEC (25) // 归零减速度 +#define SAMC_REG_CFG_RUNTOHOME_MAX_DISTANCE (26) // 归零最大移动距离 +#define SAMC_REG_CFG_RUNTOHOME_LEAVE_ZERO_POINT_DISTANCE (27) // 归零阶段1移动距离 +#define SAMC_REG_CFG_MIN_POS (28) // 最小位置 +#define SAMC_REG_CFG_MAX_POS (29) // 最大位置 +#define SAMC_REG_CFG_RUNTOHOME_KEEP_MOVE_DISTANCE (30) // 最大位置 + +namespace iflytop { +using namespace std; + +class SingleAxisMotorControler : public ICPSListener { + public: + typedef enum { + kstate_idle = 0, + kstate_rotate = 1, + kstate_moveTo = 2, + kstate_moveBy = 3, + kstate_runToHome = 4, + kstate_exception = 65535, + } state_t; + + typedef enum { + kexcep_motorBlocking = 1, + kexcep_loseZeroPoint = 2, + } exception_id_t; + + class RunToHomeContext { + public: + bool stopByIrq = false; + int32_t zeroPointPos; + }; + + private: + const char* m_name; + IflytopCanProtocolStackProcesser* m_processer; + int m_regaddoff; + // res + ZGPIO* m_homegpio; + ZGPIO* m_maxlimitgpio; + ZGPIO* m_minlimitgpio; + IStepperMotor* m_motor; + + ICPSSlaveModule* m_slave; + + state_t m_state = kstate_idle; + + icps::Reg_t* m_statusReg; + icps::Reg_t* m_currentVelocityReg; + icps::Reg_t* m_currentPosReg; + icps::Reg_t* m_exceptionReg; + + RunToHomeContext m_runToHomeContext; + + StepActionScheduler m_step_scheduler; + + ZGPIO::onirq_t m_gpioirqprocesser; + + public: + icps::Reg_t* cfg_acc_reg; + icps::Reg_t* cfg_dec_reg; + icps::Reg_t* cfg_velocity_reg; + icps::Reg_t* cfg_zero_shift_reg; + icps::Reg_t* cfg_runhome_velocity_reg; + icps::Reg_t* cfg_runtohome_dec_reg; + icps::Reg_t* cfg_runtohome_max_distance_reg; + icps::Reg_t* cfg_runtohome_leave_zero_point_distance_reg; + icps::Reg_t* cfg_min_pos_reg; + icps::Reg_t* cfg_max_pos_reg; + icps::Reg_t* cfg_runtohome_keep_move_distance_reg; + + public: + SingleAxisMotorControler(){}; + virtual ~SingleAxisMotorControler(){}; + + void initialize(const char* name, IflytopCanProtocolStackProcesser* processer, int regaddoff, // + ZGPIO* homegpio, // + ZGPIO* mingpio, // + ZGPIO* rightgpio, // + IStepperMotor* motor); + + public: + virtual icps::error_t onHostRegisterWriteEvent(icps::WriteEvent* event); + + icps::error_t rotate(int32_t velocity); + icps::error_t moveTo(int32_t pos); + icps::error_t moveBy(int32_t pos); + icps::error_t runToHome(); + icps::error_t stop(); + icps::error_t clearException(); + + private: + void motor_moveTo(int32_t pos, int32_t velocity, int32_t acc, int32_t dec); + void motor_moveBy(int32_t pos, int32_t velocity, int32_t acc, int32_t dec); + void motor_rotate(int32_t velocity, int32_t acc, int32_t dec); + + void periodJob(ZHALCORE::Context* context); + void changeToState(state_t state); + + private: + bool motorIsStop(StepActionContext* context); + void regGpioIrqProcesser(ZGPIO::onirq_t irqprocesser) { m_gpioirqprocesser = irqprocesser; } + void clearGpioIrqProcesser() { m_gpioirqprocesser = NULL; } +}; + +} // namespace iflytop +#endif diff --git a/components/single_axis_motor_control_v2/i_iflytop_can_slave_module.cpp b/components/single_axis_motor_control_v2/i_iflytop_can_slave_module.cpp index 71d746f..19d5097 100644 --- a/components/single_axis_motor_control_v2/i_iflytop_can_slave_module.cpp +++ b/components/single_axis_motor_control_v2/i_iflytop_can_slave_module.cpp @@ -20,7 +20,7 @@ void icsm::ConfigPara::initialize(IIflytopCanSlaveModule* module, uint32_t regof int32_t regstart = module->getRegStartAdd(); IflytopCanProtocolStackProcesser* processer = module->getProtocolProcesser(); - processer->activeReg(regstart + regoff, mask, defaultValue); + processer->activeReg(module->m_slave, regoff, mask, defaultValue); } icps::error_t icsm::ConfigPara::processIflytopCanRegisterWriteEvent(IflytopCanProtocolStackWriteEvent* writeEvent) { @@ -101,7 +101,7 @@ void icsm::Action::initialize(IIflytopCanSlaveModule* module, uint32_t regoff) { int32_t regstart = module->getRegStartAdd(); IflytopCanProtocolStackProcesser* processer = module->getProtocolProcesser(); - processer->activeReg(regstart + regoff, icps::kw, 0); + processer->activeReg(module->m_slave, regoff, icps::kw, 0); } void icsm::Action::setName(const char* name) { m_name = name; } const char* icsm::Action::getName() { return m_name; } @@ -165,7 +165,8 @@ void IIflytopCanSlaveModule::initialize(const char* name, IflytopCanProtocolStac m_stateticket = 0; m_listener = NULL; - m_protocolProcesser->registerListener(this); + + m_slave = m_protocolProcesser->createICPSSlaveModule(name, this, regoff); } void IIflytopCanSlaveModule::regConfigParas(icsm::ConfigPara* configParas, int configParaNum) { @@ -196,13 +197,13 @@ void IIflytopCanSlaveModule::writeRegValueByRegOff(uint16_t regaddoff, int32_t v m_protocolProcesser->writeRegValue(m_regStart + regaddoff, value, forceupdate); } uint32_t IIflytopCanSlaveModule::getRegOff(IflytopCanProtocolStackWriteEvent* writeEvent) { return writeEvent->reg->add - m_regStart; } -bool IIflytopCanSlaveModule::isThisRegOwnToMe(IflytopCanProtocolStackProcesser* processer, icps::Reg_t* reg) { - if (reg->add >= getRegStartAdd() && // - reg->add <= getRegEndAdd()) { - return true; - } - return false; -} +// bool IIflytopCanSlaveModule::isThisRegOwnToMe(IflytopCanProtocolStackProcesser* processer, icps::Reg_t* reg) { +// if (reg->add >= getRegStartAdd() && // +// reg->add <= getRegEndAdd()) { +// return true; +// } +// return false; +// } void IIflytopCanSlaveModule::stateMachine_pushEvent(icsm::Event event) { m_eventCacheIsEmpty = false; m_eventCache = event; @@ -254,11 +255,11 @@ icsm::Action* IIflytopCanSlaveModule::initializeAction(icsm::Action* action, IIf void IIflytopCanSlaveModule::activeReg(uint16_t mappingAdd, uint16_t mask, int32_t defaultValue) { if (m_protocolProcesser) { - m_protocolProcesser->activeReg(mappingAdd + m_regStart, mask, defaultValue); + m_protocolProcesser->activeReg(m_slave, mappingAdd + m_regStart, mask, defaultValue); } } -icps::error_t IIflytopCanSlaveModule::onHostRegisterWriteEvent(IflytopCanProtocolStackProcesser* processer, IflytopCanProtocolStackWriteEvent* writeEvent) { +icps::error_t IIflytopCanSlaveModule::onHostRegisterWriteEvent(IflytopCanProtocolStackWriteEvent* writeEvent) { /** * @brief * 1.遍历所有配置参数，如果是配置地址，则进行配置 diff --git a/components/single_axis_motor_control_v2/i_iflytop_can_slave_module.hpp b/components/single_axis_motor_control_v2/i_iflytop_can_slave_module.hpp index 9698cbe..5965c51 100644 --- a/components/single_axis_motor_control_v2/i_iflytop_can_slave_module.hpp +++ b/components/single_axis_motor_control_v2/i_iflytop_can_slave_module.hpp @@ -135,7 +135,7 @@ class IIflytopCanSlaveModuleListener { virtual void onException(IIflytopCanSlaveModule* service, int32_t exceptionId) = 0; }; -class IIflytopCanSlaveModule : public IflytopCanProtocolStackProcesserListener { +class IIflytopCanSlaveModule : public ICPSListener { public: const char* m_name; @@ -180,6 +180,8 @@ class IIflytopCanSlaveModule : public IflytopCanProtocolStackProcesserListener { int32_t m_exception_state_index; public: + ICPSSlaveModule* m_slave; + IIflytopCanSlaveModule(/* args */){}; virtual ~IIflytopCanSlaveModule(){}; @@ -229,8 +231,8 @@ class IIflytopCanSlaveModule : public IflytopCanProtocolStackProcesserListener { // bool isMyWriteEvent(IflytopCanProtocolStackWriteEvent* writeEvent); // icps::error_t processIflytopCanRegisterWriteEvent(IflytopCanProtocolStackWriteEvent* writeEvent); - virtual bool isThisRegOwnToMe(IflytopCanProtocolStackProcesser* processer, icps::Reg_t* reg); - virtual icps::error_t onHostRegisterWriteEvent(IflytopCanProtocolStackProcesser* processer, IflytopCanProtocolStackWriteEvent* writeEvent); + // virtual bool isThisRegOwnToMe(IflytopCanProtocolStackProcesser* processer, icps::Reg_t* reg); + virtual icps::error_t onHostRegisterWriteEvent(IflytopCanProtocolStackWriteEvent* writeEvent); /******************************************************************************* * 协议助手 * *******************************************************************************/ diff --git a/components/single_axis_motor_control_v2/single_axis_motor_control_v2.cpp b/components/single_axis_motor_control_v2/single_axis_motor_control_v2.cpp index f6505dc..e059217 100644 --- a/components/single_axis_motor_control_v2/single_axis_motor_control_v2.cpp +++ b/components/single_axis_motor_control_v2/single_axis_motor_control_v2.cpp @@ -531,7 +531,7 @@ icps::error_t SingleAxisMotorControlerV2::stop() { writeEvent.reg = reg; writeEvent.newvalue = 1; writeEvent.oldvalue = reg->getValue(); - onHostRegisterWriteEvent(getProtocolProcesser(), &writeEvent); + onHostRegisterWriteEvent(&writeEvent); return icps::kSuccess; } #endif diff --git a/components/step_action_scheduler/step_scheduler.cpp b/components/step_action_scheduler/step_scheduler.cpp new file mode 100644 index 0000000..4554023 --- /dev/null +++ b/components/step_action_scheduler/step_scheduler.cpp @@ -0,0 +1,117 @@ +#include "step_scheduler.hpp" + +#include +#include +#include + +#include "sdk/hal/zhal.hpp" + +using namespace iflytop; +using namespace std; + +void StepActionScheduler::initialize(int actionCacheSize, int call_period_ms) { + this->m_actionCacheSize = actionCacheSize; + m_actionCache = (StepAction *)calloc(1, sizeof(StepAction) * actionCacheSize); + m_actionNum = 0; + m_nowActionIndex = 0; + m_workingFlag = false; + + ZHALCORE::getInstance()->regPeriodJob([this](ZHALCORE::Context &con) { this->periodcall(con); }, call_period_ms); +} + +void StepActionScheduler::regDoneCallback(function callback) { done_callback = callback; } +void StepActionScheduler::regBreakCallback(function callback) { break_callback = callback; } + +void StepActionScheduler::pushAction(function action, function isDone) { + ZASSERT(m_actionNum < m_actionCacheSize); + this->m_actionCache[m_actionNum].action = action; + this->m_actionCache[m_actionNum].isDone = isDone; + m_actionNum++; +} + +void StepActionScheduler::pushActionAction(function action) { + ZASSERT(m_actionNum < m_actionCacheSize); + this->m_actionCache[m_actionNum].action = action; + m_actionNum++; +} +void StepActionScheduler::pushActionIsDone(function isDone) { + ZASSERT(m_actionNum > 0); + this->m_actionCache[m_actionNum - 1].isDone = isDone; +} +void StepActionScheduler::pushActionDone(function done) { + ZASSERT(m_actionNum > 0); + this->m_actionCache[m_actionNum - 1].done = done; +} + +void StepActionScheduler::startSchedule() { + m_nowActionIndex = 0; + for (int i = 0; i < m_actionNum; i++) { + m_actionCache[i].isDoActionFlag = false; + } + m_workingFlag = true; +} + +void StepActionScheduler::reset() { + m_nowActionIndex = 0; + m_actionNum = 0; + done_callback = nullptr; + break_callback = nullptr; + memset(m_actionCache, 0, sizeof(StepAction) * m_actionCacheSize); +} + +void StepActionScheduler::stop() { + if (m_workingFlag) { + if (done_callback) done_callback(); + } + m_workingFlag = false; +} + +void StepActionScheduler::breakSchedule() { + if (m_workingFlag) { + if (break_callback) break_callback(); + } + m_workingFlag = false; +} + +bool StepActionScheduler::isDone() { + if (m_workingFlag == false) { + return true; + } + if (m_nowActionIndex >= m_actionNum) { + return true; + } else { + return false; + } +} + +void StepActionScheduler::periodcall(ZHALCORE::Context &halcore_con) { + if (m_workingFlag == false) { + return; + } + + if (m_nowActionIndex >= m_actionNum) { + return; + } + + StepActionContext context = {0}; + context.coreContext = &halcore_con; + + if (m_actionCache[m_nowActionIndex].isDoActionFlag == false) { + m_actionCache[m_nowActionIndex].action(&context); + m_actionCache[m_nowActionIndex].isDoActionFlag = true; + } else { + if (m_actionCache[m_nowActionIndex].isDone(&context)) { + if (m_actionCache[m_nowActionIndex].done) { + m_actionCache[m_nowActionIndex].done(&context); + } + m_nowActionIndex++; + } + } + if (context.breakflag) { + if (break_callback) break_callback(); + m_workingFlag = false; + } else if (m_nowActionIndex >= m_actionNum) { + if (done_callback) done_callback(); + m_workingFlag = false; + } +} \ No newline at end of file diff --git a/components/step_action_scheduler/step_scheduler.hpp b/components/step_action_scheduler/step_scheduler.hpp new file mode 100644 index 0000000..3b03c6e --- /dev/null +++ b/components/step_action_scheduler/step_scheduler.hpp @@ -0,0 +1,57 @@ +#pragma once +#include + +#include + +#include "sdk/hal/zhal.hpp" + +namespace iflytop { +using namespace std; +class StepActionContext { + public: + bool breakflag = false; + ZHALCORE::Context *coreContext; +}; + +class StepAction { + public: + function action = {nullptr}; + function isDone = {nullptr}; + function done = {nullptr}; + bool isDoActionFlag = false; +}; + +class StepActionScheduler { + public: + StepAction *m_actionCache = NULL; + int m_actionCacheSize = 0; + + int m_nowActionIndex = 0; + int m_actionNum = 0; + bool m_workingFlag = false; + + function done_callback; + function break_callback; + + public: + void initialize(int actionCacheSize, int call_period_ms); + + void pushAction(function action, function isDone); + + void pushActionAction(function action); + void pushActionIsDone(function isDone); + void pushActionDone(function done); + + void regDoneCallback(function callback); // 自然结束，或者调用stop结束 + void regBreakCallback(function callback); + + void startSchedule(); + void reset(); + void stop(); + void breakSchedule(); + + private: + bool isDone(); + void periodcall(ZHALCORE::Context &con); +}; +} // namespace iflytop \ No newline at end of file diff --git a/hal/zhal_core.hpp b/hal/zhal_core.hpp index 829f497..7c94713 100644 --- a/hal/zhal_core.hpp +++ b/hal/zhal_core.hpp @@ -21,6 +21,7 @@ class ZHALCORE { PeriodJob* periodJob; public: + uint32_t getScheduleTimes() { return periodJob->schedule_times; } }; typedef struct {