10 changed files with 362 additions and 737 deletions
-
10.cproject
-
2.project
-
4.settings/language.settings.xml
-
4liquid_path_control.ioc
-
22liquid_path_control.launch
-
20usrc/main.cpp
-
282usrc/one_dimensional_code_laser_scanner.cpp
-
101usrc/one_dimensional_code_laser_scanner.hpp
@ -1,282 +0,0 @@ |
|||||
#include "one_dimensional_code_laser_scanner.hpp"
|
|
||||
|
|
||||
#include <string.h>
|
|
||||
using namespace iflytop; |
|
||||
|
|
||||
#define ONE_BIT_WIDTH ((int32_t)(51200 * 1.20003 / 8.0))
|
|
||||
#define MIN_POS ((int32_t)(102733))
|
|
||||
|
|
||||
#define TAG "OneDimensionalCodeLaserScanner"
|
|
||||
|
|
||||
void OneDimensionalCodeLaserScanner::initialize(IflytopCanProtocolStackProcesser* protocolProcesser, int32_t regStartOff, cfg_t* cfg) { |
|
||||
m_protocolProcesser = protocolProcesser; |
|
||||
m_regStartOff = regStartOff; |
|
||||
m_cfg = *cfg; |
|
||||
|
|
||||
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_item = m_protocolProcesser->activeReg(m_slave, REG_CODE_SCANER_ITEM, icps::kr, 0); |
|
||||
m_lot = m_protocolProcesser->activeReg(m_slave, REG_CODE_SCANER_LOT, icps::kr, 0); |
|
||||
|
|
||||
m_triggerGpio.initAsInput(m_cfg.triggerPin, ZGPIO::kMode_nopull, ZGPIO::kIRQ_risingAndFallingIrq, true /*mirror*/); |
|
||||
m_triggerGpio.regListener([this](ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) { onGpioIrq(GPIO_Pin, irqevent); }); |
|
||||
m_readder = m_cfg.readder; |
|
||||
} |
|
||||
|
|
||||
icps::error_t OneDimensionalCodeLaserScanner::onHostRegisterWriteEvent(icps::WriteEvent* writeEvent) { |
|
||||
int32_t regoff = writeEvent->reg->add - m_regStartOff; |
|
||||
switch (regoff) { |
|
||||
case REG_CODE_SCANER_ACT_CTRL: |
|
||||
if (writeEvent->newvalue == 1) { |
|
||||
startScan(); |
|
||||
} else if (writeEvent->newvalue == 0) { |
|
||||
stopScan(); |
|
||||
parseResult(); |
|
||||
} else { |
|
||||
return icps::kIllegalValue; |
|
||||
} |
|
||||
return icps::kSuccess; |
|
||||
case REG_CODE_SCANER_ACT_CLEAR_EXCEPTION: |
|
||||
return icps::kSuccess; |
|
||||
default: |
|
||||
break; |
|
||||
} |
|
||||
return icps::kRegNotFound; |
|
||||
} |
|
||||
|
|
||||
void OneDimensionalCodeLaserScanner::startScan() { |
|
||||
CriticalContext cc; |
|
||||
m_off = 0; |
|
||||
m_workflag = true; |
|
||||
m_idleLevel = m_triggerGpio.getState(); |
|
||||
m_startpos = m_readder(); |
|
||||
ZLOGI(TAG, "start scan startpos:%d", m_startpos); |
|
||||
} |
|
||||
void OneDimensionalCodeLaserScanner::stopScan() { |
|
||||
CriticalContext cc; |
|
||||
m_workflag = false; |
|
||||
m_endpos = m_readder(); |
|
||||
ZLOGI(TAG, "stop scan endpos:%d", m_endpos); |
|
||||
} |
|
||||
|
|
||||
bool OneDimensionalCodeLaserScanner::getPosLevel(int pos) { |
|
||||
bool level = m_idleLevel; |
|
||||
bool nowlevel = m_idleLevel; |
|
||||
|
|
||||
if (pos < m_posChache[0]) { |
|
||||
return m_idleLevel; |
|
||||
} |
|
||||
|
|
||||
for (int i = 0; i < m_off; i++) { |
|
||||
nowlevel = !nowlevel; |
|
||||
if (i + 1 < m_off) { |
|
||||
if (pos >= m_posChache[i] && pos < m_posChache[i + 1]) { |
|
||||
level = nowlevel; |
|
||||
break; |
|
||||
} |
|
||||
} else { |
|
||||
level = nowlevel; |
|
||||
} |
|
||||
} |
|
||||
|
|
||||
return level; |
|
||||
} |
|
||||
|
|
||||
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; |
|
||||
} |
|
||||
|
|
||||
void OneDimensionalCodeLaserScanner::dumpcodecache(code_cache_t* cache) { |
|
||||
#if 0
|
|
||||
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("\n"); |
|
||||
#endif
|
|
||||
} |
|
||||
|
|
||||
#define BITVAL(code, off) ((code & (1 << off)) >> off)
|
|
||||
|
|
||||
void OneDimensionalCodeLaserScanner::decode(uint32_t rawcode) { //
|
|
||||
// Lot:3:6
|
|
||||
// bit3->bit3, bit4->bit2, bit5->bit1, bit6->bit0
|
|
||||
uint32_t lot = 0; |
|
||||
lot = BITVAL(rawcode, 3) << 3 | BITVAL(rawcode, 4) << 2 | BITVAL(rawcode, 5) << 1 | BITVAL(rawcode, 6) << 0; |
|
||||
|
|
||||
// =(bit1)*2^6+(bit2)*2^5+(bit11)*2^4+(bit10)*2^0+(bit9)*2^1+(bit8)*2^2+(bit7)*2^3
|
|
||||
uint32_t item = 0; |
|
||||
item = (BITVAL(rawcode, 1) << 6) //
|
|
||||
| (BITVAL(rawcode, 2) << 5) //
|
|
||||
| (BITVAL(rawcode, 11) << 4) //
|
|
||||
| (BITVAL(rawcode, 10) << 0) //
|
|
||||
| (BITVAL(rawcode, 9) << 1) //
|
|
||||
| (BITVAL(rawcode, 8) << 2) //
|
|
||||
| (BITVAL(rawcode, 7) << 3); |
|
||||
|
|
||||
ZLOGI(TAG, "item-lot: %d-%d\n", item, lot); |
|
||||
m_item->setValue(item); |
|
||||
m_lot->setValue(lot); |
|
||||
} |
|
||||
|
|
||||
void OneDimensionalCodeLaserScanner::parsecode(code_cache_t* cache) { //
|
|
||||
//
|
|
||||
|
|
||||
/**
|
|
||||
* @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)]; |
|
||||
} |
|
||||
|
|
||||
int32_t code = 0; |
|
||||
for (int i = 0; i < 15; i++) { |
|
||||
code |= (bits[i] << i); |
|
||||
} |
|
||||
m_errReg->setValue(kerr_success); |
|
||||
m_codeReg->setValue(code); |
|
||||
printf("code:"); |
|
||||
for (int i = 0; i < 15; i++) { |
|
||||
printf("%d", bits[i]); |
|
||||
} |
|
||||
printf("\n"); |
|
||||
ZLOGI(TAG, "parse success,code:0x%0x", (uint32_t)code); |
|
||||
decode(code); |
|
||||
// return;
|
|
||||
} |
|
||||
|
|
||||
void OneDimensionalCodeLaserScanner::parseResult() { //
|
|
||||
m_codeReg->setValue(-1); |
|
||||
m_item->setValue(-1); |
|
||||
m_lot->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", 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_startpos; |
|
||||
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; |
|
||||
memmove(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) { |
|
||||
return; |
|
||||
} |
|
||||
if (m_off >= 50) return; |
|
||||
m_posChache[m_off] = m_readder(); |
|
||||
m_off++; |
|
||||
} |
|
||||
@ -1,101 +0,0 @@ |
|||||
#pragma once
|
|
||||
#include <stdint.h>
|
|
||||
#include <stdio.h>
|
|
||||
|
|
||||
#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_ERROR (6) // 上次采集结果是否有误
|
|
||||
#define REG_CODE_SCANER_CODE (7) // 码存放的地方
|
|
||||
#define REG_CODE_SCANER_ITEM (8) // 码存放的地方
|
|
||||
#define REG_CODE_SCANER_LOT (9) // 码存放的地方
|
|
||||
|
|
||||
namespace iflytop { |
|
||||
using namespace std; |
|
||||
|
|
||||
#define POS_CACHE_SIZE 50
|
|
||||
|
|
||||
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<int32_t()> PosReadder_t; |
|
||||
|
|
||||
typedef struct { |
|
||||
Pin_t triggerPin; |
|
||||
PosReadder_t readder; |
|
||||
int32_t codestartpos; |
|
||||
int32_t codeendpos; |
|
||||
} cfg_t; |
|
||||
|
|
||||
private: |
|
||||
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; |
|
||||
icps::Reg_t* m_item = NULL; |
|
||||
icps::Reg_t* m_lot = 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, cfg_t* cfg); |
|
||||
|
|
||||
virtual icps::error_t onHostRegisterWriteEvent(icps::WriteEvent* event); |
|
||||
|
|
||||
void startScan(); |
|
||||
void stopScan(); |
|
||||
void parseResult(); |
|
||||
|
|
||||
private: |
|
||||
void parsecode(code_cache_t* cache); |
|
||||
void decode(uint32_t rawcode); |
|
||||
|
|
||||
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); |
|
||||
}; |
|
||||
} // namespace iflytop
|
|
||||
Write
Preview
Loading…
Cancel
Save
Reference in new issue