robotmath.cpp

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

// RobotMath.cpp
//
// Misc. math functions
#include <limits.h>
#include <assert.h>
#include <math.h>
#include "RobotMath.h"

// These declarations are just for debug logging:
#include "Executive.h"
#include "boost\lexical_cast.hpp"
#include "boost\format.hpp"
using namespace boost;
using boost::format;
using boost::io::group;
using boost::io::str;

int ClipAndRoundToINT(double FloatValue)
{
	if (FloatValue < 0.0)
		FloatValue -= 0.5;
	else
		FloatValue += 0.5;

	if (FloatValue < INT_MIN)
		return INT_MIN;
	else if (FloatValue > INT_MAX)
		return INT_MAX;
	else
		return (int)FloatValue;
}

unsigned long ClipAndRoundToULONG(double FloatValue)
{
	if (FloatValue < 0)
		return 0;

	FloatValue += 0.5;
	if (FloatValue > ULONG_MAX)
		return ULONG_MAX;
	else
		return (unsigned long)FloatValue;
}

short ClipAndRoundToSHORT(double FloatValue)
{

	if (FloatValue < 0.0)
		FloatValue -= 0.5;
	else
		FloatValue += 0.5;

	if (FloatValue < SHRT_MIN)
		return SHRT_MIN;
	else if (FloatValue > SHRT_MAX)
		return SHRT_MAX;
	else
		return (short)FloatValue;
}

IntMovingAverage::IntMovingAverage(int NumSamples)
{
	m_NumSamples = NumSamples;
	m_SamplePosition = 0;
	m_Samples.resize(m_NumSamples, 0);
}

void IntMovingAverage::AddValue(int NewValue)
{
	m_Samples[m_SamplePosition] = NewValue;
	m_SamplePosition++;
	if (m_SamplePosition == m_NumSamples)
		m_SamplePosition = 0;
}

int IntMovingAverage::GetAverage()
{
	int Result = 0;
	for (int i = 0; i < m_NumSamples; i++)
		Result += m_Samples[i];
	Result = (int)((double)Result/(double)m_NumSamples + 0.5);
	return Result;
}

void SensorArc::Reset(int NumDegrees)
{
	m_Size = NumDegrees + 1;
	m_BestRange = 0;
	m_BestAngle = -1;
	m_Readings.resize(m_Size);
	for (int i = 0; i < m_Size; i++)
		m_Readings[i] = -1;
}

void SensorArc::AddReading(int DegreeAngle, int mmReading)
{
	mmReading = abs(mmReading);
//	assert(mmReading >= 0);  /// hit this one once when I put my hand under the beak to Stop...  ???
	assert(DegreeAngle >= 0);
	assert(DegreeAngle < m_Size);

	if (m_Readings[DegreeAngle] == -1)
		m_Readings[DegreeAngle] = mmReading;  // if there is no reading, store this one
	else
		if (m_Readings[DegreeAngle] > mmReading)
			m_Readings[DegreeAngle] = mmReading;  // keep the shorter of the reported readings
}

int SensorArc::Interpolate(int Angle)
{
	int BeforeVal = m_Readings[Angle - 1];
	int AfterVal = m_Readings[Angle + 1];
	while (AfterVal == -1)
	{
		Angle++;  // scan ahead
		AfterVal = m_Readings[Angle + 1];
	}
	return ((BeforeVal + AfterVal) / 2);
}

bool SensorArc::GetBestArc(int Range, int *pStart, int *pEnd)
{
	int Angle;
	int BestStart = 0;
	int BestEnd = 0;
	int Start = -1;
	int End = -1;

	for (Angle = 0; Angle < m_Size; Angle++)
	{
		if (m_Readings[Angle] >= Range)
		{
			// We're in a valid arc.
			if (Start == -1)
				Start = Angle;
		}
		else
		{
			// We're not in a valid arc.
			if (Start != -1)
			{
				// We just left a valid arc, was it the best one?
				End = Angle - 1;
				if ((End - Start) > (BestEnd - BestStart))
				{
					BestStart = Start;
					BestEnd = End;
					// Look for another one.
					Start = -1;
					End = -1;
				}
			}
		}
	}

	// Are we still tracing an open arc?
	if (Start != -1)
	{
		End = m_Size - 1;
		if ((End - Start) > (BestEnd - BestStart))
		{
			BestStart = Start;
			BestEnd = End;
		}
	}

	if ((BestEnd - BestStart) > 0)
	{
		*pStart = BestStart;
		*pEnd = BestEnd;
		return true;
	}
	else
		return false;
}

int SensorArc::ComputeChordLength(int Range, int ArcAngle)
{
	// We want the length of a chord from a circle with Radius = (Range + ciRobotFrontSensorRadius)
	// and chord's "Central Angle" is ArcAngle.
	// From here:  http://www.1728.com/circpart.htm, we get the formula we need:
	// 1/2 ChordLength = sin (1/2 ArcAngle) * radius

	double dRadius = ciRobotFrontSensorRadius + (double)Range;
	double dArc = (double)ArcAngle / 2.0;
	dArc = dArc * cdDegreesToRadians;
	dArc = sin(dArc) * dRadius;
	return ClipAndRoundToINT(dArc * 2.0);
}

void SensorArc::Process(int RequiredWidth, int DesiredRange)
{
	int Angle;
	int Range;
	int Start,End;

	// Step 1.  Interpolate any gaps in the readings:
	if (m_Readings[0] == -1)
		m_Readings[0] = 0;					// establish valid data at either bound if necessary
	if (m_Readings[m_Size - 1] == -1)
		m_Readings[m_Size - 1] = 0;
	for (Angle = 1; Angle < m_Size - 1; Angle++)
	{
		if (m_Readings[Angle] == -1)
			m_Readings[Angle] = Interpolate(Angle);
	}

	// Step 2.  Find a chord of RequiredWidth, starting at DesiredRange and working backwards until we find one.
	//			Stop at 400 mm, there's no need to move a shorter distance.
	for (Range = DesiredRange; Range >= 400; Range -= 100)
	{
		if (GetBestArc(Range, &Start, &End))
		{
			int ChordLength = ComputeChordLength(Range, End - Start);
//			gController->LogDebugString("Range = " + lexical_cast<string>(Range) +
//										" Start = " + lexical_cast<string>(Start) +
//										" End = " + lexical_cast<string>(End) +
//										" ChordLength = " + lexical_cast<string>(ChordLength));
			if (ChordLength >= RequiredWidth)
			{
				m_BestRange = Range;
				m_BestAngle = ClipAndRoundToINT((double)(End + Start) / 2.0);
				break;
			}
		}
	}
}

int SensorArc::GetBestAngle()
{
	return m_BestAngle;
}

int SensorArc::GetBestRange()
{
	return m_BestRange;
}