upoverlookbeh.cpp

足球机器人功能demo

// UPOverLookBeh.cpp: implementation of the UPOverLookBeh class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "UP_BALLTRACK.h"
#include "UPOverLookBeh.h"

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

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

UPOverLookBeh::UPOverLookBeh()
{
	m_BehShow = NULL;
	m_ClrShow = NULL;
	m_pcatching = NULL;

	m_CV.SetParam(&m_para);
	m_para.ReSet();
	m_TargetDoor.ReSet();
}

UPOverLookBeh::~UPOverLookBeh()
{

}

void UPOverLookBeh::AfterUpdateOverlook(BYTE *pBuffer, long lWidth, long lHeight, double dbTime, UINT nState)
{
	//摄像头图像祯更新
	m_CV.ImageBuf(lWidth,lHeight,pBuffer);
	m_CV.FindSplash(TRUE);

	//像素太少，可认为丢失了目标
	if (m_para.Vsum < 10)
	{
		m_strShow.Format("(%d,%d)%d",m_para.Vx,m_para.Vy,m_para.Vsum);
		output(&m_strShow,m_ClrShow);
		m_strShow.Format("目标无效！");
		output(&m_strShow,m_BehShow);
		return;
	}

	//车体中心坐标
	int cen_x = lWidth/2;
	int cen_y = lHeight/2;
	int speed = 800;
	int TargetX = 430;
	int TargetY = 240;
	int range = 10;
	//int spk = 10;

	//正对目标判别条件
	if (10*abs(m_para.Vy-cen_y)/abs(m_para.Vx-cen_x) < 3)
	{
		/*if (m_pcatching != NULL)
		{
			m_pCmd->SetBehavior(m_pcatching);	
			m_strShow.Format("目标在前方,切换到前置摄像头");
			output(&m_strShow,m_BehShow);
		}
		else
		{
			m_pCmd->SetBothMotorsSpeed(30*abs(m_para.Vx-cen_x),30*abs(m_para.Vx-cen_x));
			m_strShow.Format("目标在前方");
			output(&m_strShow,m_BehShow);
		}*/
		if (m_para.Vx<TargetX+range && m_para.Vx>TargetX-range && m_para.Vy < TargetY+range && m_para.Vy > TargetY - range)
		{
			m_pCmd->Brake(1);
			m_strShow.Format("捕捉到目标！");
			output(&m_strShow,m_BehShow);
		} 
		else
		{
			m_pCmd->SetBothMotorsSpeed(30*abs(m_para.Vx-cen_x),30*abs(m_para.Vx-cen_x));
			m_strShow.Format("目标在前方");
			output(&m_strShow,m_BehShow);
		}
		return;
	}

	if (m_para.Vy-cen_y < 0)
	{	//球在车体右侧
		m_pCmd->SetBothMotorsSpeed(30*abs(m_para.Vy-cen_y),-abs(m_para.Vy-cen_y));
	}
	else
	{	//球在车体左侧
		m_pCmd->SetBothMotorsSpeed(-abs(m_para.Vy-cen_y),30*abs(m_para.Vy-cen_y));
	}
}

void UPOverLookBeh::AfterUpdateVideoSample(BYTE *pBuffer, long lWidth, long lHeight, double dbTime, UINT nState)
{
	
}

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