c-motion.c

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

	PMDAxis axis = axis_intf->axis;
	PMDuint16 result;

	// set the axis number to zero
	axis_intf->axis = 0;

	result = SendCommandWord(axis_intf, PMDOPSetTraceMode, mode);

	axis_intf->axis = axis;

	return result;
}

PMDresult PMDGetTraceMode(PMDAxisInterface axis_intf, PMDuint16* mode)
{
	PMDAxis axis = axis_intf->axis;
	PMDuint16 result;

	// set the axis number to zero
	axis_intf->axis = 0;

	result = SendCommandGetWord(axis_intf, PMDOPGetTraceMode, mode);

	axis_intf->axis = axis;

	return result;
}

PMDresult PMDSetTracePeriod(PMDAxisInterface axis_intf, PMDuint16 tracePeriod)
{
	PMDAxis axis = axis_intf->axis;
	PMDuint16 result;

	// set the axis number to zero
	axis_intf->axis = 0;

	result = SendCommandWord(axis_intf, PMDOPSetTracePeriod, tracePeriod);

	axis_intf->axis = axis;

	return result;
}

PMDresult PMDGetTracePeriod(PMDAxisInterface axis_intf, PMDuint16* tracePeriod)
{
	PMDAxis axis = axis_intf->axis;
	PMDuint16 result;

	// set the axis number to zero
	axis_intf->axis = 0;

	result = SendCommandGetWord(axis_intf, PMDOPGetTracePeriod, tracePeriod);

	axis_intf->axis = axis;

	return result;
}

PMDresult PMDSetTraceVariable(PMDAxisInterface axis_intf, PMDuint16 traceID, PMDAxis traceAxis, PMDuint8 variable)
{
	PMDAxis axis = axis_intf->axis;
	PMDuint16 result;
	PMDuint16 value;

	// set the axis number to zero
	axis_intf->axis = 0;

	value = (PMDuint16)((traceAxis & nibbleMask) | ((variable & byteMask) << 8));
	result = SendCommandWordWord(axis_intf, PMDOPSetTraceVariable, traceID, value);

	axis_intf->axis = axis;

	return result;
}

PMDresult PMDGetTraceVariable(PMDAxisInterface axis_intf, PMDuint16 traceID, PMDAxis *traceAxis, PMDuint8* variable)
{
	PMDAxis axis = axis_intf->axis;
	PMDuint16 result;
	PMDuint16 value;

	// set the axis number to zero
	axis_intf->axis = 0;

	result = SendCommandWordGetWord(axis_intf, PMDOPGetTraceVariable, traceID, &value);
	*traceAxis = (PMDAxis)(value & nibbleMask);
	*variable = (PMDuint8)((value >> 8) & byteMask);

	axis_intf->axis = axis;

	return result;
}

PMDresult PMDSetTraceStart(PMDAxisInterface axis_intf, PMDAxis traceAxis, PMDuint8 condition, PMDuint8 bit, PMDuint8 state)
{
	PMDuint16 value = (PMDuint16)((traceAxis & nibbleMask) | ((condition & nibbleMask) << 4));
	PMDAxis axis = axis_intf->axis;
	PMDuint16 result;

	// set the axis number to zero
	axis_intf->axis = 0;
	
	switch(condition)
	{
	case PMDTraceConditionEventStatus:
	case PMDTraceConditionActivityStatus:
	case PMDTraceConditionSignalStatus:
		value |= ((bit & nibbleMask) << 8) | ((state & nibbleMask) << 12);
		break;
	default:
		break;
	}
	result = SendCommandWord(axis_intf, PMDOPSetTraceStart, value);

	axis_intf->axis = axis;

	return result;
}

PMDresult PMDGetTraceStart(PMDAxisInterface axis_intf, PMDAxis *traceAxis, PMDuint8* condition, PMDuint8* bit, PMDuint8* state)
{
	PMDuint16 result;
	PMDuint16 value;
	PMDAxis axis = axis_intf->axis;

	// set the axis number to zero
	axis_intf->axis = 0;

	result = SendCommandGetWord(axis_intf, PMDOPGetTraceStart, &value);
	*traceAxis = (PMDAxis)((value & nibbleMask));
	*condition = (PMDuint8)((value >> 4) & nibbleMask);
	*bit = (PMDuint8)((value >> 8) & nibbleMask);
	*state = (PMDuint8)((value >> 12) & nibbleMask);

	axis_intf->axis = axis;

	return result;
}

PMDresult PMDSetTraceStop(PMDAxisInterface axis_intf, PMDAxis traceAxis, PMDuint8 condition, PMDuint8 bit, PMDuint8 state)
{
	PMDuint16 value = (PMDuint16)((traceAxis & nibbleMask) | ((condition & nibbleMask) << 4));
	PMDAxis axis = axis_intf->axis;
	PMDuint16 result;

	// set the axis number to zero
	axis_intf->axis = 0;
	
	switch(condition)
	{
	case PMDTraceConditionEventStatus:
	case PMDTraceConditionActivityStatus:
	case PMDTraceConditionSignalStatus:
		value |= ((bit & nibbleMask) << 8) | ((state & nibbleMask) << 12);
		break;
	default:
		break;
	}
	result = SendCommandWord(axis_intf, PMDOPSetTraceStop, value);

	axis_intf->axis = axis;

	return result;
}

PMDresult PMDGetTraceStop(PMDAxisInterface axis_intf, PMDAxis *traceAxis, PMDuint8* condition, PMDuint8* bit, PMDuint8* state)
{
	PMDuint16 result;
	PMDuint16 value;
	PMDAxis axis = axis_intf->axis;

	// set the axis number to zero
	axis_intf->axis = 0;

	result = SendCommandGetWord(axis_intf, PMDOPGetTraceStop, &value);
	*traceAxis = (PMDAxis)((value & nibbleMask));
	*condition = (PMDuint8)((value >> 4) & nibbleMask);
	*bit = (PMDuint8)((value >> 8) & nibbleMask);
	*state = (PMDuint8)((value >> 12) & nibbleMask);

	axis_intf->axis = axis;

	return result;
}


PMDresult PMDGetTraceStatus(PMDAxisInterface axis_intf, PMDuint16* status)
{
	PMDAxis axis = axis_intf->axis;
	PMDuint16 result;

	// set the axis number to zero
	axis_intf->axis = 0;

	result = SendCommandGetWord(axis_intf, PMDOPGetTraceStatus, status);

	axis_intf->axis = axis;

	return result;
}

PMDresult PMDGetTraceCount(PMDAxisInterface axis_intf, PMDuint32* count)
{
	PMDAxis axis = axis_intf->axis;
	PMDuint16 result;

	// set the axis number to zero
	axis_intf->axis = 0;

	result = SendCommandGetLong(axis_intf, PMDOPGetTraceCount, count);

	axis_intf->axis = axis;

	return result;
}


///////////////////////////////////////////////////////////////////////////
// Miscellaneous
PMDresult PMDSetAxisMode(PMDAxisInterface axis_intf, PMDuint16 mode)
{
	return SendCommandWord(axis_intf, PMDOPSetAxisMode, mode);
}

PMDresult PMDGetAxisMode(PMDAxisInterface axis_intf, PMDuint16* mode)
{
	return SendCommandGetWord(axis_intf, PMDOPGetAxisMode, mode);
}

PMDresult PMDSetAxisOutSource(PMDAxisInterface axis_intf, PMDAxis sourceAxis, PMDuint8 bit, PMDuint8 sourceRegister)
{
	PMDuint16 value = (PMDuint16)((sourceAxis & nibbleMask) | ((bit & nibbleMask) << 4) 
		| ((sourceRegister & nibbleMask) << 8));
		
	return SendCommandWord(axis_intf, PMDOPSetAxisOutSource, value);
}

PMDresult PMDGetAxisOutSource(PMDAxisInterface axis_intf, PMDAxis* sourceAxis, PMDuint8* bit, PMDuint8* sourceRegister)
{
	PMDuint16 result;
	PMDuint16 value;

	result = SendCommandGetWord(axis_intf, PMDOPGetAxisOutSource, &value);

	*sourceAxis = (PMDAxis)(value & nibbleMask);

	value = (PMDuint16)(value >> 4);
	*bit = (PMDuint8)(value & nibbleMask);

	value = (PMDuint16)(value >> 4);
	*sourceRegister = (PMDuint8)(value & nibbleMask);

	return result;
}

PMDresult PMDWriteIO(PMDAxisInterface axis_intf, PMDuint16 address, PMDuint16 data)
{
	return SendCommandWordWord(axis_intf, PMDOPWriteIO, address, data);
}

PMDresult PMDReadIO(PMDAxisInterface axis_intf, PMDuint16 address, PMDuint16* data)
{
	return SendCommandWordGetWord(axis_intf, PMDOPReadIO, address, data);
}

PMDresult PMDReadAnalog(PMDAxisInterface axis_intf, PMDuint16 analogID, PMDuint16* data)
{
	return SendCommandWordGetWord(axis_intf, PMDOPReadAnalog, analogID, data);
}

PMDresult PMDReset(PMDAxisInterface axis_intf)
{
	PMDAxis axis = axis_intf->axis;
	PMDuint16 result;

	// for the reset command, the axis number MUST be zero
	axis_intf->axis = 0;

	result = SendCommand(axis_intf, PMDOPReset);

	axis_intf->axis = axis;

	return result;
}

PMDresult PMDNoOperation(PMDAxisInterface axis_intf)
{
	return SendCommand(axis_intf, PMDOPNoOperation);
}

PMDresult PMDGetVersion(PMDAxisInterface axis_intf, PMDuint16 *family, PMDuint16 *motorType, 
						PMDuint16 *numberAxes, PMDuint16 *special_and_chip_count,
						PMDuint16 *custom, PMDuint16 *major, PMDuint16 *minor)
{
	PMDuint16 chip = 0, software = 0;
	PMDuint16 result;

	result = SendCommandGetWordWord(axis_intf, PMDOPGetVersion, &chip, &software);

	*family = (PMDuint16)((chip & 0xF000) >> 12);
	*motorType  = (PMDuint16)((chip & 0x0F00) >> 8);
	*numberAxes = (PMDuint16)((chip & 0x00F0) >> 4);
	*special_and_chip_count = (PMDuint16)(chip & 0x000F);

	*custom = (PMDuint16)((software & 0xFF00) >> 8);
	*major  = (PMDuint16)((software & 0x00F0) >> 4);
	*minor  = (PMDuint16) (software & 0x000F);

	return result;
}

PMDresult PMDGetHostIOError(PMDAxisInterface axis_intf, PMDuint16 *hostIOError)
{
	return SendCommandGetWord(axis_intf, PMDOPGetHostIOError, hostIOError);
}

PMDresult PMDSetSerialPortMode(PMDAxisInterface axis_intf, PMDuint8 baud, PMDuint8 parity, 
					PMDuint8 stopBits, PMDuint8 protocol, PMDuint8 multiDropID)
{
	PMDuint16 serialData;

	serialData = (PMDuint16)(baud | (parity << 4) | (stopBits << 6) | 
		(protocol << 7) | 
		// bits 9 and 10 are reserved
		(multiDropID << 11));
	return SendCommandWord(axis_intf, PMDOPSetSerialPortMode, serialData);
}

PMDresult PMDGetSerialPortMode(PMDAxisInterface axis_intf, PMDuint8* baud, PMDuint8* parity, 
					PMDuint8* stopBits, PMDuint8* protocol, PMDuint8* multiDropID)
{
	PMDuint16 serialData;
	PMDuint16 result;

	result = SendCommandGetWord(axis_intf, PMDOPGetSerialPortMode, &serialData);

	*baud = (PMDuint8)(serialData & 0x000F);
	*parity = (PMDuint8)((serialData & 0x0030) >> 4);
	*stopBits = (PMDuint8)((serialData & 0x0040) >> 6);
	*protocol = (PMDuint8)((serialData & 0x0180) >> 7);
	*multiDropID = (PMDuint8)((serialData & 0xF800) >> 11);

	return result;
}

PMDresult PMDSetCANMode(PMDAxisInterface axis_intf, PMDuint8 baud, PMDuint8 nodeID)
{
	PMDuint16 canData;

	canData = (PMDuint16)((baud << 13) | nodeID );

	return SendCommandWord(axis_intf, PMDOPSetCANMode, canData);
}

PMDresult PMDGetCANMode(PMDAxisInterface axis_intf, PMDuint8* baud, PMDuint8* nodeID)
{
	PMDuint16 canData;
	PMDuint16 result;

	result = SendCommandGetWord(axis_intf, PMDOPGetCANMode, &canData);

	*baud = (PMDuint8)((canData & 0xE000) >> 13);
	*nodeID = (PMDuint8)(canData & 0x007F);

	return result;
}

PMDresult PMDSetSPIMode(PMDAxisInterface axis_intf, PMDuint16 mode)
{
	return SendCommandWord(axis_intf, PMDOPSetSPIMode, mode);
}

PMDresult PMDGetSPIMode(PMDAxisInterface axis_intf, PMDuint16* mode)
{
	return SendCommandGetWord(axis_intf, PMDOPGetSPIMode, mode);
}

PMDresult PMDGetTime(PMDAxisInterface axis_intf, PMDuint32* numberChipCycles)
{
	return SendCommandGetLong(axis_intf, PMDOPGetTime, numberChipCycles);
}

PMDresult PMDGetChecksum(PMDAxisInterface axis_intf, PMDuint32* checksum)
{
	return SendCommandGetLong(axis_intf, PMDOPGetChecksum, checksum);
}

PMDresult PMDSetStepRange(PMDAxisInterface axis_intf, PMDuint16 range)
{
	return SendCommandWord(axis_intf, PMDOPSetStepRange, range);
}

PMDresult PMDGetStepRange(PMDAxisInterface axis_intf, PMDuint16* range)
{
	return SendCommandGetWord(axis_intf, PMDOPGetStepRange, range);
}

PMDresult PMDSetSynchronizationMode(PMDAxisInterface axis_intf, PMDuint16 mode)
{
	return SendCommandWord(axis_intf, PMDOPSetSynchronizationMode, mode);
}

PMDresult PMDGetSynchronizationMode(PMDAxisInterface axis_intf, PMDuint16* mode)
{
	return SendCommandGetWord(axis_intf, PMDOPGetSynchronizationMode, mode);
}

///////////////////////////////////////////////////////////////////////////
// C-Motion provided functions

PMDresult PMDGetStatus(PMDAxisInterface axis_intf)
{
	return axis_intf->transport.GetStatus(axis_intf->transport_data);
}

PMDresult PMDHasError(PMDAxisInterface axis_intf)
{
	return axis_intf->transport.HasError(axis_intf->transport_data);
}

PMDresult PMDIsReady(PMDAxisInterface axis_intf)
{
	return axis_intf->transport.IsReady(axis_intf->transport_data);
}

PMDresult PMDHasInterrupt(PMDAxisInterface axis_intf)
{
	return axis_intf->transport.HasInterrupt(axis_intf->transport_data);
}

PMDresult PMDHardReset(PMDAxisInterface axis_intf)
{
	return axis_intf->transport.HardReset(axis_intf->transport_data);
}

void PMDCloseAxisInterface(PMDAxisInterface axis_intf)
{
	axis_intf->transport.Close(axis_intf->transport_data);
}

void PMDGetCMotionVersion(PMDuint8* MajorVersion, PMDuint8* MinorVersion)
{
	*MajorVersion = CMOTION_MAJOR_VERSION;
	*MinorVersion = CMOTION_MINOR_VERSION;
}