global.cpp

机器人视觉处理程序

/*========================================
*文件名:   global.cpp
*文件描述: 公用的全局函数定义
*修改记录: 
*=======================================*/

#include "stdafx.h"
#include "global.h"
#include "IRExpPlatform.h"
#include "IRExpPlatformDlg.h"
#include "memory.h"
#include "math.h"


/*================================================================================
*函数名:		LRESULT CALLBACK FrameCallbackProc(HWND ghWnd, LPVIDEOHDR lpVData)
*函数参数:		HWND ghWnd			---视频采集窗口句柄
				LPVIDEOHDR lpVData	---图像数据对应的内存首地址	
*函数功能描述:  彩色视频回调函数(默认),每次采集图像后就执行一次
*返回值:	    LRESULT==TRUE,执行成功,LRESULT==FALSE,执行失败	
*==================================================================================*/
    
LRESULT CALLBACK FrameCallbackProc(HWND ghWnd, LPVIDEOHDR lpVData)
{
	//theApp.bColor==FALSE,不显示彩色视频
	//ghWnd==FALSE，视频采集窗口没有创建成功
	if (!theApp.bColor || !ghWnd)          
       return FALSE;                //本函数不执行

	memcpy(theApp.ColorStructure, lpVData->lpData, WIDTH * HEIGHT * 3 ); //内存拷贝WIDTH * HEIGHT * 3 字节的数据至theApp.ColorStructure数组

	return (LRESULT) TRUE ;
}
/*================================================================================
*函数名:		LRESULT CALLBACK FrameCallbackProc1(HWND ghWnd, LPVIDEOHDR lpVData)
*函数参数:		HWND ghWnd			---视频采集窗口句柄
				LPVIDEOHDR lpVData	---图像数据对应的内存首地址	
*函数功能描述:  黑白视频回调视频(本程序没有采用,供程序扩展使用),每次采集图像后就执行一次
*返回值:	    LRESULT==TRUE,执行成功,LRESULT==FALSE,执行失败	
*==================================================================================*/
//详细注释见彩色视频回调函数:LRESULT CALLBACK FrameCallbackProc(HWND ghWnd, LPVIDEOHDR lpVData)
LRESULT CALLBACK FrameCallbackProc1(HWND ghWnd, LPVIDEOHDR lpVData)  
{
	if(theApp.bColor || !ghWnd) 
		return FALSE;

	memcpy(theApp.ColorStructure, lpVData->lpData, 3*WIDTH*HEIGHT);
	
	//r,g,b为彩色象素对应的红绿蓝分量,gray为对应的灰度值
	BYTE r, g, b, gray;                                         
	int i, j;
	for(i = 0; i < HEIGHT; ++i)
		for(j = 0; j < WIDTH; ++j)
		{
			r = *(lpVData -> lpData + i * WIDTH * 3 + j * 3);
			g = *(lpVData -> lpData + i * WIDTH * 3 + j * 3 + 1);
			b = *(lpVData -> lpData + i * WIDTH * 3 + j * 3 + 2);
			
			gray = BYTE(0.299 * r + 0.587 * g + 0.114 * b);       //彩色象素转灰色象素计算式

			*(lpVData -> lpData + i * WIDTH * 3 + j * 3) = gray;
			*(lpVData -> lpData + i * WIDTH * 3 + j * 3 + 1) = gray;
			*(lpVData -> lpData + i * WIDTH * 3 + j * 3 + 2) = gray;

		}		
	
	return FALSE;
}

/*================================================================================
*函数名:		void MedianFilter()
*函数参数:		无
*函数功能描述:  对图像进行中值滤波以消除图像中包含的噪声,
				滤波原理参见FAQ-->中值滤波的思想是什么？
*返回值:	    void	
*==================================================================================*/
void MedianFilter()
{
	// 循环变量
	LONG i;
	LONG j;
	LONG k;
	LONG l;

	// 掩码模版(3X3模板)
	unsigned char mask_r[9];
	unsigned char mask_g[9];
	unsigned char mask_b[9];
	COLORREF mask[9];

//TRUE 为对原始图像进行中值滤波，FALSE为对阈值分割后的图像进行滤波
if(theApp.bFirstFilter) 
{
	for (i = 0; i < HEIGHT; i ++)            //i行
	{												
		for (j = 0; j < WIDTH; j ++)		//j列
		{			
			
			theApp.tempColor[(HEIGHT - i - 1) * WIDTH * 3 + j * 3 + 2] = 
				theApp.ColorStructure[(HEIGHT - i - 1) * WIDTH * 3 + j * 3 + 2];

			theApp.tempColor[(HEIGHT - i - 1) * WIDTH * 3 + j * 3 + 1] = 
				theApp.ColorStructure[(HEIGHT - i - 1) * WIDTH * 3 + j * 3 + 1];
				
			theApp.tempColor[(HEIGHT - i - 1) * WIDTH * 3 + j * 3 + 0] = 
				theApp.ColorStructure[(HEIGHT - i - 1) * WIDTH * 3 + j * 3 + 0];
		
		}
	}
}

	// 模版滤波
	for (i = 0; i < WIDTH; i++)				//被处理像素在i列
	{
		for (j = 0; j < HEIGHT; j++)		//被处理像素在j行
		{
			// 索引
			int	id = 0;

			// 进行小区域模版滤波
			for (k = i - 1; k < i + 2; ++k)
			{
				for(l = j - 1; l < j + 2; ++l)
				{
					// 防止内存溢出
					if (k >= 0 && l >= 0 && k < WIDTH && l < HEIGHT)
					{
						mask_r[id] = theApp.tempColor[(HEIGHT - l - 1) * WIDTH * 3 + k * 3 + 2];
						mask_g[id] = theApp.tempColor[(HEIGHT - l - 1) * WIDTH * 3 + k * 3 + 1];						
						mask_b[id] = theApp.tempColor[(HEIGHT - l - 1) * WIDTH * 3 + k * 3 + 0];
				
						mask[id] = RGB(mask_r[id], mask_g[id], mask_b[id]);
						++id;
					}
				}
			}
			
			// 中间变量
			unsigned char T;
			
			// 冒泡排序法
			for (k = 0; k < 8; k++)		
			{
				for (l = 8; l > k; l--)					
				{
					if (mask_r[l] < mask_r[l - 1])
					{
						T = mask_r[l];
						mask_r[l] = mask_r[l - 1];
						mask_r[l - 1] = T;
					}
					if (mask_g[l] < mask_g[l - 1])
					{
						T = mask_g[l];
						mask_g[l] = mask_g[l - 1];
						mask_g[l - 1] = T;
					}
					if (mask_b[l] < mask_b[l - 1])
					{
						T = mask_b[l];
						mask_b[l] = mask_b[l - 1];
						mask_b[l - 1] = T;
					}
				}
			}
			theApp.tempColor[(HEIGHT - j - 1) * WIDTH * 3 + i * 3 + 2] = mask_r[4];
			theApp.tempColor[(HEIGHT - j - 1) * WIDTH * 3 + i * 3 + 1] = mask_g[4];
			theApp.tempColor[(HEIGHT - j - 1) * WIDTH * 3 + i * 3 + 0] = mask_b[4];
		}
	}
}

/*================================================================================
*函数名:		void ColorSegment()
*函数参数:		无
*函数功能描述:  对原始图像进行阈值分割以便于进一步处理
*返回值:	    void	
*==================================================================================*/
//通常采用RGB进行阈值分割,它受灯光的明暗影响很大
//本函数采用rgb进行阈值分割,它不受均匀白光的明暗影响
//注意:为了得到较好的处理效果,本函数定义的阈值在每次实验前均需要重新确定
void ColorSegment()
{
	int i,j;
	int R,G,B;
	double r,g,b;
	

	for (i = 0; i < HEIGHT; i ++)               //i行	
		for (j = 0; j < WIDTH; j ++)			//j列
		{
			R = theApp.tempColor[(HEIGHT - i - 1) * WIDTH * 3 + j * 3 + 2];
			G = theApp.tempColor[(HEIGHT - i - 1) * WIDTH * 3 + j * 3 + 1];
			B = theApp.tempColor[(HEIGHT - i - 1) * WIDTH * 3 + j * 3 + 0];

			r=R*1.0/(R+G+B);											//计算r=R/(R+G+B)				
			g=G*1.0/(R+G+B);											//计算g=G/(R+G+B)
			b=B*1.0/(R+G+B);											//计算b=B/(R+G+B)			
			
		if(r > theApp.rWhiteMin && r<theApp.rWhiteMax
		&& g > theApp.gWhiteMin && g<theApp.gWhiteMax
		&& b > theApp.bWhiteMin && b<theApp.bWhiteMax )						// 白色阈值分割  ，注意选择在光线较暗的情况下黑色，白色，和深绿色的阈值
				R = G = B = 255;
			else 
				if(r > theApp.rBlueMin && r<theApp.rBlueMax
				&& g > theApp.gBlueMin && g<theApp.gBlueMax
				&& b > theApp.bBlueMin && b<theApp.bBlueMax )							 //蓝色阈值分割
				{
				R = 0;
				G = 0;
				B = 255;
				}
			else 
				if(r > theApp.rRedMin && r<theApp.rRedMax
				&& g > theApp.gRedMin && g<theApp.gRedMax
				&& b > theApp.bRedMin && b<theApp.bRedMax )			 //红色阈值分割 
				{
				R = 255;
				G = 0;
				B = 0;

				}

			else
				if(r > theApp.rYellowMin && r<theApp.rYellowMax
				&& g > theApp.gYellowMin && g<theApp.gYellowMax
				&& b > theApp.bYellowMin && b<theApp.bYellowMax )  //黄色阈值分割 
				{
				R = 255;
				G = 255;
				B = 0;

				}
			else														//其余为背景绿色			
			{
				R = 0;
				G = 255;
				B = 0;
			}			

			theApp.tempColor[(HEIGHT - i - 1) * WIDTH * 3 + j * 3 + 2] = R;
			theApp.tempColor[(HEIGHT - i - 1) * WIDTH * 3 + j * 3 + 1] = G;
			theApp.tempColor[(HEIGHT - i - 1) * WIDTH * 3 + j * 3 + 0] = B;
		}

		
}
/*================================================================================
*函数名:		void FindOutLine()
*函数参数:		无
*函数功能描述:  发现白线后的处理函数
*返回值:	    void	
*==================================================================================*/
void FindOutLine()
{
										
	if (abs(theApp.deltaLength) <= 50)			// 小车偏离白线
	{
		if (abs(theApp.deltaAngle) <= 3)		//参数待定
			theApp.nMotiontype = FORWARD;
		else if (theApp.deltaAngle > 3)
			theApp.nMotiontype=TURNRIGHT;
		else if (theApp.deltaAngle < -3)
			theApp.nMotiontype = TURNLEFT;
	}
	else if (theApp.deltaLength > 50)           
	{
		if(theApp.deltaAngle>0)
		{
		  if (theApp.deltaAngle > theApp.deltaLength / 30)        //?theApp.deltaAngle>0  or theApp.deltaAngle<0 
			theApp.nMotiontype = TURNRIGHT;
			else
				theApp.nMotiontype = FORWARD;
		}
		else
			theApp.nMotiontype = TURNLEFT;

	}
	else														//deltalength<-50
	{
		if(theApp.deltaAngle<0)
		{
		 if (theApp.deltaAngle < theApp.deltaLength / 30)         //theApp.deltaAngle<0  or theApp.deltaAngle>0
			theApp.nMotiontype = TURNLEFT;
		else
			theApp.nMotiontype = FORWARD;
		}
		else
			theApp.nMotiontype = TURNRIGHT;
			
	}

	if (theApp.nMotiontype != theApp.nLastMotiontype && theApp.nLastMotiontype != STOP)  //为了减小对电机造成损害,每次非停止运动之前都要先停止机器人
		theApp.nMotiontype = STOP;
	return;
	
}


/*================================================================================
*函数名:		void NotFindOutLine()
*函数参数:		无
*函数功能描述:  未找到白线处理函数
*返回值:	    void	
*==================================================================================*/
void NotFindOutLine()
{
		
			static int loopCnt = 0;					//循环次数计数
			static int CenterX = 0, CenterY = 0;
	
			if (loopCnt > MAXCNT)                    
				loopCnt = MAXCNT + 1;
	
			if (theApp.nCntLinePts >= 10)
				CenterX = CenterY = 0;
			else if(theApp.bMove==TRUE)				//小车静止的时候不计数 
				++loopCnt;	
	
			for (int i = 0; theApp.nCntLinePts >= 10 && i < theApp.nCntLinePts; ++i)
			{
				CenterX += theApp.linePoints[i][0];   //计算中心坐标
				CenterY += theApp.linePoints[i][1];
			}
	
			if ( theApp.nCntLinePts >= 10 )
			{
				CenterX /= theApp.nCntLinePts;
				CenterY /= theApp.nCntLinePts;
				loopCnt = 0;
			}
	
			if (loopCnt == MAXCNT)
			{
				if (theApp.nDirection == LEFT)
					theApp.nMotiontype = TURNRIGHT;
				else
					theApp.nMotiontype = TURNLEFT;
				
				CenterX = CenterY = 0;					//下面if-else条件不满足 保持原来运动状态
	
				return;
			}
	
		if (abs(CenterX - XROBOT) < 80 && CenterY < 100)
			theApp.nMotiontype = FORWARD;
		else if ( CenterX > 0 && CenterX <= XROBOT )
			theApp.nMotiontype = TURNLEFT;
		else if ( CenterX > XROBOT )
			theApp.nMotiontype = TURNRIGHT;
        
		if (loopCnt > MAXCNT && theApp.nMotiontype == STOP)
			loopCnt = MAXCNT - 1;

		if (theApp.nMotiontype != theApp.nLastMotiontype && theApp.nLastMotiontype != STOP)
			theApp.nMotiontype = STOP;

		return;
}


/*================================================================================
*函数名:		void DecisionSearchLine()
*函数参数:		无
*函数功能描述:  寻找白线决策函数
*返回值:	    void	
*==================================================================================*/
void DecisionSearchLine()
{	
	if(SearchLine()==FALSE)                       //机器人视野范围内没有白线     
		NotFindOutLine();							//未发现白线决策函数
	else
		FindOutLine();								//发现白线决策函数

}
/*================================================================================
*函数名:		void Motion()
*函数参数:		无
*函数功能描述:  发送控制字符控制机器人运行函数
*返回值:	    void	
*==================================================================================*/
//机器人运动控制字已在下位机程序中指定,可以根据需要增加或修改
void Motion()
{
    CString	strTXData;								//向串口发送的数据
	
	CIRExpPlatformDlg *pdlgTemp = (CIRExpPlatformDlg *)theApp.m_pMainWnd;     //得到主窗口(CIRExpPlatformDlg)的句柄

	pdlgTemp->m_ctrlComm.SetOutBufferCount(0);			//清空发送缓冲区                    

	switch(theApp.nMotiontype)							
	{

	case FORWARD:			//前行
		strTXData = "a";
		pdlgTemp->m_ctrlComm.SetOutput(COleVariant(strTXData));//发送ASCII字符数据
		theApp.nLastMotiontype = FORWARD;
		theApp.nDirection = FRONT;
		theApp.strCarState= "前进";
		break;

	case BACK:				//后退							
		strTXData = "b";
		pdlgTemp->m_ctrlComm.SetOutput(COleVariant(strTXData));//发送ASCII字符数据
		theApp.nLastMotiontype = BACK;
		theApp.nDirection = BACK;
		theApp.strCarState= "后退";
		break;  

	case TURNLEFT:			//左转弯
		strTXData = "c";
		pdlgTemp->m_ctrlComm.SetOutput(COleVariant(strTXData));//发送ASCII字符数据
		theApp.nLastMotiontype = TURNLEFT;
		theApp.nDirection = LEFT;
		theApp.strCarState= "左转";
		break;

	case TURNRIGHT:			//右转弯
		strTXData = "d";
		pdlgTemp->m_ctrlComm.SetOutput(COleVariant(strTXData));//发送ASCII字符数据
		theApp.nLastMotiontype = TURNRIGHT;
		theApp.nDirection = RIGHT;
		theApp.strCarState= "右转";
		break;

	case STOP:				//停止
		//StopCar();
		strTXData = "e";
		pdlgTemp->m_ctrlComm.SetOutput(COleVariant(strTXData));//发送ASCII字符数据
		theApp.nLastMotiontype = STOP;
		theApp.nDirection = STOP;
		theApp.strCarState= "停止";
		break;


	case BEEP:				//发现目标
		strTXData="f";     
		pdlgTemp->m_ctrlComm.SetOutput(COleVariant(strTXData));//发送ASCII字符数据
		theApp.strCarState= "蜂鸣";
		break;
	case TURNLEFT90ANDRUN:	//左转90度后向前行进一段距离
		strTXData = "g";
		pdlgTemp->m_ctrlComm.SetOutput(COleVariant(strTXData));//发送ASCII字符数据
		theApp.nLastMotiontype = TURNLEFT90ANDRUN;
		theApp.nDirection = LEFT;
		theApp.strCarState= "左转90度";
		break;
	case TURNRIGHT90ANDRUN:	//右转90度后向前行进一段距离
		strTXData = "h";
		pdlgTemp->m_ctrlComm.SetOutput(COleVariant(strTXData));//发送ASCII字符数据
		theApp.nLastMotiontype = TURNRIGHT90ANDRUN;
		theApp.nDirection = RIGHT;
		theApp.strCarState= "右转90度";
		break;
	case ROTATION180ANDRUN:	//旋转180度后向前行进一段距离
		strTXData = "i";
		pdlgTemp->m_ctrlComm.SetOutput(COleVariant(strTXData));//发送ASCII字符数据
		theApp.nLastMotiontype = ROTATION180ANDRUN;
		theApp.strCarState= "旋转180度";
		break;
	default:
		break;
		
	}

	return;
}
/*================================================================================
*函数名:		void StopCar()
*函数参数:		无
*函数功能描述:  发送控制字符控制机器人停止运行函数
*返回值:	    void	
*==================================================================================*/
void StopCar()