p_transmit_disinfection
/
liquid_path_control
1
0
Fork 1
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 2 years ago
parent
c1351b7d41
commit
222cd666fb
5 changed files with 224 additions and 64 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 0
      usrc/flia_plate_box_pusher_ctl_service.cpp
  2. 0
      usrc/flia_plate_box_pusher_ctl_service.hpp
  3. 9
      usrc/main.cpp
  4. 212
      usrc/one_dimensional_code_laser_scanner.cpp
  5. 67
      usrc/one_dimensional_code_laser_scanner.hpp

0
usrc/flia_plate_box_pusher_ctl_service.cpp
View File

0
usrc/flia_plate_box_pusher_ctl_service.hpp
View File

9
usrc/main.cpp
View File

@ -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) {

212
usrc/one_dimensional_code_laser_scanner.cpp
View File

@ -1,43 +1,31 @@
#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"
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) {

67
usrc/one_dimensional_code_laser_scanner.hpp
View File

@ -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<int32_t()> 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);
};

Write Preview
Loading…
Cancel
Save