From 222cd666fbbb61d1752a60d20e995cc29949ec02 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Mon, 31 Jul 2023 19:43:55 +0800 Subject: [PATCH] update --- usrc/flia_plate_box_pusher_ctl_service.cpp | 0 usrc/flia_plate_box_pusher_ctl_service.hpp | 0 usrc/main.cpp | 9 +- usrc/one_dimensional_code_laser_scanner.cpp | 212 +++++++++++++++++++++------- usrc/one_dimensional_code_laser_scanner.hpp | 67 +++++++-- 5 files changed, 224 insertions(+), 64 deletions(-) delete mode 100644 usrc/flia_plate_box_pusher_ctl_service.cpp delete mode 100644 usrc/flia_plate_box_pusher_ctl_service.hpp diff --git a/usrc/flia_plate_box_pusher_ctl_service.cpp b/usrc/flia_plate_box_pusher_ctl_service.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/usrc/flia_plate_box_pusher_ctl_service.hpp b/usrc/flia_plate_box_pusher_ctl_service.hpp deleted file mode 100644 index e69de29..0000000 diff --git a/usrc/main.cpp b/usrc/main.cpp index d26e014..d54d493 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -225,7 +225,14 @@ void Main::run() { m_tjMotorControlService.cfg_runtohome_leave_zero_point_distance->setVal(256 * 200 * 1); // bwm_gpio - m_oneDimensionalCodeLaserScanner.initialize(&m_protocolStack, REG_LARSER_SCANNER_CTRL_ADD_BASE, FYBJL_SENSOR0, [this]() { return m_tj_motor.getXACTUAL(); }); + { + OneDimensionalCodeLaserScanner::cfg_t cfg; + cfg.triggerPin = FYBJL_SENSOR0; + cfg.codestartpos = 0; + cfg.codeendpos = 0; + cfg.readder = [this]() { return m_tj_motor.getXACTUAL(); }; + m_oneDimensionalCodeLaserScanner.initialize(&m_protocolStack, REG_LARSER_SCANNER_CTRL_ADD_BASE, &cfg); + } ZLOGI(TAG, "init done"); while (1) { diff --git a/usrc/one_dimensional_code_laser_scanner.cpp b/usrc/one_dimensional_code_laser_scanner.cpp index 4aab146..354e1ee 100644 --- a/usrc/one_dimensional_code_laser_scanner.cpp +++ b/usrc/one_dimensional_code_laser_scanner.cpp @@ -1,43 +1,31 @@ #include "one_dimensional_code_laser_scanner.hpp" +#include using namespace iflytop; #define ONE_BIT_WIDTH ((int32_t)(51200 * 1.20003 / 8.0)) #define MIN_POS ((int32_t)(102733)) #define TAG "OneDimensionalCodeLaserScanner" -int32_t m_off; -int32_t m_posChache[50]; -int32_t endpos = 0; -int32_t startpos = 0; -bool m_idleLevel = true; - -void OneDimensionalCodeLaserScanner::initialize(IflytopCanProtocolStackProcesser* protocolProcesser, int32_t regStartOff, Pin_t triggerPin, - PosReadder_t readder) { + +void OneDimensionalCodeLaserScanner::initialize(IflytopCanProtocolStackProcesser* protocolProcesser, int32_t regStartOff, cfg_t* cfg) { m_protocolProcesser = protocolProcesser; m_regStartOff = regStartOff; + m_cfg = *cfg; - // m_protocolProcesser->activeReg(m_regStartOff + REG_CODE_SCANER_ACT_CTRL, icps::kw, 0); - // m_protocolProcesser->activeReg(m_regStartOff + REG_CODE_SCANER_ACT_CLEAR_EXCEPTION, icps::kw, 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_protocolProcesser->activeReg(m_regStartOff + REG_CODE_SCANER_CODE, icps::kr, 0); + m_slave = m_protocolProcesser->createICPSSlaveModule("OneDimensionalCodeLaserScanner", this, m_regStartOff); + m_protocolProcesser->activeReg(m_slave, REG_CODE_SCANER_ACT_CTRL, icps::kw, 0); + m_protocolProcesser->activeReg(m_slave, REG_CODE_SCANER_ACT_CLEAR_EXCEPTION, icps::kw, 0); + m_statusReg = m_protocolProcesser->activeReg(m_slave, REG_CODE_SCANER_STAT_STATUS, icps::kr, 0); + m_errReg = m_protocolProcesser->activeReg(m_slave, REG_CODE_SCANER_STAT_ERROR, icps::kr, 0); + m_codeReg = m_protocolProcesser->activeReg(m_slave, REG_CODE_SCANER_CODE, icps::kr, 0); - m_triggerGpio.initAsInput(triggerPin, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, false /*mirror*/); + m_triggerGpio.initAsInput(m_cfg.triggerPin, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, false /*mirror*/); m_triggerGpio.regListener([this](ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) { onGpioIrq(GPIO_Pin, irqevent); }); - // m_protocolProcesser->registerListener(this); - m_readder = readder; + m_readder = m_cfg.readder; } -bool OneDimensionalCodeLaserScanner::isThisRegOwnToMe(IflytopCanProtocolStackProcesser* processer, icps::Reg_t* reg) { // - int32_t regoff = reg->add - m_regStartOff; - if (regoff >= 0 && regoff < REG_CODE_SCANER_SIZE) { - return true; - } - return false; -} -icps::error_t OneDimensionalCodeLaserScanner::onHostRegisterWriteEvent(IflytopCanProtocolStackProcesser* processer, - IflytopCanProtocolStackWriteEvent* writeEvent) { +icps::error_t OneDimensionalCodeLaserScanner::onHostRegisterWriteEvent(icps::WriteEvent* writeEvent) { int32_t regoff = writeEvent->reg->add - m_regStartOff; switch (regoff) { case REG_CODE_SCANER_ACT_CTRL: @@ -60,17 +48,15 @@ icps::error_t OneDimensionalCodeLaserScanner::onHostRegisterWriteEvent(IflytopCa void OneDimensionalCodeLaserScanner::startScan() { CriticalContext cc; - m_off = 0; - m_workflag = true; - + m_off = 0; + m_workflag = true; m_idleLevel = m_triggerGpio.getState(); - startpos = m_readder(); + m_startpos = m_readder(); } void OneDimensionalCodeLaserScanner::stopScan() { CriticalContext cc; m_workflag = false; - - endpos = m_readder(); + m_endpos = m_readder(); } bool OneDimensionalCodeLaserScanner::getPosLevel(int pos) { @@ -96,32 +82,158 @@ bool OneDimensionalCodeLaserScanner::getPosLevel(int pos) { return level; } -void OneDimensionalCodeLaserScanner::parseResult() { // - static uint8_t code[500] = {0}; +int32_t OneDimensionalCodeLaserScanner::compute_point_num(int32_t startpos, int32_t endpos) { + int32_t len = endpos - startpos; + float len_mm = len / 51200.0 * 8.0; // 导程8mm + int32_t pointnum = len_mm * 10; // 1mm 分成10份 + return pointnum; +} - printf("scan raw result:\n"); - printf("(%d)", m_idleLevel); - printf("%d ", startpos); - for (int i = 0; i < m_off; i++) { - printf("%d ", m_posChache[i]); +void OneDimensionalCodeLaserScanner::dumpcodecache(code_cache_t* cache) { + ZLOGI(TAG, "startpos:%d,endpos:%d,pointnum:%d", cache->startpos, cache->endpos, cache->pointnum); + for (int i = 0; i < cache->pointnum; i++) { + printf("%d", cache->codecache[i]); } - printf("%d ", endpos); - printf("\r\n"); + printf("\n"); +} + +void OneDimensionalCodeLaserScanner::parsecode(code_cache_t* cache) { // // - int32_t sp = m_posChache[0]; - int32_t ep = endpos; - int32_t len = ep - sp; - // 把 len分成500份 - for (int i = 0; i < 500; i++) { - code[i] = getPosLevel((int32_t)(sp + len / 500.0 * i)); + /** + * @brief + * 14:0 + * 100 XXXX XXXX XXX1 + * + * 解码思路 + * 1. 通过100标定起始位置 + * 2. 然后依据每一位长度为1.2mm计算出其他各个相对bit的坐标 + * + */ + + // 1. 找到100,1对应的坐标 + int bit14startpos = -1; + int bit14endpos = -1; + + for (int32_t i = cache->pointnum; i > 0; i--) { + if (bit14endpos == -1) { + if (cache->codecache[i]) { + bit14endpos = i; + } + + } else if (bit14startpos == -1) { + if (!cache->codecache[i]) { + bit14startpos = i; + } + } else { + break; + } + } + + if (bit14startpos == -1 || bit14endpos == -1) { + ZLOGE(TAG, "find bit14 failed"); + m_errReg->setValue(kerr_findbit14fail); + return; + } + + int bit14len = bit14endpos - bit14startpos; + int bit14off = bit14len / 2 + bit14startpos; + int bit0off = bit14off - 14 * 1.2 * 10.0; + + if (bit0off < 0) { + ZLOGE(TAG, "find bit0 failed"); + m_errReg->setValue(kerr_findbit0fail); + } + // + bool bits[15] = {0}; + for (int i = 0; i < 15; i++) { + bits[i] = cache->codecache[int32_t(bit0off + i * 1.2 * 10.0)]; } - printf("scanresult:\n"); - for (int i = 0; i < 500; i++) { - printf("%d", code[i]); + int32_t code = 0; + for (int i = 0; i < 15; i++) { + code |= (bits[i] << i); } - printf("\r\n"); + m_errReg->setValue(kerr_success); + m_codeReg->setValue(code); + ZLOGI(TAG, "parse success,code:%d", code); + // return; +} + +void OneDimensionalCodeLaserScanner::parseResult() { // + m_codeReg->setValue(-1); + + static code_cache_t rawcodecache = {0}; + memset(&rawcodecache, 0, sizeof(rawcodecache)); + + /** + * @brief 初步分析数据 + */ + int32_t len = m_endpos - m_startpos; + int32_t pointnum = compute_point_num(m_startpos, m_endpos); + + ZLOGI(TAG, "pointnum:%d", len, pointnum); + + if ((size_t)pointnum > sizeof(rawcodecache.codecache)) { + ZLOGE(TAG, "len too long"); + m_errReg->setVal(kerr_scanRangeTooLarge); + return; + } + + if (m_endpos <= m_cfg.codeendpos) { + ZLOGE(TAG, "stop too early"); + m_errReg->setVal(kerr_scanEndTooEarly); + return; + } + + if (m_startpos >= m_cfg.codestartpos) { + ZLOGE(TAG, "start too late"); + m_errReg->setVal(kerr_scanStartTooLate); + return; + } + + /** + * @brief 将位置信息转变成,1,0的形式,方便解析数据 + * + * 1111110000111111111100001111000 + * + */ + + int32_t sp = m_posChache[0]; + int32_t ep = m_endpos; + + for (int i = 0; i < pointnum; i++) { + rawcodecache.codecache[i] = getPosLevel((int32_t)(sp + len / (pointnum * 1.0) * i)); + } + rawcodecache.startpos = sp; + rawcodecache.pointnum = pointnum; + rawcodecache.endpos = ep; + + dumpcodecache(&rawcodecache); + /** + * @brief 由于扫描的范围要比编码的范围大,所以需要将多余的数据去掉 + * + */ + { + int32_t after_cut_code_nums = compute_point_num(m_cfg.codestartpos, m_cfg.codeendpos); + + float distance = (m_cfg.codestartpos - rawcodecache.startpos) / 51200.0 * 8.0; + int startpointoff = distance * 10; + + rawcodecache.startpos = m_cfg.codestartpos; + rawcodecache.pointnum = after_cut_code_nums; + rawcodecache.endpos = m_cfg.codeendpos; + memcpy(rawcodecache.codecache, rawcodecache.codecache + startpointoff, after_cut_code_nums); + ZLOGI(TAG, "after_cut: code_nums:%d,cutoff:%d", after_cut_code_nums, startpointoff); + } + + dumpcodecache(&rawcodecache); + + /** + * @brief 分析数据 + */ + + parsecode(&rawcodecache); } void OneDimensionalCodeLaserScanner::onGpioIrq(ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) { // if (!m_workflag) { diff --git a/usrc/one_dimensional_code_laser_scanner.hpp b/usrc/one_dimensional_code_laser_scanner.hpp index 23feb77..19361a1 100644 --- a/usrc/one_dimensional_code_laser_scanner.hpp +++ b/usrc/one_dimensional_code_laser_scanner.hpp @@ -5,49 +5,90 @@ #include "sdk/hal/zhal.hpp" #include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp" -#define REG_CODE_SCANER_ACT_CTRL (0) // 扫码器控制 -#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 (10) // +#define REG_CODE_SCANER_ACT_CTRL (0) // 扫码器控制 +#define REG_CODE_SCANER_ACT_CLEAR_EXCEPTION (1) // 清除异常 + +#define REG_CODE_SCANER_STAT_STATUS (5) // 设备状态 +#define REG_CODE_SCANER_STAT_ERROR (6) // 上次采集结果是否有误 +#define REG_CODE_SCANER_CODE (7) // 码存放的地方 namespace iflytop { using namespace std; -#define POS_CACHE_SIZE 28 +#define POS_CACHE_SIZE 50 -class OneDimensionalCodeLaserScanner { +class OneDimensionalCodeLaserScanner : public ICPSListener { public: + typedef struct { + bool codecache[500]; + int32_t pointnum; + int32_t startpos; + int32_t endpos; + } code_cache_t; + typedef enum { kidle, kworking, } code_scan_state_t; + typedef enum { + kerr_success = 0, // + kerr_scanRangeTooLarge = 1, // + kerr_scanRangeTooSmall = 2, // + kerr_scanStartTooLate = 3, // + kerr_scanEndTooEarly = 4, // + kerr_findbit14fail = 5, // + kerr_findbit0fail = 6, // + } error_type_t; + typedef function PosReadder_t; + typedef struct { + Pin_t triggerPin; + PosReadder_t readder; + int32_t codestartpos; + int32_t codeendpos; + } cfg_t; + private: - IflytopCanProtocolStackProcesser* m_protocolProcesser; - int m_regStartOff = 0; + IflytopCanProtocolStackProcesser* m_protocolProcesser = NULL; + ICPSSlaveModule* m_slave = NULL; + int m_regStartOff = 0; PosReadder_t m_readder; + icps::Reg_t* m_statusReg = NULL; + icps::Reg_t* m_errReg = NULL; + icps::Reg_t* m_codeReg = NULL; + ZGPIO m_triggerGpio; bool m_workflag = false; + int32_t m_off = 0; + int32_t m_posChache[POS_CACHE_SIZE]; + int32_t m_endpos = 0; + int32_t m_startpos = 0; + bool m_idleLevel = true; + + cfg_t m_cfg; + public: OneDimensionalCodeLaserScanner(){}; ~OneDimensionalCodeLaserScanner(){}; - void initialize(IflytopCanProtocolStackProcesser* protocolProcesser, int32_t regStartOff, Pin_t triggerPin, PosReadder_t readder); + void initialize(IflytopCanProtocolStackProcesser* protocolProcesser, int32_t regStartOff, cfg_t* cfg); - virtual bool isThisRegOwnToMe(IflytopCanProtocolStackProcesser* processer, icps::Reg_t* reg); - virtual icps::error_t onHostRegisterWriteEvent(IflytopCanProtocolStackProcesser* processer, IflytopCanProtocolStackWriteEvent* writeEvent); + virtual icps::error_t onHostRegisterWriteEvent(icps::WriteEvent* event); void startScan(); void stopScan(); void parseResult(); private: + void parsecode(code_cache_t* cache); + int32_t compute_point_num(int32_t startpos, int32_t endpos); + + void dumpcodecache(code_cache_t* cache); + void onGpioIrq(ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent); bool getPosLevel(int pos); };