robotforage.cpp

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

{

	if (m_levelInfo == 0)  // m_levelInfo = 0 : no memory
		return TRUE;				

	// insert item in the list
	if ( !FoundInList(p) ) {
		m_itemList.Add(p);
		return FALSE;
	}
	else {
		return TRUE;
	}
}


// check if the item is not already in the list
BOOL CRobotForage::FoundInList(CPoint p)
{
	BOOL found = FALSE;
	for(short j=0; j < m_itemList.GetSize(); j++) {
		if (m_itemList[j] == p) {
			found = TRUE;
			break;
		}
	}
	return found;
}


// Handel for control messages
LRESULT CRobotForage::OnControlMsg(WPARAM wParam, LPARAM lParam)
{
	CControlMsg *controlMsg;
	
	controlMsg = (CControlMsg *)wParam;

	switch (controlMsg->m_code){

	
	case NEWPOINT:
		// New item detected. Insert its position in the list
		if (controlMsg->m_from != m_id) {
			CPoint p = CPoint(round(controlMsg->m_x), round(controlMsg->m_y));
			InsertPoint(p);			
		}
		
		break;

	case GOFORIT:
		// Remove point from my list
		CPoint p = CPoint(round(controlMsg->m_x), round(controlMsg->m_y));
		for(short i=0; i<m_itemList.GetSize(); i++)
			if (m_itemList[i] == p) {
				m_itemList.RemoveAt(i);
				break;
			} 
		
		// if already going to get the point, get next point
		if (controlMsg->m_from != m_id) {
			if (m_localMap->m_goalReal == p)
				m_localMap->m_goalReal = GetNextPoint();
		}
		
		break;
	}

	CRobot::OnControlMsg(wParam, lParam);

	return 0L;
}


// 
void CRobotForage::SetItem(short i, CPoint p)
{
	m_allItems[i] = p;
}

void CRobotForage::AddItem(CPoint p)
{
	m_allItems.Add(p);
}

CPoint CRobotForage::GetItem(short i)
{
	return m_allItems[i];
}

short CRobotForage::GetNumItems()
{
	return (short) m_allItems.GetSize();
}

void CRobotForage::DeleteAllItems()
{
	m_allItems.RemoveAll();
	m_totalRescuedItems = 0;
	m_area = CSize(0,0);
}

void CRobotForage::SetArea(CSize a)
{
	m_area = a;
}

CSize CRobotForage::GetArea()
{
	return m_area;
}

CString CRobotForage::GetInfo()
{
	CString s1, s2;

	s1.Format("r: %d - list:", m_rescuedItems);
	for (short j=0; j < m_itemList.GetSize(); j++){
		s2.Format("(%d %d) ",m_itemList[j].x, m_itemList[j].y);
		s1 += s2;
	}

	return s1;
}

void CRobotForage::SerializeStatic(CArchive& ar)
{
	if(ar.IsStoring()){
		m_allItems.Serialize(ar);
		ar << m_totalRescuedItems;
		ar << m_area;
	}
	else{
		m_allItems.Serialize(ar);
		ar >> m_totalRescuedItems;
		ar >> m_area;
	}
}

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

	double fx = 0;
	double fy = 0;

	double dist;
	double angle;

	CPoint p;

	// 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 items
	for(i=0; i < GetNumItems(); i++){
		p = GetItem(i);
		if (p.x > 0) { // item is available
			dist = sqrt( ( p.x - m_x) * (p.x - m_x) + (p.y - m_y) * (p.y - m_y) );
			if (dist < m_sensorRange) { // item is inside the sensor range
				if ( (m_status == WANDER) || (m_status == GOPOSITION) ){
					m_attached = i;
					// record the current position
					InsertPoint(CPoint(round(m_x), round(m_y) ) );
					m_status = GETITEM;
				}
			}
		}
	}

	// go get the item
	if (m_status == GETITEM) {
		p = GetItem(m_attached);
		dist = sqrt( ( p.x - m_x) * (p.x - m_x) + (p.y - m_y) * (p.y - m_y) );
		angle = atan2( (p.y - m_y), (p.x - m_x) ) + PI;
		if (p.x > 0) {
			fx += Ka_rb * dist * cos(angle-PI);
			fy += Ka_rb * dist * sin(angle-PI);
			
			if (dist < m_radius){
				// Fisically get the point;
				m_status = TRANSPORT;
				m_localMap->m_goalReal = map->m_goalReal;
				SetItem(m_attached, CPoint(round(-m_x), round(-m_y)) );
			}
		}
		else {
			m_status = GOPOSITION;
			m_localMap->m_goalReal = GetNextPoint();
		}
	}

	if ( (m_status == GOPOSITION) && CloseToPosition(m_localMap->m_goalReal,5) ) {
		m_status = WANDER;
	}

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

		if ( (m_attached != -1) && (m_status == TRANSPORT) ){
			SetItem(m_attached, CPoint(round(-m_x), round(-m_y) ) );
			if (dist < 20) {
				m_attached = -1;
				m_localMap->m_goalReal = GetNextPoint();
				m_status = GOPOSITION;
				m_rescuedItems++;
				m_totalRescuedItems++;
			}
		}
	}

	CSize area = GetArea();
	if ( (m_status == WANDER) ) {
		if ( ( CloseToPosition(m_trackPoint,5) ) || (m_trackPoint.x == -1) ){
			if (m_trackPoint.x == -1) {
				m_trackPoint.x = round((area.cx / (double) m_numRobots) * m_id + m_sensorRange - 10);
				m_trackPoint.y = 1;
			}
			else if (m_trackPoint.y == 0)
			{
				if ( (m_trackPoint.x + (2 * m_sensorRange - 20) ) > area.cx)
					m_trackPoint.x = (m_trackPoint.x + (m_sensorRange - 20) );
				else
					m_trackPoint.x = (m_trackPoint.x + (2 * m_sensorRange - 20) );
				if (m_trackPoint.x > area.cx)
					m_trackPoint.x = m_sensorRange-20;
				m_trackPoint.y = 1;
			}
			else if (m_trackPoint.y == 1)
			{
				m_trackPoint.y = area.cy;
			}
			else if (m_trackPoint.y == area.cy)
			{
				if ( (m_trackPoint.x + (2 * m_sensorRange - 20) ) > area.cx)
					m_trackPoint.x = (m_trackPoint.x + (m_sensorRange - 20) );
				else
					m_trackPoint.x = (m_trackPoint.x + (2 * m_sensorRange - 20) );
				if (m_trackPoint.x > area.cx)
					m_trackPoint.x = m_sensorRange-20;
				m_trackPoint.y = area.cy - 1;
			}
			else if (m_trackPoint.y == area.cy - 1)
			{
				m_trackPoint.y = 0;
			}
		}
		dist = sqrt( (m_trackPoint.x - m_x) * (m_trackPoint.x - m_x) + (m_trackPoint.y - m_y) * (m_trackPoint.y - m_y) );
		angle = atan2( (m_trackPoint.y - m_y), (m_trackPoint.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 (mantaining 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;
		}
}

BOOL CRobotForage::CloseToPosition(CPoint p, int range)
{
	return ( (p.x >= m_x - range) && (p.x <= m_x + range) && (p.y >= m_y - range) && (p.y <= m_y + range) );
}