树莓派+OpenCV+Arduino实现二维码颜色识别检测与物料抓取总结

树莓派+OpenCV+Arduino实现二维码颜色识别检测与物料抓取

//此处代码源自CSDN，地址不记得了。。。。。。。
#include 
#include “Astar.h“
#include 

int CAStar::m_moveCostHorizontal = 5;
int CAStar::m_moveCostDiagonal = 7;

DIRECTION g_direct[] = {
	//{-1 -1} //left up
	//{-1 +1} //left down
	//{+1 -1} //right up
	//{+1 +1} //right down
	{ -1 0 } //left
	{ +1 0 } //right
	{ 0 -1 } //up
	{ 0 +1 } //down
};

CAStar::CAStar（） :m_startNode（0 0） m_endNode（0 0）
{
	m_CallBack = NULL;
}

CAStar::CAStar（int nColumn int nRow） : m_startNode（0 0） m_endNode（0 0）
{
	m_nColumn = nColumn;
	m_nRow = nRow;
	m_CallBack = NULL;
}

CAStar::~CAStar（void）
{
}

void CAStar::SetRange（int nColumn int nRow）
{
	m_nColumn = nColumn;
	m_nRow = nRow;
}

void CAStar::SetBlock（vector &vecBlock）
{
	m_barrier.clear（）;
	for （unsigned int i = 0; i < vecBlock.size（）; i++）
	{
		m_barrier.push_back（vecBlock[i]）;
	}
}

void CAStar::SetStartAndEnd（NODE startNode NODE endNode）
{
	m_startNode = startNode;
	m_endNode = endNode;
	m_open.clear（）;
	m_close.clear（）;
	CalcValue（startNode）;
	m_open.push_back（startNode）;
}

void CAStar::GetMinFromOpen（vector &minVec）
{
	minVec.clear（）;
	vector::iterator it = m_open.begin（）;
	int fMin = INT_MAX;
	for （; it != m_open.end（）; it++）
	{
		if （it->f < fMin）
			fMin = it->f;
	}
	for （it = m_open.begin（）; it != m_open.end（）; ）
	{
		if （it->f == fMin）
		{
			minVec.push_back（*it）;
			it = m_open.erase（it）;
			continue;
		}
		it++;
	}
}

int CAStar::SetCallBack（int（*pFun）（void *） void *pThis）
{
	m_CallBack = pFun;
	m_pArg = pThis;
	return 1;
}

bool CAStar::IsFindPath（）
{
	return m_bFind;
}

void CAStar::FindPath（）
{
	m_bFind = false;
	vector minVec;
	while （!m_bFind）
	{
		minVec.clear（）;
		GetMinFromOpen（minVec）;
		if （minVec.empty（））
			break;
		m_close.insert（m_close.end（） minVec.begin（） minVec.end（））;
		for （vector::iterator it = minVec.begin（）; it != minVec.end（）; it++）
			DoNeighbors（*it）;

		if （m_bFind）
			GetFinalShortestPath（）;

		if （m_CallBack） m_CallBack（m_pArg）;
	}
}

void CAStar::GetFinalShortestPath（）
{
	NODE node = m_close[m_close.size（） - 1];
	m_path.clear（）;
	while （true）
	{
		DIRECTION direct = node.parent;
		NODE parentNode = node;
		parentNode.nX -= direct.x;
		parentNode.nY -= direct.y;

		for （unsigned int i = 0; i < m_close.size（）; i++）
		{
			if （m_close[i].nX == parentNode.nX && m_close[i].nY == parentNode.nY）
			{
				node = m_close[i];
				if （node.nX == m_startNode.nX && node.nY == m_startNode.nY）
					return;
				m_path.push_back（node）;
				break;
			}
		}
	}
}

void CAStar::CalcValue（NODE &node）
{
	node.g = GetDistance（node m_startNode）;
	node.h = GetDistance（node m_endNode）;
	node.f = node.g + node.h;
}

int CAStar::GetDistance（NODE &node1 NODE &node2）
{
	int xSub = abs（node1.nX - node2.nX）;
	int ySub = abs（node1.nY - node2.nY）;

	return  abs（xSub - ySub） * m_moveCostHorizontal +
		min（xSub ySub） * m_moveCostDiagonal;
}

void CAStar::DoNeighbors（NODE &node）
{

	for （int i = 0; i < sizeof（g_direct） / sizeof（g_direct[0]）; i++）

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2019-02-01 20:48  Arduino\
     文件        8781  2019-02-01 20:13  Arduino\Arduino.ino
     文件        8571  2019-02-01 20:01  Arduino\stepper.ino
     目录           0  2019-02-01 20:47  QR_Code_Car\
     文件        5067  2018-08-08 07:43  QR_Code_Car\Astar.cpp
     文件        1667  2018-08-08 07:43  QR_Code_Car\Astar.h
     文件        2047  2018-08-08 07:43  QR_Code_Car\QR_Code_Car.cbp
     文件        6978  2019-01-09 17:07  QR_Code_Car\QR_Code_Car.depend
     文件        1091  2019-01-15 11:12  QR_Code_Car\QR_Code_Car.layout
     文件        3900  2018-08-08 07:43  QR_Code_Car\Serial.cpp
     文件        1055  2018-08-08 07:43  QR_Code_Car\Serial.h
     目录           0  2019-02-01 20:47  QR_Code_Car\bin\
     目录           0  2019-02-01 20:47  QR_Code_Car\bin\Debug\
     文件      428556  2018-08-08 07:43  QR_Code_Car\bin\Debug\QR_Code_Car
     文件        9541  2019-01-11 21:19  QR_Code_Car\main.cpp
     目录           0  2019-02-01 20:47  QR_Code_Car\obj\
     目录           0  2019-02-01 20:47  QR_Code_Car\obj\Debug\
     文件      184696  2018-08-08 07:43  QR_Code_Car\obj\Debug\Astar.o
     文件       60948  2018-08-08 07:43  QR_Code_Car\obj\Debug\Serial.o
     文件      452784  2018-08-08 07:43  QR_Code_Car\obj\Debug\main.o