pmdtrans.c

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

//  PMDTranps.c -- generic transport interface
//
//  Performance Motion Devices, Inc.
//

#ifndef _LINUX
#include <dos.h>
#include <conio.h>
#endif

#ifdef __cplusplus
	extern "C" {
#endif

#include "PMDtypes.h"
#include "PMDocode.h"
#include "PMDecode.h"
#include "PMDtrans.h"

PMDuint16 BuildCommand(PMDuint16 OPCode, PMDAxis axis)
{
	return ((PMDuint16)((PMDuint32)OPCode | ((PMDuint32)axis << 8)));
}

PMDresult SendCommand(PMDAxisInterface axis_intf, PMDuint16 OPCode)
{
        PMDuint16 buffer[1];

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);

        return axis_intf->transport.SendCommand(axis_intf->transport_data, 1, buffer, 0, PMDnull);
}


PMDresult SendCommandWord(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint16 data1)
{
        PMDuint16 buffer[2];

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);
        buffer[1] = (PMDuint16)data1;

        return axis_intf->transport.SendCommand(axis_intf->transport_data, 2, buffer, 0, PMDnull);
}


PMDresult SendCommandWordWord(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint16 data1, PMDuint16 data2)
{
        PMDuint16 buffer[3];

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);
        buffer[1] = (PMDuint16)data1;
        buffer[2] = (PMDuint16)data2;

        return axis_intf->transport.SendCommand(axis_intf->transport_data, 3, buffer, 0, PMDnull);
}

PMDresult SendCommandWordWordWord(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint16 data1, PMDuint16 data2, PMDuint16 data3)
{
        PMDuint16 buffer[4];

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);
        buffer[1] = (PMDuint16)data1;
        buffer[2] = (PMDuint16)data2;
        buffer[3] = (PMDuint16)data3;

        return axis_intf->transport.SendCommand(axis_intf->transport_data, 4, buffer, 0, PMDnull);
}

PMDresult SendCommandLong(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint32 data1)
{
        PMDuint16 buffer[3];
        PMDuint16 *ptr;

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);
        ptr = (PMDuint16 *)&data1;
        buffer[1]=*(ptr+1); //Write MSW first
        buffer[2]=*ptr;

        return axis_intf->transport.SendCommand(axis_intf->transport_data, 3, buffer, 0, PMDnull);
}

PMDresult SendCommandLongWord(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint32 data1, PMDuint16 data2)
{
        PMDuint16 buffer[4];
        PMDuint16 *ptr;

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);
        ptr = (PMDuint16 *)&data1;
        buffer[1]=*(ptr+1); //Write MSW first
        buffer[2]=*ptr;
        buffer[3] = data2;

        return axis_intf->transport.SendCommand(axis_intf->transport_data, 4, buffer, 0, PMDnull);
}

PMDresult SendCommandWordLong(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint16 data1, PMDuint32 data2)
{
        PMDuint16 buffer[4];
        PMDuint16 *ptr;

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);
        buffer[1] = data1;
        ptr = (PMDuint16 *)&data2;
        buffer[2]=*(ptr+1); //Write MSW first
        buffer[3]=*ptr;

        return axis_intf->transport.SendCommand(axis_intf->transport_data, 4, buffer, 0, PMDnull);
}


PMDresult SendCommandGetWord(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint16 *data1)
{
        PMDresult result = PMD_ERR_OK;
        PMDuint16 buffer[2];

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);

        result = axis_intf->transport.SendCommand(axis_intf->transport_data, 1, buffer, 1, buffer+1);

        *data1 = (PMDuint16)buffer[1];

        return result;
}

PMDresult SendCommandGetWordWord(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint16 *data1, PMDuint16 *data2)
{
        PMDresult result = PMD_ERR_OK;
        PMDuint16 buffer[3];

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);

        result = axis_intf->transport.SendCommand(axis_intf->transport_data, 1, buffer, 2, buffer+1);

        *data1 = (PMDuint16)buffer[1];
        *data2 = (PMDuint16)buffer[2];

        return result;
}

PMDresult SendCommandGetWordWordWord(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint16 *data1, PMDuint16 *data2, PMDuint16 *data3)
{
        PMDresult result = PMD_ERR_OK;
        PMDuint16 buffer[4];

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);

        result = axis_intf->transport.SendCommand(axis_intf->transport_data, 1, buffer, 3, buffer+1);

        *data1 = (PMDuint16)buffer[1];
        *data2 = (PMDuint16)buffer[2];
        *data3 = (PMDuint16)buffer[3];

        return result;
}

PMDresult SendCommandGetLong(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint32 *data1)
{
        PMDresult result = PMD_ERR_OK;
        PMDuint16 buffer[3];

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);

        result = axis_intf->transport.SendCommand(axis_intf->transport_data, 1, buffer, 2, buffer+1);

        *data1 = ((PMDuint32)buffer[1])<<16 | ((PMDuint32)buffer[2]);

        return result;
}

PMDresult SendCommandGetWordLong(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint16 *data1, PMDuint32 *data2)
{
        PMDresult result = PMD_ERR_OK;
        PMDuint16 buffer[4];

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);

        result = axis_intf->transport.SendCommand(axis_intf->transport_data, 1, buffer, 3, buffer+1);

        *data1 = (PMDuint16)buffer[1];
        *data2 = ((PMDuint32)buffer[2])<<16 | ((PMDuint32)buffer[3]);

        return result;
}

PMDresult SendCommandWordGetWord(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint16 data1, PMDuint16 *data2)
{
        PMDresult result = PMD_ERR_OK;
        PMDuint16 buffer[3];

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);
        buffer[1] = (PMDuint16)data1;

        result = axis_intf->transport.SendCommand(axis_intf->transport_data, 2, buffer, 1, buffer+2);

        *data2 = (PMDuint16)buffer[2];

        return result;
}

PMDresult SendCommandWordGetLong(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint16 data1, PMDuint32 *data2)
{
        PMDresult result = PMD_ERR_OK;
        PMDuint16 buffer[4];

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);
        buffer[1] = (PMDuint16)data1;

        result = axis_intf->transport.SendCommand(axis_intf->transport_data, 2, buffer, 2, buffer+2);

        *data2 = ((PMDuint32)buffer[2])<<16 | ((PMDuint32)buffer[3]);

        return result;
}

PMDresult SendCommandLongGetWord(PMDAxisInterface axis_intf, PMDuint16 OPCode, PMDuint32 data1, PMDuint16* data2)
{
		PMDresult result = PMD_ERR_OK;
		PMDuint16 buffer[4];
        PMDuint16 *ptr;

		buffer[0] = BuildCommand(OPCode,axis_intf->axis);
        ptr = (PMDuint16 *)&data1;
        buffer[0]=*(ptr+1); //Write MSW first
        buffer[1]=*ptr;

		result = axis_intf->transport.SendCommand(axis_intf->transport_data, 2, buffer, 1, buffer+3);

		*data2 = (PMDuint16)buffer[2];

		return result;
}

PMDresult SendCommandWordWordGetWord(PMDAxisInterface axis_intf, PMDuint16 OPCode, 
									 PMDuint16 data1, PMDuint16 data2, PMDuint16 *data3)
{
        PMDresult result = PMD_ERR_OK;
        PMDuint16 buffer[4];

        buffer[0] = BuildCommand(OPCode,axis_intf->axis);
        buffer[1] = (PMDuint16)data1;
        buffer[2] = (PMDuint16)data2;

        result = axis_intf->transport.SendCommand(axis_intf->transport_data, 3, buffer, 1, buffer+3);

        *data3 = (PMDuint16)buffer[3];

        return result;
}

#ifdef __cplusplus
}
#endif