pmdinterface.cpp

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

/*--------------------------------------------------*
 * PMDInterface.cpp:								*
 * DML interface for PMD chipset-based robot		*
 * (uses PMD's C-Motion library)					*
 *--------------------------------------------------*/

#define PMD_SERIAL_INTERFACE
#include "C-Motion.h"
#include "PMDTypes.h"
#include "PMDutil.h"

#include "dml.h"
#include "dmlchanref.h"
#include <string>
#include "Executive.h"
#include "PMDInterface.h"
#include "RobotMath.h"

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

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

// from PMDdiag.c:
//*****************************************************************************
const char *PMDGetErrorMessage(PMDresult errorCode)
{
	return errorCode == PMD_ERR_OK					? "No Error" : 

		errorCode == PMD_ERR_Reset					? "Processor Reset" :
		errorCode == PMD_ERR_InvalidInstruction		? "Invalid Instruction" :
		errorCode == PMD_ERR_InvalidAxis			? "Invalid Axis" :
		errorCode == PMD_ERR_InvalidParameter		? "Invalid Data Parameter" :
		errorCode == PMD_ERR_TraceRunning			? "Trace Currently Running" :
		errorCode == PMD_ERR_Flash					? "Flash Error" :
		errorCode == PMD_ERR_BlockOutOfBounds		? "Block Out of Bounds" :
		errorCode == PMD_ERR_TraceBufferZero		? "Zero Length Trace Buffer" :
		errorCode == PMD_ERR_BadSerialChecksum		? "Invalid Checksum" :
		errorCode == PMD_ERR_NotPrimaryPort			? "Invalid Instruction For Diagnostic Port" :
		errorCode == PMD_ERR_InvalidNegativeValue	? "Invalid Negative Value for Profile Mode" :
		errorCode == PMD_ERR_InvalidParameterChange ? "Invalid Parameter Change for Profile Mode" :
		errorCode == PMD_ERR_LimitEventPending		? "Invalid Move with Limit Event Pending" :
		errorCode == PMD_ERR_InvalidMoveIntoLimit	? "Invalid Move into Limit" :

		errorCode == PMD_ERR_NotConnected			? "Not Connected" :
		errorCode == PMD_ERR_ChipsetNotResponding	? "Chipset Not Responding" :
		errorCode == PMD_ERR_CommPortRead			? "Comm Port Read" :
		errorCode == PMD_ERR_CommPortWrite			? "Comm Port Write" :
		errorCode == PMD_ERR_InvalidSerialPort		? "Invalid Serial Port" :
		errorCode == PMD_ERR_InvalidDK2000IOBase	? "Invalid DK2000 IO Base" :
		errorCode == PMD_ERR_InvalidDK2000IRQ		? "Invalid DK2000 IRQ" :
		errorCode == PMD_ERR_InvalidDK2000IOMode	? "Invalid DK2000 IO Mode" :
		errorCode == PMD_ERR_DK2000NotInitialized	? "DK2000 Not Initialized" :
		errorCode == PMD_ERR_OpeningWindowsDriver	? "Cannot Open Windows Driver" :

		errorCode == PMD_ERR_Driver					? "Windows Driver Error" :
		errorCode == PMD_ERR_CommunicationsError	? "Communication Error" :
		errorCode == PMD_ERR_CommTimeoutError		? "Communication Timeout Error" :
		errorCode == PMD_ERR_ChecksumError			? "Checksum Error" :
		errorCode == PMD_ERR_CommandError			? "Command Error" :
		"Undefined error occurred";
}
//*****************************************************************************

// Log any unusual PMDresult as an error
void WatchPMDresultExpanded(PMDresult PMDr, char *pszFile, int Line)
{
	if (PMDr != PMD_NOERROR)
	{
		string sFile = pszFile;
		string sPMDMessage = PMDGetErrorMessage(PMDr);
		gController->LogErrorString("Unexpected PMDresult at file " + sFile + " line " +
										lexical_cast<string>(Line) + ": " + sPMDMessage);
	}
}

PMD_Axis::PMD_Axis(PMDAxis AxisNumber, string AxisName, bool fIOBoardPresent)
{
	string sName;
	m_AxisNumber = AxisNumber;
	m_pAxis = &m_hAxis;
	m_pAxis->transport_data = 0;
	m_fConnected = false;
	m_fIOBoardPresent = fIOBoardPresent;

	// Create DML channels for this axis:
	sName = AxisName + "-c_Acceleration";
	m_pAcceleration = new DML_ChannelRef(gController, sName, IntType,
										new DML_Data((int)0), "microsteps/cycle^2", false);
	sName = AxisName + "-c_CurrentPosition";
	m_pSetCurrentPosition = new DML_ChannelRef(gController, sName, IntType,
										new DML_Data((int)0), "microsteps", false);	
	sName = AxisName + "-c_Deceleration";
	m_pDeceleration = new DML_ChannelRef(gController, sName, IntType,
										new DML_Data((int)0), "microsteps/cycle^2", false);
	sName = AxisName + "-c_DestinationPosition";
	m_pSetDestinationPosition = new DML_ChannelRef(gController, sName, IntType,
										new DML_Data((int)0), "microsteps", false);
	sName = AxisName + "-c_Jerk";
	m_pJerk = new DML_ChannelRef(gController, sName, IntType,
										new DML_Data((int)0), "microsteps/cycle^3", false);
	sName = AxisName + "-c_PowerLevel";
	m_pPowerLevel = new DML_ChannelRef(gController, sName, IntType,
										new DML_Data((int)0), "0 to 32767", false);
	sName = AxisName + "-c_ProfileMode";
	m_pProfileMode = new DML_ChannelRef(gController, sName, IntType,
										new DML_Data((int)0), "", false);
	sName = AxisName + "-c_StopMode";
	m_pStopMode = new DML_ChannelRef(gController, sName, IntType,
										new DML_Data((int)0), "", false);
	sName = AxisName + "-c_Velocity";
	m_pVelocity = new DML_ChannelRef(gController, sName, IntType,
										new DML_Data((int)0), "microsteps/cycle", false);
	sName = AxisName + "-r_ActivityStatus";
	m_pActivityStatus = new DML_ChannelRef(gController, sName, IntType,
										new DML_Data((int)0), "", false);
	if (m_fIOBoardPresent)
	{
		for (int i = 0; i < ciNumAnalogInputs; i++)
		{
			sName = AxisName + "-r_AnalogInput" + lexical_cast<string>(i);
			m_pAnalogInputs[i] = new DML_ChannelRef(gController, sName, IntType,
										new DML_Data((int)0), "10-bit", false);
		}
	}
	sName = AxisName + "-r_CurrentPosition";
	m_pReadCurrentPosition = new DML_ChannelRef(gController, sName, IntType,
										new DML_Data((int)0), "microsteps", false);
	sName = AxisName + "-r_EventStatus";
	m_pEventStatus = new DML_ChannelRef(gController, sName, IntType,
										new DML_Data((int)0), "", false);
	sName = AxisName + "-r_fMotionComplete";
	m_pfMotionComplete = new DML_ChannelRef(gController, sName, BoolType,
										new DML_Data(true), "", false);
}

PMD_Axis::~PMD_Axis()
{
	if (m_pAxis->transport_data)
	{
		if (m_AxisNumber == PMDAxis1)
			PMDCloseAxisInterface(m_pAxis);  // free transport_data and close COM port
		else
			free(m_pAxis->transport_data);
	}

	delete m_pAcceleration;
	delete m_pSetCurrentPosition;
	delete m_pDeceleration;
	delete m_pSetDestinationPosition;
	delete m_pJerk;
	delete m_pPowerLevel;
	delete m_pProfileMode;
	delete m_pStopMode;
	delete m_pVelocity;
	delete m_pActivityStatus;
	if (m_fIOBoardPresent)
	{
		for (int i = 0; i < ciNumAnalogInputs; i++)
		{
			delete m_pAnalogInputs[i];
		}
	}
	delete m_pReadCurrentPosition;
	delete m_pEventStatus;
	delete m_pfMotionComplete;
}

bool PMD_Axis::Connect(int ComPortNumber)
{
	PMDresult RetVal;
	m_fConnected = false;

	assert(m_AxisNumber == PMDAxis1);  // set up serial interface on Axis 1, make this connection first
	RetVal = PMDSetupAxisInterface_Serial(m_pAxis, PMDAxis1, (PMDuint8)ComPortNumber, DEVICE_TYPE_M3);
	if (RetVal == PMD_NOERROR)
	{
		RetVal = PMDChipsetReset(m_pAxis);
		if (RetVal == PMD_NOERROR)
		{
			PMDSerial_SetMultiDropAddress(m_pAxis->transport_data, 0);
			gController->LogInformationString("Connected to primary PMD axis on COM port #" + lexical_cast<string>(ComPortNumber));
			m_fConnected = true;
		}
		else
		{
			gController->LogErrorString("Unable to reset primary PMD axis on COM port #" + lexical_cast<string>(ComPortNumber));
		}
	}
	else
	{
		gController->LogErrorString("Unable to set up primary PMD axis on COM port #" + lexical_cast<string>(ComPortNumber));
	}

	return m_fConnected;
}

bool PMD_Axis::Connect(PMD_Axis *pParentAxis)
{
	PMDresult RetVal;

	assert(m_AxisNumber == PMDAxis2);  // connect second axis
	if (m_pAxis->transport_data == 0)
	{
		PMDCreateMultiDropHandle(m_pAxis, pParentAxis->m_pAxis, PMDAxis1, 1);
	}
	RetVal = PMDChipsetReset(m_pAxis);
    if (RetVal == PMD_NOERROR)
	{
		m_fConnected = true;
	}
	else
	{
		gController->LogErrorString("Unable to reset secondary PMD axis");
		m_fConnected = false;
	}

	return m_fConnected;
}

void PMD_Axis::Reset()
{
	PMDuint16 CycleTime;  // in microseconds

	WatchPMD(PMDChipsetReset(m_pAxis));
	WatchPMD(PMDSetOutputMode(m_pAxis, PMDMotorOutputPWMSignMagnitude));
	WatchPMD(PMDSetLimitSwitchMode(m_pAxis, 0));  // no limit switch events to worry about	
	WatchPMD(PMDSetPhaseCounts(m_pAxis, (PMDuint16)(ciMicrostepsPerStep<<2)));
	WatchPMD(PMDGetSampleTime(m_pAxis, &CycleTime));
	if (CycleTime != ciPMD3410CycleTime)
	{
		gController->LogErrorString("PMD chip reporting unexpected cycle time: " + lexical_cast<string>(CycleTime));
	}
	WatchPMD(PMDSetActualPositionUnits(m_pAxis, 1));  // use microsteps for position, not counts
	WatchPMD(PMDSetEncoderSource(m_pAxis, PMDEncoderSourceNone));
	WatchPMD(PMDSetAutoStopMode(m_pAxis, PMDAutoStopDisabled));
	WatchPMD(PMDSetMotorMode(m_pAxis, PMDMotorOn));
}

void PMD_Axis::CommitMotionChanges()
{
	WatchPMD(PMDUpdate(m_pAxis));
}

void PMD_Axis::PerformAbruptStop()
{
	m_pStopMode->SetValue((int)PMDAbruptStopMode);
	WatchPMD(PMDSetStopMode(m_pAxis, PMDAbruptStopMode));
	CommitMotionChanges();
}

void PMD_Axis::PerformSmoothStop()
{
	m_pStopMode->SetValue((int)PMDSmoothStopMode);
	WatchPMD(PMDSetStopMode(m_pAxis, PMDSmoothStopMode));
	CommitMotionChanges();
}

void PMD_Axis::PollActivityStatus()
{
	PMDuint16 StatusValue;
	WatchPMD(PMDGetActivityStatus(m_pAxis, &StatusValue));
	m_pActivityStatus->SetValue(StatusValue);
}

void PMD_Axis::PollEventStatus()
{
	PMDuint16 StatusValue;
	WatchPMD(PMDGetEventStatus(m_pAxis, &StatusValue));
	m_pEventStatus->SetValue(StatusValue);
	bool fMotionComplete = (StatusValue & PMDEventMotionCompleteMask);
	m_pfMotionComplete->SetValue(fMotionComplete);
}

void PMD_Axis::ResetEventStatus(PMDuint16 PMDEventMask)
{
	WatchPMD(PMDResetEventStatus(m_pAxis, PMDEventMask));
	PollEventStatus();
}

void PMD_Axis::PollAnalogInputs()
{
	PMDuint16 AnalogValue;

	if (m_fIOBoardPresent)
	{
		for (int i = 0; i < ciNumAnalogInputs; i++)
		{
			WatchPMD(PMDReadAnalog(m_pAxis, i, &AnalogValue));
			// Result is shifted left 6 bits, so shift it right:
			assert((AnalogValue & 0x003F) == 0);
			AnalogValue = AnalogValue >> 6;
			m_pAnalogInputs[i]->SetValue(AnalogValue);
		}
	}
}

void PMD_Axis::PollCurrentPosition()
{
	PMDint32 PositionValue;
	// Note that the "commanded position" is just where the trajectory generator THINKS the axis is currently at, based on
	// whatever trajectory it is executing, and relative to whatever position was initially set via the SetCurrentPosition
	// method.
	WatchPMD(PMDGetCommandedPosition(m_pAxis, &PositionValue));
	m_pReadCurrentPosition->SetValue(PositionValue);
}

void PMD_Axis::SetCurrentPosition(int SetPoint)
{
	// SetPoint is in mm
	// PMDSetActualPosition units are microsteps (signed 32 bit integer)
	// Motion constants are expressed in double precision to maximize precision.

	int PMDSetValue = ClipAndRoundToINT((double)SetPoint * cdMillimetersToMicrosteps);
	// Record the value we are setting for display/debug purposes:
	m_pSetCurrentPosition->SetValue(PMDSetValue);
	// PMDSetActualPosition basically "resets the odometer".  All subsequent trajectories will be referenced
	// from the new setpoint.
	WatchPMD(PMDSetActualPosition(m_pAxis, PMDSetValue));
	CommitMotionChanges();
}

void PMD_Axis::SetAcceleration(int SetPoint)
{
	const double cdHumanToPMDShiftedAcceleration = cdHumanToPMDAcceleration * 65536.0;  // compute at compile-time
	int SetValue = ClipAndRoundToINT((double)SetPoint * cdHumanToPMDAcceleration);
	m_pAcceleration->SetValue(SetValue);
	PMDuint32 PMDShiftedValue = ClipAndRoundToULONG((double)SetPoint * cdHumanToPMDShiftedAcceleration);
	WatchPMD(PMDSetAcceleration(m_pAxis, PMDShiftedValue));
}

void PMD_Axis::SetDeceleration(int SetPoint)
{
	const double cdHumanToPMDShiftedAcceleration = cdHumanToPMDAcceleration * 65536.0;  // compute at compile-time
	int SetValue = ClipAndRoundToINT((double)SetPoint * cdHumanToPMDAcceleration);
	m_pDeceleration->SetValue(SetValue);
	PMDuint32 PMDShiftedValue = ClipAndRoundToULONG((double)SetPoint * cdHumanToPMDShiftedAcceleration);
	WatchPMD(PMDSetDeceleration(m_pAxis, PMDShiftedValue));
}

void PMD_Axis::SetJerk(int SetPoint)
{
	const double cdHumanToPMDShiftedJerk = cdHumanToPMDJerk * 4294967296.0;  // compute at compile-time
	int SetValue = ClipAndRoundToINT((double)SetPoint * cdHumanToPMDJerk);
	m_pJerk->SetValue(SetValue);
	PMDuint32 PMDShiftedValue = ClipAndRoundToULONG((double)SetPoint * cdHumanToPMDShiftedJerk);
	WatchPMD(PMDSetJerk(m_pAxis, PMDShiftedValue));
}

void PMD_Axis::SetMoveDistance(int SetPoint)
{
	int PMDSetValue = ClipAndRoundToINT((double)SetPoint * cdMillimetersToMicrosteps);
	m_pSetDestinationPosition->SetValue(PMDSetValue);
	WatchPMD(PMDSetPosition(m_pAxis, PMDSetValue));
}

void PMD_Axis::SetPowerLevel(int SetPoint)  // SetPoint is 0 to 100 (%)
{
	const double cdHumanToPMDPowerLevel = 32767.0 / 100.0;  // compute at compile-time
	short SetValue = ClipAndRoundToSHORT((double)SetPoint * cdHumanToPMDPowerLevel);
	m_pPowerLevel->SetValue((int)SetValue);
	WatchPMD(PMDSetMotorCommand(m_pAxis, SetValue));
}

void PMD_Axis::SetSCurveProfile()
{
	m_pProfileMode->SetValue((int)PMDSCurveProfile);
	WatchPMD(PMDSetProfileMode(m_pAxis, PMDSCurveProfile));
}

void PMD_Axis::SetTrapezoidalProfile()
{
	m_pProfileMode->SetValue((int)PMDTrapezoidalProfile);
	WatchPMD(PMDSetProfileMode(m_pAxis, PMDTrapezoidalProfile));
}

void PMD_Axis::SetVelocityContouringProfile()
{
	m_pProfileMode->SetValue((int)PMDVelocityContouringProfile);
	WatchPMD(PMDSetProfileMode(m_pAxis, PMDVelocityContouringProfile));
}

void PMD_Axis::SetVelocity(int SetPoint)
{
	const double cdHumanToPMDShiftedVelocity = cdHumanToPMDVelocity * 65536.0;  // compute at compile-time
	int SetValue = ClipAndRoundToINT((double)SetPoint * cdHumanToPMDVelocity);
	m_pVelocity->SetValue(SetValue);
	PMDint32 PMDShiftedValue = ClipAndRoundToINT((double)SetPoint * cdHumanToPMDShiftedVelocity);
	WatchPMD(PMDSetVelocity(m_pAxis, PMDShiftedValue));
}