home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
World of Shareware - Software Farm 2
/
wosw_2.zip
/
wosw_2
/
CPROG
/
GLOVDRIV.ZIP
/
THEGLOVE.C
< prev
Wrap
C/C++ Source or Header
|
1992-02-17
|
11KB
|
444 lines
/**********************************************************************
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 0x279 /* i/o port addresses <CHANGED> */
#define OUTPORT 0x278
/* bits for i/o ports <CHANGED> */
#define GDATA 0x10 /* 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)
{
long i;
i=(long)(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, "d:\\tpas5\\bgidrvs\\"); /* 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)>>4);
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;*/
}