global.cpp

机器人视觉处理程序

			{
				if(tempCntObstacle > theApp.nObstacleParm1 && bFoundObstacle == TRUE)
				{
					nCntObstacle += 1;           
					break;
				}
				bFoundObstacle = FALSE;
				tempCntObstacle = 0;
			}
				
		}
	}

	if(nCntObject >=theApp.nObjectParm2)    //modified 20->10
	{
		// 发现目标,得到位置参数,并返回TRUE
		theApp.xCoordinate /= nCntObject;
		theApp.yCoordinate /= nCntObject;				  
		theApp.bFlagObject=TRUE;
	}
	else							
		theApp.bFlagObject=FALSE; 
	
	if(nCntObstacle>=theApp.nObstacleParm2)	
		theApp.bFlagObstacle=TRUE;
	else
		theApp.bFlagObstacle=FALSE;		

}

/*================================================================================
*函数名:		BOOL SearchObjectSucess()
*函数参数:		无
*函数功能描述:  机器人找到目标物后处理函数
*返回值:	    void	
*==================================================================================*/
BOOL SearchObjectSucess()
{
		// 找到目标,停止运动					
		if(abs(theApp.xCoordinate - XROBOT) < 100 && abs(theApp.yCoordinate - YROBOT) < 200)
		{										//yCoordinate<YROBOT  ==1
			theApp.nMotiontype = STOP;			//先停止机器人
			Motion();
			::Sleep(100);                       //延时100ms
			theApp.nMotiontype = BEEP;			//再蜂鸣,表示找到目标物
			Motion();
			return  TRUE;
		}
		else
			return FALSE;		

}

/*================================================================================
*函数名:		BOOL SearchLine()
*函数参数:		无
*函数功能描述:	搜索图像以确定白线函数
*返回值:	    BOOL,TRUE-扫描到白线,FALSE-未扫描到白线	
*==================================================================================*/
//隔五行扫描,若每行白色象素点超过35,而且满足这样条件的行有16行,则认为由这些行围成的区域是白色区域
BOOL SearchLine()         
{
	int i, j, r, g, b;
	int tempCnt;
	int xTemp, yTemp;
	BOOL bFound = FALSE;
	theApp.nCntLinePts = 0;

	//处理图像白线
	for (i = 5; i < HEIGHT; i += 5)    //隔五行扫描
	{
		bFound = FALSE;
		tempCnt = 0;
		for (j = 0; j < WIDTH; ++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];

			if(r == 255 && g == 255 && b == 255)
			{
				++tempCnt;
				if(bFound == FALSE)
				{
					bFound = TRUE;
					xTemp = j;
					yTemp = i;
				}
				if(tempCnt > theApp.nWhiteParm1 && bFound == TRUE && j > WIDTH - 6 && j < WIDTH - 2)     //? 至少减一  (j < WIDTH - 4)--> (j>WIDTH - 4) is  right  白色区域延伸至右边缘
				{
					theApp.linePoints[theApp.nCntLinePts][0] = (j + xTemp) / 2;
					theApp.linePoints[theApp.nCntLinePts][1] = (i + yTemp) / 2;
					++theApp.nCntLinePts;
					break;
				}
			}
			else
			{
				if(tempCnt > theApp.nWhiteParm1 && bFound == TRUE)                                      //??   白色区域在中间
				{
					theApp.linePoints[theApp.nCntLinePts][0] = (j + xTemp) / 2;
					theApp.linePoints[theApp.nCntLinePts][1] = (i + yTemp) / 2;
					++theApp.nCntLinePts;
					break;
				}
				bFound = FALSE;
				tempCnt = 0;
			}
		}
	}
		
	if(theApp.nCntLinePts >= theApp.nWhiteParm2/5)                         //每隔五行扫描
	{
		int centerX = 0,centerY = 0;
		int deltaX = 0;

		for(i = 0;i < theApp.nCntLinePts; ++i)
		{
			centerX += theApp.linePoints[i][0];
			centerY += theApp.linePoints[i][1];
		}
		centerX /= theApp.nCntLinePts;
		centerY /= theApp.nCntLinePts;

		// 归一化为相隔5行的横坐标差值，白线方向是\时,值为正,方向是/时,值为负
		for(i = 0;i < theApp.nCntLinePts - 1;++i)            //防止数组越界,theApp.linePoints[theApp.nCntLinePts][1]
			if(theApp.linePoints[i + 1][1] != theApp.linePoints[i][1])
				deltaX += (theApp.linePoints[i][0]          //deltaX--平均斜率 deltaX   =x/y  right
				- theApp.linePoints[i + 1][0]) / (theApp.linePoints[i + 1][1]  //*5-->删去
				- theApp.linePoints[i][1]);

		theApp.deltaAngle = deltaX / (theApp.nCntLinePts - 1);    //theApp.deltaAngle>0表明斜率为正，白线中线沿左下角到右上角的直线
																  //theApp.deltaAngle<0表明斜率为负，白线中线右下角左上角的直线

		// 公式：(centerX-X)/(YROBOT-centerY)=deltaAngle/5
		theApp.deltaLength = XROBOT - (centerX - (YROBOT - centerY) * theApp.deltaAngle ); //   /5-->删去
																	//theApp.deltaLength>0表明白线区域中心在机器人左边，否则在机器人右边
		return TRUE;
	}
	
	return FALSE;
}

/*================================================================================
*函数名:		void SobelEdgeDection()
*函数参数:		无
*函数功能描述:  sobel边缘检测算法,在形状识别中用到
				具体算法思想参见FAQ-->sobel边缘检测算法的思想是什么？				
*返回值:	    void	
*==================================================================================*/
void SobelEdgeDection()
{

	//边缘检测卷积核,对水平方向边缘响应最大
	int Gx[9];
	Gx[0]=-1;
	Gx[1]=0;
	Gx[2]=1;
	Gx[3]=-2;
	Gx[4]=0;
	Gx[5]=2;
	Gx[6]=-1;
	Gx[7]=0;
	Gx[8]=1;
	//边缘检测卷积核,对垂直方向边缘响应最大
	int Gy[9];
	Gy[0]=1;
	Gy[1]=2;
	Gy[2]=1;
	Gy[3]=0;
	Gy[4]=0;
	Gy[5]=0;
	Gy[6]=-1;
	Gy[7]=-2;
	Gy[8]=-1;
	

	int gradSum[WIDTH*HEIGHT]={0};
	int i,j;
	int Gradx,Grady;                       //注意类型转换
	for(i=1;i<HEIGHT-1;i++)
		for(j=1;j<WIDTH-1;j++)
		{
			Gradx=Gx[0]*theApp.tempColor[(i-1 ) * WIDTH * 3 + (j-1) * 3 + 0]
				+Gx[1]*theApp.tempColor[(i-1 ) * WIDTH * 3 + j * 3 + 0]
				+Gx[2]*theApp.tempColor[(i-1 ) * WIDTH * 3 + (j+1) * 3 + 0]
				+Gx[3]*theApp.tempColor[( i ) * WIDTH * 3 + (j-1) * 3 + 0]
				+Gx[4]*theApp.tempColor[( i ) * WIDTH * 3 + j * 3 + 0]
				+Gx[5]*theApp.tempColor[(i ) * WIDTH * 3 + (j+1) * 3 + 0]
				+Gx[6]*theApp.tempColor[(i+1 ) * WIDTH * 3 + (j-1) * 3 + 0]
				+Gx[7]*theApp.tempColor[(i+1) * WIDTH * 3 + j * 3 + 0]
				+Gx[8]*theApp.tempColor[(i+1 ) * WIDTH * 3 + (j+1) * 3 + 0];
			Grady=Gy[0]*theApp.tempColor[(i-1 ) * WIDTH * 3 + (j-1) * 3 + 0]
				+Gy[1]*theApp.tempColor[(i-1 ) * WIDTH * 3 + j * 3 + 0]
				+Gy[2]*theApp.tempColor[(i-1 ) * WIDTH * 3 + (j+1) * 3 + 0]
				+Gy[3]*theApp.tempColor[( i ) * WIDTH * 3 + (j-1) * 3 + 0]
				+Gy[4]*theApp.tempColor[( i ) * WIDTH * 3 + j * 3 + 0]
				+Gy[5]*theApp.tempColor[(i ) * WIDTH * 3 + (j+1) * 3 + 0]
				+Gy[6]*theApp.tempColor[(i+1 ) * WIDTH * 3 + (j-1) * 3 + 0]
				+Gy[7]*theApp.tempColor[(i+1) * WIDTH * 3 + j * 3 + 0]
				+Gy[8]*theApp.tempColor[(i+1 ) * WIDTH * 3 + (j+1) * 3 + 0];
			
			gradSum[( i ) * WIDTH  + j]=abs(Gradx)+abs(Grady);
				
		}
		for(i=1;i<HEIGHT-1;i++)
			for(j=1;j<WIDTH-1;j++)
			if(gradSum[( i ) * WIDTH  + j]>35)                 //35--边缘检测阈值,可根据需要改变
			{
				theApp.tempColor[( i ) * WIDTH * 3 + j * 3 + 0]=0;
				theApp.tempColor[( i ) * WIDTH * 3 + j * 3 + 1]=0;
				theApp.tempColor[( i ) * WIDTH * 3 + j * 3 + 2]=0;
			}
			else
			{
				theApp.tempColor[( i ) * WIDTH * 3 + j * 3 + 0]=255;
				theApp.tempColor[( i ) * WIDTH * 3 + j * 3 + 1]=255;
				theApp.tempColor[( i ) * WIDTH * 3 + j * 3 + 2]=255;
			}
	
}

/*================================================================================
*函数名:		void GrayScale()
*函数参数:		无
*函数功能描述:  彩色图像灰度化,在形状识别中用到
*返回值:	    void	
*==================================================================================*/
void GrayScale()
{
	BYTE r, g, b,gray;
	for(int i=0;i<HEIGHT;i++)
		for(int j=0;j<WIDTH;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];

			gray = BYTE(0.299 * r + 0.587 * g + 0.114 * b);          //转化公式   

			theApp.tempColor[(HEIGHT-i-1) * WIDTH * 3 + j * 3 + 2]=gray;
			theApp.tempColor[(HEIGHT-i-1) * WIDTH * 3 + j * 3 + 1]=gray;
			theApp.tempColor[(HEIGHT-i-1) * WIDTH * 3 + j * 3 + 0]=gray;
		}
}
/*================================================================================
*函数名:		void FindOutObstacle()
*函数参数:		无
*函数功能描述:  发现障碍物后的处理函数
*返回值:	    void	
*==================================================================================*/
//处理策略是随机向左旋转90后前行或向右旋转90后前行
void FindOutObstacle()
{
	srand((unsigned)time(NULL));      
	int nRand=rand()%2;
	if(nRand == 1)
		theApp.nMotiontype = TURNLEFT90ANDRUN;     //向左旋转90后前行
	else
		theApp.nMotiontype = TURNRIGHT90ANDRUN;    //向右旋转90后前行

	Motion();
	return;

}

/*================================================================================
*函数名:		void FindOutCornerEdge()
*函数参数:		无
*函数功能描述:  发现拐角处边缘后的处理函数
*返回值:	    void	
*==================================================================================*/
void FindOutCornerEdge()
{
	if((theApp.bLeftEdge == TRUE) && (theApp.bRightEdge == TRUE))
		if(theApp.nLastMotiontype != STOP)						
			theApp.nMotiontype = STOP;
		else
			theApp.nMotiontype = ROTATION180ANDRUN;         
		
	Motion();
	return;

}
/*================================================================================
*函数名:		void FindOutEdge()
*函数参数:		无
*函数功能描述:  发现除拐角处边缘以外的边缘后处理函数
*返回值:	    void	
*==================================================================================*/
void FindOutEdge()
{
	if(((theApp.bTopEdge == TRUE) && (theApp.bLeftEdge == TRUE)) 
		|| ((theApp.bBottomEdge == TRUE) && (theApp.bLeftEdge == TRUE)))			//(左边缘和上边缘) 或 (左边缘和下行边缘) 同时在机器人视野
	{	
		if(theApp.nLastMotiontype != STOP)						
			theApp.nMotiontype = STOP;
		else
			theApp.nMotiontype = TURNRIGHT90ANDRUN;								//向右旋转	
	}
	else if(((theApp.bTopEdge == TRUE) && (theApp.bRightEdge == TRUE)) 
		|| ((theApp.bBottomEdge == TRUE) && (theApp.bRightEdge == TRUE))) 	//(右边缘和上边缘) 或 (右边缘和下行边缘)同时在机器人视野
	{	
		if(theApp.nLastMotiontype != STOP)
			theApp.nMotiontype = STOP;
		else
			theApp.nMotiontype = TURNLEFT90ANDRUN;								//向左旋转	
	}
	else if((theApp.bTopEdge == TRUE)									//机器若遇到了上行边缘或下行边缘
		|| (theApp.bBottomEdge == TRUE))
	{	
		if(( theApp.deltaxyEdgeLine >= XROBOT*2/30) && (theApp.deltaxyEdgeLine <= -XROBOT*2/30))					//行边缘近似是水平线
		{		         
			if(theApp.nLastMotiontype != STOP)
				theApp.nMotiontype = STOP;									//只是上边缘
			else
			{
				srand((unsigned)time(NULL));							
				int nRand=rand()%2;
				if(nRand==1)
					theApp.nMotiontype = TURNLEFT90ANDRUN;					//向左旋转90度
				else
					theApp.nMotiontype = TURNRIGHT90ANDRUN;					//向右旋转90度
			}				
    	
		}
		else if((theApp.deltaxyEdgeLine >- XROBOT*2/30)				
			&& (theApp.deltaxyEdgeLine <- (XROBOT*2)/(YROBOT/2)))
			{
			if(theApp.nLastMotiontype != STOP)
			theApp.nMotiontype = STOP;
			else
			theApp.nMotiontype = TURNLEFT;								//向左旋转
			}
	
		else if((theApp.deltaxyEdgeLine < XROBOT*2/30)			
			&& (theApp.deltaxyEdgeLine > (XROBOT*2)/(YROBOT/2)))
			{
			if(theApp.nLastMotiontype != STOP)
				theApp.nMotiontype = STOP;
			else
				theApp.nMotiontype = TURNRIGHT;							//向右旋转	
			}
			
	}
	else if(theApp.bLeftEdge == TRUE)
		{

		if(theApp.deltaxyEdgeColumn > XROBOT/YROBOT)                  
		{
				if( theApp.nLastMotiontype != STOP)
					theApp.nMotiontype = STOP;			
				else if(theApp.nLastMotiontype == STOP)				
					theApp.nMotiontype = TURNRIGHT;

		}
		else if((theApp.deltaxyEdgeColumn >= 30/YROBOT) 
				&& (theApp.deltaxyEdgeColumn <= XROBOT/YROBOT))			//左列边缘 /，斜率大于某个值
			{
				if( theApp.nLastMotiontype != STOP)
					theApp.nMotiontype = STOP;			
				else if(theApp.nLastMotiontype == STOP)				
					theApp.nMotiontype = TURNRIGHT;			
	

			}
			else if((theApp.deltaxyEdgeColumn <30 /YROBOT)
				&& (theApp.deltaxyEdgeColumn >= 0))
			{

				if(theApp.nLastMotiontype != STOP)
						theApp.nMotiontype = STOP;
				else														
					theApp.nMotiontype = FORWARD;				
			
			}
		}
	else if(theApp.bRightEdge == TRUE)
		{
			
		if(theApp.deltaxyEdgeColumn <- XROBOT/YROBOT)                
		{
				if( theApp.nLastMotiontype != STOP)
					theApp.nMotiontype = STOP;			
				else if(theApp.nLastMotiontype == STOP)					//这几行可能存在问题	
					theApp.nMotiontype = TURNLEFT;

		}
		else if((theApp.deltaxyEdgeColumn <= -30/YROBOT)
				&& (theApp.deltaxyEdgeColumn >=-XROBOT/YROBOT))  //左列边缘 /，斜率大于某个值
			{
				if(theApp.nLastMotiontype != STOP)			
					theApp.nMotiontype = STOP;				
				else if(theApp.nLastMotiontype == STOP)						//可能存在问题	
					theApp.nMotiontype = TURNLEFT;				

			}
			else if((theApp.deltaxyEdgeColumn >-30/YROBOT)
				&& (theApp.deltaxyEdgeColumn <=0))
			{
				if(theApp.nLastMotiontype != STOP)
					theApp.nMotiontype = STOP;	
				else 					
					theApp.nMotiontype = FORWARD;			
			                                      
			}
		}
		
		Motion();
		return;
}
/*================================================================================
*函数名:		void NotFindOutEdge()
*函数参数:		无
*函数功能描述:  未发现边缘处理函数
*返回值:	    void	
*==================================================================================*/
//处理策略是60%(3/5=0.6)的可能向前直走,20%(1/5=0.2)的可能向左转,20%(1/5=0.2)的可能向右转
void NotFindOutEdge()
{
	     
	    srand((unsigned)time(NULL));   
		int nRand = rand()%5;
		switch( nRand )
		{
		case 0:
		case 1:
		case 2:
			theApp.nMotiontype = FORWARD;   
			break;
		case 3:
			theApp.nMotiontype = TURNLEFT;   
			break;
		case 4:
			theApp.nMotiontype = TURNRIGHT; 
			break;
		}
		Motion();
		return;
}

/*================================================================================
*函数名:		void DecisionSearchObject()
*函数参数:		无
*函数功能描述:  寻找目标决策函数
*返回值:	    void	
*==================================================================================*/
void DecisionSearchObject()					//先后处理顺序
{

	SearchObjectAndObstacle();              //搜索目标和障碍物
	if(theApp.bFlagObject == TRUE)          //机器人视野范围内同时有障碍物和目标或边缘的处理原则
		return;								//只要发现目标物，则立即停止机器人.策略：发现目标物优先响应
			
		
	if(theApp.bFlagObstacle==TRUE)          //如果遇到障碍物则执行FindOutObstacle()
		FindOutObstacle();

//	SearchCornerEdge();
//		if((theApp.bLeftEdge == TRUE) && (theApp.bRightEdge == TRUE))
//		{
//		FindOutCornerEdge();
//		return;
//		}
	SearchEdge();							//搜索边缘
	if(theApp.bFlagEdge==TRUE)				//如果遇到边缘则执行FindOutEdge()
		FindOutEdge();
	else
		NotFindOutEdge();						//如果未遇到障碍物则执行NotFindEdge()
	
}