/* * POWERGLOVE ROUTINES Version 3.1 * Public Release Spring 1993 * * use file_id = init_glove(); to set up the glove * * use query_glove(file_id, &pglove); to get the glove data * and pglove.gesture * and pglove.key * * * If you have any questions, please contact: * * David Barberi * dbarberi@sunsite.unc.edu * * * Output format: * * Data Time_stamp gesture key x y z thumb one two three twist key1 key2 key3 key4 * | * 1= GOOD * 0= CORRUPT */ /*******************************************************************/ /* stream.c: Get PowerGlove data and display data values */ /* */ /* Originally written at the Syracuse University Virtual Realiat */ /* Laboratory by Greg Newby, Will Taroli, Bob Zeh, and David */ /* Barberi. Version 1.0 May 1991. */ /* */ /* Upgraded to include gesture recognition and glove keypad input */ /* by David Barberi. Version 2.0 - 2.7 November 1991. */ /* */ /* Added noise reduction code from David Stampe (posted to the */ /* Powerglove mailing list (glove-list@karazm.math.uh.edu) on */ /* 11/15/91 by Greg Newby. */ /* */ /* */ /* Contact information: */ /* David Barberi: dbarberi@sunsite.unc.edu */ /* Greg Newby: gbnewby@alexia.lis.uiuc.edu */ /*******************************************************************/ #include "glove.h" /* This include is very important.. contains glove structure */ /* the Port definition, and #defines */ #include #include #include #include #include "sys/callo.h" #include #include /*******************************************************************/ /* Routine to initialize the serial port */ /*******************************************************************/ int init_glove() /* returns the file_id for the open file */ { extern char *sys_errlist[]; int rslt; struct termios tbuf; printf(" Welcome to Glove Stream.... \n"); file_id = open(PORT_ID, O_RDWR, 0); if (file_id == -1) { printf("Error on open call to %s as Glove port, must be read/writable\n", PORT_ID); perror(*sys_errlist); return(1); } else printf(" %s is the Glove port\n", PORT_ID); tbuf.c_iflag = 0; tbuf.c_oflag = 0; tbuf.c_lflag = 0; tbuf.c_cflag = B9600 | CS8 | CLOCAL | CREAD; /* new timewait */ tbuf.c_cc[VMIN] = 0; /* Return read even if no chars */ tbuf.c_cc[VTIME] = 0; /* Return without waiting */ rslt = ioctl(file_id, TCSETA, &tbuf); if (rslt == -1) { fprintf(stderr, "ioctl failed\n"); perror(*sys_errlist); return(1); } ioctl(file_id, TCFLSH,2); /* flush the port */ return(file_id); } /******************************************************************* New Time sleep commands by Jan, ih3e@wilbury.cs.Virginia.EDU *******************************************************************/ void t_sleep(wait) unsigned int wait; { static struct timeval timeout; if ( wait < 1000 ) { timeout.tv_sec = 0; timeout.tv_usec = wait * 1000; } else { /* integral seconds */ timeout.tv_sec = wait / 1000; /* get millisecond part and * 1000 for usecs */ timeout.tv_usec = (wait % 1000) * 1000; } select(0, 0, 0, 0, &timeout); } /* t_sleep */ /*******************************************************************/ /* Puts current glove data in the structure */ /*******************************************************************/ int query_glove(file_id, pglove) pglove_data *pglove; int file_id; { int ret, temp; char buf[BUFSIZ]; char command[BUFSIZ]; /* Types of Commands for the PowerGlove 0x01 = Constand Upload (default) 0x02 = Upload only on Command 0x04 = Send Default Template (reset) 0x07 = Reset to Defaults 0x08 = Rotation Filter ON (default) 0x09 = Rotation Filter OFF */ command[0] = 2; write(file_id,command,1); t_sleep(sleepval); ret = read(file_id,buf,14); /* Check header bytes to see if the data is good, if bad return a 0 */ if (!((ret > 3) && (buf[0] == (0x81 - 127)) && (buf[1] == (0xDE - 127)) && (buf[2] == (0xDE - 127)))) { return(0); } /* Assign X, Y, and Z (need to query 1st bit for sign on Iris) */ pglove->x = buf[3]; if (buf[3] > 127) pglove->x -= 256; pglove->y = buf[4]; if (buf[4] > 127) pglove->y -= 256; pglove->z = buf[5]; if (buf[5] > 127) pglove->z -= 256; /* Adjust scale of Z */ /* pglove->z *= ZSCALER; */ /* Assign rotation and (bit-shifted) rotation values */ pglove->twist = buf[6]; temp = buf[7]; pglove->three = temp & 3; temp = temp >> 2; pglove->two = temp & 3; temp = temp >> 2; pglove->one = temp & 3; temp = temp >> 2; pglove->thumb = temp & 3; /* This is data for the Keypad buttons on the glove */ temp = buf[8]; pglove->key1 = temp & 3; temp = temp >> 2; pglove->key2 = temp & 3; temp = temp >> 2; pglove->key3 = temp & 3; temp = temp >> 2; pglove->key4 = temp & 3; /* new Noise reduction */ deglitch(pglove); dehyst(pglove); get_modes(); return(1); } /*******************************************************************/ /* dehyst: low noise removal by D. Stampe */ /*******************************************************************/ #define XHYST 2 /* hysterisis for X, Y low noise reduction */ #define YHYST 2 /* 2 eliminates +/-3 quanta of noise */ static int ox = -1000; /* last x,y for hysterisis */ static int oy = -1000; dehyst(pglove) pglove_data *pglove; { int x = pglove->x; int y = pglove->y; if( (pglove->key1==0) && (pglove->key2==0) && (pglove->key3==0) && (pglove->key4==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; pglove->x = ox; /* replace present X,Y data */ pglove->y = oy; } /*******************************************************************/ /* deglitch: Identify and discard noisy data by D. Stampe */ /*******************************************************************/ #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 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; deglitch(pglove) pglove_data *pglove; { int vx, vy; int x = pglove->x; int y = pglove->y; if( (pglove->key1==0) && (pglove->key2==0) && (pglove->key3==0) && (pglove->key4==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 = pglove->x; y2 = y1; y1 = pglove->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 */ { pglove->x = lsx; lax--; } if (lay != 0) /* hold Y pos'n if glitch */ { lay--; pglove->y = lsy; } lsx = pglove->x; /* save position for X,Y hold */ lsy = pglove->y; } /******************************************************************************/ /** Defines pglove.gesture and pglove.key for easier programming **/ /******************************************************************************/ get_modes() { pglove.gesture = STRANGE; pglove.key = BAD; get_hand_mode(); get_key_mode(); } /********************* assign pglove.gesture *************************/ /*** Gesture Recognition ***/ /*** May have to be modified for each person ***/ /*** dbarberi@sunsite.unc.edu /**********************************************************************/ get_hand_mode() { if( (pglove.one==0) && (pglove.two==0) && (pglove.three==0) ) { pglove.gesture = OPEN; return(0); } if( (pglove.one==3) && (pglove.two==3) && (pglove.three==3) ) { pglove.gesture = FIST; return(0); } /* if( (pglove.one>=2) && (pglove.two>=2) && (pglove.three>=2) ) { pglove.gesture = FIST; return(0); } */ if( (pglove.one==0) && (pglove.two>=2) && (pglove.three>=2) ) { pglove.gesture = POINT; return(0); } if( (pglove.one==0) && (pglove.two==0) && (pglove.three>=2) ) { pglove.gesture = BIGPOINT; return(0); } if( (pglove.one==3) && (pglove.two==0) && (pglove.three==3) ) { pglove.gesture = BIRD; return(0); } /* if( (pglove.one>=2) && (pglove.two==0) && (pglove.three>=2) ) { pglove.gesture = BIRD; return(0); } */ if( (pglove.thumb >=1) && (pglove.one>=2) && (pglove.two==0) && (pglove.three==0) ) { pglove.gesture = AOK; return(0); } if( (pglove.one>=2) && (pglove.two==0) && (pglove.three==0) ) { pglove.gesture = ONEDOWN; return(0); } if( (pglove.one==0) && (pglove.two==3) && (pglove.three==0) ) { pglove.gesture = TWODOWN; return(0); } if( (pglove.one==0) && (pglove.two==0) && (pglove.three==3) ) { pglove.gesture = THREEDOWN; return(0); } if( (pglove.one==3) && (pglove.two==3) && (pglove.three<=1) ) { pglove.gesture = THREEUP; return(0); } } /********************** pglove.key **********************/ get_key_mode() { if( (pglove.key1==3) && (pglove.key2==3) && (pglove.key3==3) && (pglove.key4==3) ) { pglove.key = NO_KEY; return(0); } if( (pglove.key1==0) && (pglove.key2==0) && (pglove.key2==0) && (pglove.key4==0) ) { pglove.key = CENTER; return(0); } if( (pglove.key1==3) && (pglove.key2==0) && (pglove.key3==0) && (pglove.key4==2) ) { pglove.key = SELECT; return(0); } if( (pglove.key1==2) && (pglove.key2==0) && (pglove.key3==0) && (pglove.key4==2) ) { pglove.key = START; return(0); } if( (pglove.key1==0) && (pglove.key2==0) && (pglove.key3==0) && (pglove.key4==2) ) { pglove.key = ENTER; return(0); } if( (pglove.key1==1) && (pglove.key2==0) ) pglove.key = ONE; if( (pglove.key1==2) && (pglove.key2==0) ) pglove.key = TWO; if( (pglove.key1==3) && (pglove.key2==0) ) pglove.key = THREE; if( (pglove.key1==0) && (pglove.key2==1) ) pglove.key = FOUR; if( (pglove.key1==1) && (pglove.key2==1) ) pglove.key = FIVE; if( (pglove.key1==2) && (pglove.key2==1) ) pglove.key = SIX; if( (pglove.key1==3) && (pglove.key2==1) ) pglove.key = SEVEN; if( (pglove.key1==0) && (pglove.key2==2) ) pglove.key = EIGHT; if( (pglove.key1==1) && (pglove.key2==2) ) pglove.key = NINE; if( (pglove.key1==1) && (pglove.key2==3) ) pglove.key = UP; /* for now UP is towards to red LED light */ if( (pglove.key1==2) && (pglove.key2==3) ) pglove.key = DOWN; if( (pglove.key1==3) && (pglove.key2==3) ) pglove.key = RIGHT; if( (pglove.key1==0) && (pglove.key2==3) ) pglove.key = LEFT; if( (pglove.key1==2) && (pglove.key2==2) ) pglove.key = A; if( (pglove.key1==3) && (pglove.key2==2) ) pglove.key = B; /* how to read the data from the keypad key4 key4 key3 | key3 | key2 | | key2 | | key1 | | | key1 | | | | | | | | | | | NO_KEY 3 3 3 3 0 0 0 0 0 UP 1 3 0 0 1 1 0 0 0 DOWN 2 3 0 0 2 2 0 0 0 LEFT 0 3 0 0 3 3 0 0 0 RIGHT 3 3 0 0 4 0 1 0 0 A 2 2 0 0 5 1 1 0 0 B 3 2 0 0 6 2 1 0 0 SELECT 3 0 0 2 7 3 1 0 0 START 2 0 0 2 8 0 2 0 0 CENTER 0 0 0 0 9 1 2 0 0 PROG ? ? ? ? ENTER 0 0 0 2 ?BAD? 0 0 0 1 */ } /******************/ print_modes() { /* printf(" gesture= "); */ /* 9 chars */ if(pglove.gesture==OPEN) printf("OPEN "); if(pglove.gesture==FIST) printf("FIST "); if(pglove.gesture==POINT) printf("POINT "); if(pglove.gesture==BIGPOINT) printf("BIGPOINT "); if(pglove.gesture==BIRD) printf("BIRD "); if(pglove.gesture==AOK) printf("AOK "); if(pglove.gesture==ONEDOWN) printf("ONEDOWN "); if(pglove.gesture==TWODOWN) printf("TWODOWN "); if(pglove.gesture==THREEDOWN) printf("THREEDOWN"); if(pglove.gesture==THREEUP) printf("THREEUP "); if(pglove.gesture==STRANGE) printf("STRANGE "); /* printf(" key= "); */ /* 6 chars */ if(pglove.key==NO_KEY) printf("NO_KEY"); if(pglove.key==ONE) printf("ONE "); if(pglove.key==TWO) printf("TWO "); if(pglove.key==THREE) printf("THREE "); if(pglove.key==FOUR) printf("FOUR "); if(pglove.key==FIVE) printf("FIVE "); if(pglove.key==SIX) printf("SIX "); if(pglove.key==SEVEN) printf("SEVEN "); if(pglove.key==EIGHT) printf("EIGHT "); if(pglove.key==NINE) printf("NINE "); if(pglove.key==ENTER) printf("ENTER "); if(pglove.key==UP) printf("UP "); if(pglove.key==DOWN) printf("DOWN "); if(pglove.key==RIGHT) printf("RIGHT "); if(pglove.key==LEFT) printf("LEFT "); if(pglove.key==A) printf("A "); if(pglove.key==B) printf("B "); if(pglove.key==SELECT) printf("SELECT"); if(pglove.key==START) printf("START "); if(pglove.key==CENTER) printf("CENTER"); if(pglove.key==BAD) printf("BAD "); } /*******************************************************************/ /* A small test program to write the data to stdandard output */ /* An optional argument determines time between samples */ /*******************************************************************/ main(argc, argv) int argc; char *argv[]; { time_t clock; int counter; sleepval = 70; /* new sleep timer */ /* should be in glove.h */ /* use given argument for delay time, else use default (above) */ if (argc > 1) sleepval = atoi(argv[1]); file_id = init_glove(); /* open the serial port */ while (1) { /* do forever */ printf("%i ", query_glove(file_id, &pglove) ); counter = time(&clock); printf("%i ", counter - ( (counter / 100) * 100 ) ); print_modes(); printf(" X=%4i Y=%4i Z=%4i Fingers: %i %i %i %i Rot: %i Keys: %i %i %i %i\n", pglove.x, pglove.y, pglove.z, pglove.thumb, pglove.one, pglove.two, pglove.three, pglove.twist, pglove.key1, pglove.key2, pglove.key3, pglove.key4); } } /*** End of stream.c ***/