pmdutil.c

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

// ****************************************************************
// PMDutil.c : Defines sample test and utility functions
//
// Performance Motion Devices, Inc.
//

#ifdef WIN32
#include <windows.h>
#endif

#include <stdio.h>
#include <memory.h>

#include "PMDconio.h"
#include "PMDutil.h"
#include "PMDdiag.h"
#include "PMDocode.h"
#include "PMDecode.h"


//*****************************************************************************
// Initialize array of axis handles
// define the PMD_?_INTERFACE definitions in the project file
// as a preprocessor definition based on the interface used
int InitializeAxisHandles(PMDAxisHandle* phAxis)
{
    PMDAxisHandle* pAxis0 = phAxis;
    PMDAxis axis;
    PMDuint16 status;
    PMDuint16 result = PMD_ERR_OK;

#ifdef PMD_DRIVER_INTERFACE
	if (pAxis0->transport_data == NULL) // prevent multiple initializations
		PMDSetupAxisInterface_Driver( pAxis0, PMDAxis1 );
	PMDprintf("Using PMDMCG2 parallel ISA driver interface\n");
#endif

#ifdef PMD_PARALLEL_INTERFACE
	if (pAxis0->transport_data == NULL) // prevent multiple initializations
		PMDSetupAxisInterface_Parallel( pAxis0, PMDAxis1, 0x340 );
	PMDprintf("Using direct parallel IO interface\n");
#endif

#ifdef PMD_W32SERIAL_INTERFACE
	if (pAxis0->transport_data == NULL) // prevent multiple initializations
		PMDSetupAxisInterface_Serial( pAxis0, PMDAxis1, 0 );
	PMDprintf("Using serial interface\n");
#endif

#ifdef PMD_PCI_INTERFACE
	if (pAxis0->transport_data == NULL) // prevent multiple initializations
		PMDSetupAxisInterface_PCI( pAxis0, PMDAxis1, 0 );
	PMDprintf("Using PCI interface\n");
#endif

#ifndef PMD_DRIVER_INTERFACE
#ifndef PMD_PARALLEL_INTERFACE
#ifndef PMD_W32SERIAL_INTERFACE
#ifndef PMD_PCI_INTERFACE
    PMDprintf("Error: No PMD Interface defined in PMDutil.c \n");
#endif
#endif
#endif
#endif

    if (pAxis0->transport_data == NULL)
	{
		PMDprintf("Failed to initialize board interface\n");
		return 0;
	}

    // create handles to the other axes
	for (axis=PMDAxis2; axis<PMD_MAX_AXES; axis++)
		PMDCopyAxisInterface(&phAxis[axis], pAxis0, axis);

    // the command error bit may have been set after the last
	// reset, so clear it
	result = PMDGetHostIOError( pAxis0, &status );

/*  enable this if you always want the processor in a known state at start up
	// attempt board reset 
	if (!PMDChipsetReset(pAxis0))
	{
		PMDprintf("Reset Failed.\n");
		return 0;
	}
*/

    return (result == PMD_ERR_OK);
}


//*****************************************************************************
void PMDCopyAxisInterface(PMDAxisHandle* dest_axis_handle, PMDAxisHandle* src_axis_handle, PMDAxis axis_number)
{
	// copy the handle
	memcpy(dest_axis_handle, src_axis_handle, sizeof( PMDAxisHandle ) );

	// set the axis we are talking to with this handle
	dest_axis_handle->axis = axis_number;
}

//*****************************************************************************
PMDresult PMDChipsetReset(PMDAxisHandle* phAxis)
{
	PMDresult result;
	PMDuint16 status;

	// reset the PMD chip set that this axis resides on
	// if more than one chip set is present, all of them should be
	// reset here
	result = PMDReset(phAxis);

	// If diagnostics is enabled in the interface c module or 
	// if it is the serial interface the result will be PMD_ERR_Reset 
	// otherwise the result should be PMD_ERR_CommandError
	// meaning the Command Error bit is set in the status word
	// If it isn't either of these then there is some other error
	if ( (result != PMD_ERR_Reset) && (result != PMD_ERR_CommandError))
	{
//		PMDprintf("Error1: %s\n", PMDGetErrorMessage(result));
		return result;
	}

    // wait for chip to reset
    HostDelay(20); // ms

	// after the reset the HostIOError should return PMD_ERR_Reset
	// this will also reset the HostIOError and the Command Error bit
	result = PMDGetHostIOError(phAxis, &status);

	// the above command should execute without error but we need to check
	if ( (result != PMD_ERR_OK) || (status != PMD_ERR_Reset && status != PMD_ERR_OK) )
	{
		PMDprintf("Error: %s\n", PMDGetErrorMessage(result));
		PMDprintf("Status: %s\n", PMDGetErrorMessage(status));
	}
	return result;
}

//*****************************************************************************
void SetupSingleAxisTrace(PMDAxisHandle* phAxis, PMDuint32 bufferlength)
{

	// Set tracing parameters
	PMDSetBufferLength( phAxis, 0, bufferlength );
	PMDSetTraceVariable( phAxis, PMDTrace1, PMDAxis1, PMDTraceCommandedPosition );
	PMDSetTraceVariable( phAxis, PMDTrace2, PMDAxis1, PMDTraceCommandedVelocity );
	PMDSetTraceVariable( phAxis, PMDTrace3, PMDAxis1, PMDTraceCommandedAcceleration );
	PMDSetTraceVariable( phAxis, PMDTrace4, PMDAxis1, PMDTraceCommandedPosition );
	PMDSetTraceMode( phAxis, PMDTraceOneTime); 
	PMDSetTracePeriod( phAxis, 10); // cycle(s) per trace
	PMDSetTraceStart( phAxis, PMDAxis1, PMDTraceConditionUpdate, 0, 0 );

}

//*****************************************************************************
void SetupTrace(PMDAxisHandle* phAxis, PMDuint32 bufferlength)
{
	PMDSetTraceStop( phAxis, 0, PMDTraceConditionImmediate, 0, PMDTraceTriggerStateHigh );
	
	// setup the trace buffer
	PMDSetBufferStart( phAxis, 0, DK_MEMORY_BASE );
	PMDSetBufferLength( phAxis, 0, bufferlength );

	PMDSetTraceVariable( phAxis, PMDTrace1, PMDAxis1, PMDTraceNoVariable );
	PMDSetTraceVariable( phAxis, PMDTrace2, PMDAxis1, PMDTraceNoVariable );
	PMDSetTraceVariable( phAxis, PMDTrace3, PMDAxis1, PMDTraceNoVariable ); 
	PMDSetTraceVariable( phAxis, PMDTrace4, PMDAxis1, PMDTraceNoVariable );
/*
	// single axis dynamics trace
	PMDSetTraceVariable( phAxis, PMDTrace1, PMDAxis1, PMDTraceCommandedPosition );
	PMDSetTraceVariable( phAxis, PMDTrace2, PMDAxis1, PMDTraceCommandedVelocity );
	PMDSetTraceVariable( phAxis, PMDTrace3, PMDAxis1, PMDTraceCommandedAcceleration ); 
	PMDSetTraceVariable( phAxis, PMDTrace4, PMDAxis1, PMDTraceCommandedJerk );

    // 4 axis position trace
	PMDSetTraceVariable( phAxis, PMDTrace1, PMDAxis1, PMDTraceCommandedPosition );
	PMDSetTraceVariable( phAxis, PMDTrace1, PMDAxis2, PMDTraceCommandedPosition );
	PMDSetTraceVariable( phAxis, PMDTrace1, PMDAxis3, PMDTraceCommandedPosition );
	PMDSetTraceVariable( phAxis, PMDTrace1, PMDAxis4, PMDTraceCommandedPosition );

	// 2 axis commanded vs actual position trace
	PMDSetTraceVariable( phAxis, PMDTrace1, PMDAxis1, PMDTraceCommandedPosition );
	PMDSetTraceVariable( phAxis, PMDTrace2, PMDAxis1, PMDTraceActualPosition );
	PMDSetTraceVariable( phAxis, PMDTrace3, PMDAxis2, PMDTraceCommandedPosition );
	PMDSetTraceVariable( phAxis, PMDTrace4, PMDAxis2, PMDTraceActualPosition );
*/
	// 2 axis commanded position vs velocity trace
	PMDSetTraceVariable( phAxis, PMDTrace1, PMDAxis1, PMDTraceCommandedPosition );
	PMDSetTraceVariable( phAxis, PMDTrace2, PMDAxis2, PMDTraceCommandedPosition );
	PMDSetTraceVariable( phAxis, PMDTrace3, PMDAxis1, PMDTraceCommandedVelocity );
	PMDSetTraceVariable( phAxis, PMDTrace4, PMDAxis2, PMDTraceCommandedVelocity );
	PMDSetTraceMode( phAxis, PMDTraceOneTime );
	
	// trace will start on the next update command on Axis 1
	PMDSetTraceStart( phAxis, PMDAxis1, PMDTraceConditionUpdate, 0, PMDTraceTriggerStateHigh );
    // trigger a trace start on InMotion bit instead of an update so that it is 
    // not prematurely started when initializing axis parameters
	//PMDSetTraceStart( phAxis, PMDAxis1, PMDTraceConditionActivityStatus, PMDActivityInMotionBit, PMDTraceTriggerStateHigh );
}

//*****************************************************************************
void DisplayTraceResults(PMDAxisHandle* phAxis, PMDuint32 bufferlength, PMDuint32 nVariables)
{
	PMDuint32 i;
	PMDuint32 readindex;
	PMDlong32 bufferdata;
	PMDuint16 status;

	do 
	{
		PMDGetTraceStatus(phAxis, &status);
		PMDprintf("%d\t", status);
		PMDGetBufferReadIndex(phAxis, 0, &readindex);
		PMDprintf("%ld", readindex);

		for (i=0; i<nVariables; i++) 
		{
			PMDReadBuffer(phAxis, 0, &bufferdata);
			PMDprintf("\t%ld", bufferdata);
		}
		PMDprintf("\n");
	}
	while (!PMDkbhit() && readindex < (bufferlength - nVariables));
}

//*****************************************************************************
// WaitForEventWithReset
//
//   Waits for the specified event to be set in the EventStatus register.
//   Assumes that the event will not have occured before called because
//   the event is reset before checking.
//
//*****************************************************************************
int WaitForEventWithReset(PMDAxisHandle* phAxis, PMDuint16 eventmask)
{
	PMDResetEventStatus( phAxis, (PMDuint16)~eventmask );

	return WaitForEvent( phAxis, eventmask );
}
//*****************************************************************************
// WaitForEvent 
//
//   Waits for the specified event to be set in the EventStatus register.
//
//*****************************************************************************
int WaitForEvent(PMDAxisHandle* phAxis, PMDuint16 eventmask)
{
	int eventoccured = FALSE;
	PMDuint16 status = 0;
	PMDresult result = PMD_NOERROR;

	PMDprintf("Waiting for event %04X on axis %d.\n", eventmask, phAxis->axis + 1);
	PMDprintf("Hit space to abort.\n");

	while (!eventoccured && result == PMD_NOERROR)
	{
		if (PMDkbhit())
        {
        	PMDprintf("Aborted!\n");
			break;
        }
		result = PMDGetEventStatus( phAxis, &status );
		if (status & eventmask)
		{
			eventoccured=TRUE;
			PMDprintf("Event(s) %04X Set.\n", status);
		}
	}
	if (status & PMDEventMotionErrorMask)
	{
		PMDprintf("-------------- Motion Error!\n");
	}
	// clear the event for the next time
	PMDResetEventStatus( phAxis, (PMDuint16)~eventmask );

	return eventoccured;
}