executive.cpp

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

		m_CommandQueue.pop();				// remove it from the queue
		LeaveCriticalSection(&m_QueueLock);  // unlock the queue
		
		ExecuteCommand(Cmd);
	}

	// Lastly, increment the number of services performed:
	m_ServiceCount++;
}

void Executive::TerminateService()
{
	TerminateVersionSupport();
}

STATUS Executive::ProcessStringCommand(HWND hWndSource, char *pszStringCommand)
{
	string sCmd = pszStringCommand;		// Copy command to string
	stringstream ssCmd;
	int CmdValue;
		
	ssCmd << sCmd;						// Load command into stringstream for parsing
	if (ssCmd.good())
	{
		// Get first token:
		ssCmd >> sCmd;
		if (sCmd == "Commit")
		{
			EnqueueCommand(CmdCommit);
		}
		else if (sCmd == "SCurveMove")
		{
			EnqueueCommand(CmdSCurveMove);
		}
		else if (sCmd == "TrapezoidalMove")
		{
			EnqueueCommand(CmdTrapezoidalMove);
		}
		else if (sCmd == "VelocityContourMove")
		{
			EnqueueCommand(CmdVelocityMove);
		}
		else if (sCmd == "SmoothStop")
		{
			ExecuteCommand(CmdSmoothStop);
		}
		else if (sCmd == "AbruptStop")
		{
			ExecuteCommand(CmdAbruptStop);
		}
		else if (sCmd == "ZeroPosition")
		{
			ExecuteCommand(CmdZeroPosition);
		}
		else if (sCmd == "ResetEventStatus")
		{
			ExecuteCommand(CmdResetEventStatus);
		}
		else if (sCmd == "SetAcceleration")
		{
			ssCmd >> CmdValue;
			gpAcceleration->SetValue(CmdValue);
			EnqueueCommand(CmdSetAcceleration);
		}
		else if (sCmd == "SetJerk")
		{
			ssCmd >> CmdValue;
			gpJerk->SetValue(CmdValue);
			EnqueueCommand(CmdSetJerk);
		}
		else if (sCmd == "SetPowerLevel")
		{
			ssCmd >> CmdValue;
			gpPowerLevel->SetValue(CmdValue);
			EnqueueCommand(CmdSetPowerLevel);
		}
		else if (sCmd == "SetLeftVelocity")
		{
			ssCmd >> CmdValue;
			gpLeftVelocity->SetValue(CmdValue);
			EnqueueCommand(CmdSetLeftVelocity);
		}
		else if (sCmd == "SetRightVelocity")
		{
			ssCmd >> CmdValue;
			gpRightVelocity->SetValue(CmdValue);
			EnqueueCommand(CmdSetRightVelocity);
		}
		else if (sCmd == "SetLeftMovePosition")
		{
			ssCmd >> CmdValue;
			gpLeftMoveDistance->SetValue(CmdValue);
			EnqueueCommand(CmdSetLeftMoveDistance);
		}
		else if (sCmd == "SetRightMovePosition")
		{
			ssCmd >> CmdValue;
			gpRightMoveDistance->SetValue(CmdValue);
			EnqueueCommand(CmdSetRightMoveDistance);
		}
		else
		{
			gController->LogErrorString("Unknown command: " + sCmd);
		}
	}
	else
	{
		gController->LogErrorString("Command parsing error");
	}

	return DML_SUCCESS;
}

void ProcessAnalogReading(int RawValue, DML_ChannelRef *pSensorChan, AnalogSensorType SensorType)
{
	double k,m,b;  // linear approximation values obtained from spreadsheet
	double dCalcRange = 0.0;
	int MinRawThreshold;	// only values greater than this can be plugged into the linear approximation
	double dRangeAtThreshold;  // observed range in cm at MinRawThreshold

	switch (SensorType)
	{
	case SharpDP2G12:
		k = 1.0;
		m = 0.000179;
		b = -0.00342;
		MinRawThreshold = 68;
		dRangeAtThreshold = 110.0;
		break;
	case Sharp2Y0A02:
		k = 8.0;
		m = 0.00006862;
		b = 0.000802;
		MinRawThreshold = 76;
		dRangeAtThreshold = 160.0;
		break;
	default:
		assert(false);
	}

	if (RawValue > MinRawThreshold)
	{
		// Use a linear approximation to compute the range:
		dCalcRange = m * (double)RawValue + b;
		if (dCalcRange != 0.0)  // test for an extremely unlikely divide by 0
		{
			dCalcRange = (1.0 / dCalcRange) - k;  // this value is in "approximate cm"
		}
		else
		{
			dCalcRange = 1000.0;  // return 10 meters in this horribly unlikely situation
			gController->LogErrorString("ProcessAnalogReading:  Extremely unlikely divide by zero has occurred");
		}
	}
	else
	{
		// Approximate the range based on observed A/D values at extreme ranges
		// For both sensor types, the range increases approximately 10 cm for every 4 points that the A2D value drops past
		// the threshold value.
		int Decay = MinRawThreshold - RawValue;
		dCalcRange = dRangeAtThreshold + ((double)Decay * 2.5);
	}
	
	dCalcRange = dCalcRange * 10.0;  // convert to "approximate mm"
	pSensorChan->SetValue((int)dCalcRange);  // just truncate this result, sensors aren't that accurate anyway
}

void Executive::ReceiveUpdate(DML_ChannelRef *pChan, DML_Data &NewData)
{
	if (pChan == gpServicePeriod)
	{
		// Update the controller's service period:
		gController->SetServiceInterval(NewData.GetInt());
	}
	else
	{
		gController->LogErrorString("ReceiveUpdate:  unexpected channel update received, ignored");
	}
}

ChannelMonitor::ChannelMonitor()
{
	gpMonitor = this;
}

void ChannelMonitor::WatchChannel(DML_ChannelRef *pChan, bool UpdateOnAllWrites)
{
	MonitorInfo NewMonitor;
	DWORD		Index;
	STATUS		Status;
	LPCD		lpCDMonitorChan;

	if (UpdateOnAllWrites)
		NewMonitor.MonitorType = CT_DataWrite;
	else
		NewMonitor.MonitorType = CT_DataChange;
	NewMonitor.pChan = pChan;
	m_Monitors.push_back(NewMonitor);
	Index = m_Monitors.size() - 1;
	lpCDMonitorChan = pChan->CreateCDCopy();

	Status = Monitor(lpCDMonitorChan, TheMonitorProc, NewMonitor.MonitorType,
		             NewMonitor.pChan->GetController()->GetRouting(), Index);
	if (Status != DML_SUCCESS)
		throw DML_Exception(Status, lpCDMonitorChan);

	FreeCD(lpCDMonitorChan);
}

void ChannelMonitor::DestroyMonitors()
{
	MonitorInfo OldMonitor;
	LPCD		lpCDMonitorChan;
	DWORD		Index;

	while (m_Monitors.size())
	{
		OldMonitor = m_Monitors.back();
		Index = m_Monitors.size() - 1;
		m_Monitors.pop_back();
		lpCDMonitorChan = OldMonitor.pChan->CreateCDCopy();
		DestroyMonitor(lpCDMonitorChan, TheMonitorProc, OldMonitor.MonitorType,
			           OldMonitor.pChan->GetController()->GetRouting(), Index);
		FreeCD(lpCDMonitorChan);
	}
}

CEXTERN void __declspec(dllexport) TheMonitorProc(LPCCD lpCCD)
{
	DML_Data NewData;

	while(lpCCD)
	{
		if(lpCCD->lpCD)
		{
			NewData.UpdateWithDD(lpCCD->lpCD->lpDD);
			gpMonitor->ReceiveUpdate(gpMonitor->m_Monitors[lpCCD->UserData].pChan, NewData);
		}
		lpCCD = lpCCD->lpNext;
	}
}

// Must be executed in service loop (in sync with all of the background polling)
void Executive::ExecuteCommand(CommandID Cmd)
{
	static bool fTickTock = true;

	EnterCriticalSection(&m_PMDLock);
	switch (Cmd)
	{
	case CmdCommit:
		// alternate which axis moves first:
		if (fTickTock)
		{
			m_pLeft->CommitMotionChanges();
			m_pRight->CommitMotionChanges();
		}
		else
		{
			m_pRight->CommitMotionChanges();
			m_pLeft->CommitMotionChanges();
		}
		fTickTock = !fTickTock;
		break;
	case CmdZeroPosition:
		m_pLeft->SetCurrentPosition(0);
		m_pRight->SetCurrentPosition(0);
		break;
	case CmdSCurveMove:
		m_pLeft->SetSCurveProfile();
		m_pRight->SetSCurveProfile();
		break;
	case CmdTrapezoidalMove:
		m_pLeft->SetTrapezoidalProfile();
		m_pRight->SetTrapezoidalProfile();
		break;
	case CmdVelocityMove:
		m_pLeft->SetVelocityContouringProfile();
		m_pRight->SetVelocityContouringProfile();
		break;
	case CmdSmoothStop:
		m_pLeft->PerformSmoothStop();
		m_pRight->PerformSmoothStop();
		ClearCommandQueue();
		break;
	case CmdAbruptStop:
		m_pLeft->PerformAbruptStop();
		m_pRight->PerformAbruptStop();
		ClearCommandQueue();
		break;
	case CmdSetAcceleration:
		// Update acceleration and deceleration on both axes:
		m_pLeft->SetAcceleration(gpAcceleration->GetIntValue());
		m_pLeft->SetDeceleration(gpAcceleration->GetIntValue());
		m_pRight->SetAcceleration(gpAcceleration->GetIntValue());
		m_pRight->SetDeceleration(gpAcceleration->GetIntValue());
		break;
	case CmdSetJerk:
		m_pLeft->SetJerk(gpJerk->GetIntValue());
		m_pRight->SetJerk(gpJerk->GetIntValue());
		break;
	case CmdSetPowerLevel:
		m_pLeft->SetPowerLevel(gpPowerLevel->GetIntValue());
		m_pRight->SetPowerLevel(gpPowerLevel->GetIntValue());
		break;
	case CmdSetLeftVelocity:
		m_pLeft->SetVelocity(gpLeftVelocity->GetIntValue());
		break;
	case CmdSetRightVelocity:
		m_pRight->SetVelocity(gpRightVelocity->GetIntValue());
		break;
	case CmdSetLeftMoveDistance:
		m_pLeft->SetMoveDistance(gpLeftMoveDistance->GetIntValue() * -1);  // reverse sign of left wheel distance
		break;
	case CmdSetRightMoveDistance:
		m_pRight->SetMoveDistance(gpRightMoveDistance->GetIntValue());
		break;
	case CmdResetEventStatus:
		m_pLeft->ResetEventStatus(0);  // reset all event status register bits
		m_pRight->ResetEventStatus(0);
		break;
	default:
		assert(false);
	}
	LeaveCriticalSection(&m_PMDLock);
}

void Executive::EnqueueCommand(CommandID Cmd)
{
	EnterCriticalSection(&m_QueueLock);  // lock the queue
	m_CommandQueue.push(Cmd);
	LeaveCriticalSection(&m_QueueLock);  // unlock the queue
}

void Executive::ClearCommandQueue()
{
	EnterCriticalSection(&m_QueueLock);	// lock the queue
	while (!m_CommandQueue.empty())
	{
		m_CommandQueue.pop();
	}
	LeaveCriticalSection(&m_QueueLock);  // unlock the queue
}

void Executive::SetMotionDefaults()
{
//	gpAcceleration->SetValue(ciDefaultAcceleration);
//	gpLeftVelocity->SetValue(ciDefaultVelocity);
//	gpRightVelocity->SetValue(ciDefaultVelocity);
//	gpLeftMoveDistance->SetValue(0);
//	gpRightMoveDistance->SetValue(0);
//	gpPowerLevel->SetValue(0);

//	ExecuteCommand(CmdZeroPosition);
//	ExecuteCommand(CmdResetEventStatus);
//	ExecuteCommand(CmdSetAcceleration);
//	ExecuteCommand(CmdSetLeftVelocity);
//	ExecuteCommand(CmdSetRightVelocity);
//	ExecuteCommand(CmdSetLeftMoveDistance);
//	ExecuteCommand(CmdSetRightMoveDistance);
//	ExecuteCommand(CmdSetPowerLevel);
//	ExecuteCommand(CmdTrapezoidalMove);
//	ExecuteCommand(CmdCommit);
}

void Executive::ResetVelocity()
{
	gpLeftVelocity->SetValue(ciDefaultVelocity);
	gpRightVelocity->SetValue(ciDefaultVelocity);
	ExecuteCommand(CmdSetLeftVelocity);
	ExecuteCommand(CmdSetRightVelocity);
	ExecuteCommand(CmdCommit);
}

void Executive::Move(int mmLeftDistance, int mmRightDistance)
{
	gController->LogDebugString("Move:  left = " + lexical_cast<string>(mmLeftDistance) +
								", right = " + lexical_cast<string>(mmRightDistance));

	gpAcceleration->SetValue(ciDefaultAcceleration);
	gpLeftVelocity->SetValue(ciDefaultVelocity);
	gpRightVelocity->SetValue(ciDefaultVelocity);
	gpPowerLevel->SetValue(100);
	gpLeftMoveDistance->SetValue(mmLeftDistance);
	gpRightMoveDistance->SetValue(mmRightDistance);

	ExecuteCommand(CmdZeroPosition);
	ExecuteCommand(CmdResetEventStatus);
	ExecuteCommand(CmdTrapezoidalMove);
	ExecuteCommand(CmdSetAcceleration);
	ExecuteCommand(CmdSetLeftVelocity);
	ExecuteCommand(CmdSetRightVelocity);
	ExecuteCommand(CmdSetPowerLevel);
	ExecuteCommand(CmdSetLeftMoveDistance);
	ExecuteCommand(CmdSetRightMoveDistance);
	ExecuteCommand(CmdCommit);
}

// BIG ASSUMPTIONS:
// (1) We are doing trapezoidal moves starting with the position zeroed
// (2) Left and right axes are moving the same distance (we'll only check the left axis)
// (3) Robot isn't stalling -- need stasis detection behavior to confirm this
// (4) We are regularly polling for current commanded position
double Executive::GetMoveProgress()
{
	double dCurrentPos = (double) m_pLeft->m_pReadCurrentPosition->GetIntValue(); // readback (axis current calculated position in microsteps)
	double dDestPos = (double) m_pLeft->m_pSetDestinationPosition->GetIntValue(); // control (commanded destination position, trapezoidal/S-curve only)

	return dCurrentPos / dDestPos;
}

void Executive::MoveForward(int mmDistance)
{
	Move(mmDistance, mmDistance);
}

void Executive::Turn(int degreesAngle)
{
	int mmTurn = abs(ClipAndRoundToINT((double)degreesAngle * cdOneDegreeTurn));

	if (degreesAngle > 0)
	{
		// Left turn
		Move(mmTurn * -1, mmTurn);
	}
	else if (degreesAngle < 0)
	{
		// Right turn
		Move(mmTurn, mmTurn * -1);
	}
}

bool Executive::RobotInMotion()
{
	bool fAxesStopped = m_pLeft->m_pfMotionComplete->GetBoolValue() &&
						m_pRight->m_pfMotionComplete->GetBoolValue();
	return !fAxesStopped;
}

void Executive::AdjustPowerLevel(int PowerLevel)
{
	gpPowerLevel->SetValue(PowerLevel);
	ExecuteCommand(CmdSetPowerLevel);
	ExecuteCommand(CmdCommit);
}