executive.cpp

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

// Executive.cpp

#include <vector>
#include "Executive.h"
#include "DMLChanRef.h"
#include "version.h"
#include "PMDInterface.h"
#include "RobotMath.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;

#include "DMLMonitorInclude.h"

const int ciNumAnalogSamples = 3;  // number of times to sample each analog input -- per service interval

ChannelMonitor *gpMonitor = 0;		// allows access by C monitor proc
CEXTERN void __declspec(dllexport) TheMonitorProc(LPCCD lpCCD);

Executive		*gController;

// Executive controller channels:
DML_ChannelRef *gpComPort;
DML_ChannelRef *gpServicePeriod;
DML_ChannelRef *gpServiceElapsedTime;
DML_ChannelRef *gpServicesPerSecond;

// Robot control and status readback channels:
DML_ChannelRef *gIRLeftSide;
DML_ChannelRef *gIRRightSide;
DML_ChannelRef *gIRForward;
DML_ChannelRef *gIRLeft30;
DML_ChannelRef *gIRRight30;
DML_ChannelRef *gIRBeakDownLeft;
DML_ChannelRef *gIRBeakDownCenter;
DML_ChannelRef *gIRBeakDownRight;

DML_ChannelRef *gpAcceleration;			// Control robot's acceleration AND deceleration for BOTH wheels
DML_ChannelRef *gpJerk;					// Control jerk setting for BOTH wheels
DML_ChannelRef *gpPowerLevel;			// Control robot's power level (0 to 100%)
DML_ChannelRef *gpLeftVelocity;			// Control robot's left wheel velocity (positive value = forward)
DML_ChannelRef *gpRightVelocity;		// Control robot's right wheel velocity (positive value = forward)
DML_ChannelRef *gpLeftMoveDistance;		// Control robot's left wheel move distance (positive value = forward)
DML_ChannelRef *gpRightMoveDistance;	// Control robot's right wheel move distance (positive value = forward)

IntMovingAverage *gpSmoothedAnalog[ciNumAnalogInputs];

// Arbiter and Behaviors:
Arbiter gArbiter;
StopBehavior *gpStopBehaviors[ciNumBeakBehaviors];
StartBehavior *gpStartBehaviors[ciNumBeakBehaviors];
AvoidBehavior *gpAvoidBehaviors[ciNumAvoidBehaviors];
ScanBehavior *gpScanBehavior;
DriveBehavior *gpDriveBehavior;
//TestDriveBehavior *gpTestDriveBehavior;

enum AnalogSensorType
{
	SharpDP2G12,		// IR range-finding sensor, minimum range 10 cm, maximum range 80 cm
	Sharp2Y0A02,		// IR range-finding sensor, minimum range 20 cm, maximum range 150 cm
};

// Using the raw analog value, perform conversion based on SensorType and store result to pSensorChan
void ProcessAnalogReading(int RawValue, DML_ChannelRef *pSensorChan, AnalogSensorType SensorType);

// use 100 ms service interval - last parameter of constructor
Executive::Executive() :
	DMLController("1000",AUTHOR_OCS,"EXEC",NO_SPECIFIC_APPLICATION,"WBR Control Center Executive", ciDefaultControllerServicePeriod)
	{
	}

// The required DllMain function
#pragma warning(disable : 4100)
BOOL   WINAPI   DllMain (HANDLE hInst,ULONG ul_reason_for_call,LPVOID lpReserved)
{
	switch(ul_reason_for_call)
	{
		case DLL_PROCESS_ATTACH:
			/*A new process is attempting to access the DLL; 
				one thread is assumed.*/
			gController = new Executive();

		    return 1;

		case DLL_THREAD_ATTACH:
				/*A new thread of an existing process is attempting to access 
				the DLL; this call is made beginning with the second 
				thread of a process attaching to the DLL.*/
			    return 1;
		
		case DLL_PROCESS_DETACH:
				/*A process is detaching from the DLL.*/
				delete(gController);
				gController = NULL;
			    return 1;
		
		case DLL_THREAD_DETACH:
			/*One of the additional threads (not the first thread) of a process is
			detaching from the DLL.*/
		    return 1;
	}
	return 0;
}
#pragma warning(default : 4100)

STATUS Executive::CreateChannels()
{
	string sChannelName;
	CreateVersionChannels();

	for (int i = 0; i < ciNumAnalogInputs; i++)
	{
		gpSmoothedAnalog[i] = new IntMovingAverage(ciNumAnalogSamples);
	}

	// Setup our critical section:
	InitializeCriticalSection(&m_QueueLock);
	InitializeCriticalSection(&m_PMDLock);

	// Attempt to keep this list in alphabetical order by channel name

	gpServicePeriod = new DML_ChannelRef(gController,
							"Controllers-Executive-c_ServicePeriod",
							IntType,
							new DML_Data(ciDefaultControllerServicePeriod),
							"ms",
							true);		// Persistent channel
	
	gpServiceElapsedTime = new DML_ChannelRef(gController,
							"Controllers-Executive-r_ServiceElapsedTime",
							IntType,
							new DML_Data((int)0),
							"ms",
							false);

	gpServicesPerSecond = new DML_ChannelRef(gController,
							"Controllers-Executive-r_ServicesPerSecond",
							IntType,
							new DML_Data((int)0),
							"",
							false);

	gpComPort = new DML_ChannelRef(gController,
							"M3-ComPort",
							IntType,
							new DML_Data(3),
							"",
							true);		// Persistent channel

	gIRLeftSide = new DML_ChannelRef(gController,
									"IRSensor-LeftSide",
									IntType,
									new DML_Data(0),
									"mm",
									false);
	gIRRightSide = new DML_ChannelRef(gController,
									"IRSensor-RightSide",
									IntType,
									new DML_Data(0),
									"mm",
									false);
	gIRLeft30 = new DML_ChannelRef(gController,
									"IRSensor-Left30",
									IntType,
									new DML_Data(0),
									"mm",
									false);
	gIRRight30 = new DML_ChannelRef(gController,
									"IRSensor-Right30",
									IntType,
									new DML_Data(0),
									"mm",
									false);
	gIRForward = new DML_ChannelRef(gController,
									"IRSensor-Forward",
									IntType,
									new DML_Data(0),
									"mm",
									false);
	gIRBeakDownCenter = new DML_ChannelRef(gController,
									"IRSensor-BeakDownCenter",
									IntType,
									new DML_Data(0),
									"mm",
									false);
	gIRBeakDownLeft = new DML_ChannelRef(gController,
									"IRSensor-BeakDownLeft",
									IntType,
									new DML_Data(0),
									"mm",
									false);
	gIRBeakDownRight = new DML_ChannelRef(gController,
									"IRSensor-BeakDownRight",
									IntType,
									new DML_Data(0),
									"mm",
									false);

	gpAcceleration = new DML_ChannelRef(gController,
									"Motor-Acceleration",
									IntType,
									new DML_Data(0),
									"mm/second^2",
									false);
	gpJerk = new DML_ChannelRef(gController,
									"Motor-Jerk",
									IntType,
									new DML_Data(0),
									"mm/second^3",
									false);
	gpPowerLevel = new DML_ChannelRef(gController,
									"Motor-PowerLevel",
									IntType,
									new DML_Data(0),
									"percent",
									false);
	gpLeftVelocity = new DML_ChannelRef(gController,
									"Motor-LeftVelocity",
									IntType,
									new DML_Data(0),
									"mm/second",
									false);
	gpRightVelocity = new DML_ChannelRef(gController,
									"Motor-RightVelocity",
									IntType,
									new DML_Data(0),
									"mm/second",
									false);
	gpLeftMoveDistance = new DML_ChannelRef(gController,
									"Motor-LeftMoveDistance",
									IntType,
									new DML_Data(0),
									"mm",
									false);
	gpRightMoveDistance = new DML_ChannelRef(gController,
									"Motor-RightMoveDistance",
									IntType,
									new DML_Data(0),
									"mm",
									false);


	m_pRight = new PMD_Axis(PMDAxis1, "PMD-RightAxis", false);
	m_pLeft  = new PMD_Axis(PMDAxis2, "PMD-LeftAxis", true);

	return DML_SUCCESS;
}

STATUS Executive::DestroyChannels()
{
	int i;

	DestroyMonitors();

	// Delete behaviors:
	for (i = 0; i < ciNumBeakBehaviors; i++)
	{
		delete gpStopBehaviors[i];
		delete gpStartBehaviors[i];
	}
	for (i = 0; i < ciNumAvoidBehaviors; i++)
	{
		delete gpAvoidBehaviors[i];
	}

	delete gpScanBehavior;
	delete gpDriveBehavior;
//	delete gpTestDriveBehavior;

	// Delete DML channels:
	delete m_pLeft;
	delete m_pRight;

	delete gpServiceElapsedTime;
	delete gpServicePeriod;
	delete gpServicesPerSecond;
	delete gpComPort;

	delete gIRLeftSide;
	delete gIRRightSide;
	delete gIRLeft30;
	delete gIRRight30;
	delete gIRForward;
	delete gIRBeakDownCenter;
	delete gIRBeakDownLeft;
	delete gIRBeakDownRight;

	delete gpAcceleration;
	delete gpJerk;
	delete gpPowerLevel;
	delete gpLeftVelocity;
	delete gpRightVelocity;
	delete gpLeftMoveDistance;
	delete gpRightMoveDistance;

	for (i = 0; i < ciNumAnalogInputs; i++)
	{
		delete gpSmoothedAnalog[i];
	}

	DeleteCriticalSection(&m_QueueLock);
	DeleteCriticalSection(&m_PMDLock);

	return DML_SUCCESS;
}

STATUS Executive::CreateMonitors()
{
	WatchChannel(gpServicePeriod);
	gArbiter.ClearBehaviorList();

	// Instantiate our behaviors.
	// We will watch for a Stop signal from a human on all three beak sensors:
	gpStopBehaviors[0] = new StopBehavior(&gArbiter, gIRBeakDownLeft, 0);
	gArbiter.AddBehavior(gpStopBehaviors[0]);
	gpStopBehaviors[1] = new StopBehavior(&gArbiter, gIRBeakDownCenter, 1);
	gArbiter.AddBehavior(gpStopBehaviors[1]);
	gpStopBehaviors[2] = new StopBehavior(&gArbiter, gIRBeakDownRight, 2);
	gArbiter.AddBehavior(gpStopBehaviors[2]);

	gpScanBehavior = new ScanBehavior(&gArbiter);
	gArbiter.AddBehavior(gpScanBehavior);

	gpAvoidBehaviors[0] = new AvoidBehavior(&gArbiter, gIRLeft30, 200);
	gArbiter.AddBehavior(gpAvoidBehaviors[0]);
	gpAvoidBehaviors[1] = new AvoidBehavior(&gArbiter, gIRForward, 200);
	gArbiter.AddBehavior(gpAvoidBehaviors[1]);
	gpAvoidBehaviors[2] = new AvoidBehavior(&gArbiter, gIRRight30, 200);
	gArbiter.AddBehavior(gpAvoidBehaviors[2]);
	gpAvoidBehaviors[3] = new AvoidBehavior(&gArbiter, gIRBeakDownLeft, 200);
	gArbiter.AddBehavior(gpAvoidBehaviors[3]);
	gpAvoidBehaviors[4] = new AvoidBehavior(&gArbiter, gIRBeakDownCenter, 200);
	gArbiter.AddBehavior(gpAvoidBehaviors[4]);
	gpAvoidBehaviors[5] = new AvoidBehavior(&gArbiter, gIRBeakDownRight, 200);
	gArbiter.AddBehavior(gpAvoidBehaviors[5]);

	gpDriveBehavior = new DriveBehavior(&gArbiter);
	gArbiter.AddBehavior(gpDriveBehavior);

//	gpTestDriveBehavior = new TestDriveBehavior(&gArbiter);
//	gArbiter.AddBehavior(gpTestDriveBehavior);

	gpStartBehaviors[0] = new StartBehavior(&gArbiter, gIRBeakDownLeft, 0);
	gArbiter.AddBehavior(gpStartBehaviors[0]);
	gpStartBehaviors[1] = new StartBehavior(&gArbiter, gIRBeakDownCenter, 1);
	gArbiter.AddBehavior(gpStartBehaviors[1]);
	gpStartBehaviors[2] = new StartBehavior(&gArbiter, gIRBeakDownRight, 2);
	gArbiter.AddBehavior(gpStartBehaviors[2]);

	return DML_SUCCESS;
}

void Executive::StartService()
{
	// Loop until we successfully connect to the M3 (user may have change the COM port)
	while (m_pRight->Connect(gpComPort->GetIntValue()) == false)
	{
		Sleep(2000);
	}

	m_pLeft->Connect(m_pRight);
	m_pLeft->Reset();
	m_pRight->Reset();

	m_pLeft->SetCurrentPosition(0);
	m_pRight->SetCurrentPosition(0);

	m_ServiceCount = 0;
	DMLTime(&m_CountStartTime);

	SetMotionDefaults();

	// Enable starting behaviors:
	gArbiter.DisableBehaviors(cAllBehaviors);
	gArbiter.EnableBehaviors(cBehaviorStart);

	// for TestDriveBehavior...
	//	gArbiter.EnableBehaviors(cBehaviorDrive | cBehaviorSafetyCritical);

}

void Executive::Service()
{
	DML_TIME Now;
		
	// Keep this part at the start of Service():
	// Calculate actual number of service loops per second that we are achieving:
	DMLTime(&Now);
	if (TimeDiff(&m_CountStartTime, &Now) >= 1000)
	{
		gpServicesPerSecond->SetValue(m_ServiceCount);
		DMLTime(&m_CountStartTime);
		m_ServiceCount = 0;
	}

	// Record the previous service loop's elapsed time:
	gpServiceElapsedTime->SetValue((int)gController->GetServiceElapsedTime());

	// Body of service loop:

	// Status polling first:
	EnterCriticalSection(&m_PMDLock);
	m_pLeft->PollActivityStatus();
	m_pRight->PollActivityStatus();
	m_pLeft->PollEventStatus();
	m_pRight->PollEventStatus();
	m_pLeft->PollCurrentPosition();
	m_pRight->PollCurrentPosition();
	LeaveCriticalSection(&m_PMDLock);

	// Acquire and process new analog input data -- oversampled to reduce noise
	for (int iOverSample = 0; iOverSample < ciNumAnalogSamples; iOverSample++)
	{
		EnterCriticalSection(&m_PMDLock);
		m_pLeft->PollAnalogInputs();
		LeaveCriticalSection(&m_PMDLock);
		for (int i = 0; i < ciNumAnalogInputs; i++)
		{
			gpSmoothedAnalog[i]->AddValue(m_pLeft->m_pAnalogInputs[i]->GetIntValue());
		}
	}
	// Update sensor channels from new analog input values.  This block also maps analog inputs to sensor locations,
	// as well as specifying the type of sensor connected to each input:
	ProcessAnalogReading(gpSmoothedAnalog[0]->GetAverage(), gIRRightSide,		SharpDP2G12);
	ProcessAnalogReading(gpSmoothedAnalog[1]->GetAverage(), gIRForward,			SharpDP2G12);
	ProcessAnalogReading(gpSmoothedAnalog[2]->GetAverage(), gIRRight30,			Sharp2Y0A02);
	ProcessAnalogReading(gpSmoothedAnalog[3]->GetAverage(), gIRLeft30,			Sharp2Y0A02);
	ProcessAnalogReading(gpSmoothedAnalog[4]->GetAverage(), gIRLeftSide,		SharpDP2G12);
	ProcessAnalogReading(gpSmoothedAnalog[5]->GetAverage(), gIRBeakDownRight,	SharpDP2G12);
	ProcessAnalogReading(gpSmoothedAnalog[6]->GetAverage(), gIRBeakDownCenter,	SharpDP2G12);
	ProcessAnalogReading(gpSmoothedAnalog[7]->GetAverage(), gIRBeakDownLeft,	SharpDP2G12);

	// Allow the arbiter to give any enabled behaviors a time slice to act now:
	gArbiter.Tick();

	// Execute all commands that have queued up, in order:
	while (!m_CommandQueue.empty())
	{
		CommandID Cmd;

		EnterCriticalSection(&m_QueueLock);	// lock the queue
		Cmd = m_CommandQueue.front();		// grab a local copy of the first command