From 914b4c75556afc4137ed32bb341bf4377f2765b1 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 27 Jul 2023 19:58:09 +0800 Subject: [PATCH] add idcard_reader_service.hpp --- components/datastructure/datastructure.hpp | 3 + components/datastructure/loopqueue.cpp | 0 components/datastructure/loopqueue.hpp | 67 +++ components/datastructure/static_loop_queue.hpp | 51 ++ .../device_base_control_service.cpp | 49 ++ .../device_base_control_service.hpp | 42 ++ .../idcard_reader_service.cpp | 195 ++++++++ .../idcard_reader_service.hpp | 89 ++++ .../pri/state_machine.cpp | 110 +++++ .../pri/state_machine.hpp | 79 +++ components/m3078/m3078_code_scaner.cpp | 24 +- components/m3078/m3078_code_scaner.hpp | 16 +- .../i_iflytop_can_slave_module.cpp | 373 ++++++++++++++ .../i_iflytop_can_slave_module.hpp | 276 +++++++++++ .../single_axis_motor_control_v2.cpp | 537 +++++++++++++++++++++ .../single_axis_motor_control_v2.hpp | 191 ++++++++ hal/gpio.hpp | 6 - 17 files changed, 2096 insertions(+), 12 deletions(-) create mode 100644 components/datastructure/datastructure.hpp create mode 100644 components/datastructure/loopqueue.cpp create mode 100644 components/datastructure/loopqueue.hpp create mode 100644 components/datastructure/static_loop_queue.hpp create mode 100644 components/iflytop_can_slave_modules/device_base_control_service.cpp create mode 100644 components/iflytop_can_slave_modules/device_base_control_service.hpp create mode 100644 components/iflytop_can_slave_modules/idcard_reader_service.cpp create mode 100644 components/iflytop_can_slave_modules/idcard_reader_service.hpp create mode 100644 components/iflytop_can_slave_modules/pri/state_machine.cpp create mode 100644 components/iflytop_can_slave_modules/pri/state_machine.hpp create mode 100644 components/single_axis_motor_control_v2/i_iflytop_can_slave_module.cpp create mode 100644 components/single_axis_motor_control_v2/i_iflytop_can_slave_module.hpp create mode 100644 components/single_axis_motor_control_v2/single_axis_motor_control_v2.cpp create mode 100644 components/single_axis_motor_control_v2/single_axis_motor_control_v2.hpp diff --git a/components/datastructure/datastructure.hpp b/components/datastructure/datastructure.hpp new file mode 100644 index 0000000..70e5e88 --- /dev/null +++ b/components/datastructure/datastructure.hpp @@ -0,0 +1,3 @@ +#pragma once +#include "loopqueue.hpp" +#include "static_loop_queue.hpp" \ No newline at end of file diff --git a/components/datastructure/loopqueue.cpp b/components/datastructure/loopqueue.cpp new file mode 100644 index 0000000..e69de29 diff --git a/components/datastructure/loopqueue.hpp b/components/datastructure/loopqueue.hpp new file mode 100644 index 0000000..101b3e7 --- /dev/null +++ b/components/datastructure/loopqueue.hpp @@ -0,0 +1,67 @@ +#pragma once +#include +#include +namespace iflytop { +using namespace std; + +template +class LoopQueue { + private: + T *arr; + int maxSize; + int rear; + int front; + + public: + LoopQueue(/* args */) { + maxSize = 1; + rear = 0; + front = 0; + } + ~LoopQueue() {} + + void initialize(int size) { + arr = (T *)malloc(size * sizeof(T)); + ZASSERT(arr != NULL); + maxSize = size; + } + + bool push(T *data) { + if (isFull()) { + return false; + } + memcpy(&arr[rear], data, sizeof(T)); + rear = (rear + 1) % maxSize; + return true; + } + bool pop(T *data) { + if (isEmpty()) { + return false; + } + memcpy(data, &arr[front], sizeof(T)); + front = (front + 1) % maxSize; + return true; + } + + bool popMuti(T *data, int numdata) { + if (numdata > numElements()) return false; + for (int i = 0; i < numdata; i++) { + pop(&data[i]); + } + return true; + } + + bool getHeader(T *data) { + if (isEmpty()) { + return false; + } + memcpy(data, &arr[front], sizeof(T)); + return true; + } + int size() { return maxSize; } + int numElements() { return (rear + maxSize - front) % maxSize; } + bool isEmpty() { return rear == front; } + bool isFull() { return (rear + 1) % maxSize == front; } +}; + +} // namespace iflytop \ No newline at end of file diff --git a/components/datastructure/static_loop_queue.hpp b/components/datastructure/static_loop_queue.hpp new file mode 100644 index 0000000..8f1ff6f --- /dev/null +++ b/components/datastructure/static_loop_queue.hpp @@ -0,0 +1,51 @@ +#pragma once +#include +#include +namespace iflytop { +using namespace std; + +template +class StaticLoopQueue { + private: + T arr[num]; + int maxSize; + int rear; + int front; + + public: + StaticLoopQueue(/* args */) { + maxSize = num; + rear = 0; + front = 0; + } + ~StaticLoopQueue() {} + bool push(T *data) { + if (isFull()) { + return false; + } + memcpy(&arr[rear], data, sizeof(T)); + rear = (rear + 1) % maxSize; + return true; + } + bool pop(T *data) { + if (isEmpty()) { + return false; + } + memcpy(data, &arr[front], sizeof(T)); + front = (front + 1) % maxSize; + return true; + } + bool getHeader(T *data) { + if (isEmpty()) { + return false; + } + memcpy(data, &arr[front], sizeof(T)); + return true; + } + int size() { return num; } + int numElements() { return (rear + maxSize - front) % maxSize; } + bool isEmpty() { return rear == front; } + bool isFull() { return (rear + 1) % maxSize == front; } +}; + +} // namespace iflytop \ No newline at end of file diff --git a/components/iflytop_can_slave_modules/device_base_control_service.cpp b/components/iflytop_can_slave_modules/device_base_control_service.cpp new file mode 100644 index 0000000..43fcb4a --- /dev/null +++ b/components/iflytop_can_slave_modules/device_base_control_service.cpp @@ -0,0 +1,49 @@ +#include "device_base_control_service.hpp" +#ifdef HAL_CAN_MODULE_ENABLED +using namespace std; +using namespace iflytop; +void DeviceBaseControlService::initialize(IflytopCanProtocolStackProcesser* protocol_processer, uint8_t deviceId) { + m_protocol_processer = protocol_processer; + m_deviceId = deviceId; + ZASSERT(m_protocol_processer != NULL); + activeRegs(); +} +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(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; +} + +icps::error_t DeviceBaseControlService::processIflytopCanRegisterWriteEvent(IflytopCanProtocolStackWriteEvent* writeEvent) { + switch (writeEvent->reg->add) { + case REG_DEVICE_ID: + return icps::kRegNotWritable; + case REG_DEVICE_REBOOT: + NVIC_SystemReset(); + return icps::kSuccess; + case REG_DEVICE_ENGINEER_MODE_MODE: + if (m_listener != NULL) { + m_listener(writeEvent->newvalue); + return icps::kSuccess; + } + return icps::kSuccess; + default: + break; + } + return icps::kRegNotFound; +} + +bool DeviceBaseControlService::isInEngineerMode() { return m_protocol_processer->readRegValue(REG_DEVICE_ENGINEER_MODE_MODE) != 0; } +#endif \ No newline at end of file diff --git a/components/iflytop_can_slave_modules/device_base_control_service.hpp b/components/iflytop_can_slave_modules/device_base_control_service.hpp new file mode 100644 index 0000000..490382f --- /dev/null +++ b/components/iflytop_can_slave_modules/device_base_control_service.hpp @@ -0,0 +1,42 @@ +#pragma once +#include "sdk/hal/zhal.hpp" +#ifdef HAL_CAN_MODULE_ENABLED +#include + +#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp" + +namespace iflytop { + +#define REG_DEVICE_ID (0) // 设备ID +#define REG_DEVICE_REBOOT (1) // 重启标识 +#define REG_DEVICE_ENGINEER_MODE_MODE (2) // 工程师模式标识 +class DeviceBaseControlService; +// class DeviceBaseControlServiceListener { +// public: +// virtual icps::error_t onSetEngineerMode(DeviceBaseControlService* service, int32_t engineer_mode) = 0; +// }; +class DeviceBaseControlService { + public: + typedef function SetEngineerModeCallback_t; + + private: + IflytopCanProtocolStackProcesser* m_protocol_processer; + SetEngineerModeCallback_t m_listener; + uint8_t m_deviceId; + + public: + DeviceBaseControlService() {} + virtual ~DeviceBaseControlService() {} + + void initialize(IflytopCanProtocolStackProcesser* protocol_processer, uint8_t deviceId); + void setListener(SetEngineerModeCallback_t listener) { m_listener = listener; } + + bool isThisRegOwnToMe(icps::Reg_t* reg); + icps::error_t processIflytopCanRegisterWriteEvent(IflytopCanProtocolStackWriteEvent* writeEvent); + + private: + void activeRegs(); + bool isInEngineerMode(); +}; +} // namespace iflytop +#endif \ No newline at end of file diff --git a/components/iflytop_can_slave_modules/idcard_reader_service.cpp b/components/iflytop_can_slave_modules/idcard_reader_service.cpp new file mode 100644 index 0000000..c5efcef --- /dev/null +++ b/components/iflytop_can_slave_modules/idcard_reader_service.cpp @@ -0,0 +1,195 @@ +#include "idcard_reader_service.hpp" +#ifdef HAL_CAN_MODULE_ENABLED + +#include "idcard_reader_service.hpp" +using namespace iflytop; + +#define TAG "IDCardReaderService" + +static int32_t getPacketNumByBytesNum(int num) { return num / 4 + (num % 4 == 0 ? 0 : 1); } +static int32_t littleEndianBigEnd(int32_t data) { + int32_t ret = 0; + ret |= (data & 0xff) << 24; + ret |= (data & 0xff00) << 8; + ret |= (data & 0xff0000) >> 8; + ret |= (data & 0xff000000) >> 24; + return ret; +} + +void IDCardReaderService::initialize(const char* name, IflytopCanProtocolStackProcesser* protocolProcesser, int32_t regStartOff, + M3078CodeScanner* codeScanner) { + /******************************************************************************* + * 通用成员变量初始化 * + *******************************************************************************/ + m_name = name; + m_protocolProcesser = protocolProcesser; + m_exceptionId = 0; + m_regStartOff = regStartOff; + m_stateMachine.initialize(m_name, 10, &IDCardReaderService::onProcessState, this); + + /******************************************************************************* + * 专用成员变量初始化 * + *******************************************************************************/ + 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); +} + +icps::error_t IDCardReaderService::processWriteEvent(icps::WriteEvent* event) { + int regoff = event->reg->add - m_regStartOff; + int val = event->newvalue; + switch (regoff) { + case REG_CODE_SCANER_ACT_CTRL: + if (val == 0) { // 停止扫码 + m_stateMachine.triggerEvent(kEvent_stopEvent, 0); + ZLOGI(TAG, "stop scan"); + } else if (val == 1) { // 单次扫码 + m_stateMachine.triggerEvent(kEvent_oneShutScanEvent, 0); + ZLOGI(TAG, "start one shut scan"); + } else if (val == 2) { // 连续扫码 + m_stateMachine.triggerEvent(kEvent_continueScanEvent, 0); + ZLOGI(TAG, "start continue scan"); + return icps::kIllegalValue; + } else { + return icps::kIllegalValue; + } + return icps::kSuccess; + case REG_CODE_SCANER_ACT_CLEAR_EXCEPTION: + m_stateMachine.triggerEvent(kEvent_clearExceptionEvent, 0); + return icps::kSuccess; + } + return icps::kRegNotWritable; +} +icps::error_t IDCardReaderService::processReportEvent(icps::ReportEvent* event) { + int regoff = event->reg->add - m_regStartOff; + int32_t* val = &event->value; + + /** + * @brief 上报扩展控制点的信息 + */ + if (regoff == REG_CODE_SCANER_CODE) { + if (event->extRegSubOff == 0) { // 第一包上报当前包长度 + *val = m_codeScanner->getIdinfoLen(); + } else { // 上报数据 + int32_t* data = (int32_t*)m_codeScanner->getIdinfo(); + *val = data[event->extRegSubOff - 1]; + } + } + return icps::kSuccess; +} + +void IDCardReaderService::onProcessState(StateMachine::StateProcessContext* context, StateMachine::Event* event) { + int nowState = m_stateMachine.getNowState(); + switch (nowState) { + case kidle: + return processIdleState(context, event); + case kcodeScanning: + return processCodeScanning(context, event); + case kCodeReporting: + return processCodeReporting(context, event); + case kexception: + return processExceptionState(context, event); + } + return; +} + +void IDCardReaderService::processExceptionState(StateMachine::StateProcessContext* context, StateMachine::Event* event) { + if (event->eventIndex == StateMachine::kEnterState) { // 进入异常状态 + writeRegVal(REG_CODE_SCANER_STAT_EXCEPTION, m_exceptionId); + } else if (event->eventIndex == StateMachine::kExitState) { // 退出异常状态 + m_exceptionId = 0; + writeRegVal(REG_CODE_SCANER_STAT_EXCEPTION, m_exceptionId); + } else if (event->eventIndex == kEvent_clearExceptionEvent) { // 单次扫码 + m_exceptionId = 0; + // context->changeToStateIndex = kidle; + context->changeToState(kidle, 0); + } +} +/******************************************************************************* + * 空闲状态 * + *******************************************************************************/ +void IDCardReaderService::processIdleState(StateMachine::StateProcessContext* context, StateMachine::Event* event) { + if (event->eventIndex == kEvent_oneShutScanEvent) { // 单次扫码 + // context->changeToStateIndex = kcodeScanning; + context->changeToState(kcodeScanning, 0); + m_oneShutScan = true; + } else if (event->eventIndex == kEvent_continueScanEvent) { + // context->changeToStateIndex = kcodeScanning; + context->changeToState(kcodeScanning, 0); + m_oneShutScan = false; + } +} + +/******************************************************************************* + * 扫码中 * + *******************************************************************************/ +void IDCardReaderService::processCodeScanning(StateMachine::StateProcessContext* context, StateMachine::Event* event) { + if (event->eventIndex == StateMachine::kEnterState) { // 开始一次扫码 + m_codeScanner->trigger(); + } else if (event->eventIndex == StateMachine::kExitState) { // 电平拉低 + m_codeScanner->stopTrigger(); + } else if (event->eventIndex == StateMachine::kPeriodicCallEvent) { // 周期调用 + if (m_codeScanner->idInfoIsReady() && // 等待扫码完成 + !m_codeReg->is(icps::kreadyReport) // 等待上一次上报完成 + ) { + m_protocolProcesser->writeRegValue(m_codeReg, m_codeScanner->getIdinfoLen(), false); + m_protocolProcesser->setRegReadyToReport(m_codeReg); + // context->changeToStateIndex = kCodeReporting; + context->changeToState(kCodeReporting, 0); + } + } else if (event->eventIndex == kEvent_stopEvent) { // 停止扫码 + // context->changeToStateIndex = kidle; + context->changeToState(kidle, 0); + } else if (event->eventIndex == kEvent_oneShutScanEvent) { // 单次扫码 + m_oneShutScan = true; + } else if (event->eventIndex == kEvent_continueScanEvent) { + m_oneShutScan = false; + } +} +/******************************************************************************* + * 扫码上报中 * + *******************************************************************************/ +void IDCardReaderService::processCodeReporting(StateMachine::StateProcessContext* context, StateMachine::Event* event) { + if (event->eventIndex == StateMachine::kEnterState) { + } else if (event->eventIndex == StateMachine::kExitState) { + } else if (event->eventIndex == StateMachine::kPeriodicCallEvent) { + if (!m_codeReg->is(icps::kreadyReport)) { + // 上报完成 + if (m_oneShutScan) { + // context->changeToStateIndex = kidle; + context->changeToState(kidle, 0); + } else { + // context->changeToStateIndex = kcodeScanning; + context->changeToState(kcodeScanning, 0); + } + } + } else if (event->eventIndex == kEvent_stopEvent) { // 停止扫码 + // context->changeToStateIndex = kidle; + context->changeToState(kidle, 0); + } else if (event->eventIndex == kEvent_oneShutScanEvent) { // 单次扫码 + m_oneShutScan = true; + } else if (event->eventIndex == kEvent_continueScanEvent) { + m_oneShutScan = false; + } +} +void IDCardReaderService::onProcessState(void* module, StateMachine::StateProcessContext* context, StateMachine::Event* event) { // + ((IDCardReaderService*)module)->onProcessState(context, event); +} +/******************************************************************************* + * 通用方法 * + *******************************************************************************/ +void IDCardReaderService::writeRegVal(uint16_t regoff, uint16_t regval) { m_protocolProcesser->writeRegValue(uint16_t(m_regStartOff + regoff), regval, false); } +bool IDCardReaderService::isThisRegOwnToMe(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 new file mode 100644 index 0000000..dcca3b0 --- /dev/null +++ b/components/iflytop_can_slave_modules/idcard_reader_service.hpp @@ -0,0 +1,89 @@ + +#pragma once +// #include "libiflytop_micro\stm32\basic\stm32_header.h" +#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp" +#ifdef HAL_CAN_MODULE_ENABLED +#include + +#include "pri/state_machine.hpp" +#include "sdk/components/datastructure/datastructure.hpp" +#include "sdk/components/m3078/m3078_code_scaner.hpp" + +#define REG_CODE_SCANER_ACT_CTRL (0) // 扫码器控制 写0停止扫码，写1开始扫码 +#define REG_CODE_SCANER_ACT_CLEAR_EXCEPTION (1) // 清除异常 +#define REG_CODE_SCANER_STAT_STATUS (5) // 设备状态 +#define REG_CODE_SCANER_STAT_EXCEPTION (6) // 异常状态 +#define REG_CODE_SCANER_CODE (7) // 码存放的地方 + +#define REG_CODE_SCANER_SIZE (20) // + +namespace iflytop { +using namespace std; +using namespace iflytop_can_slave_modules_pri; + +/** + * @brief + * + * 协议参考: + * https://iflytop1.feishu.cn/wiki/OYZFwSNf2i0Hyek1zouc8eSsnsg + * + */ +class IDCardReaderService { + public: + typedef enum { + kidle = 0, + kcodeScanning = 1, + kCodeReporting = 2, + kexception = 65535, + } state_t; + + typedef enum { + kEvent_stopEvent = 1, + kEvent_oneShutScanEvent, + kEvent_continueScanEvent, + kEvent_clearExceptionEvent, + } event_t; + + private: + /******************************************************************************* + * 通用成员变量 * + *******************************************************************************/ + const char* m_name; + IflytopCanProtocolStackProcesser* m_protocolProcesser; + StateMachine m_stateMachine; + int32_t m_exceptionId; + int32_t m_regStartOff; + + /******************************************************************************* + * 专用成员变量 * + *******************************************************************************/ + M3078CodeScanner* m_codeScanner; + icps::Reg_t* m_codeReg; + + bool m_oneShutScan; + + public: + void initialize(const char* name, IflytopCanProtocolStackProcesser* protocolProcesser, int32_t regStartOff, + M3078CodeScanner* codeScanner); + + bool isThisRegOwnToMe(icps::Reg_t* reg); + + icps::error_t processWriteEvent(icps::WriteEvent* event); + icps::error_t processReportEvent(icps::ReportEvent* event); + + void periodicJob(); + + public: + private: + static void onProcessState(void* module, StateMachine::StateProcessContext* context, StateMachine::Event* event); + + void onProcessState(StateMachine::StateProcessContext* context, StateMachine::Event* event); + void processIdleState(StateMachine::StateProcessContext* context, StateMachine::Event* event); + void processCodeScanning(StateMachine::StateProcessContext* context, StateMachine::Event* event); + void processCodeReporting(StateMachine::StateProcessContext* context, StateMachine::Event* event); + void processExceptionState(StateMachine::StateProcessContext* context, StateMachine::Event* event); + + void writeRegVal(uint16_t regoff, uint16_t regval); +}; +} // namespace iflytop +#endif diff --git a/components/iflytop_can_slave_modules/pri/state_machine.cpp b/components/iflytop_can_slave_modules/pri/state_machine.cpp new file mode 100644 index 0000000..69ef067 --- /dev/null +++ b/components/iflytop_can_slave_modules/pri/state_machine.cpp @@ -0,0 +1,110 @@ +#include "state_machine.hpp" +using namespace iflytop; +using namespace std; +using namespace iflytop_can_slave_modules_pri; + +StateMachine::StateMachine() { + m_name = NULL; + // m_eventCache + + m_onProcessState = NULL; + m_father_module = NULL; + + m_nowStateIndex = 0; + m_nowStateTicket = 0; + + m_lastCallTicket = 0; + m_stateMachineTicket = 0; + + isChangeState = false; + m_changeToStateIndex = 0; +} +StateMachine::~StateMachine() {} +void StateMachine::initialize(const char* name, int32_t periodicCallPeriod, onProcessState_t onProcessState, void* module) { + m_name = name; + m_periodicCallPeriod = periodicCallPeriod; + m_onProcessState = onProcessState; + m_father_module = module; +} +void StateMachine::triggerEvent(int evenIndex, int32_t eventVal) { + Event event; + event.eventIndex = evenIndex; + event.eventVal = eventVal; + bool push = m_eventCache.push(&event); + if (!push) { + ZLOGE(m_name, "StateMachine::triggerEvent failed, event cache is full"); + } +} +void StateMachine::regOnProcessState(void* module, onProcessState_t onProcessState) { + m_father_module = module; + m_onProcessState = onProcessState; +} +int32_t StateMachine::getNowState() { return m_nowStateIndex; } + +void StateMachine::periodicCall() { + /** + * @brief 从队列中取出事件处理事件 + */ + if (isChangeState) { + isChangeState = false; + int changeToStateIndex = m_changeToStateIndex; + m_changeToStateIndex = 0; + changeState(changeToStateIndex, m_changeToStateVal); + } else if (!m_eventCache.isEmpty()) { + Event event; + m_eventCache.pop(&event); + callProcessState(&event); + } else { + // if (m_lastCallTicket != m_os->getTicket()) { + if (haspassedms(m_lastCallTicket) >= (uint32_t)m_periodicCallPeriod) { + m_lastCallTicket = chip_get_ticket(); + m_stateMachineTicket++; + m_nowStateTicket++; + + Event event; + event.eventIndex = kPeriodicCallEvent; + event.eventVal = m_nowStateTicket; + callProcessState(&event); + } + } +} + +void StateMachine::changeState(int32_t stateIndex, int32_t enterStateVal) { + { + StateMachine::StateProcessContext context; + Event event(kExitState, 0); + if (m_onProcessState) { + m_onProcessState(m_father_module, &context, &event); + } + } + + ZLOGI(m_name, "change state from %d to %d", m_nowStateIndex, stateIndex); + m_nowStateIndex = stateIndex; + m_stateMachineTicket = 0; + + { + StateMachine::StateProcessContext context; + Event event(kEnterState, enterStateVal); + if (m_onProcessState) { + m_onProcessState(m_father_module, &context, &event); + } + if (context.getChangeToStateIndex() != -1) { + isChangeState = true; + m_changeToStateIndex = context.getChangeToStateIndex(); + m_changeToStateVal = context.getEnterStateVal(); + } + } +} + +void StateMachine::callProcessState(Event* event) { // + StateMachine::StateProcessContext context; + if (m_onProcessState) { + m_onProcessState(m_father_module, &context, event); + } + + if (context.getChangeToStateIndex() != -1) { + isChangeState = true; + m_changeToStateIndex = context.getChangeToStateIndex(); + m_changeToStateVal = context.getEnterStateVal(); + } +} \ No newline at end of file diff --git a/components/iflytop_can_slave_modules/pri/state_machine.hpp b/components/iflytop_can_slave_modules/pri/state_machine.hpp new file mode 100644 index 0000000..533c244 --- /dev/null +++ b/components/iflytop_can_slave_modules/pri/state_machine.hpp @@ -0,0 +1,79 @@ +#pragma once +#include +#include + +#include "sdk/components/datastructure/datastructure.hpp" +#include "sdk/hal/zhal.hpp" + +namespace iflytop_can_slave_modules_pri { +using namespace std; + +class StateMachine { + public: + typedef enum { + kEnterState = 10001, + kExitState = 10002, + kPeriodicCallEvent = 10003, + } EventIndex_t; + + class Event { + public: + int32_t eventIndex; + int32_t eventVal; + Event() : eventIndex(0), eventVal(0) {} + Event(int32_t index, int32_t val) : eventIndex(index), eventVal(val) {} + }; + + class StateProcessContext { + int32_t changeToStateIndex; + int32_t enterStateVal; + + public: + void changeToState(int32_t stateIndex, int32_t enterStateVal) { changeToStateIndex = stateIndex; } + + int32_t getChangeToStateIndex() { return changeToStateIndex; } + int32_t getEnterStateVal() { return enterStateVal; } + + void setEnterStateVal(int32_t val) { enterStateVal = val; } + + StateProcessContext() : changeToStateIndex(-1) {} + }; + + typedef void (*onProcessState_t)(void* module, StateMachine::StateProcessContext* context, Event* event); + + private: + const char* m_name; + iflytop::StaticLoopQueue m_eventCache; + + onProcessState_t m_onProcessState; + void* m_father_module; + + int m_nowStateIndex; + int m_nowStateTicket; + + int32_t m_lastCallTicket; + int32_t m_stateMachineTicket; + + bool isChangeState; + int32_t m_changeToStateIndex; + int32_t m_changeToStateVal; + + int32_t m_periodicCallPeriod; + + public: + StateMachine(); + ~StateMachine(); + + void initialize(const char* name, int32_t periodicCallPeriod, onProcessState_t onProcessState, void* module); + + void triggerEvent(int evenIndex, int32_t eventVal); + void regOnProcessState(void* module, onProcessState_t onProcessState); + void periodicCall(); + + int32_t getNowState(); + + private: + void callProcessState(Event* event); + void changeState(int32_t stateIndex, int32_t enterStateVal); +}; +} // namespace iflytop \ No newline at end of file diff --git a/components/m3078/m3078_code_scaner.cpp b/components/m3078/m3078_code_scaner.cpp index 2f42cd4..17f6a4f 100644 --- a/components/m3078/m3078_code_scaner.cpp +++ b/components/m3078/m3078_code_scaner.cpp @@ -1,4 +1,7 @@ #include "m3078_code_scaner.hpp" + +#include +#include using namespace iflytop; using namespace std; @@ -10,7 +13,12 @@ void M3078CodeScanner::initialize(UART_HandleTypeDef* huart, Pin_t triggerPin) { cfg.rxovertime_ms = 10; m_uart.initialize(&cfg, [this](uint8_t* data, size_t len) { - if (m_onidinfo) m_onidinfo((char*)data); + // if (m_onidinfo) m_onidinfo((char*)data); + if (len < sizeof(m_buf) - 1) { + memcpy(m_buf, data, len); + m_bufLen = len; + m_isready = true; + } }); m_triggerGpio.initAsOutput(triggerPin, ZGPIO::kMode_nopull, false, false); @@ -18,10 +26,10 @@ void M3078CodeScanner::initialize(UART_HandleTypeDef* huart, Pin_t triggerPin) { m_uart.startRxIt(); m_triggerGpio.setState(0); - m_onidinfo = NULL; + // m_onidinfo = NULL; } -void M3078CodeScanner::regListener(onidinfo_t listener) { m_onidinfo = listener; } +// void M3078CodeScanner::regListener(onidinfo_t listener) { m_onidinfo = listener; } void M3078CodeScanner::trigger() { if (!m_trigger) { m_triggerGpio.setState(0); @@ -35,3 +43,13 @@ void M3078CodeScanner::stopTrigger() { m_triggerGpio.setState(0); m_trigger = false; } + +bool M3078CodeScanner::idInfoIsReady() { return m_isready; } + +char* M3078CodeScanner::getIdinfo() { return m_buf; } +size_t M3078CodeScanner::getIdinfoLen() { return m_bufLen; } +void M3078CodeScanner::clearIdinfo() { + m_isready = false; + m_bufLen = 0; + memset(m_buf, 0, sizeof(m_buf)); +} diff --git a/components/m3078/m3078_code_scaner.hpp b/components/m3078/m3078_code_scaner.hpp index 6a1a551..fb1579a 100644 --- a/components/m3078/m3078_code_scaner.hpp +++ b/components/m3078/m3078_code_scaner.hpp @@ -25,14 +25,18 @@ namespace iflytop { using namespace std; class M3078CodeScanner { - typedef function onidinfo_t; + // typedef function onidinfo_t; private: ZGPIO m_triggerGpio; ZUART m_uart; bool m_trigger; - onidinfo_t m_onidinfo; + char m_buf[128] = {0}; + int m_bufLen = 0; + bool m_isready = false; + + // onidinfo_t m_onidinfo; public: M3078CodeScanner(){}; @@ -50,10 +54,16 @@ class M3078CodeScanner { * 2. 接收超时建议大于10ms */ void initialize(UART_HandleTypeDef* huart, Pin_t triggerPin); - void regListener(onidinfo_t listener); + // void regListener(onidinfo_t listener); void trigger(); void stopTrigger(); + + bool idInfoIsReady(); + + char* getIdinfo(); + size_t getIdinfoLen(); + void clearIdinfo(); }; } // namespace iflytop \ No newline at end of file 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 new file mode 100644 index 0000000..373078a --- /dev/null +++ b/components/single_axis_motor_control_v2/i_iflytop_can_slave_module.cpp @@ -0,0 +1,373 @@ +#include "i_iflytop_can_slave_module.hpp" +#ifdef HAL_CAN_MODULE_ENABLED +#include +#include + +using namespace iflytop; +#define STATE_NULL -1 + +static bool s_dumpConfig; + +/******************************************************************************* + * ConfigPara * + *******************************************************************************/ +void icsm::ConfigPara::initialize(IIflytopCanSlaveModule* module, uint32_t regoff, uint16_t mask, int32_t defaultValue) { + // + m_module = module; + m_regAddOff = regoff; + m_val = defaultValue; + m_name = "unset"; + + int32_t regstart = module->getRegStartAdd(); + IflytopCanProtocolStackProcesser* processer = module->getProtocolProcesser(); + processer->activeReg(regstart + regoff, mask, defaultValue); +} + +icps::error_t icsm::ConfigPara::processIflytopCanRegisterWriteEvent(IflytopCanProtocolStackWriteEvent* writeEvent) { + int32_t val = writeEvent->newvalue; + if (!m_module->getEngineerMode() && m_enableLimit) { + if (val > m_limitMax) { + val = m_limitMax; + } else if (val < m_limitMin) { + val = m_limitMin; + } + } + m_val = val; + writeEvent->newvalue = m_val; + return icps::kSuccess; +} + +icps::error_t icsm::ConfigPara::setValWitchLimit(int32_t initVal, int32_t min, int32_t max) { + m_limitMin = min; + m_limitMax = max; + m_enableLimit = true; + return setVal(initVal); +} + +icps::error_t icsm::ConfigPara::setVal(int32_t val) { + val = val; + if (s_dumpConfig) { + ZLOGI(m_module->m_name, "set %s = %d", m_name, val); + } + if (!m_module->getEngineerMode() && m_enableLimit) { + if (val > m_limitMax) { + val = m_limitMax; + } else if (val < m_limitMin) { + val = m_limitMin; + } + } + m_val = val; + m_module->writeRegValueByRegOff(m_regAddOff, val); + return icps::kSuccess; +} +int32_t icsm::ConfigPara::getVal() { return m_val; } + +icps::error_t icsm::ConfigPara::setEnableLimit(bool enable) { + m_enableLimit = enable; + return icps::kSuccess; +} +bool icsm::ConfigPara::getEnableLimit() { return m_enableLimit; } + +icps::error_t icsm::ConfigPara::setLimitMin(int32_t val) { + m_limitMin = val; + return icps::kSuccess; +} +int32_t icsm::ConfigPara::getLimitMin() { return m_limitMin; } +icps::error_t icsm::ConfigPara::setLimitMax(int32_t val) { + m_limitMax = val; + return icps::kSuccess; +} +int32_t icsm::ConfigPara::getLimitMax() { return m_limitMax; } +int32_t icsm::ConfigPara::getRegAddOff() { return m_regAddOff; } + +icps::error_t icsm::ConfigPara::enableLimit(bool enable, int32_t min, int32_t max) { + m_enableLimit = enable; + m_limitMin = min; + m_limitMax = max; + return icps::kSuccess; +} + +void icsm::ConfigPara::setName(const char* name) { m_name = name; } + +/******************************************************************************* + * ACTION * + *******************************************************************************/ + +void icsm::Action::initialize(IIflytopCanSlaveModule* module, uint32_t regoff) { + // + m_module = module; + m_regAddOff = regoff; + m_name = "unkownaction"; + + int32_t regstart = module->getRegStartAdd(); + IflytopCanProtocolStackProcesser* processer = module->getProtocolProcesser(); + processer->activeReg(regstart + regoff, icps::kw, 0); +} +void icsm::Action::setName(const char* name) { m_name = name; } +const char* icsm::Action::getName() { return m_name; } +icps::error_t icsm::Action::setVal(int32_t val) { + m_val = val; + return icps::kSuccess; +} +int32_t icsm::Action::getVal() { return m_val; } +int32_t icsm::Action::getRegAddOff() { return m_regAddOff; } + +/******************************************************************************* + * IIflytopCanSlaveModule * + *******************************************************************************/ +void IIflytopCanSlaveModule::stateMachine_periodicJob(int ticket) { + if (m_lastcallticket != chip_get_ticket()) { + m_lastcallticket = chip_get_ticket(); + m_module_ticket++; + m_stateticket++; + // + stateMachine_onPeriodic(m_module_ticket); + stateMachine_triggerEvent(icsm::Event(icsm::kPublicEvent, icsm::kevent_periodicJob)); + } + stateMachine_onAsFarAsPossibleCall(); + if (!m_eventCacheIsEmpty) { + stateMachine_triggerEvent(m_eventCache); + m_eventCacheIsEmpty = true; + } +} +void IIflytopCanSlaveModule::setListener(IIflytopCanSlaveModuleListener* listener) { m_listener = listener; } + +void IIflytopCanSlaveModule::initialize(const char* name, IflytopCanProtocolStackProcesser* processer, int32_t regoff, int32_t regNum) { + m_name = name; + + m_regStart = regoff; + m_regNum = regNum; + + m_statusRegOff = -1; + m_ExceptionRegOff = -1; + + m_configParas = NULL; + m_configParaNum = 0; + + m_action = NULL; + m_actionNum = 0; + + engineer_mode = false; + + m_eventCacheIsEmpty = true; + // m_eventCache; + + m_protocolProcesser = processer; + + m_lastState = 0; + m_state = 0; + m_exceptionId = 0; + + m_lastcallticket = 0; + m_periodms = 10; + m_module_ticket = 0; + + m_stateticket = 0; + + m_listener = NULL; +} + +void IIflytopCanSlaveModule::regConfigParas(icsm::ConfigPara* configParas, int configParaNum) { + m_configParas = configParas; + m_configParaNum = configParaNum; +} +void IIflytopCanSlaveModule::regActions(icsm::Action* action, int actionNum) { + m_action = action; + m_actionNum = actionNum; +} +void IIflytopCanSlaveModule::setStatusRegOff(int32_t regoff) { + m_statusRegOff = regoff; + IIflytopCanSlaveModule::activeReg(regoff, icps::kr, 0); +} +void IIflytopCanSlaveModule::setExceptionRegOff(int32_t regoff) { + m_ExceptionRegOff = regoff; + IIflytopCanSlaveModule::activeReg(m_ExceptionRegOff, icps::kr, 0); +} + +void IIflytopCanSlaveModule::setIdelStateIndex(int32_t stateIndex) { m_idle_state_index = stateIndex; } +void IIflytopCanSlaveModule::setExceptionStateIndex(int32_t stateIndex) { m_exception_state_index = stateIndex; } + +void IIflytopCanSlaveModule::periodicJob() { stateMachine_periodicJob(chip_get_ticket()); } + +void IIflytopCanSlaveModule::setEngineerMode(bool engineer_mode) { this->engineer_mode = engineer_mode; } +bool IIflytopCanSlaveModule::getEngineerMode() { return engineer_mode; } +void IIflytopCanSlaveModule::writeRegValueByRegOff(uint16_t regaddoff, int32_t value, bool forceupdate) { + m_protocolProcesser->writeRegValue(m_regStart + regaddoff, value, forceupdate); +} +uint32_t IIflytopCanSlaveModule::getRegOff(IflytopCanProtocolStackWriteEvent* writeEvent) { return writeEvent->reg->add - m_regStart; } +bool IIflytopCanSlaveModule::isMyWriteEvent(IflytopCanProtocolStackWriteEvent* writeEvent) { + if (writeEvent->reg->add >= getRegStartAdd() && // + writeEvent->reg->add <= getRegEndAdd()) { + return true; + } + return false; +} +void IIflytopCanSlaveModule::stateMachine_pushEvent(icsm::Event event) { + m_eventCacheIsEmpty = false; + m_eventCache = event; +} + +int32_t IIflytopCanSlaveModule::stateMachine_getStateIndex() { return m_state; } +int32_t IIflytopCanSlaveModule::stateMachine_getStateDuration() { return m_stateticket; } +int32_t IIflytopCanSlaveModule::getExceptionId() { return m_exceptionId; } +bool IIflytopCanSlaveModule::stateMachine_isIdle() { return m_state == m_idle_state_index; } + +void IIflytopCanSlaveModule::disablePermission(int32_t actionRegoff, uint32_t permission) { + m_protocolProcesser->disablePermission(m_regStart + actionRegoff, permission); +} + +icsm::ConfigPara* IIflytopCanSlaveModule::getConfig(int regoff) { + for (size_t i = 0; i < m_configParaNum; i++) { + if (m_configParas[i].getRegAddOff() == regoff) { + return &m_configParas[i]; + } + } + ZLOGE(m_name, "Can't find config by reg,reg == %d", regoff); + return &m_zeroCfg; +} +icsm::Action* IIflytopCanSlaveModule::getAction(int regoff) { + for (size_t i = 0; i < m_actionNum; i++) { + if (m_action[i].getRegAddOff() == regoff) { + return &m_action[i]; + } + } + ZLOGE(m_name, "Can't find action by reg,reg == %d", regoff); + + return &m_zeroaction; +} +void IIflytopCanSlaveModule::initializeCfg(icsm::ConfigPara* cfg, icsm::ConfigPara** shadowcfg, IIflytopCanSlaveModule* module, uint32_t regoff, uint16_t mask, + int32_t defaultValue) { + cfg->initialize(module, regoff, mask, defaultValue); + *shadowcfg = cfg; +} +void IIflytopCanSlaveModule::initializeCfg(const char* name, icsm::ConfigPara* cfg, icsm::ConfigPara** shadowcfg, IIflytopCanSlaveModule* module, + uint32_t regoff, uint16_t mask, int32_t defaultValue) { + initializeCfg(cfg, shadowcfg, module, regoff, mask, defaultValue); + cfg->setName(name); +} +icsm::Action* IIflytopCanSlaveModule::initializeAction(icsm::Action* action, IIflytopCanSlaveModule* module, const char* actionName, uint32_t regoff) { + action->initialize(module, regoff); + action->setName(actionName); + return action; +} + +void IIflytopCanSlaveModule::activeReg(uint16_t mappingAdd, uint16_t mask, int32_t defaultValue) { + if (m_protocolProcesser) { + m_protocolProcesser->activeReg(mappingAdd + m_regStart, mask, defaultValue); + } +} + +icps::error_t IIflytopCanSlaveModule::processIflytopCanRegisterWriteEvent(IflytopCanProtocolStackWriteEvent* writeEvent) { + /** + * @brief + * 1.遍历所有配置参数，如果是配置地址，则进行配置 + * 2.遍历所有动作参数，如果是动作地址，则检查动作参数后，则执行相应的动作。 + * 3.如果是状态参数，则返回非法操作。 + */ + icps::error_t err; + int regoff = writeEvent->reg->add - m_regStart; + for (size_t i = 0; i < m_configParaNum; i++) { + if (m_configParas[i].getRegAddOff() == regoff) { + err = m_configParas[i].processIflytopCanRegisterWriteEvent(writeEvent); + ZLOGI(m_name, "update Config %d %d", m_configParas[i].getRegAddOff(), writeEvent->newvalue); + return err; + } + } + + for (size_t i = 0; i < m_actionNum; i++) { + if (m_action[i].getRegAddOff() == regoff) { + /** + * @brief + * 1. 判断是否可以执行Action + * 2. 执行Action + */ + err = stateMachine_onTryDoAction(&m_action[i], writeEvent->newvalue); + if (err != icps::kSuccess) { + return err; + } + m_actionVal = writeEvent->newvalue; + // m_action[i].getRegAddOff(); + ZLOGI(m_name, "Action %d %d", m_action[i].getRegAddOff(), writeEvent->newvalue); + stateMachine_pushEvent(icsm::Event(icsm::kPublicEvent, icsm::kevent_doAction, m_action[i].getRegAddOff(), writeEvent->newvalue)); + return icps::kSuccess; + } + } + return icps::kRegNotFound; +} + +void IIflytopCanSlaveModule::_stateMachine_onPocessEvent(icsm::StateMachineEventContext* context) { // + bool processed = stateMachine_onPocessEvent(context); + if (!processed) { + stateMachine_onPocessRemindEvent(context); + } +} +// int32_t IIflytopCanSlaveModule::stateMachine_getIdelStateIndex() { return 0; } +// int32_t IIflytopCanSlaveModule::stateMachine_getExceptionStateIndex() { return 1; } +void IIflytopCanSlaveModule::_stateMachine_processChangeStateEvent(icsm::StateMachineEventContext* oldcontext, icsm::StateMachineEventContext* newcontext) { + // 触发退出状态事件 + icsm::StateMachineEventContext exit_event_context; + exit_event_context.event.eventType = icsm::kPublicEvent; + exit_event_context.event.eventIndex = icsm::kevent_exitState; + exit_event_context.changeToState = STATE_NULL; + exit_event_context.exceptionId = 0; + _stateMachine_onPocessEvent(&exit_event_context); + + // 更新状态 + m_lastState = m_state; + m_state = oldcontext->changeToState; + + ZLOGI(m_name, "state change from %s to %s", stateMachine_stateToString(m_lastState), stateMachine_stateToString(m_state)); + + if (m_statusRegOff >= 0) { + writeRegValueByRegOff(m_statusRegOff, m_state, false); + } + // 更新异常 + if (m_state == m_exception_state_index) { + m_exceptionId = oldcontext->exceptionId; + } else { + m_exceptionId = 0; + } + if (m_ExceptionRegOff >= 0) { + writeRegValueByRegOff(m_ExceptionRegOff, m_exceptionId, false); + } + + // 处理切换到新的状态任务 + newcontext->event.eventType = icsm::kPublicEvent; + newcontext->event.eventIndex = icsm::kevent_enterstate; + newcontext->changeToState = STATE_NULL; + newcontext->exceptionId = 0; + m_stateticket = 0; + _stateMachine_onPocessEvent(newcontext); +} + +void IIflytopCanSlaveModule::stateMachine_triggerEvent(icsm::Event event) { // + icsm::StateMachineEventContext context; + icsm::StateMachineEventContext newcontext; + context.event = event; + context.changeToState = STATE_NULL; + context.exceptionId = 0; + _stateMachine_onPocessEvent(&context); + if (context.changeToState == STATE_NULL) { + return; + } + + while (true) { + _stateMachine_processChangeStateEvent(&context, &newcontext); + if (newcontext.changeToState == STATE_NULL) { + return; + } + memcpy(&context, &newcontext, sizeof(newcontext)); + memset(&newcontext, 0, sizeof(newcontext)); + } +} +int32_t IIflytopCanSlaveModule::getModuleTicket() { return m_module_ticket; } + +void IIflytopCanSlaveModule::stateMachine_triggerPrivateEvent(int32_t eventIndex) { + icsm::Event event; + event.eventIndex = eventIndex; + event.eventType = icsm::kPrivateEvent; + stateMachine_triggerEvent(event); +} + +void IIflytopCanSlaveModule::setDumpConfig(bool dumpConfig) { s_dumpConfig = dumpConfig; } +#endif 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 new file mode 100644 index 0000000..0618c21 --- /dev/null +++ b/components/single_axis_motor_control_v2/i_iflytop_can_slave_module.hpp @@ -0,0 +1,276 @@ +#pragma once +#include "sdk/hal/zhal.hpp" +#ifdef HAL_CAN_MODULE_ENABLED +#include + +#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp" + +namespace iflytop { +using namespace std; + +#define CONFIG_PARA_REG_NUM + +class IIflytopCanSlaveModule; +namespace icsm { + +class ConfigPara { + IIflytopCanSlaveModule* m_module; + const char* m_name; + + uint32_t m_regAddOff; + int32_t m_val; + + bool m_enableLimit; + int32_t m_limitMax; + int32_t m_limitMin; + + public: + void initialize(IIflytopCanSlaveModule* module, uint32_t regoff, uint16_t mask, int32_t defaultValue); + + icps::error_t processIflytopCanRegisterWriteEvent(IflytopCanProtocolStackWriteEvent* writeEvent); + + icps::error_t setVal(int32_t val); + int32_t getVal(); + icps::error_t setEnableLimit(bool enable); + bool getEnableLimit(); + icps::error_t setLimitMax(int32_t val); + int32_t getLimitMax(); + icps::error_t setLimitMin(int32_t val); + int32_t getLimitMin(); + + icps::error_t enableLimit(bool enable, int32_t min, int32_t max); + icps::error_t setValWitchLimit(int32_t initVal, int32_t min, int32_t max); + + void setName(const char* name); + + int32_t getRegAddOff(); +}; + +class Action { + IIflytopCanSlaveModule* m_module; + + uint32_t m_regAddOff; + int32_t m_val; + const char* m_name; + + public: + void initialize(IIflytopCanSlaveModule* module, uint32_t regoff); + + void setName(const char* name); + const char* getName(); + + icps::error_t setVal(int32_t val); + int32_t getVal(); + + int32_t getRegAddOff(); +}; + +typedef enum { + kevent_enterstate, + kevent_exitState, + kevent_periodicJob, + kevent_doAction, +} PublicEventIndex_t; + +typedef enum { + kPublicEvent, + kPrivateEvent, +} EventType_t; + +class Event { + public: + EventType_t eventType; + int32_t eventIndex; + + private: + int32_t eventVal0; + int32_t eventVal1; + + public: + Event(){}; + Event(EventType_t eventType, int32_t eventIndex) { + this->eventType = eventType; + this->eventIndex = eventIndex; + this->eventVal0 = 0; + this->eventVal1 = 0; + } + + Event(EventType_t eventType, int32_t eventIndex, int32_t action, int32_t actionVal) { + this->eventType = eventType; + this->eventIndex = eventIndex; + this->eventVal0 = action; + this->eventVal1 = actionVal; + } + + bool isEq(EventType_t eventType, int32_t eventIndex) { // + return (this->eventType == eventType) && (this->eventIndex == eventIndex); + } + bool isPublicEvent(PublicEventIndex_t eventIndex) { // + return (this->eventType == kPublicEvent) && (this->eventIndex == (int32_t)eventIndex); + } + bool isPrivateEvent(int32_t eventIndex) { // + return (this->eventType == kPrivateEvent) && (this->eventIndex == eventIndex); + } + + int32_t getAction() { return this->eventVal0; } + int32_t getActionVal() { return this->eventVal1; } +}; + +class StateMachineEventContext { + public: + Event event; + int32_t changeToState; + int32_t exceptionId; +}; + +} // namespace icsm + +/******************************************************************************* + * IIflytopCanSlaveModule * + *******************************************************************************/ +class IIflytopCanSlaveModule; +class IIflytopCanSlaveModuleListener { + public: + virtual void onStateChange(IIflytopCanSlaveModule* service, int32_t state) = 0; + virtual void onException(IIflytopCanSlaveModule* service, int32_t exceptionId) = 0; +}; + +class IIflytopCanSlaveModule { + public: + const char* m_name; + + private: + + int32_t m_regStart; + int32_t m_regNum; + + int32_t m_statusRegOff; + int32_t m_ExceptionRegOff; + + icsm::ConfigPara* m_configParas; + int m_configParaNum; + + icsm::Action* m_action; + int m_actionNum; + + bool engineer_mode; + + bool m_eventCacheIsEmpty; + icsm::Event m_eventCache; + + IflytopCanProtocolStackProcesser* m_protocolProcesser; + + int32_t m_lastState; + int32_t m_state; + int32_t m_exceptionId; + + int32_t m_lastcallticket; + int32_t m_periodms; + int32_t m_module_ticket; + + int32_t m_stateticket; + + IIflytopCanSlaveModuleListener* m_listener; + + icsm::Action m_zeroaction; + icsm::ConfigPara m_zeroCfg; + + int32_t m_actionVal; + + int32_t m_idle_state_index; + int32_t m_exception_state_index; + + public: + IIflytopCanSlaveModule(/* args */){}; + virtual ~IIflytopCanSlaveModule(){}; + + protected: + void initialize(const char* name, IflytopCanProtocolStackProcesser* processer, int32_t regoff, int32_t retNum); + void regConfigParas(icsm::ConfigPara* configParas, int configParaNum); + void regActions(icsm::Action* action, int actionNum); + + void setStatusRegOff(int32_t regoff); + void setExceptionRegOff(int32_t regoff); + + void setIdelStateIndex(int32_t stateIndex); + void setExceptionStateIndex(int32_t stateIndex); + + public: + void setListener(IIflytopCanSlaveModuleListener* listener); + void periodicJob(); + + static void setDumpConfig(bool dumpConfig); + + /******************************************************************************* + * 子类复写 * + *******************************************************************************/ + + virtual icps::error_t stateMachine_onTryDoAction(icsm::Action* action, int32_t actionVal) = 0; + virtual bool stateMachine_onPocessEvent(icsm::StateMachineEventContext* context) = 0; + virtual void stateMachine_onPocessRemindEvent(icsm::StateMachineEventContext* context) = 0; + virtual void stateMachine_onPeriodic(int stateMachineTicket) = 0; + virtual void stateMachine_onAsFarAsPossibleCall() = 0; + + virtual const char* stateMachine_stateToString(int32_t state) = 0; + + /******************************************************************************* + * 通用状态 * + *******************************************************************************/ + + void setEngineerMode(bool engineer_mode); + bool getEngineerMode(); + + int32_t getExceptionId(); + + int32_t getModuleTicket(); + + + /******************************************************************************* + * 协议对接 * + *******************************************************************************/ + bool isMyWriteEvent(IflytopCanProtocolStackWriteEvent* writeEvent); + icps::error_t processIflytopCanRegisterWriteEvent(IflytopCanProtocolStackWriteEvent* writeEvent); + /******************************************************************************* + * 协议助手 * + *******************************************************************************/ + + const char* getActionNameByState(int32_t state); + int getActionByState(int32_t state); + int getActionStateIndexByState(int32_t state); + IflytopCanProtocolStackProcesser* getProtocolProcesser() { return m_protocolProcesser; } + + icsm::ConfigPara* getConfig(int regoff); + icsm::Action* getAction(int regoff); + + void initializeCfg(icsm::ConfigPara* cfg, icsm::ConfigPara** shadowcfg, IIflytopCanSlaveModule* module, uint32_t regoff, uint16_t mask, int32_t defaultValue); + void initializeCfg(const char* name, icsm::ConfigPara* cfg, icsm::ConfigPara** shadowcfg, IIflytopCanSlaveModule* module, uint32_t regoff, uint16_t mask, + int32_t defaultValue); + icsm::Action* initializeAction(icsm::Action* action, IIflytopCanSlaveModule* module, const char* actionName, uint32_t regoff); + + uint32_t getRegOff(IflytopCanProtocolStackWriteEvent* writeEvent); + uint32_t getRegStartAdd() { return m_regStart; } + uint32_t getRegEndAdd() { return m_regStart + m_regNum; } + + void writeRegValueByRegOff(uint16_t regaddoff, int32_t value, bool forceupdate = false); + void activeReg(uint16_t mappingAdd, uint16_t mask, int32_t defaultValue); + /******************************************************************************* + * StateMachine * + *******************************************************************************/ + int32_t stateMachine_getStateIndex(); + int32_t stateMachine_getStateDuration(); + void stateMachine_triggerEvent(icsm::Event event); + void stateMachine_triggerPrivateEvent(int32_t eventIndex); + bool stateMachine_isIdle(); + + void disablePermission(int32_t actionRegoff, uint32_t permission); + void stateMachine_pushEvent(icsm::Event event); + + private: + // void stateMachine_triggerChangeStateEvent(int32_t state); + void stateMachine_periodicJob(int ticket); + void _stateMachine_processChangeStateEvent(icsm::StateMachineEventContext* oldcontext, icsm::StateMachineEventContext* newcontext); + void _stateMachine_onPocessEvent(icsm::StateMachineEventContext* context); +}; + +} // namespace iflytop +#endif 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 new file mode 100644 index 0000000..22be105 --- /dev/null +++ b/components/single_axis_motor_control_v2/single_axis_motor_control_v2.cpp @@ -0,0 +1,537 @@ + +#include "single_axis_motor_control_v2.hpp" +#ifdef HAL_CAN_MODULE_ENABLED +using namespace iflytop; +#define TAG "SingleAxisMotorControlerV2" + +void SingleAxisMotorControlerV2::initialize(const char* name, IflytopCanProtocolStackProcesser* processer, int regaddoff, // + ZGPIO* homegpio, // + ZGPIO* mingpio, // + ZGPIO* maxgpio, // + IStepperMotor* motor) { + IIflytopCanSlaveModule::initialize(name, processer, regaddoff, SINGLE_AXIS_MOTOR_CTRL_REG_NUM); + + m_homegpio = homegpio; + m_minlimitgpio = mingpio; + m_maxlimitgpio = maxgpio; + m_motor = motor; + + if (m_homegpio) { + ZASSERT(m_homegpio->isItRisingAndItFallingEXITGPIO()); + } + + IIflytopCanSlaveModule::setStatusRegOff(SAMC_REG_STAT_STATUS); + IIflytopCanSlaveModule::setExceptionRegOff(SAMC_REG_STAT_EXCEPTION); + IIflytopCanSlaveModule::setIdelStateIndex(kstate_idle); + IIflytopCanSlaveModule::setExceptionStateIndex(kstate_exception); + /******************************************************************************* + * 注册状态 * + *******************************************************************************/ + + IIflytopCanSlaveModule::activeReg(SAMC_REG_STAT_CURRENT_VELOCITY, icps::kr, 0); + IIflytopCanSlaveModule::activeReg(SAMC_REG_STAT_CURRENT_POS, icps::kr, 0); + IIflytopCanSlaveModule::activeReg(SAMC_REG_STAT_STATUS, icps::kr, 0); + IIflytopCanSlaveModule::activeReg(SAMC_REG_STAT_EXCEPTION, icps::kr, 0); + /** + * @brief + */ + int i = 0; + /******************************************************************************* + * 注册动作 * + *******************************************************************************/ + i = 0; + act_velocity_ctrl = IIflytopCanSlaveModule::initializeAction(&actions[i++], this, "velocity_ctrl", SAMC_REG_ACT_ROTATE); + act_position_ctrl = IIflytopCanSlaveModule::initializeAction(&actions[i++], this, "position_ctrl", SAMC_REG_ACT_MOVE_TO); + act_rela_position_ctrl = IIflytopCanSlaveModule::initializeAction(&actions[i++], this, "rela_position_ctrl", SAMC_REG_ACT_MOVE_BY); + act_zero_ctrl = IIflytopCanSlaveModule::initializeAction(&actions[i++], this, "zero_ctrl", SAMC_REG_ACT_RUN_TO_HOME); + act_calibrezero = IIflytopCanSlaveModule::initializeAction(&actions[i++], this, "calibrezero", SAMC_REG_ACT_DO_NOTHING); + act_stop_ctrl = IIflytopCanSlaveModule::initializeAction(&actions[i++], this, "stop_ctrl", SAMC_REG_ACT_STOP); + act_clear_error = IIflytopCanSlaveModule::initializeAction(&actions[i++], this, "clear_error", SAMC_REG_ACT_CLEAR_EXCEPTION); + ZASSERT(ZARRAY_SIZE(actions) == i); + IIflytopCanSlaveModule::regActions(actions, i); + + /******************************************************************************* + * 注册配置 * + *******************************************************************************/ + i = 0; + IIflytopCanSlaveModule::initializeCfg("acc", &cfgs[i++], &cfg_acc, this, SAMC_REG_CFG_ACC, icps::kwr, 0); + IIflytopCanSlaveModule::initializeCfg("dec", &cfgs[i++], &cfg_dec, this, SAMC_REG_CFG_DEC, icps::kwr, 0); + IIflytopCanSlaveModule::initializeCfg("velocity", &cfgs[i++], &cfg_velocity, this, SAMC_REG_CFG_VELOCITY, icps::kwr, 0); + IIflytopCanSlaveModule::initializeCfg("zero_shift", &cfgs[i++], &cfg_zero_shift, this, SAMC_REG_CFG_ZERO_SHIFT, icps::kwr, 0); + IIflytopCanSlaveModule::initializeCfg("runhome_velocity", &cfgs[i++], &cfg_runhome_velocity, this, SAMC_REG_CFG_RUNHOME_VELOCITY, icps::kwr, 0); + IIflytopCanSlaveModule::initializeCfg("runtohome_dec", &cfgs[i++], &cfg_runtohome_dec, this, SAMC_REG_CFG_RUNTOHOME_DEC, icps::kwr, 0); + IIflytopCanSlaveModule::initializeCfg("runtohome_max_distance", &cfgs[i++], &cfg_runtohome_max_distance, this, SAMC_REG_CFG_RUNTOHOME_MAX_DISTANCE, icps::kwr, + 0); + IIflytopCanSlaveModule::initializeCfg("runtohome_leave_zero_point_distance", &cfgs[i++], &cfg_runtohome_leave_zero_point_distance, this, + SAMC_REG_CFG_RUNTOHOME_LEAVE_ZERO_POINT_DISTANCE, icps::kwr, 0); + IIflytopCanSlaveModule::initializeCfg("min_pos", &cfgs[i++], &cfg_min_pos, this, SAMC_REG_CFG_MIN_POS, icps::kwr, INT32_MIN); + IIflytopCanSlaveModule::initializeCfg("max_pos", &cfgs[i++], &cfg_max_pos, this, SAMC_REG_CFG_MAX_POS, icps::kwr, INT32_MAX); + IIflytopCanSlaveModule::initializeCfg("runtohome_min_distance", &cfgs[i++], &cfg_runtohome_keep_move_distance, this, + SAMC_REG_CFG_RUNTOHOME_KEEP_MOVE_DISTANCE, icps::kwr, 0); + + ZASSERT(ZARRAY_SIZE(cfgs) == i); + IIflytopCanSlaveModule::regConfigParas(cfgs, i); + // ZGPIO::regListener(this); + + if (m_homegpio) m_homegpio->regListener([this](ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) { STM32_GPIO_onIRQ(GPIO_Pin, irqevent); }); + if (m_minlimitgpio) m_minlimitgpio->regListener([this](ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) { STM32_GPIO_onIRQ(GPIO_Pin, irqevent); }); + if (m_maxlimitgpio) m_maxlimitgpio->regListener([this](ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) { STM32_GPIO_onIRQ(GPIO_Pin, irqevent); }); + m_moveToHomeCheck = true; + + ZHALCORE::getInstance()->regPeriodJob([this](ZHALCORE::Context& context) { periodicJob(); }, 1); +} +int32_t SingleAxisMotorControlerV2::getPos() { return m_motor->getXACTUAL(); } + +void SingleAxisMotorControlerV2::lock() { m_lock = true; } +void SingleAxisMotorControlerV2::unlock() { m_lock = false; } +bool SingleAxisMotorControlerV2::islock() { return m_lock; } + +const char* SingleAxisMotorControlerV2::stateMachine_stateToString(int32_t state) { + switch (state) { + case kstate_idle: + return "state_idle"; + case kstate_exception: + return "state_exception"; + + case kstate_findZero_runToZero: + return "state_findZero_runToZero"; + case kstate_findZero_keepRunningToZero: + return "state_findZero_keepRunningToZero"; + case kstate_findZero_leaveFromZero: + return "state_findZero_leaveFromZero"; + case kstate_findZero_waittingToStop: + return "state_findZero_waittingToStop"; + + case kstate_moveTo: + return "state_moveTo"; + case kstate_moveBy: + return "state_moveBy"; + case kstate_rotating: + return "state_rotating"; + default: + break; + } + return "state_unkown"; +} +/******************************************************************************* + * 参数检查 * + *******************************************************************************/ +icps::error_t SingleAxisMotorControlerV2::checkTargetPos(int32_t targetPos) { + if (getEngineerMode()) { + return icps::kSuccess; + } + if (targetPos > cfg_max_pos->getVal()) { + // ZLOGI(m_name, "targetPos:%d, cfg_max_pos:%d", targetPos, cfg_max_pos->getVal()); + return icps::kParameterOutOfLimit; + } else if (targetPos < cfg_min_pos->getVal()) { + // ZLOGI(m_name, "targetPos:%d, cfg_max_pos:%d", targetPos, cfg_min_pos->getVal()); + return icps::kParameterOutOfLimit; + } + return icps::kSuccess; +} +icps::error_t SingleAxisMotorControlerV2::stateMachine_onTryDoAction(icsm::Action* action, int32_t actionVal) { + if (m_lock) { + return icps::kDeviceLockByInternalControler; + } + + if (action->getRegAddOff() != SAMC_REG_ACT_CLEAR_EXCEPTION && getExceptionId() != 0) { + return icps::kDeviceException; + } + switch (action->getRegAddOff()) { + case SAMC_REG_ACT_ROTATE: + if (m_homegpio && m_moveToHomeCheck) { + if (!m_hasRunToHomed && !getEngineerMode()) { + return icps::kmotorNotRunToHome; + } + } + return icps::kSuccess; + case SAMC_REG_ACT_MOVE_TO: + if (m_homegpio && m_moveToHomeCheck) { + if (!m_hasRunToHomed && !getEngineerMode()) { + return icps::kmotorNotRunToHome; + } + } + return checkTargetPos(actionVal); + case SAMC_REG_ACT_MOVE_BY: + if (m_homegpio && m_moveToHomeCheck) { + if (!m_hasRunToHomed && !getEngineerMode()) { + return icps::kmotorNotRunToHome; + } + } + return checkTargetPos(getPos() + actionVal); + case SAMC_REG_ACT_RUN_TO_HOME: + if (!m_homegpio) { + return icps::kIllegalOperation; + } + return icps::kSuccess; + case SAMC_REG_ACT_DO_NOTHING: + return icps::kSuccess; + case SAMC_REG_ACT_STOP: + return icps::kSuccess; + case SAMC_REG_ACT_CLEAR_EXCEPTION: + return icps::kSuccess; + default: + return icps::kSuccess; + } +} + +void SingleAxisMotorControlerV2::runToHome() { + if (stateMachine_onTryDoAction(act_zero_ctrl, 0) != icps::kSuccess) { + return; + } + stateMachine_pushEvent(icsm::Event(icsm::kPublicEvent, icsm::kevent_doAction, act_zero_ctrl->getRegAddOff(), 0)); +} +void SingleAxisMotorControlerV2::rotate(int direction) { + m_actionVal = direction; + if (stateMachine_onTryDoAction(act_velocity_ctrl, direction) != icps::kSuccess) { + return; + } + stateMachine_pushEvent(icsm::Event(icsm::kPublicEvent, icsm::kevent_doAction, act_velocity_ctrl->getRegAddOff(), direction)); +} + +/******************************************************************************* + * 状态更新&事件生成 * + *******************************************************************************/ +void SingleAxisMotorControlerV2::stateMachine_onPeriodic(int stateMachineTicket) { + if (stateMachineTicket % 100 == 0) { + writeRegValueByRegOff(SAMC_REG_STAT_CURRENT_VELOCITY, getActuralVelocity(), false); + writeRegValueByRegOff(SAMC_REG_STAT_CURRENT_POS, getActuralPosition(), false); + } + + if (stateMachine_getStateIndex() != kstate_idle && stateMachine_getStateIndex() != kstate_exception) { + if (stateMachineTicket % 33 == 0 && m_motor->isReachTarget()) { + stateMachine_triggerPrivateEvent(kevent_reachTarget); + } + } +} + +void SingleAxisMotorControlerV2::stateMachine_onAsFarAsPossibleCall() { + if (m_zeroSwitchTrigged) { + ZLOGI(m_name, "zero switch trigged"); + m_zeroSwitchTrigged = false; + stateMachine_triggerPrivateEvent(kevent_triggerZeroSwitch); + } + if (m_minlimitgpioSwitchTrigged) { + ZLOGI(m_name, "min switch trigged"); + m_minlimitgpioSwitchTrigged = false; + stateMachine_triggerPrivateEvent(kevent_triggerRightSwitch); + } + if (m_maxlimitgpioSwitchTrigged) { + ZLOGI(m_name, "max switch trigged"); + m_maxlimitgpioSwitchTrigged = false; + stateMachine_triggerPrivateEvent(kevent_triggerLelfSwitch); + } +} +void SingleAxisMotorControlerV2::STM32_GPIO_onIRQ(ZGPIO* gpio, ZGPIO::IrqTypeEvent_t irqevent) { + if (gpio == m_homegpio) { + m_zeroSwitchTrigged = true; + stateMachine_triggerPrivateEvent(kevent_triggerZeroSwitchIrq); + } else if (gpio == m_minlimitgpio) { + m_minlimitgpioSwitchTrigged = true; + stateMachine_triggerPrivateEvent(kevent_triggerRightSwitchIrq); + } else if (gpio == m_maxlimitgpio) { + m_maxlimitgpioSwitchTrigged = true; + stateMachine_triggerPrivateEvent(kevent_triggerLelfSwitchIrq); + } +} +/******************************************************************************* + * 事件处理 * + *******************************************************************************/ +bool SingleAxisMotorControlerV2::stateMachine_onPocessEvent(icsm::StateMachineEventContext* context) { + int nowstate = IIflytopCanSlaveModule::stateMachine_getStateIndex(); + if (nowstate == kstate_idle) { + return process_state_idle(context); + } else if (nowstate == kstate_exception) { + return process_state_exception(context); + } else if (nowstate == kstate_findZero_runToZero) { + return process_state_findZero_runToZero(context); + } else if (nowstate == kstate_findZero_keepRunningToZero) { + return process_state_findZero_keepRunningToZero(context); + } else if (nowstate == kstate_findZero_leaveFromZero) { + return process_state_findZero_leaveFromZero(context); + } else if (nowstate == kstate_findZero_waittingToStop) { + return process_state_findZero_waittingToStop(context); + } else if (nowstate == kstate_moveTo) { + return process_state_moveTo(context); + } else if (nowstate == kstate_moveBy) { + return process_state_moveBy(context); + } else if (nowstate == kstate_rotating) { + return process_state_rotating(context); + } + return false; +} +/******************************************************************************* + * 公共事件处理 * + *******************************************************************************/ +void SingleAxisMotorControlerV2::stateMachine_onPocessRemindEvent(icsm::StateMachineEventContext* context) { + icsm::Event& event = context->event; + if (event.isPublicEvent(icsm::kevent_doAction)) { + ZLOGI(m_name, "doAction %d,%d", event.getAction(), event.getActionVal()); + m_actionVal = event.getActionVal(); + switch (event.getAction()) { + case SAMC_REG_ACT_ROTATE: + context->changeToState = kstate_rotating; + break; + case SAMC_REG_ACT_MOVE_TO: + context->changeToState = kstate_moveTo; + break; + case SAMC_REG_ACT_MOVE_BY: + context->changeToState = kstate_moveBy; + break; + case SAMC_REG_ACT_RUN_TO_HOME: + context->changeToState = kstate_findZero_runToZero; + break; + case SAMC_REG_ACT_DO_NOTHING: + context->changeToState = kstate_idle; + break; + case SAMC_REG_ACT_STOP: + context->changeToState = kstate_idle; + break; + case SAMC_REG_ACT_CLEAR_EXCEPTION: + context->changeToState = kstate_idle; + context->exceptionId = 0; + break; + default: + break; + } + // ZLOGI(m_name, "xxxxcontext->changeToState %d", context->changeToState); + } +} +bool SingleAxisMotorControlerV2::process_state_idle(icsm::StateMachineEventContext* context) { + if (context->event.isPublicEvent(icsm::kevent_enterstate)) { + ZLOGI(m_name, "change state -> idle"); + m_motor->stop(); + return true; + } else if (context->event.isPublicEvent(icsm::kevent_periodicJob)) { + return true; + } + return false; +} + +/** + * @brief + * 归零逻辑 + * 0. 如果设备没有处于零点，则设备往反方向移动，靠近零点，当零点触发时，停止运动，然后执行步骤1 + * 如果设备处于零点则直接执行步骤1. + * 1. 设备往正方向移动，远离零点，当中断触发时，记录当前零点位置，校准零点 + */ + +bool SingleAxisMotorControlerV2::process_state_findZero_runToZero(icsm::StateMachineEventContext* context) { + if (context->event.isPublicEvent(icsm::kevent_enterstate)) { + // 如果当前在零点，则直接进入下一步 + ZLOGI(m_name, "change state -> findZero_runToZero"); + if (m_homegpio->getState()) { + ZLOGI(m_name, "try search home point falling edge"); + motor_moveBy(cfg_runtohome_max_distance->getVal(), // + cfg_runhome_velocity->getVal() * 2, // + cfg_acc->getVal(), // + cfg_runtohome_dec->getVal()); + m_runToHomeContext_runDirection = 1; + } else { + // 回到零点 + ZLOGI(m_name, "try search home point rising edge"); + motor_moveBy(-cfg_runtohome_max_distance->getVal(), // + cfg_runhome_velocity->getVal() * 2, // + cfg_acc->getVal(), // + cfg_runtohome_dec->getVal()); + m_runToHomeContext_runDirection = -1; + } + return true; + } else if (context->event.isPrivateEvent(kevent_triggerZeroSwitchIrq)) { + // 锁存当前零点位置,且立马停止设备 + m_motor->stop(); + return true; + } else if (context->event.isPrivateEvent(kevent_triggerZeroSwitch)) { + m_runToHomeContext_zeroTriggerPos = m_motor->getXACTUAL(); + context->changeToState = kstate_findZero_keepRunningToZero; + return true; + } else if (m_motor->isReachTarget() && context->event.isPrivateEvent(kevent_reachTarget)) { + // 移动了最大位移后，依然找不到原点，报告异常 + ZLOGE(TAG, "run to home failed %d", stateMachine_getStateIndex()); + context->exceptionId = kexcep_loseZeroPoint; + context->changeToState = kstate_exception; + return true; + } + return false; +} + +bool SingleAxisMotorControlerV2::process_state_findZero_keepRunningToZero(icsm::StateMachineEventContext* context) { + if (context->event.isPublicEvent(icsm::kevent_enterstate)) { + // 如果当前在零点，则直接进入下一步 + ZLOGI(m_name, "change state -> runToHomeStep0"); + + int32_t hasMovedDistance = m_motor->getXACTUAL() - m_runToHomeContext_zeroTriggerPos; + + if (abs(hasMovedDistance) >= cfg_runtohome_keep_move_distance->getVal()) { + context->changeToState = kstate_findZero_leaveFromZero; + } else { + int32_t reminddistance = cfg_runtohome_keep_move_distance->getVal() - abs(hasMovedDistance); + if (m_runToHomeContext_runDirection > 0) { + motor_moveBy(reminddistance, cfg_runhome_velocity->getVal() * 2, cfg_acc->getVal(), cfg_runtohome_dec->getVal()); + } else { + motor_moveBy(-reminddistance, cfg_runhome_velocity->getVal() * 2, cfg_acc->getVal(), cfg_runtohome_dec->getVal()); + } + } + return true; + } else if (m_motor->isReachTarget() && context->event.isPrivateEvent(kevent_reachTarget)) { + context->changeToState = kstate_findZero_leaveFromZero; + return true; + } + return false; +} + +bool SingleAxisMotorControlerV2::process_state_findZero_leaveFromZero(icsm::StateMachineEventContext* context) { + if (context->event.isPublicEvent(icsm::kevent_enterstate)) { + // 进入状态 + ZLOGI(m_name, "change state -> runToHomeStep1"); + if (m_runToHomeContext_runDirection < 0) { + motor_moveBy(cfg_runtohome_leave_zero_point_distance->getVal(), // + cfg_runhome_velocity->getVal(), // + cfg_acc->getVal(), // + cfg_runtohome_dec->getVal()); // 此时处于零点位置，先离开原点一定位置 + } else { + motor_moveBy(-cfg_runtohome_leave_zero_point_distance->getVal(), // + cfg_runhome_velocity->getVal(), // + cfg_acc->getVal(), // + cfg_runtohome_dec->getVal()); // 此时处于零点位置，先离开原点一定位置 + } + return true; + } else if (context->event.isPrivateEvent(kevent_triggerZeroSwitchIrq)) { + // 锁存当前零点位置,且立马停止设备 + m_zeroPointPosition = m_motor->getXACTUAL(); + m_motor->stop(); + return true; + } else if (context->event.isPrivateEvent(kevent_triggerZeroSwitch)) { + // 找到原点，停止设备，进入idle状态，之所以没有在IRQ切换状态，是为了避免异步操作带来的问题 + m_motor->stop(); + context->changeToState = kstate_findZero_waittingToStop; + return true; + } else if (m_motor->isReachTarget() && context->event.isPrivateEvent(kevent_reachTarget)) { + // 移动了最大位移后，依然找不到原点，报告异常 + ZLOGE(TAG, "run to home failed %d", stateMachine_getStateIndex()); + context->exceptionId = kexcep_loseZeroPoint; + context->changeToState = kstate_exception; + return true; + } + return false; +} + +bool SingleAxisMotorControlerV2::process_state_findZero_waittingToStop(icsm::StateMachineEventContext* context) { + if (context->event.isPublicEvent(icsm::kevent_periodicJob)) { + if (m_zeroPointPosition != 0) { + if (m_motor->getVACTUAL() == 0) { + int32_t nowposition = m_motor->getXACTUAL(); + int32_t nowposition_real = nowposition - m_zeroPointPosition; + ZLOGI(m_name, "nowposition:%d,zeroPointPosition:%d", nowposition, m_zeroPointPosition); + m_motor->setXACTUAL(nowposition_real - cfg_zero_shift->getVal()); + m_zeroPointPosition = 0; + motor_moveTo(0, cfg_runhome_velocity->getVal(), cfg_acc->getVal(), cfg_acc->getVal()); + m_hasRunToHomed = true; + // context->changeToState = kstate_idle; + } + } else { + if (m_motor->getVACTUAL() == 0) { + context->changeToState = kstate_idle; + } + } + + return true; + } + return false; +} + +bool SingleAxisMotorControlerV2::process_state_moveTo(icsm::StateMachineEventContext* context) { + if (context->event.isPublicEvent(icsm::kevent_enterstate)) { + // 进入状态b + ZLOGI(m_name, "change state -> moveTo %d", m_actionVal); + motor_moveTo(m_actionVal, cfg_velocity->getVal(), cfg_acc->getVal(), cfg_dec->getVal()); + return true; + + } else if (m_motor->isReachTarget() && context->event.isPrivateEvent(kevent_reachTarget)) { + context->changeToState = kstate_idle; + return true; + } + return false; +} +bool SingleAxisMotorControlerV2::process_state_moveBy(icsm::StateMachineEventContext* context) { + if (context->event.isPublicEvent(icsm::kevent_enterstate)) { + // 进入状态 + ZLOGI(m_name, "change state -> moveBy %d", m_actionVal); + motor_moveBy(m_actionVal, cfg_velocity->getVal(), cfg_acc->getVal(), cfg_dec->getVal()); + return true; + + } else if (m_motor->isReachTarget() && context->event.isPrivateEvent(kevent_reachTarget)) { + context->changeToState = kstate_idle; + return true; + } + return false; +} +bool SingleAxisMotorControlerV2::process_state_rotating(icsm::StateMachineEventContext* context) { + if (context->event.isPublicEvent(icsm::kevent_enterstate)) { + // 进入状态 + ZLOGI(m_name, "change state -> rotating %d", cfg_velocity->getVal()); + if (m_actionVal > 0) { + motor_rotate(cfg_velocity->getVal(), cfg_acc->getVal(), cfg_dec->getVal()); + } else if (m_actionVal < 0) { + motor_rotate(-cfg_velocity->getVal(), cfg_acc->getVal(), cfg_dec->getVal()); + } else { + context->changeToState = kstate_idle; + } + return true; + } + return false; +} +bool SingleAxisMotorControlerV2::process_state_exception(icsm::StateMachineEventContext* context) { + if (context->event.isPublicEvent(icsm::kevent_enterstate)) { + ZLOGI(m_name, "change state -> exception %d", getExceptionId()); + m_motor->stop(); + return true; + } else if (context->event.isPublicEvent(icsm::kevent_doAction) && m_actionVal == SAMC_REG_ACT_CLEAR_EXCEPTION) { + context->exceptionId = 0; + context->changeToState = kstate_idle; + return true; + } + return false; +} + +void SingleAxisMotorControlerV2::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 SingleAxisMotorControlerV2::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 SingleAxisMotorControlerV2::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); +} +int32_t SingleAxisMotorControlerV2::getActuralVelocity() { return m_motor->getVACTUAL(); } +int32_t SingleAxisMotorControlerV2::getActuralPosition() { return m_motor->getXACTUAL(); } + +IStepperMotor* SingleAxisMotorControlerV2::getMotor() { return m_motor; } + +int SingleAxisMotorControlerV2::getDirection() { return m_direction; } + +icps::error_t SingleAxisMotorControlerV2::stop() { + IflytopCanProtocolStackWriteEvent writeEvent; + icps::Reg_t* reg = getProtocolProcesser()->findReg(getRegStartAdd() + SAMC_REG_ACT_STOP); + ZASSERT(reg != NULL); + + writeEvent.reg = reg; + writeEvent.newvalue = 1; + writeEvent.oldvalue = reg->getValue(); + processIflytopCanRegisterWriteEvent(&writeEvent); + return icps::kSuccess; +} +#endif diff --git a/components/single_axis_motor_control_v2/single_axis_motor_control_v2.hpp b/components/single_axis_motor_control_v2/single_axis_motor_control_v2.hpp new file mode 100644 index 0000000..9470004 --- /dev/null +++ b/components/single_axis_motor_control_v2/single_axis_motor_control_v2.hpp @@ -0,0 +1,191 @@ +#pragma once +#include "sdk/hal/zhal.hpp" +#ifdef HAL_CAN_MODULE_ENABLED +#include + +#include "i_iflytop_can_slave_module.hpp" +#include "sdk\components\tmc\basic\tmc_ic_interface.hpp" + +/** + * @brief + * + * service: SingleAxisMotorControlerV2 + * + */ +#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 SingleAxisMotorControlerListener; +class SingleAxisMotorControlerV2 : public IIflytopCanSlaveModule { + private: + // res + ZGPIO* m_homegpio; + ZGPIO* m_maxlimitgpio; + ZGPIO* m_minlimitgpio; + IStepperMotor* m_motor; + + typedef enum { + kevent_reachTarget, + kevent_triggerZeroSwitch, + kevent_triggerZeroSwitchIrq, + kevent_triggerRightSwitch, + kevent_triggerRightSwitchIrq, + kevent_triggerLelfSwitch, + kevent_triggerLelfSwitchIrq, + } event_t; + + typedef enum { + kstate_idle = 0, + kstate_exception = 65535, + + kstate_findZero_runToZero = 10, // 向零点移动 + kstate_findZero_keepRunningToZero = 11, // 继续向原点移动，保证电机没有刚好停在零点, 因为如果刚好停在零点，零点电平可能是跳动的） + kstate_findZero_leaveFromZero = 12, // 远离零点 + kstate_findZero_waittingToStop = 13, // 等待电机停止运动，校准零点偏移 + + kstate_moveTo = 20, + kstate_moveBy = 30, + kstate_rotating = 40, + } state_t; + + typedef enum { + kexcep_motorBlocking = 1, + kexcep_loseZeroPoint = 2, + } exception_id_t; + + public: + icsm::ConfigPara cfgs[11]; // 状态 + icsm::Action actions[7]; + + icsm::Action* act_velocity_ctrl; + icsm::Action* act_position_ctrl; + icsm::Action* act_rela_position_ctrl; + icsm::Action* act_zero_ctrl; + icsm::Action* act_calibrezero; + icsm::Action* act_stop_ctrl; + icsm::Action* act_clear_error; + + icsm::ConfigPara* cfg_acc; + icsm::ConfigPara* cfg_dec; + icsm::ConfigPara* cfg_velocity; + icsm::ConfigPara* cfg_zero_shift; + icsm::ConfigPara* cfg_runhome_velocity; + icsm::ConfigPara* cfg_runtohome_dec; + icsm::ConfigPara* cfg_runtohome_max_distance; + icsm::ConfigPara* cfg_runtohome_leave_zero_point_distance; + icsm::ConfigPara* cfg_min_pos; + icsm::ConfigPara* cfg_max_pos; + icsm::ConfigPara* cfg_runtohome_keep_move_distance; // 归零零点触发后，继续移动的距离 + + bool m_hasRunToHomed; + int m_direction; + bool m_zeroSwitchTrigged; + bool m_maxlimitgpioSwitchTrigged; + bool m_minlimitgpioSwitchTrigged; + int32_t m_zeroPointPosition; + + int32_t m_actionVal; + bool m_lock; + + int32_t m_runToHomeContext_moveDirection; + int32_t m_runToHomeContext_zeroTriggerPos; + int32_t m_runToHomeContext_startPos; + int32_t m_runToHomeContext_runDirection; + + bool m_moveToHomeCheck; + + public: + SingleAxisMotorControlerV2(){}; + virtual ~SingleAxisMotorControlerV2(){}; + + void initialize(const char* name, IflytopCanProtocolStackProcesser* processer, int regaddoff, // + ZGPIO* homegpio, // + ZGPIO* mingpio, // + ZGPIO* rightgpio, // + IStepperMotor* motor); + + public: + /** + * @brief 内部模块接管电机时，需要调用此函数 + */ + void lock(); + void unlock(); + bool islock(); + + void disableMoveHomeCheck() { m_moveToHomeCheck = false; }; + + IStepperMotor* getMotor(); + + void runToHome(); + void rotate(int direction); + bool hasRunToHomed() { return m_hasRunToHomed; }; + + virtual icps::error_t stateMachine_onTryDoAction(icsm::Action* action, int32_t actionVal); + virtual bool stateMachine_onPocessEvent(icsm::StateMachineEventContext* context); + virtual void stateMachine_onPocessRemindEvent(icsm::StateMachineEventContext* context); + virtual void stateMachine_onPeriodic(int stateMachineTicket); + virtual void stateMachine_onAsFarAsPossibleCall(); + virtual const char* stateMachine_stateToString(int32_t state); + + bool process_state_idle(icsm::StateMachineEventContext* context); + + bool process_state_findZero_runToZero(icsm::StateMachineEventContext* context); + bool process_state_findZero_keepRunningToZero(icsm::StateMachineEventContext* context); + bool process_state_findZero_leaveFromZero(icsm::StateMachineEventContext* context); + bool process_state_findZero_waittingToStop(icsm::StateMachineEventContext* context); + + bool process_state_moveTo(icsm::StateMachineEventContext* context); + bool process_state_moveBy(icsm::StateMachineEventContext* context); + bool process_state_rotating(icsm::StateMachineEventContext* context); + bool process_state_exception(icsm::StateMachineEventContext* context); + + /******************************************************************************* + * GPIO LISTENER * + *******************************************************************************/ + void STM32_GPIO_onIRQ(ZGPIO *GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent); + icps::error_t checkTargetPos(int32_t targetPos); + + /******************************************************************************* + * UTILS * + *******************************************************************************/ + int getDirection(); + icps::error_t stop(); + + private: + int32_t getActuralVelocity(); + int32_t getActuralPosition(); + 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); + int32_t getPos(); +}; + +} // namespace iflytop +#endif diff --git a/hal/gpio.hpp b/hal/gpio.hpp index a91dafd..981fee2 100644 --- a/hal/gpio.hpp +++ b/hal/gpio.hpp @@ -78,10 +78,4 @@ class ZGPIO { private: bool enableClock(); }; - -class STM32_GPIO_LISTENER { - public: - virtual void STM32_GPIO_onIRQ(ZGPIO *GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) {} -}; - } // namespace iflytop \ No newline at end of file