circledlg.cpp

兄弟编的程序

// circleDlg.cpp : implementation file
//

#include "stdafx.h"
#include "ASRSystem.h"
#include "circle.h"
#include "circleDlg.h"
#include <math.h>
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

const int MAX = 5;
/////////////////////////////////////////////////////////////////////////////
// CAboutDlg dialog used for App About

class CAboutDlg : public CDialog
{
	//CASRSystem* m_pRobot;
	//IMotionControl* m_pMotion;
	//IASRSensorPsd* m_pPsd;
	//IASRSensorSonar* m_pSonar;
	//void Move1( double dData[],int );
    //void Move2( double dData[],int );
	//static unsigned __stdcall PsdProc(LONG nOwner,void* lpParam);
    //static unsigned __stdcall SonarProc(LONG nOwner,void* lpParam);
	//BOOL InitEnv();
	//void EndEnv();
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()

/////////////////////////////////////////////////////////////////////////////
// CCircleDlg dialog

CCircleDlg::CCircleDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CCircleDlg::IDD, pParent)
{
	//{{AFX_DATA_INIT(CCircleDlg)
		// 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;

}

void CCircleDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CCircleDlg)
		// NOTE: the ClassWizard will add DDX and DDV calls here
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CCircleDlg, CDialog)
	//{{AFX_MSG_MAP(CCircleDlg)
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_BN_CLICKED(IDC_BUTTON1, OnButton1)
	ON_WM_DESTROY()
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CCircleDlg message handlers

BOOL CCircleDlg::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 CCircleDlg::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 CCircleDlg::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();
	}
}

BOOL CCircleDlg::InitEnv()
{
	m_pRobot = CASRSystem::GetInstance();
	
	HERROR hres;
	IASRDevice* pDevice = NULL;
	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("RTX_MOTION",pDevice);
	if(ASRFAIL(hres))
	{
		return FALSE;
	}
	hres = pDevice->QueryInterface(ASR_IID_MOTION_CONTROL,(LPVOID*)&m_pMotion);
	if(ASRFAIL(hres))
	{
		return FALSE;
	}

	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 desc;
	strcpy(desc.szMode,"1,3,5");        //使用1,3,5路超声
//	desc.nIndex = 0;
    desc.nMaxRange= 7;                //设置最大工作范围
    desc.nTime= 50;                   //设置采样周期
    m_pSonar->SetParam(desc);        //设置设备参数信息
    m_pSonar->SetCallback(SonarProc,(LONG)this); //设置回调函数及回调参数

	if( ASRFAIL(m_pMotion->Init()) )
	{
		AfxMessageBox("运动端口无效或被其他程序占用"); 
		return FALSE;
	}
	if( ASRFAIL(m_pMotion->Start()) )
	{
        
		return FALSE;
	}

	if( ASRFAIL(m_pSonar->Init()) )
	{
		AfxMessageBox("声纳端口无效或被其他程序占用"); 
		return FALSE;
	}
	if( ASRFAIL(m_pSonar->Start()) )
	{
		return FALSE;
	}

	
	if( ASRFAIL(m_pPsd->Init()) )
	{
		AfxMessageBox("红外端口无效或被其他程序占用"); 
		return FALSE;
	}
	if( ASRFAIL(m_pPsd->Start()) )
	{
		return FALSE;
	}


	
	
	return TRUE;

}

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

    if( m_pSonar )
	{ 
		m_pSonar->End();
		ReleaseInterface(m_pSonar);
	}

	if( m_pMotion )
	{
		m_pMotion->Abort(WHEEL_LEFT);
		m_pMotion->Abort(WHEEL_RIGHT);

		m_pMotion->End();
		ReleaseInterface(m_pMotion);
	}

	if( m_pRobot )
	{
		m_pRobot->DestroyInstance();
		m_pRobot = NULL;
	}
}

void CCircleDlg::Move2( double DData[],int )
{
	if( !m_pMotion )
	{
		return;
	}

	if( DData[0]>0.65 )  
	{
		m_pMotion->Pos(WHEEL_LEFT,1.0,-0.2);             //向着障碍物 转动
		m_pMotion->Pos(WHEEL_RIGHT,1.0,0.2);
	}

	else
	{
		m_pMotion->Move(WHEEL_LEFT,0.1);              //沿障碍物边缘前进      
		m_pMotion->Move(WHEEL_RIGHT,0.1);
	}
}

void CCircleDlg::Move1( double dData[],int )
{
	if( !m_pMotion )
	{
		return;
	}

	if( dData[0]<20||dData[1]<15 )  
	{
		m_pMotion->Pos(WHEEL_LEFT,1.0,0.2);             //背着障碍物 转动 
		m_pMotion->Pos(WHEEL_RIGHT,1.0,-0.2);
	}

//	else  if(dData[1]<15||dData[2]<15)
//	{
//		m_pMotion->Move(WHEEL_LEFT,0.3);                 
//		m_pMotion->Move(WHEEL_RIGHT,-0.3);
//	}
}


unsigned __stdcall CCircleDlg::PsdProc(LONG nOwner,void* lpParam)
{
	float* pValue1 = (float*)lpParam;
	CCircleDlg* pThis = (CCircleDlg*)nOwner;
	double dPsdData[MAX];
	memset(dPsdData,0,sizeof(dPsdData));

	for( int i = 0;i<MAX;i++ )
	{
		dPsdData[i] = pValue1[i]>80?80:pValue1[i];
	}

	//pThis->m_graph.SetDataP(dsdData,sizeof(dPsdData));
	pThis->Move1(dPsdData,MAX);

	return 0;
}

unsigned __stdcall CCircleDlg::SonarProc(LONG nOwner,void* lpParam)
{
	float* pValue2 = (float*)lpParam;
	CCircleDlg* pThis = (CCircleDlg*)nOwner;
	double dSonarData[MAX];
	memset(dSonarData,0,sizeof(dSonarData));

	for( int i = 0;i<MAX;i++ )
	{
		dSonarData[i] = pValue2[i]<0?0:pValue2[i];
	}

	//pThis->m_graph.SetDataP(dsdData,sizeof(dPsdData));
	pThis->Move2(dSonarData,MAX);

	return 0;
}

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

void CCircleDlg::OnButton1() 
{
	// TODO: Add your control notification handler code here
	if(m_pMotion)
	{
		m_pMotion->SetACC(WHEEL_LEFT,0.1);
		m_pMotion->SetACC(WHEEL_RIGHT,0.1);
	}
}



void CCircleDlg::OnDestroy() 
{
	EndEnv();
	CDialog::OnDestroy();
	
	// TODO: Add your message handler code here
	
}