home *** CD-ROM | disk | FTP | other *** search
/ Virtual Reality Zone / VRZONE.ISO / mac / PC / PCGLOVE / GLOVE / GLOVE.C < prev    next >
C/C++ Source or Header  |  1991-11-07  |  14KB  |  491 lines
  1. /**********************************************************************
  2.  
  3.   Originally "power.c" (c) manfredo 9/91 (manfredo@opal.cs.tu-berlin.de)
  4.   Developed on an ATARI 1040ST with TC 1.1 using a logic analyzer to get
  5.   the correct timings.
  6.  
  7. **********************************************************************/
  8. /*********************************************************************
  9.               ported to PC compatibles by
  10.                  Greg Alt 10/91
  11.  
  12.                 galt@peruvian.utah.edu
  13.              or galt@es.dsd.com
  14.  
  15. **********************************************************************/
  16. /*********************************************************************
  17.  
  18.  Substantially rewritten by Dave Stampe (c) 1991: PWRFILT.C
  19.   No cash, no warranty, no flames.
  20.   This stuff works great, so gimme credit.
  21.  
  22.  Goals <achieved> were:
  23.  
  24.   Higher speed, smaller code.
  25.   Polled operation is now possible.
  26.   Graphics test (VGA)
  27.   Noise reduction added, gets rid of 99.5% of noise with NO DELAY!
  28.  
  29.   This runs on a 486/25 with an i/o card. 
  30.   Someone should adapt it for the usual printer port adapter.
  31.   It was compiled with Turbo C++ 2.0 but will probably
  32.   work on any Turbo C directly. MSC will need library calls checked.
  33.  
  34.  
  35.  dstamp@watserv1.uwaterloo.ca            17/10/91
  36. **********************************************************************/
  37.  
  38. /*********************************************************************
  39.   Re-converted to use printer port by Dave Stampe and Bernie Roehl.
  40.                                         October 18, 1991.
  41.  
  42.   I also split off Dave's graphics code into a separate file, and put some
  43.   of the stuff that was shared between the two into glove.h
  44.  
  45.   I also added a little judicious whitespace here and there to enhance
  46.   readability, and made a whole bunch of globals static.
  47.  
  48.   I also added init_glove(mode) and glove_read(glov), the latter of which
  49.   calls getglove(glov), deglitch(glov) and dehyst(glov).
  50.  
  51.                                       --Bernie, October 18-19 1991
  52.  ********************************************************************/
  53.  
  54. /* More changes:
  55.  
  56.    init_glove() now auto-calibrates.  A new function (available outside
  57.    of this module) called "udelay" delays for a certain number of micro-
  58.    seconds.  Calls to fdelay have been replaced by udelay(),
  59.    and the D2BITS and D2BYTES values are now in microseconds.
  60.  
  61.    init_glove() now takes an additional parameter, a pointer to a
  62.    function (currently unused).
  63.  
  64.                                       --Bernie Roehl, October 21 1991
  65.  */
  66.  
  67. /* Interrupt-driven operation is now implemented!  Simply specify IHIRES
  68.    (for interrupt-driven high-resolution mode).
  69.  
  70.    Also, based on suggestions by Dave Stampe, I've:
  71.        - put the calibration stuff into a separate routine
  72.        - gone back to using counts instead of microseconds in most
  73.          places (for performance reasons; it saves a MULT instruction);
  74.        - udelay() is now gone, and a uconvert() macro does the conversion
  75.  
  76.     I have also:
  77.        - renamed the functions so they all begin with "glove_"; they
  78.          are now named glove_init(), glove_read() and glove_quit()
  79.  
  80.     Many thanks to Dave for all his help, especially in finding some
  81.     annonying timing problems.
  82.  
  83.                                  -- Bernie Roehl, November 8 1991
  84.  */
  85.  
  86. #include <stdio.h>
  87. #include <stdlib.h>
  88. #include <dos.h>
  89. #include <bios.h>
  90. #include <signal.h>
  91.  
  92. #include "glove.h"
  93.  
  94. #define PC_PRINTER  1   /* use the PC Printer port for i/o */
  95.  
  96. #define D2BITS        4            /* microseconds */
  97. #define D2BYTES        96            /* microseconds */
  98. #define SAMPLE_TIME    20            /* milliseconds */
  99.  
  100. #ifdef PC_PRINTER
  101. #define INPORT  0x379           /* i/o port addresses */
  102. #define OUTPORT 0x378
  103. /* bits from parallel port */
  104. #define        GDATA           0x10    /* PG data in */
  105. #define        GLATCH          0x02    /* PG latch out */
  106. #define        GCLOCK          0x01    /* PG clock out */
  107. #define        GCLOLAT         0x03    /* clock + latch */
  108. #define     SHIFTVAL        4       /* shift data right 4 bits */
  109. #endif
  110.  
  111. #ifdef DSTAMPE         /* stuff from here down to #else is i/o card-specific */
  112. #define INPORT  0x2A0           /* i/o port addresses */
  113. #define OUTPORT 0x2A0
  114. /* bits for i/o ports */
  115. #define        GDATA           0x01    /* PG data in */
  116. #define        GLATCH          0x02    /* PG latch out */
  117. #define        GCLOCK          0x01    /* PG clock out */
  118. #define        GCLOLAT         0x03    /* clock + latch */
  119. #define     SHIFTVAL        0       /* don't shift */
  120. #endif
  121.  
  122. static unsigned long bitdelay, bytedelay, longdelay;
  123.  
  124. static void fdelay(unsigned long val)
  125.     {
  126.     while (val--);
  127.     }
  128.  
  129. static unsigned long microfactor = 0L;  /* usec/iteration times 91 */
  130.  
  131. #define uconvert(microseconds) (((microseconds) * microfactor)/91)
  132.  
  133. void glove_delay()
  134.     {
  135.     fdelay(longdelay);
  136.     }
  137.  
  138. /* defines for output line pair control */
  139.  
  140. #define     C0L0()       outportb(OUTPORT, 0)        /* clock 0 latch 0 */
  141. #define     C0L1()       outportb(OUTPORT, GLATCH)   /* clock 0 latch 1 */
  142. #define     C1L0()       outportb(OUTPORT, GCLOCK)   /* clock 1 latch 0 */
  143. #define     C1L1()       outportb(OUTPORT, GCLOLAT)  /* clock 1 latch 1 */
  144.  
  145. static unsigned char getbyte()  /* read a byte from glove <rolled code> */
  146.     {
  147.     register int i;
  148.     register unsigned char x = 0;
  149.     
  150.     C1L0();                /* generate a reset (latch) pulse */
  151.     C1L1();
  152.     fdelay(bitdelay);            /* hold for 3 us */
  153.     C1L0();
  154.     fdelay(bitdelay);            /* hold for 3 us */
  155.  
  156.     for(i = 0; i < 8; i++)
  157.         {
  158.         x = (x << 1) + ((inportb(INPORT) & GDATA) >> SHIFTVAL); 
  159.         C0L0();
  160.         fdelay(bitdelay);
  161.         C1L0();  /* pulse */
  162.         fdelay(bitdelay);
  163.         }
  164.     return x;  /* return the byte */
  165.     }
  166.  
  167. void getglove(glove_data *buf)      /* read 6 byte data packet */
  168.     {
  169.     register unsigned char *bp = (char *) buf;
  170.     register int i;
  171.     for (i = 0; i < 6; ++i) {
  172.         *bp++ = getbyte();    /* read data */
  173.         fdelay(bytedelay);
  174.         }
  175.     /* throwaways (speeds up polling later) */
  176.     getbyte();    
  177.     fdelay(bytedelay);
  178.     getbyte();
  179.     }
  180.  
  181. /*  HIRES ENTRY CODES 
  182. byte:
  183. 1- any value between $05 and $31
  184. 2- only $C1 and $81 work OK
  185. 3- no effect
  186. 4- no effect
  187. 5- no effect
  188. 6- only $FF works
  189. 7- seems to affect read rate slightly, 1 fastest
  190. */
  191.  
  192. static int hires_code[7] = { 0x06, 0xC1, 0x08, 0x00, 0x02, 0xFF, 0x01 };
  193.  
  194. void Hires()    /* enter HIRES mode <rolled code- speed unimportant> */
  195.     {
  196.     int i,j,k;
  197.               /* dummy read 4 bits from glove:  */
  198.     C1L0(); C1L1();    /* generate a reset (latch) pulse */
  199.     fdelay(bitdelay);    /* delay for 6 us */
  200.     fdelay(bitdelay);
  201.     C1L0();
  202.     fdelay(bitdelay);    /* delay for 6 us */
  203.     fdelay(bitdelay);
  204.  
  205.     C0L0(); C1L0();    /* pulse clock */
  206.     fdelay(bitdelay);
  207.     C0L0(); C1L0();    /* pulse clock */
  208.     fdelay(bitdelay);
  209.     C0L0(); C1L0();    /* pulse clock */
  210.     fdelay(bitdelay);
  211.     C0L0(); C1L0();    /* pulse clock */
  212.  
  213.          /* handshake for command code? */
  214.     C1L0();
  215.     fdelay(uconvert(7212));   /* 7212 us delay */
  216.     C1L1();
  217.     fdelay(uconvert(2260));   /* 2260 us delay */
  218.  
  219.     for (i = 0; i < 7; i++)   /* send 7 bytes */
  220.         {
  221.         k = hires_code[i];
  222.         for (j = 0; j < 8; j++)     /* 8 bits per byte, MSB first */
  223.             {
  224.             if (k & 0x80) 
  225.                 {
  226.                 C1L1(); 
  227.                 C0L1();
  228.                 C1L1();
  229.                 }
  230.             else
  231.                 {
  232.                 C1L0(); 
  233.                 C0L0();
  234.                 C1L0();
  235.                 }
  236.             k <<= 1;
  237.             fdelay(bitdelay);
  238.             }
  239.         fdelay(bytedelay);
  240.         }
  241.  
  242.     fdelay(uconvert(892));    /* 892 us delay (end of 7. byte) */
  243.  
  244.     C1L0();         /* drop the reset line */
  245.     fdelay(uconvert(40000));   /* some time for the glove controller to relax */
  246.     }
  247.  
  248. #define XHYST 2            /* hysterisis for X, Y low noise reduction */
  249. #define YHYST 2            /* 2 eliminates +/-3 quanta of noise */
  250.  
  251. static int ox = -1000;            /* last x,y for hysterisis */
  252. static int oy = -1000;
  253.  
  254. static void dehyst(glove_data *g)        /* hysterisis deglitch (low noise removal) */
  255.     {
  256.     int x = g->x;
  257.     int y = g->y;
  258.  
  259.     if(g->keys==0) ox = oy = 0;    /* handle recentering ("0"key or "Center") */
  260.  
  261.     if(x-ox>XHYST) ox = x-XHYST;    /* X hysterisis */
  262.     if(ox-x>XHYST) ox = x+XHYST;
  263.  
  264.     if(y-oy>YHYST) oy = y-YHYST;    /* Y hysterisis */
  265.     if(oy-y>YHYST) oy = y+YHYST;
  266.  
  267.     g->x = ox;            /* replace present X,Y data */
  268.     g->y = oy;
  269.     }
  270.  
  271. #define XACC 8          /* X, Y maximum accel/decel level. Should */
  272. #define YACC 8            /* be 6-10, but too high limits gesturing */
  273.  
  274. #define XXTEND 1        /* stretches deglitching time */
  275. #define YXTEND 1
  276.  
  277. static int x1 = 0;        /* delayed 1 sample (for smoothed velocity test) */
  278. static int y1 = 0;
  279. static int x2 = 0;        /* delayed 2 samples */
  280. static int y2 = 0;
  281. static int lx = 0;        /* last good X,Y speed */
  282. static int ly = 0;
  283. static int lax = 0;        /* bad data "stretch" counter */
  284. static int lay = 0;
  285. static int lsx = 0;        /* X,Y "hold" values to replace bad data */
  286. static int lsy = 0;
  287. static int lcx = 0;        /* last X,Y speed for accel. calc. */
  288. static int lcy = 0;
  289.  
  290. static void deglitch(glove_data *g)
  291. {
  292.     int vx, vy;
  293.  
  294.     int x = g->x;
  295.     int y = g->y;
  296.  
  297.     if(g->keys == 0)        /* reset on recentering ("0" or "Center" key) */
  298.         {
  299.         x1 = x2 = y1 = y2 = 0;
  300.         lx = ly = lax = lay = 0;
  301.         lsx = lsy = lcx = lcy = 0;
  302.         }
  303.  
  304.     vx = x-((x1+x2)>>1);        /* smoothed velocity */
  305.     vy = y-((y1+y2)>>1);
  306.  
  307.     x2 = x1;            /* update last values */
  308.     x1 = g->x;
  309.  
  310.     y2 = y1;
  311.     y1 = g->y;
  312.  
  313.     if (abs(lcx-vx) > XACC) lax = XXTEND;    /* check for extreme acceleration */
  314.     if (lax == 0) lx = vx;                  /* save only good velocity        */
  315.     lcx = vx;                              /* save velocity for next accel.  */
  316.  
  317.     if (abs(lcy-vy) > YACC) lay = YXTEND;    /* same deal for Y accel. */
  318.     if (lay == 0) ly = vy;
  319.     lcy = vy;
  320.  
  321.     if (lax != 0)        /* hold X pos'n if glitch */
  322.         {
  323.         g->x = lsx;
  324.         lax--;
  325.         }
  326.  
  327.     if (lay != 0)             /* hold Y pos'n if glitch */
  328.         {
  329.         g->y = lsy;
  330.         lay--;
  331.         }
  332.  
  333.     lsx = g->x;        /* save position for X,Y hold */
  334.     lsy = g->y;
  335.  
  336.     /* g->y = x;*/
  337.     }
  338.  
  339. static void calibrate()
  340.     {
  341.     unsigned long n;
  342.     /* calibrate timing loop; note that instead of dividing by 18.2,
  343.        we multiply by 5 now and divide by 91 later */
  344.     n = biostime(0, 0L);
  345.     fdelay(1000000);  /* divide by a milllion to get microfactor */
  346.     microfactor = (biostime(0, 0L) - n) * 5;
  347.     bitdelay = uconvert(D2BITS);
  348.     bytedelay = uconvert(D2BYTES);
  349.     longdelay = uconvert(4000);
  350.     }
  351.  
  352. static int glovemode = HIRES;        /* operating mode of glove */
  353. static void (*upcall)() = NULL;        /* user's function to call up to */
  354. static unsigned char uses_ints = 0;    /* non-zero if we're interrupt-driven */
  355. static glove_data glove_int_data;    /* our copy of the most recent data */
  356.  
  357. /* Following is the number of our ticks per real tick, rounded up */
  358. #define DIVISOR ((55+SAMPLE_TIME/2)/SAMPLE_TIME)
  359.  
  360. #define UNUSED_VECT 128                /* unused interrupt vector (we hope!) */ 
  361.  
  362. static void interrupt (*oldint8)() = NULL;    /* previous interrupt handler */
  363. static void interrupt (*oldunused)() = NULL;  /* previous unused vector */
  364.  
  365. static unsigned n_ints = 0;        /* number of interrupts since oldint8 called */
  366. static unsigned unready = 0;    /* number of times glove has been not ready */
  367.  
  368. static char samples[7500];        /* samples of X values */
  369. static char dsamples[7500];        /* samples of X values after deglitching */
  370. static int nsamples = 0;
  371.  
  372. static void interrupt int8()
  373.     {
  374.     disable();
  375.     if (getbyte() != 0xA0) {
  376.         if (++unready > 500) {        /* glove not responding... reset it */
  377.             unready = 0;
  378.             if (glovemode == IHIRES)
  379.                 Hires();
  380.             }
  381.         }
  382.     else {  /* data ready! */
  383.         fdelay(bytedelay);
  384.         unready = 0;
  385.         getglove(&glove_int_data);
  386.         deglitch(&glove_int_data);        /* remove spikes and jumps */
  387.         dehyst(&glove_int_data);          /* add hysteresis to remove LL noise */
  388.         ++glove_int_data.nmissed;        /* flag data as new */
  389.         if (upcall) (*upcall)();        /* upcall to application */
  390.         }
  391.     if (++n_ints >= DIVISOR) {   /* call previous int 8 handler periodically */
  392.         n_ints = 0;
  393.         geninterrupt(UNUSED_VECT);
  394.         }
  395.     else
  396.         outportb(0x20, 0x20);        /* signal EOI */
  397.     }
  398.  
  399. #define TIMER_CONTROL 0x43        /* timer control register */
  400. #define TIMER_0       0x40        /* timer zero data register */
  401. #define TIMER_MODE    0x36        /* byte to write to control register */
  402.  
  403. int glove_init(int mode, void (*function)())
  404.     {
  405.     calibrate();
  406.     if (mode == LORES) mode = HIRES;
  407.     if (mode == ILORES) mode = IHIRES;    
  408.     if (mode == HIRES || mode == IHIRES) Hires();
  409.     glove_int_data.nmissed = 0;  /* go this one, start counter again! */
  410.     if (mode == ILORES || mode == IHIRES) {        /* an interrupt mode */
  411.         void glove_quit();
  412.         unsigned rate;
  413.         uses_ints = 1;
  414.         oldint8 = getvect(8);
  415.         oldunused = getvect(UNUSED_VECT);
  416.         setvect(UNUSED_VECT, oldint8);
  417.         setvect(8, int8);
  418.         rate = 65535/DIVISOR;
  419.         /* reprogram timer */
  420.         disable();
  421.         outportb(TIMER_CONTROL, TIMER_MODE);    /* timer control port */
  422.         outportb(TIMER_0, rate & 0xFF);            /* low byte to timer zero */
  423.         outportb(TIMER_0, (rate >> 8) & 0xFF);    /* high byte to timer zero */
  424.         /* make sure we clean up at exit or on errors */
  425.         atexit(glove_quit);
  426.         signal(SIGABRT, glove_quit);
  427.         signal(SIGFPE, glove_quit);
  428.         signal(SIGINT, glove_quit);
  429.         enable();
  430.         }
  431.     else
  432.         uses_ints = 0;
  433.     glovemode = mode;
  434.     upcall = function;
  435.     return glovemode;     /* returns mode actually set */
  436.     }
  437.  
  438. int glove_read(glove_data *glov)
  439.     {
  440.     if (uses_ints) {    /* interrupt-driven */
  441.         disable();
  442.         *glov = glove_int_data;
  443.         glove_int_data.nmissed = 0;
  444.         enable();
  445.         return glov->nmissed;
  446.         }
  447.     /* polled operation */
  448.     getglove(glov);        /* retrieve the data */
  449.     deglitch(glov);        /* remove spikes and jumps */
  450.     dehyst(glov);          /* add hysteresis to remove LL noise */
  451.     glove_int_data = *glov;
  452.     return 1;            /* always new data */
  453.     }
  454.  
  455. int glove_ready()    /* returns 1 if glove ready, 0 otherwise */
  456.     {
  457.     if (uses_ints)  /* interrupt handler makes sure there's always data */
  458.         return 1;
  459.     if (getbyte() == 0xA0) {    /* ready */
  460.         unready = 0;
  461.         if (upcall) (*upcall)();
  462.         return 1;
  463.         }
  464.     /* not ready */
  465.     if (++unready > 500) {  /* glove not responding... reset it */
  466.         unready = 0;
  467.         if (glovemode == HIRES || glovemode == IHIRES)
  468.             Hires();
  469.         }
  470.     return 0;
  471.     }
  472.  
  473. void glove_quit()
  474.     {
  475.     if (uses_ints) {
  476.         disable();
  477.         /* reprogram timer */
  478.         outportb(TIMER_CONTROL, TIMER_MODE);
  479.         outportb(TIMER_0, 0);    /* low byte */
  480.         outportb(TIMER_0, 0);    /* high byte */
  481.         setvect(8, oldint8);
  482.         setvect(UNUSED_VECT, oldunused);
  483.         signal(SIGABRT, SIG_DFL);
  484.         signal(SIGFPE, SIG_DFL);
  485.         signal(SIGINT, SIG_DFL);
  486.         enable();
  487.         }
  488.     upcall = NULL;
  489.     }
  490.  
  491.