Received: from watserv1.uwaterloo.ca (watserv1.waterloo.edu) by karazm.math.UH.EDU with SMTP id AA24854
  (5.65c/IDA-1.4.4 for <glove-list@karazm.math.uh.edu>); Thu, 17 Oct 1991 20:51:46 -0500
Received: by watserv1.uwaterloo.ca
	id <AA12204>; Thu, 17 Oct 91 21:47:39 -0400
Date: Thu, 17 Oct 91 21:47:39 -0400
From: Dave Stampe-Psy+Eng <dstamp@watserv1.uwaterloo.ca>
Message-Id: <9110180147.AA12204@watserv1.uwaterloo.ca>
To: glove-list@karazm.math.uh.edu
Program: PWRFILT.C

This is a new HIRES glove driver program that includes NOISE REDUCTION!
I hung the receiver array in the *worst* corner of my room, and got
almost no glitches and rock-solid operation.  It's hard to believe it's
the same system!  And the NR adds no delay, and is invisible to the user.

This driver rolls up some of the loops in the original HIRES driver, 
exchanging speed in unimportant areas for much smaller code.  It was 
written to use a PC-LabCard I/O device, since I happened to have one.
Someone else will have to change the port addresses, bit masks, etc.
so it's usable with the printer port.

Interrupt polling of the Glove should be easy, given this code.  One 
warning: don't test the power glove too often (I recommend about 2-4 mS
between pollings) or the Glove will start trashing data severely. 

You non-PC users will want to transplant the NR code at least, so go ahead.

------------------------- cut here -----------------------------------

/**********************************************************************
 
  Originally "power.c" (c) manfredo 9/91 (manfredo@opal.cs.tu-berlin.de)
  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
 
			    galt@peruvian.utah.edu
			 or galt@es.dsd.com
 
**********************************************************************/
/*********************************************************************

 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.


 dstamp@watserv1.uwaterloo.ca			17/10/91
**********************************************************************/

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

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

#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

#define N 1			/* delay scaled by N/D <CHANGED> */
#define D 1     		/* these are 1,1 for 486 PC with i/o card */
#define INPORT  0x2A0       	/* i/o port addresses <CHANGED> */
#define OUTPORT 0x2A0
 
/* bits for i/o ports <CHANGED> */

#define		GDATA           0x01    /* PG data in */
#define		GLATCH          0x02    /* PG latch out */
#define		GCLOCK          0x01    /* PG clock out */
#define		GCLOLAT         0x03    /* clock + latch */

/* delay values for sending and sampling data <CHANGED> */
 
#define     D2BYTES      150      /* delay between 2 bytes = 96 us  */
#define     D2BITS       6        /* delay between 2 bits = 3 us    */
#define     D2SLOW       8000     /* intertest delay = 2000-4000 us */

/* Delay timing: may not work in some IBM C's due to problems with LONGs */

void fdelay(unsigned int val)
{
 register long i=N*val;
 for(;i>0;i-=D);
}

/* 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 */
 

/* prototypes */
 
void Hires (void);		 	/* puts glove in hires mode   */
void getglove (unsigned char *);	/* get data packet from glove */
int  glove_ready();			/* returns 0 if not ready     */
					/* delay repeats by 2-4 ms    */
unsigned char getbyte (void);		/* read byte from glove */
 

/***** 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;

/*********************************************/
			    
void main ()
{
 unsigned char buf[12];
 glove_data   *glov;
 unsigned unready;         /* number of unsuccessful tries to read glove */

 glov=(glove_data *)buf;
 initgraph(&gdriver, &gmode, "");	/* VGA graphics, 640x480 */
 cleardevice();
			   /* begin again here if glove crashes */
restart:		
 Hires (); 				/* set PG into 'hires' mode */

 while(!kbhit())
  {
   unready = 0; 			/* start polling glove */
   fdelay(D2SLOW);
   while(glove_ready()==0)              /* wait for glove to become ready */
    {
     if (unready++>500) goto restart; 	/* reset mode if dead glove */
     fdelay(D2SLOW);
    }
   
   getglove(buf);		 	/* read 6 byte packet */
   gotoxy(1,1);				/* print xyz at scrren top */
   printf("% 4d % 4d % 4d    ", 255&glov->x, 255&glov->y, 255&glov->z);
					/* print rot, fingers, keys */	
   printf("%-2x %-2x %-2x ", buf[3],buf[4],buf[5]);

   deglitch(glov);		/* remove spikes and jumps */
   dehyst(glov);  		/* add hysteresis to remove LL noise */

   drawp(glov);  		/* plot x,y positions */
   drawthing(glov);  		/* animate glove cursor */
  }

 getch();			/* exit when keyboard hit */
 C0L0();			/* release glove on exit */
}
 

 
void getglove(buf)      /* read 6 byte data packet */
unsigned char *buf;
{
 register unsigned char *bp;

 bp = buf;

 *bp++ = getbyte ();	/* read data */
 fdelay(D2BYTES);
 *bp++ = getbyte ();
 fdelay(D2BYTES);
 *bp++ = getbyte ();
 fdelay(D2BYTES);
 *bp++ = getbyte ();
 fdelay(D2BYTES);
 *bp++ = getbyte ();
 fdelay(D2BYTES);
 *bp++ = getbyte ();
 fdelay(D2BYTES);
			/* throwaways (speeds up polling later) */
 getbyte ();	
 fdelay(D2BYTES);
 getbyte ();
}



int glove_ready()	/* returns 1 if glove ready, 0 otherwise */
{
 int f;
 f = getbyte();
 return( (f==0xA0) ? 1 : 0);
}
 
 
 
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 ();
 fdelay(D2BITS);    		/* hold for 5 us */
 C1L0 ();

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

 return(x);  /* return the byte */
}


/*  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
*/

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 */
 fdelay(D2BITS);
 C1L0 ();

 fdelay(D2BITS);
 C0L0 (); C1L0 ();    /* pulse clock */
 fdelay(D2BITS);
 C0L0 (); C1L0 ();    /* pulse clock */
 fdelay(D2BITS);
 C0L0 (); C1L0 ();    /* pulse clock */
 fdelay(D2BITS);
 C0L0 (); C1L0 ();    /* pulse clock */
 
		 /* handshake for command code? */
 C1L0 ();
 fdelay(16950);  /* 7212 us delay */
 C1L1 ();
 fdelay(4750);   /* 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;
     fdelay(D2BITS);
    }
   fdelay(D2BYTES);
  }

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


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

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


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;
}



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);
}



int xx = 0;           		/* plot position */

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;
}



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


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;
}



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


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;*/
}