demo.cpp

来自「足球机器人功能demo」· C++ 代码 · 共 427 行

// Demo.cpp: implementation of the CDemo class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Demo.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CDemo::CDemo()
{
	m_nDistance = 30;
	m_BehShow = NULL;
	showcount = 0;
	Active = FALSE;
	pos = 0;

	m_CV.SetParam(&m_para);
}

CDemo::~CDemo()
{

}

void CDemo::AfterUpdateInfrared(UCHAR *Infrared,BOOL *EnableIR,UINT nState)
{
	//红外信号更新

}

double FindMinUSonic (DOUBLE *Ultrasonic,unsigned char StartNum,unsigned char EndNum)
{
	unsigned char i = StartNum;
	double TempDouble = 1000;

	if(StartNum<=EndNum)
	{
		for(i=StartNum;i<=EndNum;i++)
		{
			if(TempDouble > Ultrasonic[i] )	
			TempDouble = Ultrasonic[i];
		}
	}
	else
	{
		for(i=StartNum;i<=23;i++)
		{
			if(TempDouble > Ultrasonic[i] )	
			TempDouble = Ultrasonic[i];
		}

		for(i=0;i<=EndNum;i++)
		{
			if(TempDouble > Ultrasonic[i] )	
			TempDouble = Ultrasonic[i];
		}
	}

	

	return TempDouble;
}

void CDemo::AfterUpdateUSonic(DOUBLE *Ultrasonic,BOOL *EnableUS,UINT nState)
{/*
	unsigned int RoboState = 0,SpeedTurnVp = 1000,SpeedForthVp = 3000;

	signed int TempSpeedForth=0,TempSpeedTurn=0;
	signed int MaxSpeedForth = 2000,MaxSpeedTurn = 800;

	double TempMinDistance;

	double distance_F = 0.5;
	double distance_F_Max = 0.3;
	double distance_F_Nin = 0.3;
	double distance_L = 0.5;
	double distance_R = 0.5;

	//超声信号更新

	if (		//前方有障碍物
			Ultrasonic[0] < distance_F || 
			Ultrasonic[1] < distance_F || 
			Ultrasonic[2] < distance_F || 
			Ultrasonic[20] < distance_F || 
			Ultrasonic[21] < distance_F || 
			Ultrasonic[22] < distance_F || 
			Ultrasonic[23] < distance_F
			)
		{
		
			//判断较好的转动方向
			if (m_bFindDirection(Ultrasonic,EnableUS))
			{
				TempSpeedForth = 0;
				TempSpeedTurn = MaxSpeedTurn;
				
			}
			else
			{
				TempSpeedForth = 0;
				TempSpeedTurn = 0-MaxSpeedTurn;
			}

			if(nState == RIGHT) 
			{
				TempSpeedForth = 0;
				TempSpeedTurn = MaxSpeedTurn;
			}
			else if(nState == LEFT) 
			{
				TempSpeedForth = 0;
				TempSpeedTurn = 0-MaxSpeedTurn;
			}

			m_pCmd->SetBothMotorsSpeed(TempSpeedForth+TempSpeedTurn,
										TempSpeedForth-TempSpeedTurn);

			//前方distance距离内有障碍物，进入转向状态
			m_strShow.Format("前方障碍物 : %d,%d",TempSpeedForth+TempSpeedTurn,TempSpeedForth-TempSpeedTurn);
			output(&m_strShow);
		}
	else if(	//左方可能有障碍物干扰
			 
			Ultrasonic[19] < distance_L || 
			Ultrasonic[18] < distance_L 
			)
		{		
			TempMinDistance = FindMinUSonic (Ultrasonic,18,19);
			TempSpeedTurn = (signed int)((distance_L-TempMinDistance)*SpeedTurnVp);

			if(TempSpeedTurn > MaxSpeedTurn)
				TempSpeedTurn = MaxSpeedTurn/2;
			
			TempMinDistance = FindMinUSonic (Ultrasonic,22,0);

			TempSpeedForth = (signed int)((TempMinDistance-distance_F_Nin)*SpeedForthVp);
			if(TempSpeedForth > MaxSpeedForth)
				TempSpeedForth = MaxSpeedForth;

			m_pCmd->SetBothMotorsSpeed(TempSpeedForth+TempSpeedTurn,
										TempSpeedForth-TempSpeedTurn);

			//前方distance距离内有障碍物，进入转向状态
			m_strShow.Format("左有障碍 : %d,%d",TempSpeedForth+TempSpeedTurn,TempSpeedForth-TempSpeedTurn);
			output(&m_strShow);
			
		}

	else if(	//右方可能有障碍物干扰
			
			Ultrasonic[4] < distance_R || 
			Ultrasonic[5] < distance_R 
			)
		{
		
			
			TempMinDistance = FindMinUSonic (Ultrasonic,4,5);
			TempSpeedTurn = (signed int)((distance_R-TempMinDistance)*SpeedTurnVp);

			if(TempSpeedTurn > MaxSpeedTurn)
				TempSpeedTurn = MaxSpeedTurn/2;
			
			TempMinDistance = FindMinUSonic (Ultrasonic,22,1);

			TempSpeedForth = (signed int)((TempMinDistance-distance_F_Nin)*SpeedForthVp);
			if(TempSpeedForth > MaxSpeedForth)
				TempSpeedForth = MaxSpeedForth;

			m_pCmd->SetBothMotorsSpeed(TempSpeedForth-TempSpeedTurn,
										TempSpeedForth+TempSpeedTurn);

			//前方distance距离内有障碍物，进入转向状态
			m_strShow.Format("右有障碍 : %d,%d",TempSpeedForth-TempSpeedTurn,TempSpeedForth+TempSpeedTurn);
			output(&m_strShow);
			
		}
	else
	{
		TempMinDistance = FindMinUSonic (Ultrasonic,22,0);

		TempSpeedForth = (signed int)((TempMinDistance-distance_F_Nin)*SpeedForthVp);
		if(TempSpeedForth > MaxSpeedForth)
			TempSpeedForth = MaxSpeedForth;

		if(nState == LEFT || nState == RIGHT)
		{
			TempSpeedForth /=2;
		}

		TempSpeedTurn = 0;

		m_pCmd->SetBothMotorsSpeed(TempSpeedForth+TempSpeedTurn,
									TempSpeedForth-TempSpeedTurn);

		//前方distance距离内有障碍物，进入转向状态
			m_strShow.Format("无障前进 : %d,%d",TempSpeedForth+TempSpeedTurn,TempSpeedForth+TempSpeedTurn);
			output(&m_strShow);
	}
	
	
	



/*



	switch(nState)
	{
	case STOP:					//机器人处于停止状态
		//判断前方是否有障碍物
		if ( 
			Ultrasonic[0] < distance_L || 
			Ultrasonic[1] < distance_L || 
			Ultrasonic[2] < distance_L || 
			Ultrasonic[20] < distance_L || 
			Ultrasonic[21] < distance_L || 
			Ultrasonic[22] < distance_L || 
			Ultrasonic[23] < distance_L
			)
		{
			//前方distance距离内有障碍物，进入转向状态
			m_strShow.Format("前方障碍物");
			output(&m_strShow);

			//判断较好的转动方向
			if (m_bFindDirection(Ultrasonic,EnableUS))
			{
				m_pCmd->SetBothMotorsSpeed(100,-100);
			}
			else
			{
				m_pCmd->SetBothMotorsSpeed(-100,100);
			}
		}							
		else
		{
			
		}		
		break;
		
	case FORWARD:				//机器人处于往前直行状态
		//前方障碍物距离大于distance
		if (Ultrasonic[0] > distance && 
			Ultrasonic[1] > distance && 
			Ultrasonic[2] > distance && 
			Ultrasonic[20] > distance && 
			Ultrasonic[21] > distance && 
			Ultrasonic[22] > distance && 
			Ultrasonic[23] > distance)
		{
			break;
		}
		else
		{
			//前方有障碍物，根据障碍物情况选择转动方向
			m_strShow.Format("前方障碍物");
			output(&m_strShow);
			
			if (m_bFindDirection(Ultrasonic,EnableUS))
			{
				m_pCmd->SetBothMotorsSpeed(100,-100);
			}
			else
			{
				m_pCmd->SetBothMotorsSpeed(-100,100);
			}
		}
		break;

	case BACKWARD:				//机器人处于往后直行状态
		break;

	case RIGHT:					//机器人处于原地右转状态
	case RIGHTFRONT:			//机器人处于右转前行状态
		//break;

	case LEFT:					//机器人处于原地左转状态
	case LEFTFRONT:				//机器人处于左转前行状态
		if (
			Ultrasonic[20] > m_nDistance && 
			Ultrasonic[21] > m_nDistance && 
			Ultrasonic[22] > m_nDistance &&
			Ultrasonic[23] > m_nDistance && 
			Ultrasonic[1] > m_nDistance && 
			Ultrasonic[2] > m_nDistance && 
			Ultrasonic[3] > m_nDistance 	
			)
		{
			m_strShow.Format("前方无障碍物");
			output(&m_strShow);

			m_pCmd->SetBothMotorsSpeed(150,150);
		}
		break;

	default:					//机器人处于不明状态
		m_pCmd->Brake(1);
		nState = STOP;
		break;
	}
*/
}


void CDemo::AfterUpdateVideoSample(BYTE *pBuffer, long lWidth, long lHeight,double dbTime,UINT nState)
{
	if (pos != 1)
	{
		return;
	}

	//摄像头图像祯更新
	m_CV.ImageBuf(lWidth,lHeight,pBuffer);
	m_CV.FindSplash(TRUE);

	if (m_para.Uen == TRUE)
	{
		m_strShow.Format("U分量：(%d,%d):%d",m_para.Ux,m_para.Uy,m_para.Usum);
		output(&m_strShow);
	}

	if (m_para.Ven == TRUE)
	{
		m_strShow.Format("V分量：(%d,%d):%d",m_para.Vx,m_para.Vy,m_para.Vsum);
		output(&m_strShow);
	}

	if (m_para.Ysum == 0 && m_para.Usum == 0 && m_para.Vsum == 0)
	{	//针对某些线程阻塞的情况
		m_para.bBusy = FALSE;
	}
}

void CDemo::AfterSendCommand(BYTE *pBuffer, int iLength,UINT nState)
{
	//发送了新的指令
	
}

BOOL CDemo::m_bFindDirection(DOUBLE *Ultrasonic,BOOL *EnableUS)
{
	double distance = 0;	//用于计算最佳移动方向的中间变量
	int direction = 0;		//用于计算最佳方向的中间变量
	bool found = false;		//标记是否找到移动最佳方向
	for (int i=0;i<24;i++)	//循环扫描8个方向
	{
		if (Ultrasonic[i] == 120 && EnableUS[i]==true )	//如果有一个方向无障碍物，中断扫描，确定此方向为移动
		{
			direction = i;
			found = true;
			break;
		}
		if (Ultrasonic[i] > distance && EnableUS[i]==true)
		{
			distance = Ultrasonic[i];				//如果该方向距离比历史记录大，刷新历史记录
			direction = i;										//记录距离最大方向
			found = true;										//找到一个方向，做标记
		}
	}

	if (found == FALSE)
	{
		return true;
	}

	//if (dRightSum >= dLeftSum)
	if(direction < 12)
	{
		return true;
	}
	else
	{
		return false;
	}
}

void CDemo::AfterUpdateOverlook(BYTE *pBuffer, long lWidth, long lHeight, double dbTime, UINT nState)
{
	if (pos != 2)
	{
		return;
	}
		

	//全景摄像头图像祯更新
	m_CV.ImageBuf(lWidth,lHeight,pBuffer);
	m_CV.FindSplash(TRUE);

	if (m_para.Uen == TRUE)
	{
		m_strShow.Format("U分量：(%d,%d):%d",m_para.Ux,m_para.Uy,m_para.Usum);
		output(&m_strShow);
	}

	if (m_para.Ven == TRUE)
	{
		m_strShow.Format("V分量：(%d,%d):%d",m_para.Vx,m_para.Vy,m_para.Vsum);
		output(&m_strShow);
	}

	if (m_para.Ysum == 0 && m_para.Usum == 0 && m_para.Vsum == 0)
	{	//针对某些线程阻塞的情况
		m_para.bBusy = FALSE;
	}
}

void CDemo::output(CString *strShow)
{
	if (m_BehShow != NULL)
	{
		m_BehShow->AddString(*strShow);
		m_BehShow->SetCurSel(m_BehShow->GetCount()-1);
	}
}