pmdpar.c

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

//  PMDpar.c -- parallel interface command/data transfer functions for the MC2xxx
//
//  Performance Motion Devices, Inc.
//

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

// undefine __TIME_WAITFORREADY if it is not desired to have a timeout while waiting for the ready signal
#define __TIME_WAITFORREADY
#ifdef __TIME_WAITFORREADY
#include <sys/timeb.h>
#endif

#include "PMDtypes.h"
#include "PMDecode.h"
#include "PMDtrans.h"
#include "PMDconio.h"
#include "PMDpar.h"

// only include this if we are running in diagnostics mode
#include "PMDdiag.h"

#define PMDBASE  0x340

// forward declarations
PMDuint16 PMDParallel_ReceiveResponse(PMDParallelIOTransportData* transport_data, PMDuint16 length, PMDuint16 buffer[]);

void OutP16Bit(PMDuint16 port, PMDuint16 dataword)
{
        _outpw(port, dataword);
}

PMDuint16 InP16Bit(PMDuint16 port)
{
        return _inpw(port);
}

void OutP8Bit(PMDuint16 port, PMDuint16 dataword)
{
	_outp(port, (PMDuint8)(dataword>>8)&0xFF);
	_outp(port, (PMDuint8)dataword&0xFF);
}

PMDuint16 InP8Bit(PMDuint16 port)
{
	return (PMDuint16) (((_inp(port) & 0xFF) << 8) | (_inp(port) & 0xFF));
}

void OutP8BitCmd(PMDuint16 port, PMDuint16 dataword)
{
	_outp(port, (PMDuint8)dataword&0xFF);
}

PMDuint16 InP8BitStatus(PMDuint16 port)
{
	return (PMDuint16)(_inp(port) & 0xFF);
}

PMDuint16 PMDParallel_GetStatus(void* transport_data)
{
	PMDParallelIOTransportData* PIOtransport_data = (PMDParallelIOTransportData*)transport_data;

	return PIOtransport_data->InPStatus(PIOtransport_data->statusPort);
}

PMDuint16 PMDParallel_IsReady(void* transport_data)
{
	PMDParallelIOTransportData* PIOtransport_data = (PMDParallelIOTransportData*)transport_data;

	return ((PIOtransport_data->InPStatus(PIOtransport_data->statusPort) 
		& PIOtransport_data->readyMask) == PIOtransport_data->readyValue);
}

PMDuint16 PMDParallel_HasInterrupt(void* transport_data)
{
	PMDParallelIOTransportData* PIOtransport_data = (PMDParallelIOTransportData*)transport_data;

	return ((PIOtransport_data->InPStatus(PIOtransport_data->statusPort) 
		& PIOtransport_data->hostInterruptMask) == PIOtransport_data->hostInterruptValue);
}

PMDuint16 PMDParallel_HasError(void* transport_data)
{
	PMDParallelIOTransportData* PIOtransport_data = (PMDParallelIOTransportData*)transport_data;

	return ((PIOtransport_data->InPStatus(PIOtransport_data->statusPort) 
		& PIOtransport_data->commandStatusMask) == PIOtransport_data->commandStatusValue);
}

PMDuint16 PMDParallel_HardReset(void* transport_data)
{
	PMDParallelIOTransportData* PIOtransport_data = (PMDParallelIOTransportData*)transport_data;

    // reset port for the DK Board is base address + 8
    PIOtransport_data->OutPCmd((PMDuint16)(PIOtransport_data->dataPort + 8), 0);
    // reset port for the Motion Board is base address + 6
    PIOtransport_data->OutPCmd((PMDuint16)(PIOtransport_data->dataPort + 6), 0);

    return PMD_NOERROR;
}

// wait for the DB2000 to be ready for the next command
static PMDuint16 PMDParallel_WaitForReady(PMDParallelIOTransportData* transport_data)
{
	PMDuint16 in_val;
    int i;
#ifdef __TIME_WAITFORREADY
    struct timeb stopTime, currentTime;
    PMDuint32 milliSeconds, seconds;

    // For faster turnaround, check ready bit in tight loop before using timeout code
    // because ftime() is a costly function call. (approx 25祍)
    // Each InPStatus call takes 1 to 1.5 祍 (based on ISA freq) and a command may 
    // take up to 150 祍 for the PMD chip to process (based on servo loop time)
    // so check ready bit 150 times before dropping into timeout section.
    for(i=0;i<150;i++)
    {
        // poll ready port, if not ready, loop
        in_val = transport_data->InPStatus(transport_data->statusPort);

        if ((in_val != 0xFFFF) 
			&& ((in_val & transport_data->readyMask) == transport_data->readyValue))
                return PMD_ERR_OK;
    }
    ftime( &stopTime );
    milliSeconds = stopTime.millitm + 1000;
    seconds = milliSeconds/1000;
    stopTime.time += seconds;
    stopTime.millitm = (PMDuint16)(milliSeconds - seconds*1000);

    for(;;)
    {

        // poll ready port, if not ready, loop
        in_val = transport_data->InPStatus(transport_data->statusPort);

        if ((in_val != 0xFFFF) 
			&& ((in_val & transport_data->readyMask) == transport_data->readyValue))
                return PMD_ERR_OK;

        ftime( &currentTime );

        if((currentTime.time > stopTime.time) ||
            ((currentTime.time == stopTime.time) &&
            (currentTime.millitm > stopTime.millitm)))
        {
            // recheck the busy flag on last time
            in_val = transport_data->InPStatus(transport_data->statusPort);

		    if ((in_val != 0xFFFF) 
				&& ((in_val & transport_data->readyMask) == transport_data->readyValue))
				return PMD_ERR_OK;
            return PMD_ERR_CommTimeoutError;
        }
    }
#else
    // wait indefinitely
    for(;;)
    {
        // poll ready port, if not ready, loop
        in_val = transport_data->InPStatus(transport_data->statusPort);

        if ((in_val != 0xFFFF) 
			&& ((in_val & transport_data->readyMask) == transport_data->readyValue))
                return PMD_ERR_OK;
    }
#endif
}

// wait for the DK2000 to be ready for the next command
static PMDuint16 PMDParallel_GetCommandStatus(PMDParallelIOTransportData* transport_data)
{
	PMDuint16 result;
	int     in_val;

	if((result = PMDParallel_WaitForReady(transport_data)) != PMD_ERR_OK)
			return result;

	in_val = transport_data->InPStatus(transport_data->statusPort);
	if ((in_val & transport_data->commandStatusMask) == transport_data->commandStatusValue)
	{
		if (transport_data->bDiagnostics)
			PMDprintf("C-Motion: Command Error bit set.\n");
		return PMD_ERR_CommandError;
	}

	return PMD_ERR_OK;
}

// send the command and data to the DK2000
PMDuint16 PMDParallel_Send(PMDParallelIOTransportData* transport_data, 
							  PMDuint8 xCt, PMDuint16* xDat, PMDuint8 rCt, PMDuint16* rDat)
{
    PMDuint16 result;
    PMDuint16 commandstatus = PMD_ERR_OK;
    PMDuint16 index;
    long messageChecksum=0;
    PMDuint16 chipsetChecksum=0;

    result = PMD_ERR_DK2000NotInitialized;
    if((result = PMDParallel_WaitForReady(transport_data)) == PMD_ERR_OK)
    {
            // put the command into the DK2000
            transport_data->OutPCmd(transport_data->commandPort, xDat[0]);

            // put the data into the DK2000
            // one word at a time
            for(index=1; index<xCt; index++)
            {
                    if((result = PMDParallel_WaitForReady(transport_data)) != PMD_ERR_OK)
                            break;
                    transport_data->OutPData(transport_data->dataPort,xDat[index]);
            }
            // get the status
            if(result == PMD_ERR_OK)
                    commandstatus = PMDParallel_GetCommandStatus(transport_data);
    }

    // get the data from the DK2000
    for(index=0; index<rCt; index++)
    {
            if((result = PMDParallel_WaitForReady(transport_data)) != PMD_ERR_OK)
                    break;
            rDat[index] = transport_data->InPData(transport_data->dataPort);
    }

    if (transport_data->bVerifyChecksum)
	{
		for(index=0; index<xCt; index++)
				messageChecksum += xDat[index];
		for(index=0; index<rCt; index++)
				messageChecksum += rDat[index];
		messageChecksum = messageChecksum & 0xFFFF;

		result = PMDParallel_ReceiveResponse(transport_data, 1, &chipsetChecksum);

		if ( result == PMD_ERR_OK && messageChecksum != chipsetChecksum )
		{
			if (transport_data->bDiagnostics)
				PMDprintf("Checksum failure.  expected: %04x,  got: %04x\n",messageChecksum,chipsetChecksum);
			return PMD_ERR_ChecksumError;
		}
	}

	if (commandstatus!=PMD_ERR_OK)
	{
		if (transport_data->bDiagnostics)
		{
			PMDprintf("C-Motion: %s ",PMDGetOpcodeText(xDat[0]));
			for(index=0; index<xCt; index++)
				PMDprintf("%X ",xDat[index]);
			PMDprintf("\n");
		}
		return commandstatus;
	}

   	return result;
}

// send the command to and get data from the DK2000
PMDuint16 PMDParallel_ReceiveResponse(PMDParallelIOTransportData* transport_data, PMDuint16 length, PMDuint16 buffer[])
{
        PMDuint16 result;
        PMDuint16 index;

        result = PMD_ERR_DK2000NotInitialized;

        // get the data from the DK2000
        for(index=0; index<length; index++)
        {
                if((result = PMDParallel_WaitForReady(transport_data)) != PMD_ERR_OK)
                        break;
                buffer[index] = transport_data->InPData(transport_data->dataPort);
        }

        return result;
}

void PMDParallel_Close(void* transport_data)
{
    free(transport_data);
}

void PMDParallel_InitData(PMDParallelIOTransportData* transport_data, int IOMode)
{
	// assign default values
	if (transport_data->dataPort == 0)
		transport_data->dataPort = PMDBASE;
	transport_data->commandPort = transport_data->dataPort + 2;
	transport_data->statusPort = transport_data->dataPort + 2;

	transport_data->readyMask = 0x8000;
	transport_data->readyValue = 0x8000;
	transport_data->hostInterruptMask = 0x4000;
	transport_data->hostInterruptValue = 0x4000;
	transport_data->commandStatusMask = 0x2000;
	transport_data->commandStatusValue = 0x2000;

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

	// assign default handlers/masks according to IO mode
	switch (IOMode)
	{
	case PMDParallelIOMode_16_16:
		transport_data->OutPData = OutP16Bit;
		transport_data->OutPCmd = OutP16Bit;
		transport_data->InPData = InP16Bit;
		transport_data->InPStatus = InP16Bit;
		break;
	case PMDParallelIOMode_8_16:
		transport_data->OutPData = OutP8Bit;
		transport_data->OutPCmd = OutP8Bit;
		transport_data->InPData = InP8Bit;
		transport_data->InPStatus = InP8Bit;
		break;
	case PMDParallelIOMode_8_8:
		transport_data->OutPData = OutP8Bit;
		transport_data->OutPCmd = OutP8BitCmd;
		transport_data->InPData = InP8Bit;
		transport_data->InPStatus = InP8BitStatus;
		transport_data->readyMask = 0x80;
		transport_data->readyValue = 0x80;
		transport_data->hostInterruptMask = 0x40;
		transport_data->hostInterruptValue = 0x40;
		transport_data->commandStatusMask = 0x20;
		transport_data->commandStatusValue = 0x20;
		break;
	}
}

void PMDParallel_Init(PMDIOTransport* transport)
{
	// setup function pointers
	transport->SendCommand = PMDParallel_Send;

	transport->GetStatus = PMDParallel_GetStatus;
	transport->IsReady = PMDParallel_IsReady;
	transport->HasInterrupt = PMDParallel_HasInterrupt;
	transport->HasError = PMDParallel_HasError;
	transport->HardReset = PMDParallel_HardReset;

	transport->Close = PMDParallel_Close;
}

/*****************************************************************************
Set board_address to 0 to use default (0x340)
/*****************************************************************************/
PMDuint16 PMDSetupAxisInterface_Parallel(PMDAxisHandle* axis_handle, PMDAxis axis_number, PMDuint16 board_address)
{
	PMDParallelIOTransportData* transport_data;

	transport_data = (PMDParallelIOTransportData*) malloc( sizeof( PMDParallelIOTransportData ) );
	memset(transport_data, 0, sizeof(PMDParallelIOTransportData));

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

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

	// if required the transport data defaults can be changed here
	// e.g. board IO address (dataPort,commandPort,statusPort)
	transport_data->dataPort = board_address;

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

	axis_handle->transport_data = (void*) transport_data;

	// initialize the transport (inits function pointers)
	PMDParallel_Init(&axis_handle->transport);

	return PMD_ERR_OK;
}