avoiddlg.cpp

上海广茂达as-r机器人避障程序

// AVOIDDlg.cpp : implementation file
//以下为加入跟踪后的源程序，注释加在跟踪程序段CAVOIDDlg::TraceDecide

#include "stdafx.h"
#include "ASRSystem.h"
#include "AVOID.h"
#include "AVOIDDlg.h"
#include <math.h>

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
const int MAX_PSD = 4;
const int MAX_Sonar = 5;

/////////////////////////////////////////////////////////////////////////////
// CAboutDlg dialog used for App About
class CAboutDlg : public CDialog
{
public:
	CAboutDlg();

// Dialog Data
	//{{AFX_DATA(CAboutDlg)
	enum { IDD = IDD_ABOUTBOX };
	//}}AFX_DATA

	// ClassWizard generated virtual function overrides
	//{{AFX_VIRTUAL(CAboutDlg)
	protected:
	virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV support
	//}}AFX_VIRTUAL

// Implementation
protected:
	//{{AFX_MSG(CAboutDlg)
	//}}AFX_MSG
	DECLARE_MESSAGE_MAP()
};

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

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

BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
	//{{AFX_MSG_MAP(CAboutDlg)
		// No message handlers
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CAVOIDDlg dialog

CAVOIDDlg::CAVOIDDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CAVOIDDlg::IDD, pParent)
{
	//{{AFX_DATA_INIT(CAVOIDDlg)
	m_dSafeDis = 60;
	m_dTurnSpeed = 0.1;
	m_dRunSpeed = 0.1;
	m_dDcc = 1.0;
	// NOTE: the ClassWizard will add member initialization here
	//}}AFX_DATA_INIT
	// Note that LoadIcon does not require a subsequent DestroyIcon in Win32
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
	m_pRobot = NULL;
	m_pMotion = NULL;
	m_pPsd = NULL;
	m_pSonar= NULL;
	m_pImageCap=NULL;
	m_pImageProcess=NULL;
	
}
CAVOIDDlg::~CAVOIDDlg()
{
}
void CAVOIDDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CAVOIDDlg)
	DDX_Control(pDX, IDC_PSD_GRAPH, m_graph);
	// NOTE: the ClassWizard will add DDX and DDV calls here
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CAVOIDDlg, CDialog)
	//{{AFX_MSG_MAP(CAVOIDDlg)
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_WM_DESTROY()
	ON_BN_CLICKED(IDC_RUN, OnRun)
	ON_BN_CLICKED(IDC_STOP, OnStop)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CAVOIDDlg message handlers

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

	// Add "About..." menu item to system menu.

	// IDM_ABOUTBOX must be in the system command range.
	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);
		}
	}

	// Set the icon for this dialog.  The framework does this automatically
	//  when the application's main window is not a dialog
	SetIcon(m_hIcon, TRUE);			// Set big icon
	SetIcon(m_hIcon, FALSE);		// Set small icon
	
	// TODO: Add extra initialization here
	if( !InitEnv() )
	{
		PostQuitMessage(0);
		return FALSE;
	}
	return TRUE;  // return TRUE  unless you set the focus to a control
}

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

// If you add a minimize button to your dialog, you will need the code below
//  to draw the icon.  For MFC applications using the document/view model,
//  this is automatically done for you by the framework.

void CAVOIDDlg::OnPaint() 
{
	if (IsIconic())
	{
		CPaintDC dc(this); // device context for painting

		SendMessage(WM_ICONERASEBKGND, (WPARAM) dc.GetSafeHdc(), 0);

		// Center icon in client rectangle
		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;

		// Draw the icon
		dc.DrawIcon(x, y, m_hIcon);
	}
	else
	{
		CDialog::OnPaint();
	}
}


// The system calls this to obtain the cursor to display while the user drags
//  the minimized window.
HCURSOR CAVOIDDlg::OnQueryDragIcon()
{
	return (HCURSOR) m_hIcon;
}

BOOL CAVOIDDlg::InitEnv() 
{
	//build the robot,NOTE:a process can only get a instance via  GetInstance method 
	//even if you call GetInstance method more than once 
	//CFollowCommand m_pCommand;
	m_pRobot = CASRSystem::GetInstance();
	IASRDevice* pDevice = NULL;
	IASRProcess* pProcess = NULL;
	HERROR hres;
    
	PROCESS_DESC proc_desc;
    proc_desc.nType = SEQUENCE;
    m_pRobot->CreateProcess(proc_desc,pProcess); 
	
	hres = m_pRobot->CreateDevice("RTX_MOTION",pDevice);
	if(ASRFAIL(hres))
	{
		return FALSE;
	}
	hres = pDevice->QueryInterface(ASR_IID_MOTION_CONTROL,(LPVOID*)&m_pMotion);
	if(ASRFAIL(hres))
	{
		return FALSE;
	}

	hres = m_pRobot->CreateDevice("SENSOR_PSD",pDevice);
	if(ASRFAIL(hres))
	{
		return FALSE;
	}
	hres = pDevice->QueryInterface(ASR_IID_SENSOR_PSD,(LPVOID*)&m_pPsd);
	if(ASRFAIL(hres))
	{
		return FALSE;
	} 
	
	hres = m_pRobot->CreateDevice("SENSOR_SONAR",pDevice);
	if(ASRFAIL(hres))
	{
		return FALSE;
	}
	hres = pDevice->QueryInterface(ASR_IID_SENSOR_SONAR,(LPVOID*)&m_pSonar);
	if(ASRFAIL(hres))
	{
		return FALSE;
	} 
        
   
	hres =m_pRobot->CreateDevice("IMAGE_CAP_DS",pDevice); 
	if(ASRFAIL(hres))
	{
		return FALSE;
	}
     RequestInterface(pDevice,ASR_IID_IMAGECAPTURE,m_pImageCap); 
	 CheckPtr(m_pImageCap,FALSE);
	 m_pRobot->AddToProcess(pProcess,pDevice); 
	 CWnd* gWnd = (CWnd*)GetDlgItem(IDC_VIDEO);
     m_pImageCap->BindGUIWnd(gWnd->GetSafeHwnd());  
	 m_pImageCap->BindIndex(0); 
             

	hres =m_pRobot->CreateDevice("ASR_IMAGE_PROCESS",pDevice);//创建图像处理设备
    if(ASRFAIL(hres))
	{
		return FALSE;
	} 
	IASRImage* pImage = NULL;
	RequestInterface(pDevice,ASR_IID_IMAGE,pImage);
	CheckPtr(pImage,FALSE);
	m_pRobot->AddToProcess(pProcess,pDevice);
	
	IMAGEPROC_DESC ImageDesc;
	ImageDesc.nHeight = 640;
	ImageDesc.nWidth = 480;
	pImage->SetParam(ImageDesc);
    pImage->Release();
	RequestInterface(pDevice,ASR_IID_FOOTBALL,m_pImageProcess);
    CheckPtr(m_pImageProcess,FALSE);
	IASRStreamLocal* pStream =  pDevice->GetLocalStream();
	pStream->BindCommand(&m_pFollowCommand);
	
    

	        



	PSD_DESC psd_desc;
	psd_desc.nIndex = PSD_1 | PSD_2 | PSD_3 | PSD_4;
	psd_desc.nTime =  50;
	m_pPsd->SetParam(psd_desc);
	m_pPsd->SetCallback(PsdProc,(LONG)this);
	
	SONAR_DESC sonar_desc;
	sonar_desc.nMaxRange = 7;
	sonar_desc.nTime =  50;
	strcpy(sonar_desc.szMode,"1,2;3;4,5");
	m_pSonar->SetParam(sonar_desc);
	m_pSonar->SetCallback(SonarProc,(LONG)this);     

	//设置设备参数信息

    
	
	if( ASRFAIL(m_pImageCap->Init()))
	{
     return FALSE;                  
	}
    //启动视频采集

    if( ASRFAIL(m_pImageCap->Start()))
	{
     return FALSE;                  
	}
	
   	if( ASRFAIL(m_pMotion->Init()))
	{
     return FALSE;                  
	}
	if( ASRFAIL(m_pMotion->Start()) )
	{
		return FALSE;
	}
	
	if( ASRFAIL(m_pPsd->Init()) )
	{
		return FALSE;
	}
	
	if( ASRFAIL(m_pPsd->Start()) )
	{
		return FALSE;
	}

	if( ASRFAIL(m_pSonar->Init()) )
	{
		return FALSE;
	}
	
	if( ASRFAIL(m_pSonar->Start()) )
	{
		return FALSE;
	}
    
	bRun =FALSE;
	return TRUE;
}

void CAVOIDDlg::EndEnv()
{
	if( m_pPsd )
	{ 
		m_pPsd->End();
		ReleaseInterface(m_pPsd);
	}
     
	if( m_pSonar )
	{   m_pSonar->End();
		ReleaseInterface(m_pSonar);
	}

	if( m_pImageCap )

    {    m_pImageCap->End();