Absolute Position Encoder Electronics Upgrade - Appendix B.3
February 18th, 2000 - Bob Broilo

B.3 Encoder.c
/*
 * encoder.c
 * runs the encoder data reciever/buffer system for prototype
 ************************************************************************
 * Copyright (C) 2002 Bob Broilo, National Radio Astronomy Observatory
 * 
 * This program is free software;  you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation;  either version 2
 * of the License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY;  without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 * 
 * Bob Broilo - bbroilo@nrao.edu
 * PO Box 0, Socorro NM  87801
 ************************************************************************
 * This program reads the VLA encoder data stream from
 * the fiber receiver/buffers.  The program combines the
 * coarse and fine data from both azimuth and elevation
 * encoders and sends the position information to the ACU.
 * The program runs on a ZWorld BL1600 (Little Giant)
 * controller with an XP8120 digital output board connected
 * to the PLCBus and an 4x40 LCD display connected to
 * the LCD Bus.
 *
 * This program assumes that we are using the old 20-bit
 * ACU.  If a more bits are desired, another PLCBus output
 * board will need to be added, or a better comm protocol
 * designed (serial!).  Or just use the BL1600 as an ACU.
 *    
 * the program may be expanded to take over the fiber
 * receiver/buffer function - yeah right someday!  They
 * are very timing dependent...  perhaps there is a better
 * way.
 *
 * The routine which combines the coarse and fine words
 * (combine) is based on Tony Poyner's code.
 *
 * The data from the fiber recieve buffers is in 8-bit parallel
 * format with 2-bit handshaking.  The data to the ACU is 2x20bit
 * parallel with one hold bit.
 *
 * Bob Broilo
 */

#define VERSION		"Data Converter Rev. 8/11/1999_BB        "
#define VERSION2	"          'The Hootis'                  "

/*
 * Required Libraries
 */
#use ezioplc.lib	/* PLCBus stuff */
#use eziopbdv.lib	/* more PLCBus stuff */



/*
 * Constants
 */

/* LCD module defines */
#define LCD1		0x080	/* top LCD Control Register (/USER1) */
#define LCD2		0x150	/* bottom LCD Control Register (LCDX) */
#define LCD_CLEAR	0x01	/* Command to clear display, home cursor */
#define LCD_FUNC	0x3C	/* Command 8 bit, 2 lines, 5x10 dots */
#define LCD_DISP	0x0C	/* Command disp on, cursor on, blink */
#define LCD_HOME	0x02	/* Command home cursor */
#define LCD_TIMEOUT	50	/* LCD not responding */
#define LCD_FREQ	20	/* delay between LCD updates */

/* LCD Messages */
#define BLANKLINE       "                                        "

/* PLCBus defines */
#define POS_OUT		7*32	/* Address of position output board */

/* Constants */
#define AZEL_DISPLAY	0	/* display mode: az and el decimal */
#define AZ_DETAILS	1	/* display mode: az bits */
#define EL_DETAILS	2	/* display mode: el bits */
#define MAX_POS		16777215	/* maximum position possible, all 24 bits on */
#define LSBRES		720.0/33554432.0	/* angle of one LSB (720/2^25) */
#define BUF_DELAY	4	/* Data valid delay */
#define BUF_TIMEOUT	10	/* timeout waiting for data */
#define PLC_TIMEOUT	50	/* PLC not responding */
#define DSP_TIMEOUT	1000	/* time till display reverts */
#define MAXSTRING	81	/* 40x2 display has 80 char max */
#define ACU_BITS	20	/* number of bits the ACU expects */
#define AZ_OFFSET	3	/* number of LSBs to drop for azimuth */
#define EL_OFFSET	3	/* number of LSBs to drop for elevation */
#define TRUE		1	/* You can't handle the truth! */
#define FALSE		0	/* He would disguise lies with the truth, to attack */
				/* us.  The attack is psychological, powerful... */
/* I/O ports */
#define INENLO		0x110	/* 8-bit in buffer&kb	IN00-IN07 H1.1-H1.10 */
#define AZ_RDY		9	/* az data ready in	IN08 H1.9 */
#define EL_RDY		10	/* el data ready in	IN09 H1.12 */
#define AZ_HOLD		11	/* acu data hold		IN10 H1.11 */
#define EL_HOLD		12	/* acu data hold		IN10 H1.11 */
#define AZ_BUFFER	0x120	/* select az buffer out	OUT1 H2.3 */
#define EL_BUFFER	0x121	/* select el buffer out	OUT2 H2.4 */
#define SWITCH		0x122	/* select keyboard out	OUT3 H2.5 */
#define SYNC		0x123	/* sync out for testing	OUT4 H2.6 */
#define OUTBYTE		0x130	/* el data out		OUTB1-OUTB8 H2.13-H2.20 */


/*
 * Functions
 */
void lcdwait(int);		/* wait for LCD busy to clear */
void lcdinit(int);		/* initialize display */
void lcdprint(int, char *);	/* print string to LCD */
void lcdhome(int);		/* home LCD cursor */
int readpos(int);		/* read position from fiber buffer */
				/* combine coarse/fine into position */
unsigned long combine(unsigned int fword, unsigned int cword);
void bits16(unsigned int);
void bits32(unsigned long);
void update_display(int);
void update_acu(void);

/*
 * Globals
 */
char result[MAXSTRING];
struct posdata {
  unsigned int coarse, fine;	/* data words */
  unsigned int cf_diff;		/* difference btw coarse and fine overlap bits */
  unsigned long position;	/* digital position */
  int timer;			/* time counter */
  int valid;			/* data valid flag */
  /* the following data is for display and is not continuously updated */
  double angle;			/* angle in degrees */
  double degrees, minutes, seconds;	/* angle in DMS */
};
struct posdata az;		/* position data */
struct posdata el;
int data_valid;			/* incoming data valid flag */
int lcd_valid;			/* LCD diplay working OK */
unsigned int cf_diff;		/* difference btw coarse and fine overlap bits */



/*
 * send 20-bit position data to ACU
 * very hardware dependent because PLCBus=32bits and ACU=2x20bits
 */
void update_acu(void)
{
  int i, data;

  if(az.valid) {
    for(i=1;i<=ACU_BITS;i++) {				/* 20 bits AZ PLCBus out 1-20 */
      data=(int)(az.position >> (i+AZ_OFFSET)) & 1;
      plcXP81Out(POS_OUT+(ACU_BITS-i), data);
    }
  }
  if(el.valid) {
    for(i=9;i<=ACU_BITS;i++) {				/* 12 MSB EL out PLCBus 21-32 */
      data=(int)(el.position >> (i+EL_OFFSET)) & 1;
      plcXP81Out(POS_OUT+ACU_BITS+(ACU_BITS-i), data);
    }
    data=(int)((el.position >> (EL_OFFSET+1) & 0x0FF));	/* 8 LSB EL out H2, OUTB1-OUTB8 */
    outport(OUTBYTE,data);
  }
  /* Debussy realized that a work of art, or an effort to create beauty, was */
  /* always regarded by some people as a personal attack...   - Art of Noise */
}



/*
 * wait for LCD busy to clear
 */
void lcdwait(int display_address)
{
  int lcd_timer;
  lcd_timer=0;
  lcd_valid=FALSE;
  while((inport(display_address)&0x80) &&
	(lcd_timer < LCD_TIMEOUT))
    lcd_timer++;
  if(lcd_timer < LCD_TIMEOUT) lcd_valid=TRUE;
}



/*
 * Initialization for LCD module
 */
void lcdinit(int display_address)
{
  lcdwait(display_address);			/* wait for LCD not busy */
  outport(display_address,LCD_CLEAR);
  lcdwait(display_address);
  outport(display_address,LCD_FUNC);
  lcdwait(display_address);
  outport(display_address,LCD_DISP);
}



/*
 * Home cursor in display
 */
void lcdhome(int display_address)
{
  lcdwait(display_address);
  outport(display_address,LCD_HOME);
}



/*
 * print string to LCD at current cursor location
 */
void lcdprint(int display_address, char *lcstring)
{
  int indx;

  for(indx=0;indx<strlen(lcstring);indx++)
    {
    lcdwait(display_address);
    outport(display_address+1,lcstring[indx]);
    }
}



/*
 * read a byte from a fiber recieve buffer
 */
int readpos(int buffer_select)
{
  int readval, i;

  outport(buffer_select,1);	/* signal ready to read */
  for(i=0;i<BUF_DELAY;i++);	/* wait for buffer data valid */
  readval = inport(INENLO);	/* read byte */
  outport(buffer_select,0);	/* signal byte read */
  return(readval);
}



/*
 * Convert the 16-bit coarse and fine words into one 24-bit
 * number representing absolute angle from 0-720 degrees.
 * The coarse resolver is only accurate to 12 bits so we
 * don't use the low nibble.
 * Coarse : XXXXXXXX XXXX1111
 * Fine:              XXXXXXXX XXXXXXXX
 *      MSB ^         ^^^ Overlap     ^ LSB
 * Assuming that the fine encoder is more accurate than the
 * coarse and that there are only 4 bit transistions difference
 * between the two, we adjust the coarse by adding or subtracting to
 * match the fine, then combine the data.
 * Based on code by Tony Poyner
 */
unsigned long combine(unsigned int fword, unsigned int cword)
{
  int c3, f3, sign, x;
  unsigned long combined;

  cword = cword/0x10;		/* ignore low nibble */

  				/* TEMP change for syncro */
  cword = (cword>>1);  	/* REMOVE REMOVE REMOVE */
    
  c3 = cword & 0x07;		/* low 3 bits coarse */
  f3 = fword/0x2000;		/* high 3 bits fine */

  x = c3-f3;			/* how far off are we? */
  cf_diff = abs(x);		/* save the difference for display */

  if (x>4)			cword = cword + (8-x);	/* assume cword behind */
  else if ((x<=4)&&(x>=-4))	cword = cword - x;	/* twiddle cword */
  else /* if (x<-4) */		cword = cword - (8+x);	/* assume cword ahead */

  cword = cword / 0x08;	/* use top three bits from fine */
  combined = (long)cword*0x10000 + (long)fword;
  return(combined);
}



/*
 * Convert 16-bit number into binary string of 1s and 0s.
 * put the string in result[]
 */
void bits16(unsigned int dataword)
{
  int i;

  for(i=0;i<16;i++) {		/* 16 bits in word */
    if((dataword<<i)&0x8000)
      result[i]='1';		/* high = '1' */
    else
      result[i]='0';		/* lo = '0' */
  }
  result[16]='\0';		/* terminate string properly */
}


/*
 * Convert 16-bit number (from coarse) into 12-item binary string
 *  of 1s and 0s.  Put the string in result[]
 */
void bits12(unsigned int dataword)
{
  int i;

  for(i=0;i<12;i++) {		/* 16 bits in word */
    if((dataword<<i)&0x8000)
      result[i]='1';		/* high = '1' */
    else
      result[i]='0';		/* lo = '0' */
  }
  result[12]='\0';		/* terminate string properly */
}



/*
 * Convert 32-bit number into binary string of 1s and 0s.
 * put the string in result[]
 */
void bits32(unsigned long dataword)
{
  int i;

  for(i=0;i<32;i++) {		/* 16 bits in word */
    if((dataword<<i)&0x80000000)
      result[i]='1';		/* high = '1' */
    else
      result[i]='0';		/* lo = '0' */
  }
  result[32]='\0';		/* terminate string properly */
}


/*
 * Convert 32-bit number into 25-item binary string of 1s and 0s.
 * put the string in result[]
 */
void bits25(unsigned long dataword)
{
  int i;

  for(i=0;i<25;i++) {		/* 25 bits in position */
    if((dataword<<i+7)&0x80000000)
      result[i]='1';		/* high = '1' */
    else
      result[i]='0';		/* lo = '0' */
  }
  result[25]='\0';		/* terminate string properly */
}


void update_display(int mode)
{
  double degrees, minutes, seconds;
  switch(mode) {
  case AZEL_DISPLAY:		/* Az and El decimal degrees, faults */
    {
      lcdhome(LCD1);
      lcdhome(LCD2);
      lcdprint(LCD1, VERSION);	/* display version information */
      lcdprint(LCD1, VERSION2);

      if(az.valid) {
	az.angle=(double)az.position * LSBRES;
	sprintf(result, "AZ: %10.5f      ", az.angle); }
      else
	sprintf(result, "AZ: invalid         ");
      lcdprint(LCD2,result);
      if(el.valid) {
	el.angle=(double)el.position * LSBRES;
	sprintf(result, "EL: %10.5f      ", el.angle);
      }
      else
	sprintf(result, "EL: invalid         ");
      lcdprint(LCD2,result);
      lcdprint(LCD2,BLANKLINE);
      break;
    }
  case AZ_DETAILS: {				/* AZ coarse, fine, combined, and D M S */
      lcdhome(LCD1);
      lcdhome(LCD2);
      if(az.valid) {
	bits12(az.coarse);
	lcdprint(LCD1, "AZ Coarse ");
	lcdprint(LCD1, result);
	sprintf(result, " %1d", az.cf_diff);
	lcdprint(LCD1, result);
	lcdprint(LCD1, "                   Fine            ");
	bits16(az.fine);
	lcdprint(LCD1, result);
	lcdprint(LCD1, "    ");
	bits25(az.position);
	lcdprint(LCD2, "   Comb   ");
	lcdprint(LCD2, result);
	lcdprint(LCD2, "     ");

	az.angle=(double)az.position * LSBRES;
	minutes = (az.angle - floor(az.angle));
	degrees = az.angle - minutes;
	minutes = minutes * 60.0;
	seconds = (minutes - floor(minutes));
	minutes = minutes - seconds;
	seconds = seconds * 60.0;
	sprintf(result, "%10.5f    %4.0f%3.0f%4.1f     ", az.angle, degrees, minutes, seconds);
	lcdprint(LCD2, result);
	lcdprint(LCD2, "         ");
      }
      else {
	lcdprint(LCD1, "Az position data invalid               ");
	lcdprint(LCD1, BLANKLINE);
	lcdprint(LCD2, "Not receiving data from the Az encoder ");
	lcdprint(LCD2, BLANKLINE);
      }
      break;
  }
  case EL_DETAILS: {				/* EL coarse, fine, combined, and D M S */
    }
      lcdhome(LCD1);
      lcdhome(LCD2);
      if(el.valid) {
	bits12(el.coarse);
	lcdprint(LCD1, "EL Coarse ");
	lcdprint(LCD1, result);
	sprintf(result, " %1d", el.cf_diff);
	lcdprint(LCD1, result);
	lcdprint(LCD1, "                   Fine            ");
	bits16(el.fine);
	lcdprint(LCD1, result);
	lcdprint(LCD1, " ");
	bits25(el.position);
	lcdprint(LCD2, "   Comb   ");
	lcdprint(LCD2, result);
	lcdprint(LCD2, "     ");

	el.angle=(double)el.position * LSBRES;
	minutes = (el.angle - floor(el.angle));
	degrees = el.angle - minutes;
	minutes = minutes * 60.0;
	seconds = (minutes - floor(minutes));
	minutes = minutes - seconds;
	seconds = seconds * 60.0;
	sprintf(result, "%10.5f    %4.0f%3.0f%4.1f     ", el.angle, degrees, minutes, seconds);
	lcdprint(LCD2, result);
	lcdprint(LCD2, "         ");
      }
      else {
	lcdprint(LCD1, "El position data invalid               ");
	lcdprint(LCD1, BLANKLINE);
	lcdprint(LCD2, "Not receiving data from the El encoder ");
	lcdprint(LCD2, BLANKLINE);
      }
      break;
  }
}


main()
{
  unsigned int cshi, cslo, fnhi, fnlo;	/* raw bytes from fiber */
  int display_mode;			/* what should be on LCD */
  int display_timer;			/* timeout to revert to default */
  int i;
  int lcd_count;			/* count of loops before updating LCD */
  int kbd;				/* value read from keyboard/switches */

  lcdinit(LCD1);			/* Initialize LCD top */
  lcdinit(LCD2);	   		/* Initialize LCD bottom */

  display_mode=AZEL_DISPLAY;		/* set default display */
  display_timer=0;			/* init timers */
  az.timer=0;
  el.timer=0;
  az.valid=FALSE;
  el.valid=FALSE;
  lcd_count=0;

  /* Init PLCBus */
  /* Read headers, set options */
  /* Record power-up in log */

  while(1)				/* main program loop */
  {
    for(i=0;i<BUF_DELAY;i++);
    if(up_digin(AZ_RDY))		/* if Az pos available */
      {
	fnlo = readpos(AZ_BUFFER);	/* read four Az bytes */
	fnhi = readpos(AZ_BUFFER);
	cslo = readpos(AZ_BUFFER);
	if(up_digin(AZ_RDY))		/* if line still high, so far so good */
	  {
	    cshi = readpos(AZ_BUFFER);
	    for(i=0;i<BUF_DELAY;i++);
	    if(!up_digin(AZ_RDY))	/* if line went low, all is well */
	      {
		az.fine =  fnlo+(fnhi*0x100);	/* combine bytes to word */
		az.coarse = cslo+(cshi*0x100);
		az.position = MAX_POS - combine(az.fine, az.coarse); /* Az is reverse of El */
		az.cf_diff = cf_diff;	/* save coarse/fine diff */
		az.timer = 0;		/* reset az timer */
		az.valid = TRUE;
	      }
	  }
	/*	else az.valid = FALSE; */
      }
    if(up_digin(EL_RDY))		/* if El pos available */
      {
	fnlo = readpos(EL_BUFFER);	/* read four El bytes */
	fnhi = readpos(EL_BUFFER);
	cslo = readpos(EL_BUFFER);
	if(up_digin(EL_RDY))
	  {
	    cshi = readpos(EL_BUFFER);
	    for(i=0;i<BUF_DELAY;i++);
	    if(!up_digin(EL_RDY))	/* if line went low, all is well */
	      {
		el.fine =  fnlo+(fnhi*0x100);	/* combine bytes to word */
		el.coarse = cslo+(cshi*0x100);
		el.position = combine(el.fine, el.coarse);
		el.cf_diff = cf_diff;	/* save coarse/fine diff */
		el.timer = 0;		/* reset el timer */
		el.valid = TRUE;
	      }
	  }
	/*	else el.valid = FALSE; */
      }

    /* read keyboard/switches */
    outport(SWITCH,1);			/* pull switch line */
    for(i=0;i<BUF_DELAY;i++);		/* wait for buffer data valid */
    kbd=(~inport(INENLO)&0x07);	/* read keys */
    outport(SWITCH,0);			/* release switch line */
    if(kbd&1) display_mode=AZ_DETAILS;
    if(kbd&2) display_mode=EL_DETAILS;
    if(kbd==0) display_mode=AZEL_DISPLAY;
    
    /* check timers, set flags, change offsets */
    az.timer++;			/* update timers */
    el.timer++;
    /* display_timer++; */
    if (az.timer > BUF_TIMEOUT) {	/* if az or el timeouts set invalid */
      az.valid=FALSE;
      az.timer=0;
    }
    if (el.timer > BUF_TIMEOUT) {
      el.valid=FALSE;
      el.timer=0;
    }
    if(display_timer > DSP_TIMEOUT) {
      display_mode=AZEL_DISPLAY;
      display_timer=0;
    }

    lcd_count++;
    if(lcd_count > LCD_FREQ) {
      update_display(display_mode);
      lcd_count=0;
    }

    update_acu();			/* send position to ACU */
    hitwd();				/* hit watchdog timer */
  }
}