behavior.cpp

一个机器人的源代码.软件设计得超级好!是商业级代码.

// Behavior.cpp -- a first cut at defining behaviors and arbitration between them.

#include "Executive.h"
#include "Behavior.h"

#include "boost\lexical_cast.hpp"
#include "boost\format.hpp"

using namespace boost;
using boost::format;
using boost::io::group;
using boost::io::str;

// Constants for ScanBehavior and DriveBehavior:
const int ciTurnAngle = 30;
const int ciSweepAngle = ciTurnAngle + 60;  // by turning 30 degrees we can sweep out 90 degrees with the 3 front sensors
const int ciSafeOpening = 600;  // mm, minimum width of an opening that the robot can drive towards/through
const int ciMaxDriveDistance = 600;  // mm, maximum length of a forward drive
const int ciSafetyMargin = 200;  // mm, minimum open space we want to maintain in front of robot after any drive
const int ciMaxScanRange = 800;  // maximum scanning range

// Functions shared by behaviors:
void RobotSmoothStop(string sWho);

// Variables section for data shared between behaviors:

// Data prepared by ScanBehavior, used by DriveBehavior:
int gScanComputedAngle;
int gScanComputedDistance;

// Start/Stop behavior voting (need 2 out of 3 to trigger a Stop or Start)
bool gStops[ciNumBeakBehaviors];
bool gStarts[ciNumBeakBehaviors];

// Shared between Drive and Avoid:
bool gDriving;

DMLTimer::DMLTimer()
{
	Reset();
}

void DMLTimer::Reset()
{
	m_fHasStarted = false;
}

void DMLTimer::Start()
{
	m_fHasStarted = true;
	DMLTime(&m_TimerStart);
}

bool DMLTimer::HasStarted()
{
	return m_fHasStarted;
}

bool DMLTimer::HasTimeElapsed(int msElapsed)
{
	DML_TIME Now;
	DMLTime(&Now);
	long ElapsedTime = TimeDiff(&m_TimerStart, &Now);
	return (ElapsedTime >= msElapsed);
}

Behavior::Behavior(Arbiter *pMaster)
{
	m_pMaster = pMaster;
	m_pInput = 0;
}

Behavior::Behavior(Arbiter *pMaster, DML_ChannelRef *pInput)
{
	m_pMaster = pMaster;
	m_pInput = pInput;
}

Arbiter::Arbiter()
{
	m_EnabledBehaviors = 0;
	ClearBehaviorList();
}

void Arbiter::AddBehavior(Behavior *pBehavior)
{
	m_lpBehaviors.push_back(pBehavior);
	pBehavior->Reset();
}

void Arbiter::ClearBehaviorList()
{
	m_lpBehaviors.clear();
}

void Arbiter::EnableBehaviors(U32 BehaviorSet)
{
	int AlreadyEnabled = m_EnabledBehaviors;
	m_EnabledBehaviors = m_EnabledBehaviors | BehaviorSet;
	int NewlyEnabled = m_EnabledBehaviors & ~AlreadyEnabled;

	// Reset only the behaviors that were newly enabled:
	list<Behavior *>::iterator pCurrent = m_lpBehaviors.begin();
	while (pCurrent != m_lpBehaviors.end())
	{
		if (((*pCurrent)->BehaviorType() & NewlyEnabled) > 0)
		{
			(*pCurrent)->Reset();
		}
		pCurrent++;
	}
}

void Arbiter::DisableBehaviors(U32 BehaviorSet)
{
	m_EnabledBehaviors = m_EnabledBehaviors & ~BehaviorSet;
}

bool Arbiter::BehaviorIsEnabled(U32 TheBehavior)
{
	return ((m_EnabledBehaviors & TheBehavior) > 0);
}

void Arbiter::Tick()
{
	// Starting at list front, look at each behavior on the list.  If the behavior
	// is currently enabled, allow it to Act().  Note that any behavior's "Act()" call
	// could potentially enable/disable subsequent behaviors.
	list<Behavior *>::iterator pCurrent = m_lpBehaviors.begin();
	while (pCurrent != m_lpBehaviors.end())
	{
		if (BehaviorIsEnabled((*pCurrent)->BehaviorType()))
			(*pCurrent)->Act();
		pCurrent++;
	}
}

U32 StopBehavior::BehaviorType()
{
	return cBehaviorSafetyCritical;
}

void StopBehavior::Reset()
{
	m_Timer.Reset();
	m_fHoldoff = true;
	gStops[m_InstanceNumber] = false;
}

void StopBehavior::Act()
{
	// StopBehavior uses a single input which we assume is a beak sensor IR reading in mm:
	int BeakSensor = m_pInput->GetIntValue();
	const int msSwipeTime = 300;  // watch for a "hand swipe" lasting at least 300 ms
	const int mmSwipeDistance = 150;  // a hand swipe is when sensor reading drops below 150 mm
	bool fISeeSomething = (BeakSensor <= mmSwipeDistance);

	// Do not process a "Stop" until after there is no obstacle in front of our sensor.
	// This prevents Stop from triggering whenever the robot is near an obstacle.
	if (m_fHoldoff)
	{
		m_fHoldoff = fISeeSomething;
		if (m_fHoldoff)
			return;
	}

	// Reset if we don't see anything anymore
	if (gStops[m_InstanceNumber] && !fISeeSomething)
	{
		Reset();
		return;
	}

	// Trigger the Stop behavior if we see a hand swipe:
	if (m_Timer.HasStarted() == false)
	{
		if (fISeeSomething)
			m_Timer.Start();  // Sense beginning of hand swipe, start timer:
	}
	else  // timer has been started
	{
		if (fISeeSomething)
		{
			if (m_Timer.HasTimeElapsed(msSwipeTime))
			{
				// Need 2 out of 3 stop votes before we stop, because an obstacle can give a single false Stop
				gStops[m_InstanceNumber] = true;
				int StopCount = 0;
				for (int i = 0; i < ciNumBeakBehaviors; i++)
				{
					if (gStops[i])
						StopCount++;
				}
				
				if (StopCount >= 2)
				{
					// Owner says "STOP!!!"
					m_pMaster->DisableBehaviors(cAllBehaviors);
					m_pMaster->EnableBehaviors(cBehaviorStart);
					RobotSmoothStop("StopBehavior");
					gController->AdjustPowerLevel(0);  // turn the motors off
				}
			}
		}
		else
		{
			m_Timer.Reset();  // swipe is over and it wasn't long enough
		}
	}
}

U32 StartBehavior::BehaviorType()
{
	return cBehaviorStart;
}

void StartBehavior::Reset()
{
	m_Timer.Reset();
	m_fHoldoff = true;
	gStarts[m_InstanceNumber] = false;
}

void StartBehavior::Act()
{
	// StartBehavior uses a single input which we assume is a beak sensor IR reading in mm:
	int BeakSensor = m_pInput->GetIntValue();
	const int msSwipeTime = 300;  // watch for a "hand swipe" lasting at least 300 ms
	const int mmSwipeDistance = 150;  // a hand swipe is when sensor reading drops below 150 mm
	bool fISeeSomething = (BeakSensor <= mmSwipeDistance);

	// Do not process a "Start" until after there is no obstacle in front of our sensor.
	// This prevents Start from triggering whenever the robot is near an obstacle.
	if (m_fHoldoff)
	{
		m_fHoldoff = fISeeSomething;
		if (m_fHoldoff)
			return;
	}

	// Reset if we don't see anything anymore
	if (gStarts[m_InstanceNumber] && !fISeeSomething)
	{
		Reset();
		return;
	}

	// Trigger the Start behavior if we see a hand swipe:
	if (m_Timer.HasStarted() == false)
	{
		if (fISeeSomething)
			m_Timer.Start();  // Sense beginning of hand swipe, start timer:
	}
	else  // timer has been started
	{
		if (fISeeSomething)
		{
			if (m_Timer.HasTimeElapsed(msSwipeTime))
			{
				// Need 2 out of 3 start votes before we start, because an obstacle can give a single false Start
				gStarts[m_InstanceNumber] = true;
				int StartCount = 0;
				for (int i = 0; i < ciNumBeakBehaviors; i++)
				{
					if (gStarts[i])
						StartCount++;
				}
				
				if (StartCount >= 2)
				{
					// Owner says "Start!!!"
					m_pMaster->DisableBehaviors(cBehaviorStart);
					m_pMaster->EnableBehaviors(cBehaviorSafetyCritical | cBehaviorExplore);
					gController->LogInformationString("StartBehavior:  I am starting.");
					Sleep(2000);  // Pause execution for a bit, during this user can move his hand away
				}
			}
		}
		else
		{
			m_Timer.Reset();  // swipe is over and it wasn't long enough
		}
	}
}

U32 ScanBehavior::BehaviorType()
{
	return cBehaviorExplore;
}

void ScanBehavior::Reset()
{
	m_fScanStarted = false;
	m_SensorArc.Reset(ciSweepAngle);
	// Reset the data we are providing to other behaviors:
	gScanComputedDistance = 0;
	gScanComputedAngle = -1;
}

void ScanBehavior::Act()
{
	int ScanLeft = gIRLeft30->GetIntValue();
	int ScanCenter = gIRForward->GetIntValue();
	int ScanRight = gIRRight30->GetIntValue();
	int BeakLeft = gIRBeakDownLeft->GetIntValue();
	int BeakCenter = gIRBeakDownCenter->GetIntValue();
	int BeakRight = gIRBeakDownRight->GetIntValue();

	// If we haven't started to scan yet...
	if (!m_fScanStarted)
	{
		// Which way should we turn?  Check the three pairs of left/right sensors, and head in the 
		// direction of more open space:
		int LeftVotes = ScanLeft + BeakLeft + gIRLeftSide->GetIntValue();
		LeftVotes -= (ScanRight + BeakRight + gIRRightSide->GetIntValue());
		if (LeftVotes >= 0)
			m_TurnAngle = ciTurnAngle;
		else
			m_TurnAngle = ciTurnAngle * -1;

		// Command the turn:
		m_fScanStarted = true;
		gController->Turn(m_TurnAngle);
		return;
	}

	// Modify the values of the floor-level sensors if the closest beak sensor sees something closer.
	// Hopefully this will prevent turning too close to nearby obstacles.
	if ((BeakLeft < ciLeftFloorReturn) && (BeakLeft < ScanLeft))
		ScanLeft = BeakLeft - 100;  // cuz it's an angled sensor, but we ought to use Pythagoreas here...
	if ((BeakRight < ciRightFloorReturn) && (BeakRight < ScanRight))
		ScanRight = BeakRight - 100;
	if ((BeakCenter < ciCenterFloorReturn) && (BeakCenter < ScanCenter))
		ScanCenter = BeakCenter - 100;

	// Scan is now in progress, either we just started it or it started earlier.  Capture scan data:
	int degreesMoved = abs(ClipAndRoundToINT((double)m_TurnAngle * gController->GetMoveProgress()));
	if (gController->RobotInMotion())
	{
		if (m_TurnAngle > 0)
		{
			// turning left, counterclockwise
			m_SensorArc.AddReading(degreesMoved, ScanRight);		// Right sensor traces out 0-30
			m_SensorArc.AddReading(degreesMoved + 30, ScanCenter);  // Center sensor traces out 30-60
			m_SensorArc.AddReading(degreesMoved + 60, ScanLeft);	// Left sensor traces out 60-90
		}
		else
		{
			// turning right, clockwise
			m_SensorArc.AddReading(degreesMoved, ScanLeft);			// Left sensor traces out 0-30
			m_SensorArc.AddReading(degreesMoved + 30, ScanCenter);  // Center sensor traces out 30-60
			m_SensorArc.AddReading(degreesMoved + 60, ScanRight);	// Right sensor traces out 60-90
		}
//		gController->LogDebugString("Angle " + lexical_cast<string>(degreesMoved) +
//								": Left = " + lexical_cast<string>(ScanLeft) +
//								" Center = " + lexical_cast<string>(ScanCenter) +
//								" Right = " + lexical_cast<string>(ScanRight));
	}
	else
	{
		// If we're not in motion anymore, then analyze results and provide a safe travel vector if possible:
		gController->AdjustPowerLevel(0);  /// test -- will this allow the turn to finish?  Answer: it seems to help...
		Sleep(200);
		m_SensorArc.Process(ciSafeOpening, ciMaxScanRange);
		if (GetSafeTravelVector(&gScanComputedAngle, &gScanComputedDistance))
		{
			// We have a place to drive now, so let "Drive" take over:
			m_pMaster->DisableBehaviors(cBehaviorExplore);
			m_pMaster->EnableBehaviors(cBehaviorDrive);
			gController->LogInformationString("ScanBehavior:  angle = " + lexical_cast<string>(gScanComputedAngle) +
											  ", distance = " + lexical_cast<string>(gScanComputedDistance));
		}
		else
		{
			// Keep scanning in the same direction.
			m_SensorArc.Reset(ciSweepAngle);	// discard data from last sweep
			gController->Turn(m_TurnAngle);		// Continue turning and scanning
		}
	}
}

bool ScanBehavior::GetSafeTravelVector(int *pTurnAngle, int *pMoveDistance)
{
	if (m_SensorArc.GetBestRange() > 0)
	{
		*pTurnAngle = m_SensorArc.GetBestAngle();
		*pMoveDistance = m_SensorArc.GetBestRange();

		// Leave room for front sensors to operate after the move.  And cap the maximum
		// move distance to a value that reasonably reflects our sensor accuracy:
		*pMoveDistance -= ciSafetyMargin;
		if (*pMoveDistance > ciMaxDriveDistance)
			*pMoveDistance = ciMaxDriveDistance;

		// Modify pTurnAngle to produce a turn angle relative to our current heading.  Robot is centered on 60 degrees
		// SensorArc after finishing a sweep in either direction:
		if (m_TurnAngle > 0)  // if we scanned left...
			*pTurnAngle = *pTurnAngle - 60;
		else  // if we scanned right...
			*pTurnAngle = 60 - *pTurnAngle;
		return true;
	}
	else
	{
		return false;  // we'll have to keep scanning
	}
}

U32 DriveBehavior::BehaviorType()
{
	return cBehaviorDrive;
}

void DriveBehavior::Reset()
{
	m_fTurnStarted = false;
	m_fDriveStarted = false;
	gDriving = false;
}

void DriveBehavior::Act()
{
	assert(gScanComputedDistance > 0);

	if (!m_fTurnStarted)
	{
		m_fTurnStarted = true;  // start the turn
		gController->Turn(gScanComputedAngle);
	}
	else if (!m_fDriveStarted)
	{
		// Has the turn finished yet?
		if (!gController->RobotInMotion())
		{
			gController->AdjustPowerLevel(0);  /// test -- will this allow the turn to finish?  Answer: it seems to help...
			Sleep(200);

			// Turn complete, start driving:
			m_fDriveStarted = true;
			gDriving = true;  // enable Avoid behaviors
			gController->MoveForward(gScanComputedDistance);
		}
	}
	else
	{
		// Has the drive finished yet?
		if (!gController->RobotInMotion())
		{
			// All done!  Stop driving and start a new scan:
			m_pMaster->DisableBehaviors(cBehaviorDrive);
			m_pMaster->EnableBehaviors(cBehaviorExplore);
			gController->LogInformationString("DriveBehavior:  Drive completed.");
		}
	}
}

U32 TestDriveBehavior::BehaviorType()
{
	return cBehaviorDrive;
}

void TestDriveBehavior::Reset()
{
	m_fDrive1Started = false;
	m_fDrive2Started = false;
}

void TestDriveBehavior::Act()
{
	if (!m_fDrive1Started)
	{
		m_fDrive1Started = true;  // start the turn
		gController->Turn(30);
	}
	else if (!m_fDrive2Started)
	{
		// Has the turn finished yet?
		if (!gController->RobotInMotion())
		{
			// Turn complete, start driving:
			m_fDrive2Started = true;
			gController->Turn(-30);
		}
	}
	else
	{
		// Has the drive finished yet?
		if (!gController->RobotInMotion())
		{
			// All done!  Stop driving and start a new scan:
			gController->LogInformationString("TestDriveBehavior:  Drive completed.");
			Reset();
		}
	}
}

U32 AvoidBehavior::BehaviorType()
{
	return cBehaviorDrive;
}

void AvoidBehavior::Reset()
{
}

void AvoidBehavior::Act()
{
	if (!gDriving)
		return;

	int MySensor = m_pInput->GetIntValue();
	if (MySensor <= m_TriggerDistance)
	{
		m_pMaster->DisableBehaviors(cBehaviorDrive);
		m_pMaster->EnableBehaviors(cBehaviorSafetyCritical | cBehaviorExplore);
		RobotSmoothStop("AvoidBehavior");
	}
}

void RobotSmoothStop(string sWho)
{
	gController->ExecuteCommand(CmdSmoothStop);
	gController->LogInformationString(sWho + ":  I am stopping.");
	Sleep(1000);  // Pause until robot stops
}