robotscout.cpp

基于vc 的环境下机器人自主避障的算法 图形处理 具有载物功能

//////////////////////////////////////////////////////////////////////
// MuRoS - Multi Robot Simulator
//
// Luiz Chaimowicz
// GRASP Lab. University of Pennsylvania
// VERLab - DCC - UFMG - Brasil
//
// RobotHolonomic.cpp: implementation of the CRobotHolonomic class.
//
// RobotScout.cpp: implementation of the CRobotScout class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "simulator.h"
#include "RobotScout.h"
#include "const.h"
#include <math.h>


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

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
IMPLEMENT_SERIAL(CRobotScout, CRobotNonHolonomic, 1 )


CRobotScout::CRobotScout()
{

}

CRobotScout::CRobotScout(double x, double y, double theta, short status, short id, CString name) : CRobotNonHolonomic(x, y, theta, status, id, name)
{
	m_radius = 20;
}

CRobotScout::~CRobotScout()
{
}

void CRobotScout::Update(CArray<CRobot*, CRobot*> *robots, double simTime, double dt, CBox* box, CMapPath *globalMap)
{
	CRobot::Update(robots, simTime, dt, box, globalMap);

	if (m_status == LEAD) {

		m_x += m_v * cos(m_theta) * dt;
		m_y += m_v * sin(m_theta) * dt;
		m_theta += m_w * dt;

		FollowPath(simTime, m_localMap);
	}
}

void CRobotScout::Draw(CDC *pDC)
{
	m_rect = CRect( round(m_x - m_radius), round(m_y - m_radius), round(m_x + m_radius), round(m_y + m_radius) ); 
	
	if (m_status == LEAD)
		pDC->SelectStockObject(GRAY_BRUSH);
	else if (m_status == FOLLOW)
		pDC->SelectStockObject(BLACK_BRUSH);
	else if (m_status == WAIT)
		pDC->SelectStockObject(WHITE_BRUSH);
	else if (m_status == DOCK) {
		pDC->SelectStockObject(LTGRAY_BRUSH);
	}

	pDC->SelectStockObject(BLACK_PEN);
	pDC->Ellipse(m_rect);
	

	CPoint points[2];
	points[0] = CPoint( (short) m_x, (short) m_y);
	points[1] = CPoint( (short) (m_x + ( m_radius * cos(m_theta) )), (short) (m_y + ( m_radius * sin(m_theta) )));

	pDC->SelectObject(&RED_PEN);
	pDC->Polygon(points,2);  
}

void CRobotScout::Planner(double simTime)
{
/*
	static BOOL init = false;

	if (!init) {
	   srand( (unsigned)time( NULL ) );
	   init = true;
	}

	if (simTime <= 5)
		m_v = simTime * 10;				// acc
	else
		m_v = 30 + ( rand() % 40 );

*/
	if (simTime <= 5)
		m_v = simTime * 10;				// acc
	else
		m_v = 50 + 20 * sin(simTime-5); // sin

//	m_v = 50;
}

void CRobotScout::Output(double simTime)
{
	fprintf(m_out, "%10.4f %10.4f %10.4f\n", simTime, m_x, m_v);
}

void CRobotScout::Serialize(CArchive& ar)
{
	CRobotNonHolonomic::Serialize(ar);
}