Subject: Automatic calibration of delay loops

From: Bernie Roehl <[email-masked]>
Date: Mon, 21 Oct 91 17:30:44 EDT

Below is a modified version of the source, which does automatic calibration
of the delay loops.  It should work without change on any machine regardless
of clock speed.  (No more N and D -- yay!)

The D2BITS and D2BYTES values are now in microseconds.

I've also adopted the suggestion that was made about including a function
as a second parameter to init_glove(), though I currently don't do anything
with it.

My next step will be to look at reading the glove under timer interrupts.

(Everything below is on sunee.waterloo.edu in pub/glove)

--------- glove.h -------------

/***** GLOVE DATA SPECIFICATIONS **************

 The glove_data array has been simplified. These are its functions:

x =   X position, 3mm per number
y =   Y position, 3mm per number
z =   distance,  14mm per number 
rot = wrist twist. 0 is up 1 is slightly CW, 5 is down, 11 is slightly CCW.
      About 30 to 40 degrees per count.

 Note: exact scaling of all above change with distance! Closer is higher res.

fingers = packed 2-bit values, 0 is open, 3 is (tight) fist:
	  Bit format: TtIiMmRr  for Thumb, Index, Middle, and Ring fingers.

keys: $FF or $80 is no key. Responds with 0 to 9 for keys "0" thru "9"
      $82 = START, $83 = SEL, $0A = "A", $0B = "B", 0 is "Center"
      Up,down,left,right are $0D,$0E,$0C,$0F respectively.

*/

typedef struct glove_data { 
			  signed char x,y,z,rot,fingers,keys;
			  } glove_data;

/* prototypes */
 
void Hires (void);		 	/* puts glove in hires mode   */
void getglove (glove_data *);	/* get data packet from glove */
int  glove_ready(void);			/* returns 0 if not ready     */

int init_glove(int mode, void (*function)());  /* returns actual mode used */
int read_glove(glove_data *glov);	/* reads glove data, with de-glitching */
void reset_glove(void);     /* release the glove */
void udelay(unsigned microseconds);  /* delay for a number of microseconds */

/* Modes passed to init_glove() */

#define LORES 0
#define HIRES 1

/* End of glove.h */

----------- newglove.c -------------

/**********************************************************************
 
  Originally "power.c" (c) manfredo 9/91 ([email-masked])
  Developed on an ATARI 1040ST with TC 1.1 using a logic analyzer to get
  the correct timings.
 
**********************************************************************/
/*********************************************************************
		      ported to PC compatibles by
			     Greg Alt 10/91
 
			    [email-masked]
			 or [email-masked]
 
**********************************************************************/
/*********************************************************************

 Substantially rewritten by Dave Stampe (c) 1991: PWRFILT.C
  No cash, no warranty, no flames.
  This stuff works great, so gimme credit.

 Goals <achieved> were:

  Higher speed, smaller code.
  Polled operation is now possible.
  Graphics test (VGA)
  Noise reduction added, gets rid of 99.5% of noise with NO DELAY!

  This runs on a 486/25 with an i/o card. 
  Someone should adapt it for the usual printer port adapter.
  It was compiled with Turbo C++ 2.0 but will probably
  work on any Turbo C directly. MSC will need library calls checked.

 [email-masked]			17/10/91
**********************************************************************/

/*********************************************************************
  Re-converted to use printer port by Dave Stampe and Bernie Roehl.
                                        October 18, 1991.

  I also split off Dave's graphics code into a separate file, and put some
  of the stuff that was shared between the two into glove.h

  I also added a little judicious whitespace here and there to enhance
  readability, and made a whole bunch of globals static.

  I also added init_glove(mode) and glove_read(glov), the latter of which
  calls getglove(glov), deglitch(glov) and dehyst(glov).

                                      --Bernie, October 18-19 1991
 ********************************************************************/

/* More changes:

   init_glove() now auto-calibrates.  A new function (available outside
   of this module) called "udelay" delays for a certain number of micro-
   seconds.  Calls to fdelay have been replaced by udelay(),
   and the D2BITS and D2BYTES values are now in microseconds.

   init_glove() now takes an additional parameter, a pointer to a
   function (currently unused).

                                      --Bernie Roehl, October 21 1991
 */

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

#include "glove.h"

#define PC_PRINTER  1   /* use the PC Printer port for i/o */

#define D2BITS		3			/* microseconds */
#define D2BYTES		96			/* microseconds */

#ifdef PC_PRINTER
#define INPORT  0x379       	/* i/o port addresses */
#define OUTPORT 0x378
/* bits from parallel port */
#define		GDATA           0x10    /* PG data in */
#define		GLATCH          0x02    /* PG latch out */
#define		GCLOCK          0x01    /* PG clock out */
#define		GCLOLAT         0x03    /* clock + latch */
#define     SHIFTVAL        4       /* shift data right 4 bits */
#endif

#ifdef DSTAMPE         /* stuff from here down to #else is i/o card-specific */
#define INPORT  0x2A0       	/* i/o port addresses */
#define OUTPORT 0x2A0
/* bits for i/o ports */
#define		GDATA           0x01    /* PG data in */
#define		GLATCH          0x02    /* PG latch out */
#define		GCLOCK          0x01    /* PG clock out */
#define		GCLOLAT         0x03    /* clock + latch */
#define     SHIFTVAL        0       /* don't shift */
#endif

static void fdelay(unsigned long val)
	{
	while (val--);
	}

static unsigned long microfactor = 0L;  /* usec/iteration times 91 */

void udelay(unsigned microseconds)
	{
	/* we do the divide here for precision */
	fdelay((microseconds * microfactor) / 91);
	}

void slowdelay()
	{
	udelay(4000);
	}

/* defines for output line pair control */
 
#define     C0L0()       outportb(OUTPORT, 0)        /* clock 0 latch 0 */
#define     C0L1()       outportb(OUTPORT, GLATCH)   /* clock 0 latch 1 */
#define     C1L0()       outportb(OUTPORT, GCLOCK)   /* clock 1 latch 0 */
#define     C1L1()       outportb(OUTPORT, GCLOLAT)  /* clock 1 latch 1 */
 
static unsigned char getbyte ()  /* read a byte from glove <rolled code> */
	{
	register int i;
	register unsigned char x = 0;
    
	C1L0 ();	    		/* generate a reset (latch) pulse */
	C1L1 ();
	udelay(D2BITS);    		/* hold for 3 us */
	C1L0 ();

	for(i = 0; i < 8; i++)
		{
		x = (x << 1) + ((inportb(INPORT) & GDATA) >> SHIFTVAL); 
		C0L0 ();
		C1L0 ();  /* pulse */
		}
	return(x);  /* return the byte */
	}

void getglove(buf)      /* read 6 byte data packet */
	glove_data *buf;
	{
	register unsigned char *bp = (unsigned char *) buf;
	register int i;
	for (i = 0; i < 6; ++i) {
		*bp++ = getbyte ();	/* read data */
		udelay(D2BYTES);
		}
	/* throwaways (speeds up polling later) */
	getbyte ();	
	udelay(D2BYTES);
	getbyte ();
	}

int glove_ready()	/* returns 1 if glove ready, 0 otherwise */
	{
	return (getbyte() == 0xA0) ? 1 : 0;
	}
 
/*  HIRES ENTRY CODES 
byte:
1- any value between $05 and $31
2- only $C1 and $81 work OK
3- no effect
4- no effect
5- no effect
6- only $FF works
7- seems to affect read rate slightly, 1 fastest
*/

static int hires_code[7] = { 0x06, 0xC1, 0x08, 0x00, 0x02, 0xFF, 0x01 };
 
void Hires ()	/* enter HIRES mode <rolled code- speed unimportant> */
	{
	int i,j,k;
		      /* dummy read 4 bits from glove:  */
	C1L0 (); C1L1 ();    /* generate a reset (latch) pulse */
	udelay(D2BITS);
	C1L0 ();

	udelay(D2BITS);
	C0L0 (); C1L0 ();    /* pulse clock */
	udelay(D2BITS);
	C0L0 (); C1L0 ();    /* pulse clock */
	udelay(D2BITS);
	C0L0 (); C1L0 ();    /* pulse clock */
	udelay(D2BITS);
	C0L0 (); C1L0 ();    /* pulse clock */
 
		 /* handshake for command code? */
	C1L0 ();
	udelay(7212);   /* 7212 us delay */
	C1L1 ();
	udelay(2260);   /* 2260 us delay */
 
	for (i = 0; i < 7; i++)   /* send 7 bytes */
		{
		k=hires_code[i];
		for (j = 0; j < 8; j++)     /* 8 bits per byte, MSB first */
			{
			if (k & 0x80) 
				{
				C1L1(); 
				C0L1();
				C1L1();
				}
			else
				{
				C1L0(); 
				C0L0();
				C1L0();
				}
			k = k<<1;
			udelay(D2BITS);
			}
		udelay(D2BYTES);
		}

	udelay(892);    /* 892 us delay (end of 7. byte) */
 
	C1L0 ();         /* drop the reset line */
	udelay(100000);   /* some time for the glove controller to relax */
	udelay(100000);
	}

#define XHYST 2			/* hysterisis for X, Y low noise reduction */
#define YHYST 2			/* 2 eliminates +/-3 quanta of noise */

#define XACC 8                  /* X, Y maximum accel/decel level. Should */
#define YACC 8			/* be 6-10, but too high limits gesturing */

#define XXTEND 2		/* stretches deglitching time */
#define YXTEND 1

static int ox = -1000;			/* last x,y for hysterisis */
static int oy = -1000;

static void dehyst(glove_data *g)		/* hysterisis deglitch (low noise removal) */
	{
	int x = g->x;
	int y = g->y;

	if(g->keys==0) ox = oy = 0;	/* handle recentering ("0"key or "Center") */

	if(x-ox>XHYST) ox = x-XHYST;	/* X hysterisis */
	if(ox-x>XHYST) ox = x+XHYST;

	if(y-oy>YHYST) oy = y-YHYST;	/* Y hysterisis */
	if(oy-y>YHYST) oy = y+YHYST;

	g->x = ox;			/* replace present X,Y data */
	g->y = oy;
	}

static int x1 = 0;		/* delayed 1 sample (for smoothed velocity test) */
static int y1 = 0;
static int x2 = 0;		/* delayed 2 samples */
static int y2 = 0;
static int lx = 0;		/* last good X,Y speed */
static int ly = 0;
static int lax = 0;		/* bad data "stretch" counter */
static int lay = 0;
static int lsx = 0;		/* X,Y "hold" values to replace bad data */
static int lsy = 0;
static int lcx = 0;		/* last X,Y speed for accel. calc. */
static int lcy = 0;

static void deglitch(glove_data *g)
{
	int vx, vy;

	int x = g->x;
	int y = g->y;

	if(g->keys == 0)		/* reset on recentering ("0" or "Center" key) */
		{
		x1 = x2 = y1 = y2 = 0;
		lx = ly = lax = lay = 0;
		lsx = lsy = lcx = lcy = 0;
		}

	vx = x-((x1+x2)>>1);		/* smoothed velocity */
	vy = y-((y1+y2)>>1);

	x2 = x1;			/* update last values */
	x1 = g->x;

	y2 = y1;
	y1 = g->y;

	if (abs(lcx-vx) > XACC) lax = XXTEND;	/* check for extreme acceleration */
	if (lax == 0) lx = vx;                   /* save only good velocity        */
	lcx = vx;                              /* save velocity for next accel.  */

	if (abs(lcy-vy) > YACC) lay = YXTEND;	/* same deal for Y accel. */
	if (lay == 0) ly=vy;
	lcy = vy;

	if (lax != 0)		/* hold X pos'n if glitch */
		{
		g->x = lsx;
		lax--;
		}

	if (lay != 0)             /* hold Y pos'n if glitch */
		{
		lay--;
		g->y = lsy;
		}

	lsx = g->x;		/* save position for X,Y hold */
	lsy = g->y;

	/* g->y = x;*/
	}

static void (*upcall)() = NULL;

int init_glove(int mode, void (*function)())
	{
	unsigned long n;
	/* calibrate timing loop; note that instead of dividing by 18.2,
	   we multiply by 5 now and divide by 91 later */
	n = biostime(0, 0L);
	fdelay(1000000);  /* divide by a milllion to get microfactor */
	microfactor = (biostime(0, 0L) - n) * 5;
	if (mode == HIRES) Hires();
	upcall = function;
	return HIRES;     /* returns mode actually set */
	}

int glove_read(glove_data *glov)
	{
	getglove(glov);
	deglitch(glov);		/* remove spikes and jumps */
	dehyst(glov);  		/* add hysteresis to remove LL noise */
	return 1;			/* for now */
	}

void reset_glove()
	{
	upcall = NULL;
	}

-------- glovgraf.c ----------------

/* Graphics-mode demonstration code for PowerGlove */

/* Written by Dave Stampe, Modified by Bernie Roehl, October 1991 */

#include <dos.h>
#include <bios.h>
#include <stdio.h>
#include <conio.h>
#include <graphics.h>
#include "glove.h"

int gdriver = DETECT;		/* for graphics plot and cursor */
int gmode = VGAHI;

void main()
	{
	glove_data glov;		/* glove data */
	unsigned unready;       /* number of unsuccessful tries to read glove */
	void drawp(), drawthing();

	initgraph(&gdriver, &gmode, "");
	if (graphresult() < 0) {
		printf("could not initialize graphics\n");
		exit(1);
		}
	cleardevice();
restart:
	init_glove(HIRES, NULL);

	while(!kbhit())
		{
		unready = 0; 			/* start polling glove */
		slowdelay();
		while(glove_ready()==0)              /* wait for glove to become ready */
			{
			if (unready++>500) 			 	/* reset mode if dead glove */
				goto restart;
			slowdelay();
			}
		glove_read(&glov);	 	/* read 6 byte packet */
#ifdef DEBUG
		drawp(&glov);  			/* plot x,y positions */
#endif
		drawthing(&glov);  		/* animate glove cursor */
		}

	getch();			/* exit when keyboard hit */
	reset_glove();
	textmode(LASTMODE);
	}

static void drawit(glove_data *g)		/* draw/erase box cursor */
	{
	int x = 320+2*(g->x);		/* compute X,Y center */
	int y = 240-2*(g->y);
	int z = 30+(g->z);		/* size prop. to Z */

	rectangle(x-z,y-z,x+z,y+z);
	}

static glove_data oldbuf;	/* used to store old state for drawing */

static int drawn = 0;		/* set if cursor to be erased */

void drawthing(glove_data *g)	/* draw square cursor */
	{
	if(g->keys==2) return;		/* hold down "2" to stop drawing */

	if(drawn)			/* erase old box */
		{
		setcolor(0);
		drawit(&oldbuf);
		}

	setcolor(15);			/* draw new box */
	drawit(g);
	drawn = 1;

	oldbuf.x = g->x;		/* save pos'n for next erase */
	oldbuf.y = g->y;
	oldbuf.z = g->z;
	}

static int xx = 0;           		/* plot position */

void drawp(glove_data *g)	/* plot X,Y data to test smoothing */
	{
	if(g->keys==4) 	/* restart at left edge if "4" pressed */
		{
		cleardevice();
		xx=0;
		}

	setcolor(0);
	line(xx,0,xx,479);
	line(xx+1,0,xx+1,479);
	setcolor(15);
	line(xx,240-2*g->x,xx+1,240-2*g->x);
	setcolor(12);
	line(xx+1,240-2*g->y,xx+2,240-2*g->y);
	xx++;
	xx++;
	if(xx > 639) xx = 0;
	}

------------- test.c ----------------

#include <stdio.h>
#include "glove.h"

void main()
	{
	glove_data glov;		/* glove data */
	unsigned unready;       /* number of unsuccessful tries to read glove */
restart:
	init_glove(HIRES, NULL);
	while(!kbhit())
		{
		unready = 0; 			/* start polling glove */
		slowdelay();
		while(glove_ready()==0)              /* wait for glove to become ready */
			{
			if (unready++>500) 			 	/* reset mode if dead glove */
				goto restart;
			slowdelay();
			}
		glove_read(&glov);	 	/* read 6 byte packet */
		printf("%3.3d %3.3d %3.3d %2.2d %02.2X %02.2X       \r",
			glov.x, glov.y, glov.z, 255&glov.rot,
			255&glov.fingers, 255&glov.keys);
		}
	reset_glove();
	}

--------- makefile ----------

test.exe: test.obj newglove.obj
	tcc test.obj newglove.obj

glovgraf.exe: glovgraf.obj newglove.obj
	tcc glovgraf.obj newglove.obj graphics.lib

glovgraf.obj: glovgraf.c glove.h
	tcc -c glovgraf

newglove.obj: newglove.c glove.h
	tcc -c newglove

test.obj: test.c glove.h
	tcc -c test