visiondemodlg.cpp

比赛足球机器人系统中的DCT追球程序

// VisionDemoDlg.cpp : 实现文件
//

#include "stdafx.h"
#include "VisionDemo.h"
#include "VisionDemoDlg.h"

#include ".\visiondemodlg.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

CWinThread* pThread=NULL;
bool threadRun=false;
int rowCenter,columnCenter;
UINT process(LPVOID pParam);
bool readImageCenter(char* filename,int& rowCenter,int&columnCenter);
void decitionMake(Ipm& ipm,COmniVision& vision,char* command);
// 用于应用程序“关于”菜单项的 CAboutDlg 对话框

class CAboutDlg : public CDialog
{
public:
	CAboutDlg();

// 对话框数据
	enum { IDD = IDD_ABOUTBOX };

	protected:
	virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV 支持

// 实现
protected:
	DECLARE_MESSAGE_MAP()
};

CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
{
}

void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
}

BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
END_MESSAGE_MAP()


// CVisionDemoDlg 对话框



CVisionDemoDlg::CVisionDemoDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CVisionDemoDlg::IDD, pParent)
{
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

void CVisionDemoDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
}

BEGIN_MESSAGE_MAP(CVisionDemoDlg, CDialog)
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	//}}AFX_MSG_MAP
	ON_BN_CLICKED(IDC_BUTTON_START, OnBnClickedButtonStart)
	ON_BN_CLICKED(IDC_BUTTON_STOP, OnBnClickedButtonStop)
	ON_WM_CLOSE()
END_MESSAGE_MAP()


// CVisionDemoDlg 消息处理程序

BOOL CVisionDemoDlg::OnInitDialog()
{
	CDialog::OnInitDialog();

	// 将\“关于...\”菜单项添加到系统菜单中。

	// IDM_ABOUTBOX 必须在系统命令范围内。
	ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
	ASSERT(IDM_ABOUTBOX < 0xF000);

	CMenu* pSysMenu = GetSystemMenu(FALSE);
	if (pSysMenu != NULL)
	{
		CString strAboutMenu;
		strAboutMenu.LoadString(IDS_ABOUTBOX);
		if (!strAboutMenu.IsEmpty())
		{
			pSysMenu->AppendMenu(MF_SEPARATOR);
			pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
		}
	}

	// 设置此对话框的图标。当应用程序主窗口不是对话框时，框架将自动
	//  执行此操作
	SetIcon(m_hIcon, TRUE);			// 设置大图标
	SetIcon(m_hIcon, FALSE);		// 设置小图标

	// TODO: 在此添加额外的初始化代码
	
	return TRUE;  // 除非设置了控件的焦点，否则返回 TRUE
}

void CVisionDemoDlg::OnSysCommand(UINT nID, LPARAM lParam)
{
	if ((nID & 0xFFF0) == IDM_ABOUTBOX)
	{
		CAboutDlg dlgAbout;
		dlgAbout.DoModal();
	}
	else
	{
		CDialog::OnSysCommand(nID, lParam);
	}
}

// 如果向对话框添加最小化按钮，则需要下面的代码
//  来绘制该图标。对于使用文档/视图模型的 MFC 应用程序，
//  这将由框架自动完成。

void CVisionDemoDlg::OnPaint() 
{
	if (IsIconic())
	{
		CPaintDC dc(this); // 用于绘制的设备上下文

		SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0);

		// 使图标在工作矩形中居中
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;

		// 绘制图标
		dc.DrawIcon(x, y, m_hIcon);
	}
	else
	{
		CDialog::OnPaint();
	}
}

//当用户拖动最小化窗口时系统调用此函数取得光标显示。
HCURSOR CVisionDemoDlg::OnQueryDragIcon()
{
	return static_cast<HCURSOR>(m_hIcon);
}

void CVisionDemoDlg::OnBnClickedButtonStart()
{
	// TODO: 在此添加控件通知处理程序代码
	if(threadRun==false){
		threadRun=true;
		pThread = AfxBeginThread(process,this);
	}
}

void CVisionDemoDlg::OnBnClickedButtonStop()
{
	// TODO: 在此添加控件通知处理程序代码
	threadRun=false;
	WaitForSingleObject(pThread,INFINITE);
	pThread=NULL;
}


void CVisionDemoDlg::OnClose()
{
	// TODO: 在此添加消息处理程序代码和/或调用默认值
	if(threadRun==true){
		threadRun=false;
		WaitForSingleObject(pThread,INFINITE);
		pThread=NULL;
	}

	CDialog::OnClose();
}

void CVisionDemoDlg::showImage(BYTE* buffer)
{

	CDC *pDC = GetDlgItem(IDC_STATIC_VIEW)->GetDC();
	CBitmap bitmap,*pOldBitmap;
	CDC MemDC;
	MemDC.CreateCompatibleDC(pDC);
	bitmap.CreateCompatibleBitmap(pDC, 320, 240);
	pOldBitmap = (CBitmap *)MemDC.SelectObject(&bitmap);

	//画框的笔
	CPen newPen(PS_SOLID,1,RGB(157,239,249));
	CPen * pOldPen = MemDC.SelectObject(&newPen);

	COLORREF colorRef=(COLORREF)RGB(255,0,0);

	//显示原始图像
	if(buffer != NULL ) {
		BITMAPINFOHEADER bih;
		memset( &bih, 0, sizeof( bih ) );
		bih.biSize = sizeof( bih );
		bih.biWidth = 320;
		bih.biHeight = 240;
		bih.biPlanes = 1;
		bih.biBitCount = 24;

		::StretchDIBits( 
			MemDC, 0, 0, 
			320, 240, 
			0, 0, 320,240, 
			buffer, 
			(BITMAPINFO*) &bih, 
			DIB_RGB_COLORS, 
			SRCCOPY );								
	}

	MemDC.SelectObject(pOldPen);
	pDC->BitBlt(0,0,320,240,&MemDC,0,0,SRCCOPY);

	pDC->SelectObject(pOldBitmap); 
	GetDlgItem(IDC_STATIC_VIEW)->ReleaseDC(pDC);	
}

// 显示原始图像，并在红色颜色的中心画交叉符号，并在静态框中显示坐标和其他信息
void CVisionDemoDlg::showInfo(COmniVision& vision,BYTE* buffer,char* command)
{

	CDC *pDC = GetDlgItem(IDC_STATIC_VIEW)->GetDC();
	CBitmap bitmap,*pOldBitmap;
	CDC MemDC;
	MemDC.CreateCompatibleDC(pDC);
	bitmap.CreateCompatibleBitmap(pDC, 320, 240);
	pOldBitmap = (CBitmap *)MemDC.SelectObject(&bitmap);

	//画框的笔
	CPen newPen(PS_SOLID,1,RGB(100,150,200));
	CPen * pOldPen = MemDC.SelectObject(&newPen);

	COLORREF colorRef=(COLORREF)RGB(255,0,0);

	//显示原始图像
	if(buffer != NULL ) {
		BITMAPINFOHEADER bih;
		memset( &bih, 0, sizeof( bih ) );
		bih.biSize = sizeof( bih );
		bih.biWidth = 320;
		bih.biHeight = 240;
		bih.biPlanes = 1;
		bih.biBitCount = 24;

		::StretchDIBits( 
			MemDC, 0, 0, 
			320, 240, 
			0, 0, 320,240, 
			buffer, 
			(BITMAPINFO*) &bih, 
			DIB_RGB_COLORS, 
			SRCCOPY );								
	}

	//显示被选中的红色点
		BYTE k ; //做为后面移位用
		k=1;//BYTE00000001
		for(int y=0; y<240; y++)  {
			for(int x=0; x<320; x++)  {
				if( (vision.getMap()[y*320 +x] & k) != 0 ) 
					MemDC.SetPixel( x, y, colorRef );	
			}
		}


	//画交叉符号
	int tx=0,ty=0;
	if(vision.ball_conf>0){  //注意图像左右相反。
		tx=-vision.ball_imagex+columnCenter;
		ty=-vision.ball_imagey+rowCenter;
		MemDC.MoveTo(tx-5,ty);
		MemDC.LineTo(tx+6,ty);
		MemDC.MoveTo(tx,ty-5);
		MemDC.LineTo(tx,ty+6);
	}
	


	MemDC.SelectObject(pOldPen);
	pDC->BitBlt(0,0,320,240,&MemDC,0,0,SRCCOPY);

	pDC->SelectObject(pOldBitmap); 
	GetDlgItem(IDC_STATIC_VIEW)->ReleaseDC(pDC);	

	//显示坐标
	pDC = GetDlgItem(IDC_STATIC)->GetDC();
	char s1[1000]="\0";
	char s2[1000]="\0";
	char s3[100]="\0";
	sprintf(s1,"ColumnCenter=%3d,RowCenter=%3d",columnCenter,rowCenter);
	sprintf(s2,"x=%3d,y=%3d",tx,ty);
	sprintf(s3,"vision X=%3d,Y=%3d",vision.ball_imagex,vision.ball_imagey);
	pDC->TextOut(0,20,s1);
	pDC->TextOut(0,40,s2);
	pDC->TextOut(0,60,s3);
	pDC->TextOut(0,80,command);

}

UINT process(LPVOID pParam){
	char* buffer=NULL;
	Camera cam;
	ColorLimit colorLimit;
	COmniVision vision;
	CVisionDemoDlg* dlg = (CVisionDemoDlg*)pParam;
	Ipm ipm;

	//摄像头初始化
	if(cam.Init(1,buffer)==false){
		MessageBox(NULL,"摄像头初始化失败",NULL,MB_OK);
		threadRun=false;
		return 0;
	}

	//从文件读取颜色阀值
	if(colorLimit.LoadConfig("colorLimit.rob")==false){
		MessageBox(NULL,"读取颜色阀值失败",NULL,MB_OK);
		threadRun=false;
		return 0;
	}

	//视觉处理类初始化
	if(vision.initialize("omniVisionConfig.cfg",320,240)!=true){
		MessageBox(NULL,"视觉处理初始化失败",NULL,MB_OK);
		threadRun=false;
		return 0;
	}

	//读取中心点
	readImageCenter("omniVisionConfig.cfg",rowCenter,columnCenter);


	//将第一套颜色阀值赋给视觉，即视觉软件中全维视觉的阀值
	vision.setThreshold(&colorLimit,0);

	//ipm读取配置文件
	if(!ipm.LoadConfig("ipm.cfg")){
		MessageBox(NULL,"ipm读取配置文件失败",NULL,MB_OK);
		threadRun=false;
		return 0;
	}

	//ipm初始化
	if(!ipm.initIpm()){
		MessageBox(NULL,"ipm初始化失败",NULL,MB_OK);
		threadRun=false;
		return 0;
	}

	//为图像缓冲分配空间
	buffer=new char[320*240*3];
	if(buffer==NULL){
		MessageBox(NULL,"分配空间失败",NULL,MB_OK);
		threadRun=false;
		return 0;
	}

	//循环捕捉图像，显示，处理
	while(threadRun){
		//捕捉图像
		cam.CaptureImage(buffer);

		//显示
		//dlg->showImage((BYTE*)buffer);


		//处理图像
		vision.process((BYTE*)buffer);

		//决策执行
		char command[10]="停止";
		decitionMake(ipm,vision,command);
		
		//显示图像,及处理结果信息
		dlg->showInfo(vision,(BYTE*)buffer,command);

	}
	
	
	ipm.quit();

	if(buffer!=NULL)
		delete buffer;

	return 1;
}

void decitionMake(Ipm& ipm,COmniVision& vision,char* command){
	if(vision.ball_conf<=0)//没有看到球，直接返回，仍旧执行上一次的指令
		return;
	double seta = atan2(-(float)vision.ball_imagey,(float)vision.ball_imagex);
	double pi = 3.1415;
	bool isTurning=false;//true代表转弯，false代表向前
	if(seta>=pi*4/9 && seta<=pi*5/9){//球在前方10度范围内 向前
		ipm.lv=25;
		ipm.rv=25;
		isTurning=false;
		strcpy(command,"向前");
	}else if(seta>=pi/3 && seta<=pi*2/3 && isTurning==false){//球在前方30度范围内，如果上一个状态是向前，则继续向前
		ipm.lv=20;
		ipm.rv=20;
		isTurning=false;
		strcpy(command,"向前");
	}else if(seta>-pi/2 && seta<pi*4/9){//向右
		ipm.lv=30;
		ipm.rv=-5;
		isTurning=true;
		strcpy(command,"右转");
	}else if(seta<-pi/2 || seta>pi*5/9){//向左
		ipm.lv=-5;
		ipm.rv=30;
		isTurning=true;
		strcpy(command,"左转");
	}

	if(seta>=pi/3 && seta<=pi*2/3 && vision.ball_imagey<0 && abs(vision.ball_imagey)<27){
		ipm.kickerOn();
		Sleep(200);
		ipm.kickerOff();
		//threadRun=false;//射门结束后停止
	}
	ipm.SendToIPM();	

}
bool readImageCenter(char* filename,int& rowCenter,int&columnCenter){//从全向配置文件中读取中心点位置
	FILE* fp=fopen(filename,"r");
	char s[100];
	if(fp==NULL)
		return false;
	for(int i=1;i<5;i++)
		fgets(s,99,fp);
	fgets(s,99,fp);
	sscanf(s,"r_center:(%d) c_center:(%d)",&rowCenter,&columnCenter);
	return true;
	
}