c-motion.c

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

///////////////////////////////////////////////////////////////////////////
//
//  c-motion.c -- Magellan C-Motion API implementation
//
//  Performance Motion Devices, Inc.
//  
///////////////////////////////////////////////////////////////////////////


#include "c-motion.h"
#include "PMDocode.h"

///////////////////////////////////////////////////////////////////////////
// Profile Generation
PMDresult PMDSetProfileMode(PMDAxisInterface axis_intf, PMDuint16 mode)
{
	return SendCommandWord(axis_intf, PMDOPSetProfileMode, mode);
}

PMDresult PMDGetProfileMode(PMDAxisInterface axis_intf, PMDuint16* mode)
{
	return SendCommandGetWord(axis_intf, PMDOPGetProfileMode, mode);
}

PMDresult PMDSetPosition(PMDAxisInterface axis_intf, PMDint32 position)
{
	return SendCommandLong(axis_intf, PMDOPSetPosition, position);
}

PMDresult PMDGetPosition(PMDAxisInterface axis_intf, PMDint32* position)
{
	return SendCommandGetLong(axis_intf, PMDOPGetPosition, (PMDuint32*)position);
}

PMDresult PMDSetVelocity(PMDAxisInterface axis_intf, PMDint32 velocity)
{
	return SendCommandLong(axis_intf, PMDOPSetVelocity, velocity);
}

PMDresult PMDGetVelocity(PMDAxisInterface axis_intf, PMDint32* velocity)
{
	return SendCommandGetLong(axis_intf, PMDOPGetVelocity, (PMDuint32*)velocity);
}

PMDresult PMDSetStartVelocity(PMDAxisInterface axis_intf, PMDuint32 velocity)
{
	return SendCommandLong(axis_intf, PMDOPSetStartVelocity, velocity);
}

PMDresult PMDGetStartVelocity(PMDAxisInterface axis_intf, PMDuint32* velocity)
{
	return SendCommandGetLong(axis_intf, PMDOPGetStartVelocity, velocity);
}

PMDresult PMDSetAcceleration(PMDAxisInterface axis_intf, PMDuint32 acceleration)
{
	return SendCommandLong(axis_intf, PMDOPSetAcceleration, acceleration);
}

PMDresult PMDGetAcceleration(PMDAxisInterface axis_intf, PMDuint32* acceleration)
{
	return SendCommandGetLong(axis_intf, PMDOPGetAcceleration, acceleration);
}

PMDresult PMDSetDeceleration(PMDAxisInterface axis_intf, PMDuint32 deceleration)
{
	return SendCommandLong(axis_intf, PMDOPSetDeceleration, deceleration);
}

PMDresult PMDGetDeceleration(PMDAxisInterface axis_intf, PMDuint32* deceleration)
{
	return SendCommandGetLong(axis_intf, PMDOPGetDeceleration, deceleration);
}

PMDresult PMDSetJerk(PMDAxisInterface axis_intf, PMDuint32 jerk)
{
	return SendCommandLong(axis_intf, PMDOPSetJerk, jerk);
}

PMDresult PMDGetJerk(PMDAxisInterface axis_intf, PMDuint32* jerk)
{
	return SendCommandGetLong(axis_intf, PMDOPGetJerk, jerk);
}

PMDresult PMDSetGearRatio(PMDAxisInterface axis_intf, PMDint32 ratio)
{
	return SendCommandLong(axis_intf, PMDOPSetGearRatio, (PMDuint32)ratio);
}

PMDresult PMDGetGearRatio(PMDAxisInterface axis_intf, PMDint32* ratio)
{
	return SendCommandGetLong(axis_intf, PMDOPGetGearRatio, (PMDuint32*)ratio);
}

PMDresult PMDSetGearMaster(PMDAxisInterface axis_intf, PMDAxis masterAxis, PMDuint8 source)
{
	PMDuint16 value = (PMDuint16)((masterAxis & nibbleMask) | ((source & nibbleMask) << 8));
	return SendCommandWord(axis_intf, PMDOPSetGearMaster, value);
}

PMDresult PMDGetGearMaster(PMDAxisInterface axis_intf, PMDAxis* masterAxis, PMDuint8* source)
{
	PMDuint16 result;
	PMDuint16 value;

	result = SendCommandGetWord(axis_intf, PMDOPGetGearMaster, &value);
	*masterAxis = (PMDAxis)(value & nibbleMask);
	*source = (PMDuint8)((value >> 8) & nibbleMask);

	return result;
}

PMDresult PMDSetStopMode(PMDAxisInterface axis_intf, PMDuint16 mode)
{
	return SendCommandWord(axis_intf, PMDOPSetStopMode, mode);
}

PMDresult PMDGetStopMode(PMDAxisInterface axis_intf, PMDuint16* mode)
{
	return SendCommandGetWord(axis_intf, PMDOPGetStopMode, mode);
}

PMDresult PMDGetCommandedPosition(PMDAxisInterface axis_intf, PMDint32* position)
{
	return SendCommandGetLong(axis_intf, PMDOPGetCommandedPosition, (PMDuint32*)position);
}

PMDresult PMDGetCommandedVelocity(PMDAxisInterface axis_intf, PMDint32* velocity)
{
	return SendCommandGetLong(axis_intf, PMDOPGetCommandedVelocity, (PMDuint32*)velocity);
}

PMDresult PMDGetCommandedAcceleration(PMDAxisInterface axis_intf, PMDint32 *acceleration)
{
	return SendCommandGetLong(axis_intf, PMDOPGetCommandedAcceleration, (PMDuint32*)acceleration);
}


///////////////////////////////////////////////////////////////////////////
// Servo Filter
PMDresult PMDSetKp(PMDAxisInterface axis_intf, PMDuint16 kp)
{
	return SendCommandWord(axis_intf, PMDOPSetKp, kp);
}

PMDresult PMDGetKp(PMDAxisInterface axis_intf, PMDuint16* kp)
{
	return SendCommandGetWord(axis_intf, PMDOPGetKp, kp);
}

PMDresult PMDSetKd(PMDAxisInterface axis_intf, PMDuint16 kd)
{
	return SendCommandWord(axis_intf, PMDOPSetKd, kd);
}

PMDresult PMDGetKd(PMDAxisInterface axis_intf, PMDuint16* kd)
{
	return SendCommandGetWord(axis_intf, PMDOPGetKd, kd);
}

PMDresult PMDSetKi(PMDAxisInterface axis_intf, PMDuint16 ki)
{
	return SendCommandWord(axis_intf, PMDOPSetKi, ki);
}

PMDresult PMDGetKi(PMDAxisInterface axis_intf, PMDuint16* ki)
{
	return SendCommandGetWord(axis_intf, PMDOPGetKi, ki);
}

PMDresult PMDSetKvff(PMDAxisInterface axis_intf, PMDuint16 kvff)
{
	return SendCommandWord(axis_intf, PMDOPSetKvff, kvff);
}

PMDresult PMDGetKvff(PMDAxisInterface axis_intf, PMDuint16* kvff)
{
	return SendCommandGetWord(axis_intf, PMDOPGetKvff, kvff);
}

PMDresult PMDSetKaff(PMDAxisInterface axis_intf, PMDuint16 kaff)
{
	return SendCommandWord(axis_intf, PMDOPSetKaff, kaff);
}

PMDresult PMDGetKaff(PMDAxisInterface axis_intf, PMDuint16* kaff)
{
	return SendCommandGetWord(axis_intf, PMDOPGetKaff, kaff);
}

PMDresult PMDSetKout(PMDAxisInterface axis_intf, PMDuint16 kout)
{
	return SendCommandWord(axis_intf, PMDOPSetKout, kout);
}

PMDresult PMDGetKout(PMDAxisInterface axis_intf, PMDuint16* kout)
{
	return SendCommandGetWord(axis_intf, PMDOPGetKout, kout);
}

PMDresult PMDSetIntegrationLimit(PMDAxisInterface axis_intf, PMDuint32 limit)
{
	return SendCommandLong(axis_intf, PMDOPSetIntegrationLimit, limit);
}

PMDresult PMDGetIntegrationLimit(PMDAxisInterface axis_intf, PMDuint32* limit)
{
	return SendCommandGetLong(axis_intf, PMDOPGetIntegrationLimit, limit);
}

PMDresult PMDSetLimitSwitchMode(PMDAxisInterface axis_intf, PMDuint16 mode)
{
	return SendCommandWord(axis_intf, PMDOPSetLimitSwitchMode, mode);
}

PMDresult PMDGetLimitSwitchMode(PMDAxisInterface axis_intf, PMDuint16* mode)
{
	return SendCommandGetWord(axis_intf, PMDOPGetLimitSwitchMode, mode);
}

PMDresult PMDSetMotorLimit(PMDAxisInterface axis_intf, PMDuint16 limit)
{
	return SendCommandWord(axis_intf, PMDOPSetMotorLimit, limit);
}

PMDresult PMDGetMotorLimit(PMDAxisInterface axis_intf, PMDuint16* limit)
{
	return SendCommandGetWord(axis_intf, PMDOPGetMotorLimit, limit);
}

PMDresult PMDSetMotorBias(PMDAxisInterface axis_intf, PMDint16 bias)
{
	return SendCommandWord(axis_intf, PMDOPSetMotorBias, (PMDuint16)bias);
}

PMDresult PMDGetMotorBias(PMDAxisInterface axis_intf, PMDint16* bias)
{
	return SendCommandGetWord(axis_intf, PMDOPGetMotorBias, (PMDuint16*)bias);
}

PMDresult PMDSetPositionErrorLimit(PMDAxisInterface axis_intf, PMDuint32 limit)
{
	return SendCommandLong(axis_intf, PMDOPSetPositionErrorLimit, limit);
}

PMDresult PMDGetPositionErrorLimit(PMDAxisInterface axis_intf, PMDuint32* limit)
{
	return SendCommandGetLong(axis_intf, PMDOPGetPositionErrorLimit, limit);
}

PMDresult PMDSetAutoStopMode(PMDAxisInterface axis_intf, PMDuint16 mode)
{
	return SendCommandWord(axis_intf, PMDOPSetAutoStopMode, mode);
}

PMDresult PMDGetAutoStopMode(PMDAxisInterface axis_intf, PMDuint16* mode)
{
	return SendCommandGetWord(axis_intf, PMDOPGetAutoStopMode, mode);
}

PMDresult PMDSetDerivativeTime(PMDAxisInterface axis_intf, PMDuint16 derivativeTime)
{
	return SendCommandWord(axis_intf, PMDOPSetDerivativeTime, derivativeTime);
}

PMDresult PMDGetDerivativeTime(PMDAxisInterface axis_intf, PMDuint16* derivativeTime)
{
	return SendCommandGetWord(axis_intf, PMDOPGetDerivativeTime, derivativeTime);
}

PMDresult PMDSetSettleTime(PMDAxisInterface axis_intf, PMDuint16 settleTime)
{
	return SendCommandWord(axis_intf, PMDOPSetSettleTime, settleTime);
}

PMDresult PMDGetSettleTime(PMDAxisInterface axis_intf, PMDuint16* settleTime)
{
	return SendCommandGetWord(axis_intf, PMDOPGetSettleTime, settleTime);
}

PMDresult PMDSetSettleWindow(PMDAxisInterface axis_intf, PMDuint16 settleWindow)
{
	return SendCommandWord(axis_intf, PMDOPSetSettleWindow, settleWindow);
}

PMDresult PMDGetSettleWindow(PMDAxisInterface axis_intf, PMDuint16* settleWindow)
{
	return SendCommandGetWord(axis_intf, PMDOPGetSettleWindow, settleWindow);
}

PMDresult PMDSetTrackingWindow(PMDAxisInterface axis_intf, PMDuint16 trackingWindow)
{
	return SendCommandWord(axis_intf, PMDOPSetTrackingWindow, trackingWindow);
}

PMDresult PMDGetTrackingWindow(PMDAxisInterface axis_intf, PMDuint16* trackingWindow)
{
	return SendCommandGetWord(axis_intf, PMDOPGetTrackingWindow, trackingWindow);
}

PMDresult PMDSetMotionCompleteMode(PMDAxisInterface axis_intf, PMDuint16 mode)
{
	return SendCommandWord(axis_intf, PMDOPSetMotionCompleteMode, mode);
}

PMDresult PMDGetMotionCompleteMode(PMDAxisInterface axis_intf, PMDuint16* mode)
{
	return SendCommandGetWord(axis_intf, PMDOPGetMotionCompleteMode, mode);
}

PMDresult PMDClearPositionError(PMDAxisInterface axis_intf)
{
	return SendCommand(axis_intf, PMDOPClearPositionError);
}

PMDresult PMDGetDerivative(PMDAxisInterface axis_intf, PMDint16* derivative)
{
	return SendCommandGetWord(axis_intf, PMDOPGetDerivative, (PMDuint16*)derivative);
}

PMDresult PMDGetIntegral(PMDAxisInterface axis_intf, PMDint32* integral)
{
	return SendCommandGetLong(axis_intf, PMDOPGetIntegral, (PMDuint32*)integral);
}

PMDresult PMDGetPositionError(PMDAxisInterface axis_intf, PMDint32* error)
{
	return SendCommandGetLong(axis_intf, PMDOPGetPositionError, (PMDuint32*)error);
}

#ifdef PILOT
PMDresult PMDSetSampleTime(PMDAxisInterface axis_intf, PMDuint16 sampleTime)
{
	return SendCommandWord(axis_intf, PMDOPSetSampleTime, sampleTime);
}

PMDresult PMDGetSampleTime(PMDAxisInterface axis_intf, PMDuint16* sampleTime)
{
	return SendCommandGetWord(axis_intf, PMDOPGetSampleTime, sampleTime);
}
#endif
#ifdef MAGELLAN
PMDresult PMDSetSampleTime(PMDAxisInterface axis_intf, PMDuint32 sampleTime)
{
	return SendCommandLong(axis_intf, PMDOPSetSampleTime, sampleTime);
}

PMDresult PMDGetSampleTime(PMDAxisInterface axis_intf, PMDuint32* sampleTime)
{
	return SendCommandGetLong(axis_intf, PMDOPGetSampleTime, sampleTime);
}
#endif

PMDresult PMDSetBiQuadCoefficient(PMDAxisInterface axis_intf, PMDuint16 coefficientID, PMDuint16 filterID, PMDint16 value)
{
	PMDuint16 firstdataword = (PMDuint16)((filterID << 8) | coefficientID);

	return SendCommandWordWord(axis_intf, PMDOPSetBiQuadCoefficient, firstdataword, value);
}

PMDresult PMDGetBiQuadCoefficient(PMDAxisInterface axis_intf, PMDuint16 coefficientID, PMDuint16 filterID, PMDint16* value)
{
	PMDuint16 firstdataword = (PMDuint16)((filterID << 8) | coefficientID);

	return SendCommandWordGetWord(axis_intf, PMDOPGetBiQuadCoefficient, firstdataword, (PMDuint16*)value);
}


///////////////////////////////////////////////////////////////////////////
// Parameter Update & Breakpoints
PMDresult PMDSetBreakpoint(PMDAxisInterface axis_intf, PMDuint16 breakpointID, 
						   PMDAxis sourceAxis, PMDuint8 action, PMDuint8 trigger)
{
	PMDuint16 value = (PMDuint16)((sourceAxis & nibbleMask) | 
		((action & nibbleMask) << 4) | ((trigger & byteMask) << 8));
	return SendCommandWordWord(axis_intf, PMDOPSetBreakpoint, breakpointID, value);
}

PMDresult PMDGetBreakpoint(PMDAxisInterface axis_intf, PMDuint16 breakpointID, 
						   PMDAxis* sourceAxis, PMDuint8* action, PMDuint8* trigger)
{
	PMDuint16 result;
	PMDuint16 value;

	result = SendCommandWordGetWord(axis_intf, PMDOPGetBreakpoint, breakpointID, &value);

	*sourceAxis = (PMDAxis)(value & nibbleMask);
	*action = (PMDuint8)((value >> 4) & nibbleMask);
	*trigger = (PMDuint8)((value >> 8) & byteMask);

	return result;
}

PMDresult PMDSetBreakpointValue(PMDAxisInterface axis_intf, PMDuint16 breakpointID, PMDint32 breakpointValue)
{
	return SendCommandWordLong(axis_intf, PMDOPSetBreakpointValue, breakpointID, (PMDuint32)breakpointValue);
}

PMDresult PMDGetBreakpointValue(PMDAxisInterface axis_intf, PMDuint16 breakpointID, PMDint32* breakpointValue)
{
	return SendCommandWordGetLong(axis_intf, PMDOPGetBreakpointValue, breakpointID, (PMDuint32*)breakpointValue);
}

PMDresult PMDUpdate(PMDAxisInterface axis_intf)
{
	return SendCommand(axis_intf, PMDOPUpdate);
}

PMDresult PMDMultiUpdate(PMDAxisInterface axis_intf, PMDuint16 axisMask)
{
	return SendCommandWord(axis_intf, PMDOPMultiUpdate, axisMask);
}


// Interrupt Processing
PMDresult PMDSetInterruptMask(PMDAxisInterface axis_intf, PMDuint16 interruptMask)
{
	return SendCommandWord(axis_intf, PMDOPSetInterruptMask, interruptMask);
}

PMDresult PMDGetInterruptMask(PMDAxisInterface axis_intf, PMDuint16* interruptMask)
{
	return SendCommandGetWord(axis_intf, PMDOPGetInterruptMask, interruptMask);
}

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

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

	result = SendCommand(axis_intf, PMDOPClearInterrupt);

	axis_intf->axis = axis;

	return result;
}

PMDresult PMDGetInterruptAxis(PMDAxisInterface axis_intf, PMDuint16* interruptingAxisMask)
{
	return SendCommandGetWord(axis_intf, PMDOPGetInterruptAxis, interruptingAxisMask);
}


///////////////////////////////////////////////////////////////////////////
// Status Register Control