robotxr4000.cpp

多机器人合作中的动态角色分配仿真算法是多机器人合作领域的一个比较著名的仿真软件

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

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

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif
#include "myglobals.h"


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
IMPLEMENT_SERIAL(CRobotXR4000, CRobotHolonomic, 1 )

CRobotXR4000::CRobotXR4000(double x, double y, double theta, short status, short id, CString name) : CRobotHolonomic(x, y, theta, status, id, name)
{
	m_radius = 25;
}

CRobotXR4000::CRobotXR4000()
{
}

CRobotXR4000::~CRobotXR4000()
{
}

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

	Potential(robots, dt, globalMap, box, simTime);
		
	m_x += m_vx * dt;
	m_y += m_vy * dt;
}

void CRobotXR4000::Draw(CDC *pDC)
{
	CPoint line[2];

	m_rect = CRect( round(m_x - m_radius), round(m_y - m_radius), round(m_x + m_radius), round(m_y + m_radius) );

	pDC->SelectObject(&RED_PEN);

	if (m_status == TRANSPORT)
		pDC->SelectStockObject(BLACK_BRUSH);
	else if (m_status == DOCK)
		pDC->SelectStockObject(GRAY_BRUSH);
	else if (m_status == STOPPED)
		pDC->SelectStockObject(WHITE_BRUSH);
	else if (m_status == WAIT)
		pDC->SelectObject(&BLUE);

	pDC->Ellipse(m_rect);
}

void CRobotXR4000::Serialize(CArchive& ar)
{
	CRobotHolonomic::Serialize(ar);
}

void CRobotXR4000::Potential(CArray<CRobot*, CRobot*> *robots, double dt, CMapPath *map, CBox* box, double simTime)
{
	short i;

	double fx = 0;
	double fy = 0;

	double dist;
	double angle;

	// Repulsive forces from other robots
	for (i=0; i<robots->GetSize(); i++) 
		if (robots->GetAt(i)->m_id != m_id)
			ForceFromRobots(robots->GetAt(i), fx, fy);

	// Repulsive forces from obstacles
	for (i=0; i<m_localMap->m_obstacles.GetSize(); i++)
		ForceFromObstacles(m_localMap->m_obstacles[i], fx, fy);

	// Atractive force from box (Docking or waiting)
	// Repulsive if dist < 0 in all cases
	if (box->m_exist) {
		ForceFromBoxes(box, fx, fy);
	}
	else {
		m_status = TRANSPORT;
	}

	// Atractive force from goal
	if ( (m_localMap->m_goalReal.x != -1) && (m_status == TRANSPORT) ) {
		dist = sqrt( (map->m_goalReal.x - m_x) * (map->m_goalReal.x - m_x) + (map->m_goalReal.y - m_y) * (map->m_goalReal.y - m_y) );
		angle = atan2( (map->m_goalReal.y - m_y), (map->m_goalReal.x - m_x) );
		
		fx += Ka_rg * dist * cos(angle);
		fy += Ka_rg * dist * sin(angle);
	}

	// Damping
	fx += - Cr * m_vx;
	fy += - Cr * m_vy;

	// Computing velocities
	m_vx += fx * dt;
	m_vy += fy * dt;

	//Saturation (keeping scale between vx and vy)
	double factor;
	if ( (m_vx > 200) || (m_vy > 200) )
		if (m_vx > m_vy) {
		factor = m_vx / 200;
			m_vx = m_vx / factor;
			m_vy = m_vy / factor;
		}
		else{
			factor = m_vy / 200;
			m_vy = m_vy / factor;
			m_vx = m_vx / factor;
		}
	if ( (m_vx < -200) || (m_vy < -200) )
		if (m_vx < m_vy) {
			factor = m_vx / -200;
			m_vx = m_vx / factor;
			m_vy = m_vy / factor;
		}
		else{
			factor = m_vy / -200;
			m_vy = m_vy / factor;
			m_vx = m_vx / factor;
		}

	// Change to WAIT status if close to the box and the distance is small
	//dist = sqrt( (box->m_x - m_x) * (box->m_x - m_x) + (box->m_y - m_y) * (box->m_y - m_y) ) - box->m_lx - m_radius; //*****	
	if ( (m_status == DOCK) && (fabs(m_vx) < 1) && ( fabs(m_vy) < 1) && (dist < 5) ) {

		m_status = WAIT;
		m_timer = simTime;

		CControlMsg *controlMsg;
		controlMsg = new CControlMsg;
		controlMsg->m_code = BOXLOCKED;
		controlMsg->m_from = m_id;
		controlMsg->m_to = 0;
		controlMsg->m_x = m_x;
		controlMsg->m_y = m_y;
		
		SendControlMsg(controlMsg);
	}
	else if ( (m_status == WAIT) && ( (simTime - m_timer) > 10) ) {
		CControlMsg *controlMsg;
		controlMsg = new CControlMsg;
		controlMsg->m_code = TIMEOUT;
		controlMsg->m_from = m_id;
		controlMsg->m_to = 0;

		SendControlMsg(controlMsg);
	}

	if ( (box->m_exist) && (m_status == TRANSPORT) && (dist > m_sensorRange) ) {

		m_status = STOPPED;

		CControlMsg *controlMsg;
		controlMsg = new CControlMsg;
		controlMsg->m_code = LOSTBOX;
		controlMsg->m_from = m_id;
		controlMsg->m_to = 0;
		
		SendControlMsg(controlMsg);
	}

}