encodrbd.c

RTX是当前非常流行的RTX实时操作系统

/*****************************************************************************
***   ENCODRBD.C -- source file (US Digital Optical Encoder Interface Board) *
******************************************************************************
****                                                                      ****
****     Functions to interface with the US Digital board      ****
****     init_7166 -- initializes the 4 encoder interface chips ***
****               -- This function must be called at start up   ****
****	 read_encoders -- Reads encoder channels 1 and 2 and converts ****
****                   -- the counts to radians.
****     Lower Level functions
****     read_chipX    -- reads and returns the count value on the encoder **
****	 hard_latch    -- latches all four encoder counts  This must be called **
****                   -- before read_chipX.  read_encoders calls this for you  **
****     Last revision date:  1/4/99                                     ****
****     Programmer:  Dan Block                                           ****
****                                                                      ****
*****************************************************************************/
#include "windows.h"
#include "rtapi.h"  // RTSS API
#include "encodrbd.h"


/*****************************************************************************/
/*  init_7166 initializes all four encoder interface chips on the             */
/*            US Digital PC Board.  Must be called before any other encoder   */
/*            functions                                                       */
/*****************************************************************************/
VOID _stdcall init_7166(void) {                               /* initialize the LS7166s */

	union presetting
	{
	 char a[3];
	 long b;
	}ps;

	ps.b = ENCINIT;

	// Enable for Port IO to ENCBASE through ENCBASE+9
	RtEnablePortIo(ENCBASE,0x9);

	RtWritePortUchar(CONTROL1, MASTER_RESET);
	RtWritePortUchar(CONTROL1, INPUT_SETUP);
	RtWritePortUchar(CONTROL1, QUAD_X4);
	RtWritePortUchar(CONTROL1, CNTR_RESET);
	/* preset counter to ENCINIT */
	RtWritePortUchar(CONTROL1, ADDR_RESET); // reset addr
	RtWritePortUchar(DATA1, ps.a[0]); // output preset value
	RtWritePortUchar(DATA1, ps.a[1]); // output preset value
	RtWritePortUchar(DATA1, ps.a[2]); // output preset value
	RtWritePortUchar(CONTROL1, PRESET_CTR); // preset to counter

	RtWritePortUchar(CONTROL2, MASTER_RESET);
	RtWritePortUchar(CONTROL2, INPUT_SETUP);
	RtWritePortUchar(CONTROL2, QUAD_X4);
	RtWritePortUchar(CONTROL2, CNTR_RESET);
	/* preset counter to ENCINIT */
	RtWritePortUchar(CONTROL2, ADDR_RESET); // reset addr
	RtWritePortUchar(DATA2, ps.a[0]); // output preset value
	RtWritePortUchar(DATA2, ps.a[1]); // output preset value
	RtWritePortUchar(DATA2, ps.a[2]); // output preset value
	RtWritePortUchar(CONTROL2, PRESET_CTR); // preset to counter

	RtWritePortUchar(CONTROL3, MASTER_RESET);
	RtWritePortUchar(CONTROL3, INPUT_SETUP);
	RtWritePortUchar(CONTROL3, QUAD_X4);
	RtWritePortUchar(CONTROL3, CNTR_RESET);
	/* preset counter to ENCINIT */
	RtWritePortUchar(CONTROL3, ADDR_RESET); // reset addr
	RtWritePortUchar(DATA3, ps.a[0]); // output preset value
	RtWritePortUchar(DATA3, ps.a[1]); // output preset value
	RtWritePortUchar(DATA3, ps.a[2]); // output preset value
	RtWritePortUchar(CONTROL3, PRESET_CTR); // preset to counter

	RtWritePortUchar(CONTROL4, MASTER_RESET);
	RtWritePortUchar(CONTROL4, INPUT_SETUP);
	RtWritePortUchar(CONTROL4, QUAD_X4);
	RtWritePortUchar(CONTROL4, CNTR_RESET);
	/* preset counter to ENCINIT */
	RtWritePortUchar(CONTROL4, ADDR_RESET); // reset addr
	RtWritePortUchar(DATA4, ps.a[0]); // output preset value
	RtWritePortUchar(DATA4, ps.a[1]); // output preset value
	RtWritePortUchar(DATA4, ps.a[2]); // output preset value
	RtWritePortUchar(CONTROL4, PRESET_CTR); // preset to counter
}


/*****************************************************************************/
/*  hard_latch -- Latch all four encoder counts into output buffers for reading  */
/*                This function must be called before read_chipX    */
/*****************************************************************************/
VOID _stdcall hard_latch(void) { /* latch position with low pulse on pin 3 */
	RtWritePortUchar(LATCH, 0); /* toggle pin 3 of LS 7166 */
}


/*****************************************************************************/
/*  read_chip1 -- read and return counter 1's value latch by latest hard_latch call*/
/*****************************************************************************/
long _stdcall read_chip1(void) {/* read position of encoder */
	
		long int position;
	RtWritePortUchar(CONTROL1, ADDR_RESET); /* reset address pointer */
	position  = (long)(RtReadPortUchar(DATA1) & 0xFF); /* least significant byte */
	position += (long)(RtReadPortUchar(DATA1) & 0xFF) << 8;
	position += (long)(RtReadPortUchar(DATA1) & 0xFF) << 16; /* most significant byte */

	if(position >= 8388608 )  // return a 24 bit signed number 
							  // instead of a 24 bit unsigned number
	  position = position - 16777216;

	return position;
}


/*****************************************************************************/
/*  read_chip2 -- read and return counter 2's value latch by latest hard_latch call*/
/*****************************************************************************/
long _stdcall read_chip2(void) {/* read position of encoder */
	
		long int position;
	RtWritePortUchar(CONTROL2, ADDR_RESET); /* reset address pointer */
	position  = (long)(RtReadPortUchar(DATA2) & 0xFF); /* least significant byte */
	position += (long)(RtReadPortUchar(DATA2) & 0xFF) << 8;
	position += (long)(RtReadPortUchar(DATA2) & 0xFF) << 16; /* most significant byte */

	if(position >= 8388608 )
	  position = position - 16777216;

	return position;    
}


/*****************************************************************************/
/*  read_chip3 -- read and return counter 3's value latch by latest hard_latch call*/
/*****************************************************************************/
long _stdcall read_chip3(void) { /* read position of encoder */
	
	long int position;
	RtWritePortUchar(CONTROL3, ADDR_RESET); /* reset address pointer */
	position  = (long)(RtReadPortUchar(DATA3) & 0xFF); /* least significant byte */
	position += (long)(RtReadPortUchar(DATA3) & 0xFF) << 8;
	position += (long)(RtReadPortUchar(DATA3) & 0xFF) << 16; /* most significant byte */

	if(position >= 8388608 )
	  position = position - 16777216;

	return position;
}


/*****************************************************************************/
/*  read_chip4 -- read and return counter 4's value latch by latest hard_latch call*/
/*****************************************************************************/
long _stdcall read_chip4(void) { /* read position of encoder */
	
	long int position;
	RtWritePortUchar(CONTROL4, ADDR_RESET); /* reset address pointer */
	position  = (long)(RtReadPortUchar(DATA4) & 0xFF); /* least significant byte */
	position += (long)(RtReadPortUchar(DATA4) & 0xFF) << 8;
	position += (long)(RtReadPortUchar(DATA4) & 0xFF) << 16; /* most significant byte */

	if(position >= 8388608 )
	  position = position - 16777216;

	return position;
}

/*****************************************************************************/
/*  read_encoders -- read encoder channels 1 and 2 and convert the count value */
/*                   to radians.  
/*****************************************************************************/
VOID _stdcall read_encoders(float* enc1,float* enc2) {

	 long int lCntA,lCntB; 
     hard_latch();
     lCntA = read_chip1(); /* Read encoder count value */
     lCntB = read_chip2(); /* Read encoder count value */
     *enc1 = (float) ((float)lCntA*(2.0F*ENCPI)/(float)ENCDIV); /* convert counts to radians */
     *enc2 = (float) ((float)lCntB*(2.0F*ENCPI)/(float)ENCDIV);
}