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