pmddrv.c

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

//  PMDdrv.c -- parallel interface command/data transfer functions for the DK board 
//              via the MS Windows driver.
//
//  Performance Motion Devices, Inc.
//

#ifdef WIN32

#include <stdio.h>
#include <stdlib.h>

#include "PMDtypes.h"
#include "PMDecode.h"
#include "PMDocode.h"
#include "PMDtrans.h"
#include "PMDconio.h"
#include "PMDdrv.h"
#include "PMDdiag.h"
#include "ctlcode.h"

const char driverFile9x[] = "\\\\.\\PMDMCG2.VXD";
const char driverFileNT[] = "\\\\.\\PMDMCG2Device0";

PMD_ResultCodes PMDDrv_ConvertError( int error_code );

PMDresult PMDDrv_Send(void* transport_data, PMDuint8 xCt, PMDuint16* xDat, PMDuint8 rCt, PMDuint16* rDat);

PMDuint16 PMDDrv_HandleError(void* transport_data)
{
	PMDDrvIOTransportData* PIOtransport_data = (PMDDrvIOTransportData*)transport_data;
	DWORD nOut=0;
	WORD errBuf;
    PMDresult error;
    PMDresult result;
    static bGettingError = 0;

	// for NT we have to make an extra call to get error code info
	if (PIOtransport_data->OSNT)
	{
		DeviceIoControl( PIOtransport_data->hDrv, PMDGEN1_IOCTL_GET_ERR_CODE, 
						   NULL, 0, 
						   &errBuf, sizeof(WORD), &nOut, NULL );
		error = PMDDrv_ConvertError( errBuf );
	}
	else
		error = PMDDrv_ConvertError( GetLastError() );

    if (PIOtransport_data->bDiagnostics && error == PMD_ERR_CommandError && !bGettingError)
    {
        PMDuint16 xDat[2];
        PMDuint16 rDat[2];
        PMDuint8 xCt = 1;
        PMDuint8 rCt = 1;

        bGettingError = 1;
        xDat[0] = PMDOPGetHostIOError;
        result = PMDDrv_Send(transport_data, xCt, xDat, rCt, rDat);
		error = (PMDresult) rDat[0];
        bGettingError = 0;
    }
    if (PIOtransport_data->bDiagnostics)
        PMDprintf("%s\n", PMDGetErrorMessage(error));

    return error;
}

PMDuint16 PMDDrv_GetStatus(void* transport_data)
{
	return 0;
}

PMDuint16 PMDDrv_IsReady(void* transport_data)
{
	PMDDrvIOTransportData* PIOtransport_data = (PMDDrvIOTransportData*)transport_data;
	HANDLE hDrv = PIOtransport_data->hDrv;
	WORD rdy = 0;
	DWORD nOut=0;

	if( !DeviceIoControl( hDrv, PMDGEN1_IOCTL_CHECK_READY, NULL, 0,
								 &rdy, sizeof(WORD), &nOut, NULL ) )
		return PMDDrv_HandleError(transport_data);

	return  rdy;
}

PMDuint16 PMDDrv_HasInterrupt(void* transport_data)
{
	PMDDrvIOTransportData* PIOtransport_data = (PMDDrvIOTransportData*)transport_data;
	HANDLE hDrv = PIOtransport_data->hDrv;
	WORD rdy = 0;
	DWORD nOut=0;

	if( !DeviceIoControl( hDrv, PMDGEN1_IOCTL_CHECK_INTERRUPT, NULL, 0,
								 &rdy, sizeof(WORD), &nOut, NULL ) )
		return PMDDrv_HandleError(transport_data);

	return  rdy;
}

PMDuint16 PMDDrv_HasError(void* transport_data)
{
	return  0;
}

PMDuint16 PMDDrv_HardReset(void* transport_data)
{
	PMDDrvIOTransportData* PIOtransport_data = (PMDDrvIOTransportData*)transport_data;
	HANDLE hDrv = PIOtransport_data->hDrv;
	WORD rdy = 0;
	DWORD nOut=0;

	if( !DeviceIoControl( hDrv, PMDGEN1_IOCTL_RESET, NULL, 0,
								 &rdy, sizeof(WORD), &nOut, NULL ) )
		return PMDDrv_HandleError(transport_data);

	return  rdy;
}

PMD_ResultCodes PMDDrv_ConvertError( int error_code )
{
	PMDuint16 err;

	switch( error_code )
	{
		case PMDERR_OK:           return PMD_ERR_OK;
		case PMDERR_BAD_DRVR_CMD: return PMD_ERR_Driver;
		case PMDERR_MISSING_DATA: return PMD_ERR_Driver;
		case PMDERR_OUT_BUFF_LEN: return PMD_ERR_Driver;
		case PMDERR_CKSUM:        return PMD_ERR_ChecksumError;
		case PMDERR_TIMEOUT:      return PMD_ERR_CommTimeoutError;
		case PMDERR_ERRBIT:       return PMD_ERR_CommandError;
		default:                  return PMD_ERR_Driver;
	}

	return err;
}

PMDuint16 PMDDrv_GetCommandStatus(PMDDrvIOTransportData* transport_data)
{
	return PMD_ERR_OK;
}

// send the command and data to the DK2000
PMDresult PMDDrv_Send(void* transport_data, 
							  PMDuint8 xCt, PMDuint16* xDat, PMDuint8 rCt, PMDuint16* rDat)
{
	DWORD nOut=0;
	PMDuint16 i;
	PMDuint16 cksum;
	WORD cmd = xDat[0];
	PMDuint16 calcSum;
	PMDuint16 result = PMD_ERR_OK;
	PMDDrvIOTransportData* PIOtransport_data = (PMDDrvIOTransportData*)transport_data;
	HANDLE hDrv = PIOtransport_data->hDrv;

	if( hDrv == INVALID_HANDLE_VALUE )
	    return PMD_ERR_DK2000NotInitialized;

	if( !DeviceIoControl( hDrv, PMDGEN1_IOCTL_GENERIC_COMMAND, 
		                   &cmd, sizeof(WORD), 
		                   NULL, 0, &nOut, NULL ) )
    {
		result = PMDDrv_HandleError(transport_data);
    }

	if( !xCt && !rCt )
		return result;

	if( !DeviceIoControl( hDrv, PMDGEN1_IOCTL_GENERIC_DATA,
								 &xDat[1], xCt*sizeof(WORD)-1, // the -1 is because the command which we don't want to send here is the first word in xDat
                         rDat, rCt*sizeof(WORD), &nOut, NULL ) )
		return PMDDrv_HandleError(transport_data);


	if (PIOtransport_data->bVerifyChecksum)
	{
		if( !DeviceIoControl( hDrv, PMDGEN1_IOCTL_GENERIC_DATA,
									 NULL, 0, &cksum, sizeof(WORD), &nOut, NULL ) )
			return PMDDrv_HandleError(transport_data);


		calcSum = 0;

		for( i=0; i<xCt; i++ ) 
			calcSum += xDat[i];
		for( i=0; i<rCt; i++ ) 
			calcSum += rDat[i];

		if( calcSum != cksum )
		{
			PMDprintf( "Checksum error, sent: ");
			for( i=0; i<xCt; i++ ) PMDprintf( " 0x%04hx", xDat[i] );
			PMDprintf( "\n" );

			if( rCt ) PMDprintf( "            received:" );
			for( i=0; i<rCt; i++ ) PMDprintf( " 0x%04hx", rDat[i] );

			PMDprintf( "\nExpected: 0x%04hx, got: 0x%04hx\n", calcSum, cksum );

			return PMD_ERR_ChecksumError;
		}
	}

	return result;
}


PMDuint16 PMDDrv_InitPort(PMDDrvIOTransportData* transport_data)
{
	HANDLE hDrv;
	DWORD nOut=0;
	int OSNT;
	// find out if we're running under NT
	OSVERSIONINFO osvi;   // pointer to version 
                                         // information structure

	ZeroMemory(&osvi, sizeof(OSVERSIONINFO));
	osvi.dwOSVersionInfoSize = sizeof(OSVERSIONINFO);

	if ( !GetVersionEx(&osvi) )
	{
		PMDprintf( "This program does not support this version of Windows." );
		return PMD_ERR_OpeningWindowsDriver;
	}

	if ( osvi.dwPlatformId == VER_PLATFORM_WIN32_NT )
	{
		OSNT = TRUE;
		// NT create handle code
		hDrv = CreateFile( driverFileNT,
							 GENERIC_READ | GENERIC_WRITE,
							 FILE_SHARE_READ,
							 NULL,
							 OPEN_EXISTING,
							 0,
							 NULL);
	}
	else
	{
		OSNT = FALSE;
		// Create a handle to the driver
		hDrv = CreateFile( driverFile9x, 0, 0, NULL, OPEN_EXISTING, FILE_FLAG_DELETE_ON_CLOSE, NULL );
	}

	transport_data->OSNT = OSNT;
	transport_data->hDrv = hDrv;

	if( hDrv == INVALID_HANDLE_VALUE )
	{
		DWORD err = GetLastError();
		PMDprintf( "Attempt to open driver returned error %ld\n", err );
		return PMD_ERR_OpeningWindowsDriver;
	}

	// set IO mode in driver
	if( !DeviceIoControl( hDrv, PMDGEN1_IOCTL_SET_MODE, 
		                   &transport_data->busMode, sizeof(WORD),
								 NULL, 0, &nOut, NULL ) )
		return PMDDrv_HandleError(transport_data);

	return PMD_ERR_OK;
}
//*****************************************************************************
void PMDDrv_Close(void* transport_data)
{
	PMDDrvIOTransportData* PIOtransport_data = (PMDDrvIOTransportData*)transport_data;

	if ( PIOtransport_data->hDrv!=INVALID_HANDLE_VALUE )
	{
		CloseHandle( PIOtransport_data->hDrv );
		PIOtransport_data->hDrv = INVALID_HANDLE_VALUE;
	}
    free(transport_data);
}

void PMDDrv_InitData(PMDDrvIOTransportData* transport_data, int IOMode)
{
	// assign default values
	memset(transport_data, 0, sizeof(PMDDrvIOTransportData));
	transport_data->OSNT = 0;
	transport_data->busMode = IOMode;
	transport_data->hDrv = INVALID_HANDLE_VALUE;

	// by default always verify the checksum
	transport_data->bVerifyChecksum = 1;
	// by default disable diagnostics
	transport_data->bDiagnostics = 0;
}

PMDuint16 PMDDrv_Init(void* handle)
{
	PMDAxisHandle* axis_handle = (PMDAxisHandle*) handle;

	// setup function pointers
	axis_handle->transport.SendCommand = PMDDrv_Send;
	axis_handle->transport.GetStatus = PMDDrv_GetStatus;
	axis_handle->transport.IsReady = PMDDrv_IsReady;
	axis_handle->transport.HasInterrupt = PMDDrv_HasInterrupt;
	axis_handle->transport.HasError = PMDDrv_HasError;
	axis_handle->transport.HardReset = PMDDrv_HardReset;
    axis_handle->transport.Close = PMDDrv_Close;

	return PMDDrv_InitPort(axis_handle->transport_data);
}

//*****************************************************************************
PMDuint16 PMDSetupAxisInterface_Driver(PMDAxisHandle* axis_handle, PMDAxis axis_number)
{
	PMDDrvIOTransportData* transport_data;

	transport_data = (PMDDrvIOTransportData*) malloc( sizeof( PMDDrvIOTransportData ) );

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

	// set the interface type 
	axis_handle->InterfaceType = InterfaceDriver;

	// the transport data is initialized first to setup the defaults
	// make sure the IO mode is set correctly
	PMDDrv_InitData(transport_data, PMDDrvIOMode_16_16);

	axis_handle->transport_data = (void*) transport_data;

	// initialize the transport (inits function pointers)
	return PMDDrv_Init(axis_handle);
}

#endif