/* ************************************************************** * * * (c) Copyright 1999 Mariusz Zaczek * * ALL RIGHTS RESERVED * * * * Permission to use, copy, modify, and distribute this * * software for any purpose and without fee is hereby * * granted, provided that the above copyright notice * * appear in all copies and that both the copyright notice * * and this permission notice appear in supporting * * documentation, and that the name of Mariusz Zaczek. not * * be used in advertising or publicity pertaining to * * distribution of the software without specific, written * * prior permission. (zaczek@uiuc.edu) * * * * THE MATERIAL EMBODIED ON THIS SOFTWARE IS PROVIDED * * TO YOU "AS-IS" AND WITHOUT WARRANTY OF ANY KIND, * * EXPRESSED, IMPLIED OR OTHERWISE, INCLUDING WITHOUT * * LIMITATION, ANY WARRANTY OF MERCHANTABILITY OR * * FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT * * SHALL MARIUSZ ZACZEK BE LIABLE TO YOU OR ANYONE * * ELSE FOR ANY DIRECT, SPECIAL, INCIDENTAL, INDIRECT * * OR CONSEQUENTIAL DAMAGES OF ANY KIND, OR ANY DAMAGES * * WHATSOEVER, INCLUDING WITHOUT LIMITATION, LOSS OF * * PROFIT, LOSS OF USE, SAVINGS OR REVENUE, OR THE * * CLAIMS OF THIRD PARTIES, WHETHER OR NOT MARIUSZ * * ZACZEK HAS BEEN ADVISED OF THE POSSIBILITY OF * * SUCH LOSS, HOWEVER CAUSED AND ON ANY THEORY OF * * LIABILITY, ARISING OUT OF OR IN CONNECTION WITH THE * * POSSESSION, USE OR PERFORMANCE OF THIS SOFTWARE. * * * * US Government Users Restricted Rights * * Use, duplication, or disclosure by the Government is * * subject to restrictions set forth in * * FAR 52.227.19(c)(2) or subparagraph (c)(1)(ii) of the * * Rights in Technical Data and Computer Software * * clause at DFARS 252.227-7013 and/or in similar or * * successor clauses in the FAR or the DOD or NASA FAR * * Supplement. Unpublished-- rights reserved under the * * copyright laws of the United States. * * * ************************************************************** */ /* * hand.c * * This program uses the Parallel Port of a PC running Win95 * which reads the potentionmeter voltages of the glove. These * voltages are transformed into angle values and a 3D hand is * draw using OPENGL and GLUT on the screen with appropriate * angles of the fingers. * */ #include #include #include #include #include #include #include //#define _M_IX86 /* This did not help ... it was for WinNT */ /* * >debug * -d 0040:0008 L8 * * 0040:0000 BC 03 78 03 78 02 C0 9F * * I guess this means that LPT1 = 03BC */ /* 0x0378 - Win95 Computer in ADSL 0x03BC - the NT computer in ADSL */ #define DATA 0x0378 /* Output pins */ #define STATUS DATA+1 /* Input pins...interrupt */ #define CONTROL DATA+2 #define TRUE 1 #define FALSE 0 /* Never actually used... :) */ #define PI 3.1415925 int data; /* Data FROM A/D converter */ int status; /* Bits which will be set */ int control; /* Bits which will be set */ /* F2 F3 __ F1 F4 __ ( ) __ __ ( )| |( ) ( )| || || | T |__||__||__||__| ___ | || || || | ( \ |__||__||__||__| \ \ | | \ \| Right | \ \ Hand | \ \ / \ / */ int T_tip=0, T_mid=0, T_joint=0; int T_tip_ZERO=0, T_mid_ZERO=0, T_joint_ZERO=0; int F4_tip=0, F4_mid=0, F4_joint=0; int F4_tip_ZERO=0, F4_mid_ZERO=0, F4_joint_ZERO=0; /* Define the rest of the fingers */ GLenum doubleBuffer; static int shoulder = 0, elbow = 0; int position = 0, updown = 0; static int Afirst = 0, Bfirst = 0, Cfirst = 0; static int Asecond = 0, Bsecond = 0, Csecond = 0; static int Athird = 0, Bthird = 0, Cthird = 0; static int Afourth = 0, Bfourth = 0, Cfourth = 0; static int wrist = 0; static int Athumb = 0, Bthumb = 0, Cthumb = 0; int flip_bits(int value) /* ???? */ { int new_val = 0; if ( value & 128 ) new_val = new_val | 0x01; if ( value & 64 ) new_val = new_val | 0x02; if ( value & 32 ) new_val = new_val | 0x04; if ( value & 16 ) new_val = new_val | 0x08; if ( value & 8 ) new_val = new_val | 0x10; if ( value & 4 ) new_val = new_val | 0x20; if ( value & 2 ) new_val = new_val | 0x40; if ( value & 1 ) new_val = new_val | 0x80; return new_val; } void display_data(int dat) { if ( dat & 128 ) printf("\n 1"); else printf("\n 0"); if ( dat & 64 ) printf("1"); else printf("0"); if ( dat & 32 ) printf("1"); else printf("0"); if ( dat & 16 ) printf("1"); else printf("0"); if ( dat & 8 ) printf("1"); else printf("0"); if ( dat & 4 ) printf("1"); else printf("0"); if ( dat & 2 ) printf("1"); else printf("0"); if ( dat & 1 ) printf("1"); else printf("0"); } /* ++++++++++++++++++++++++++++++ + + + getkey + + + + - function to determine + + number of key pressed.+ + - parameters: none. + + + ++++++++++++++++++++++++++++++ */ int getkey(void) { int i; switch( i = (int)getch() ) { case 0xe0: case 0: return 256 + (int)getch(); default: return i; } } int angle(int ZERO, int CURRENT) { float newangle=0.0; /* 0.711 = 180/128 = 360/256 */ newangle = ( CURRENT - ZERO ) * 0.711; return (int)newangle; } /* ++++++++++++++++++++++++++++++ + + + posit + + + + - uses status & control + + data to combine them + + and get full 8bit val.+ + - parameters: none. + + + ++++++++++++++++++++++++++++++ */ int posit(void) { /* Clear variable */ int newpos = 0; int temp = 0; /* Combine bits */ /* Take bits from status */ if ( status & 128 ) /* Pin 11 */ newpos = newpos | 0x80; if ( status & 64 ) /* Pin 10 */ newpos = newpos | 0x40; if ( status & 32 ) /* Pin 12 */ newpos = newpos | 0x20; if ( status & 16 ) /* Pin 13 */ newpos = newpos | 0x10; if ( status & 8 ) /* Pin 15 */ newpos = newpos | 0x08; /* Take bits from control */ if ( control & 2 ) /* Pin 14 */ newpos = newpos | 0x04; if ( control & 4 ) /* Pin 16 */ newpos = newpos | 0x02; if ( control & 1 ) /* Pin 1 */ newpos = newpos | 0x01; // printf("\n************************\n%d",newpos); return newpos; } void give_get( int pot, int stat) { data = data & (~0x20); /* Clear bit 5 */ data = data & (~0x10); /* Clear bit 4 */ data = data & (~0x08); /* Clear bit 3 */ /* 1) Set the channel select switches (3 bits) */ _outp(DATA,data); _sleep(4); // display_data(data); /* 2) Set the ALE (address latch enable) switch to HI (wait 50 ns) */ data = data & (~0x10); /* Clear bit 4 */ data = data | 0x08; /* SET bit 3 */ _outp(DATA,data); _sleep(1); /* 50 nanoseconds ....*/ // display_data(data); /* 3) Set the START switch to HI (wait 50 ns) */ data = data | 0x10; /* SET bit 4 */ data = data | 0x08; /* keep bit 3 HI */ _outp(DATA,data); _sleep(1); /* 50 nanoseconds .... */ //display_data(data); /* 4) Set the ALE switch back to LO (wait 50 ns) */ data = data | 0x10; /* keep bit 4 HI */ data = data & (~0x08); /* Clear bit 3 */ _outp(DATA,data); _sleep(1); /* 50 nanoseconds .... */ // display_data(data); /* 5) Set the START switch to LO (wait 200 microseconds) */ data = data & (~0x10); /* Clear bit 4 */ data = data & (~0x08); /* Clear bit 3 */ _outp(DATA,data); _sleep(10); /* wait 200 microseconds....*/ data = data | 0x20; /* Set bit 5 - Output Enable*/ _outp(DATA,data); _sleep(4); // display_data(data); status = 0; control = 0; /* 6) ....begin reading data for 1 pot. */ status = _inp(STATUS); /* Convert input to data */ control = _inp(CONTROL); /* NEED A DELAY */ _sleep(2); data = data & (~0x20); /* Clear bit 5 */ _outp(DATA,data); /* * Bit value: 7 6 5 4 3 2 1 0 * x x x x x x x x * * Note: !## corresponds to INVERSE of ## * ******( DATA )******************************************** * Only 5 bits will be written to the data port. * They will control the selection of pot on the A/D * converter. * * Bits 2 1 0 will be the only needed bits with * corresponding pin numbers of: 4, 3 & 2 * * Bits 4 3 will be used to control the START and ALE * functions of the A/D converter. In order to get a * reading off the A/D converter the following sequence of * events must occur: * * 1) Set the channel select switches (3 bits) * 2) Set the ALE (address latch enable) switch to HI (wait 50 ns) * 3) Set the START switch to HI (wait 50 ns) * 4) Set the ALE switch back to LO (wait 50 ns) * 5) Set the START switch to LO (wait 100 microseconds) * 6) ....begin reading data for 1 pot. * ******( STATUS )****************************************** * From STATUS we read the following bits: * 7 6 5 4 3 * which corresponds to pins:( 11, !10, 12, 13, !15 ) * ( 10 & 11 flipped ) * * ( Yellow, Orange, Green, Red, Purple ) * ******( CONTROL )***************************************** * From CONTROL we read the following bits: * 2 1 0 * which corresponds to pins:( !14, 16, !17 ) * * ( Yellow, Orange, Green ) * * */ if ( stat == 0 ) /* Initialize all ZERO values */ { if ( pot == 0 ) T_tip_ZERO = posit(); else if ( pot == 1 ) T_mid_ZERO = posit(); else if ( pot == 2 ) T_joint_ZERO = posit(); else if ( pot == 3 ) F4_tip_ZERO = posit(); else if ( pot == 4 ) F4_mid_ZERO = posit(); else if ( pot == 5 ) F4_joint_ZERO = posit(); else printf("\n (give_get-1): Incorrect Potentionmeter was specified. "); } else if ( stat == 1 ) /* Already initialized...read normally */ { if ( pot == 0 ) T_tip = angle( T_tip_ZERO, posit() ); else if ( pot == 1 ) T_mid = angle( T_mid_ZERO, posit() ); else if ( pot == 2 ) T_joint = angle( T_joint_ZERO, posit() ); else if ( pot == 3 ) F4_tip = angle( F4_tip_ZERO, posit() ); else if ( pot == 4 ) F4_mid = angle( F4_mid_ZERO, posit() ); else if ( pot == 5 ) F4_joint = angle( F4_joint_ZERO, posit() ); else printf("\n (give_get-2): Incorrect Potentionmeter was specified. "); } else printf("\n (give_get-3): Incorrect status selection. "); } void read_pots(int stat) { // printf("\n-( read_pots )-----------------------"); /* Pot 0: xxxxx000 */ data = data & (~0x04); data = data & (~0x02); data = data & (~0x01); give_get(0,stat); /* Pot 1: xxxxx001 */ data = data & (~0x04); data = data & (~0x02); data = data | 0x01; give_get(1,stat); /* Pot 2: xxxxx010 */ data = data & (~0x04); data = data | 0x02; data = data & (~0x01); give_get(2,stat); /* Pot 3: xxxxx011 */ data = data & (~0x04); data = data | 0x02; data = data | 0x01; give_get(3,stat); /* Pot 4: xxxxx100 */ data = data | 0x04; data = data & (~0x02); data = data & (~0x01); give_get(4,stat); /* Pot 5: xxxxx101 */ data = data | 0x04; data = data & (~0x02); data = data | 0x01; give_get(5,stat); /* Pot 6: xxxxx110 - NOT USED YET*/ /* data = data | 0x04; data = data | 0x02; data = data & (~0x01); */ /* Pot 7: xxxxx111 - NOT USED YET*/ /* data = data | 0x04; data = data | 0x02; data = data | 0x01; */ } int initialize_hand(void) { int choice = 0, i = 0; /* Output introduction and instructions. */ printf("\n***************************************" "\n* *" "\n* Robotic Hand *" "\n* *" "\n* by Mariusz Zaczek & Aaron Trask *" "\n* *" "\n* (ADSL) Advanced Digital Systems Lab *" "\n* Fall 1999 *" "\n* *" "\n***************************************" ); printf("\n Please lay your hand on a flat surface" "\n and hit the ENTER key to initialize " "\n all angles to zero.\n"); // data = 0xff; // printf("\n The data is %d",data); // _outp(DATA,data); while(TRUE) { choice = getkey(); if ( choice == 13 ) /* ENTER - Lock location */ { /* Read in values of all potentionmeters */ /* COMMENT THIS OUT IF TESTING IF OPENGL WORKS */ read_pots(0); /* Done reading all pots...begin motion */ printf("\n POSITIONS LOCKED - Begin Motion \n"); return 1; } } } void idle(void) { /* COMMENT THIS OUT IF TESTING IF OPENGL WORKS */ read_pots(1); /* COMMENT THIS OUT IF TESTING IF OPENGL WORKS */ glutPostRedisplay(); } void init(void) { float light_position[] = {0.0, 3.0, 3.0, 0.0}; float local_view[] = {0.0}; // GLfloat ambient[] = {0.1745, 0.01175, 0.01175}; // GLfloat diffuse[] = {0.61424, 0.04136, 0.04136}; // GLfloat specular[] = {0.727811, 0.626959, 0.626959}; GLfloat ambient[] = { 0.0, 0.0, 0.0, 1.0 }; GLfloat diffuse[] = { 0.7, 0.0, 0.0, 1.0 }; GLfloat specular[] = { 1.0, 1.0, 1.0, 1.0 }; glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LESS); glLightfv(GL_LIGHT0, GL_POSITION, light_position); glLightModelfv(GL_LIGHT_MODEL_LOCAL_VIEWER, local_view); glFrontFace(GL_CW); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); glEnable(GL_AUTO_NORMAL); glEnable(GL_NORMALIZE); glMaterialfv(GL_FRONT, GL_AMBIENT, ambient); glMaterialfv(GL_FRONT, GL_DIFFUSE, diffuse); glMaterialfv(GL_FRONT, GL_SPECULAR, specular); glMaterialf(GL_FRONT, GL_SHININESS, 128.0); glClearColor(0.5, 0.5, 0.5, 1.0); glColor3f(1.0, 1.0, 1.0); } void display(void) { GLUquadricObj *cone; glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT); glLoadIdentity(); /* Set pot values to the corresponding values */ Cthumb = -T_tip; Bthumb = -T_mid; /* Thumb */ Athumb = -T_joint; Cfourth = -F4_tip; Bfourth = -F4_mid; /* First finger...Index finger */ Afourth = -F4_joint; gluLookAt(120.0, 120.0, -120.0, 0.0, 18.0, 15.0, 1.0, 0.0, 0.0); glRotatef(position,1.0,0.0,0.0); glColor3f(0.0,0.0,0.0); cone = gluNewQuadric(); glPushMatrix(); { glTranslatef(0.0, 0.0, 0.0); glRotatef( (GLfloat) wrist, 0.0, 1.0, 0.0); glPushMatrix(); { glTranslatef(0.0,0.0,-15.0); /* glScalef(6.0, 36.0, 30.0); glutSolidCube(1.0); */ glScalef(0.25,1.0,1.0); gluCylinder(cone, 21.0, 19.0, 30.0, 20, 20); } glPopMatrix(); /* First Finger */ glTranslatef(0.0,-13.5,15.0); glPushMatrix(); { glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Afirst, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 4.0, 20, 20); glTranslatef(0.0,0.0,4.0); glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Bfirst, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 9.0, 20, 20); glTranslatef(0.0,0.0,9.0); glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Cfirst, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 8.0, 20, 20); glTranslatef(0.0,0.0,8.0); glutSolidSphere(3.0,10,10); } glPopMatrix(); /* Second Finger */ glTranslatef(0.0,9.0,0.0); glPushMatrix(); { glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Asecond, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 12.0, 20, 20); glTranslatef(0.0,0.0,12.0); glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Bsecond, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 12.0, 20, 20); glTranslatef(0.0,0.0,12.0); glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Csecond, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 8.0, 20, 20); glTranslatef(0.0,0.0,8.0); glutSolidSphere(3.0,10,10); } glPopMatrix(); /* Third Finger */ glTranslatef(0.0,9.0,0.0); glPushMatrix(); { glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Athird, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 14.0, 20, 20); glTranslatef(0.0,0.0,14.0); glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Bthird, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 12.0, 20, 20); glTranslatef(0.0,0.0,12.0); glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Cthird, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 9.0, 20, 20); glTranslatef(0.0,0.0,9.0); glutSolidSphere(3.0,10,10); } glPopMatrix(); /* Fourth Finger */ glTranslatef(0.0,9.0,0.0); glPushMatrix(); { glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Afourth, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 12.0, 20, 20); glTranslatef(0.0,0.0,12.0); glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Bfourth, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 9.0, 20, 20); glTranslatef(0.0,0.0,9.0); glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Cfourth, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 10.0, 20, 20); glTranslatef(0.0,0.0,10.0); glutSolidSphere(3.0,10,10); } glPopMatrix(); /* THUMB */ glTranslatef(0.0,4.5,-15.0); glPushMatrix(); glRotatef(180, 1.0, 0.0, 0.0); glPushMatrix(); { glRotatef(90.0, 1.0,0.0,0.0); glRotatef( (GLfloat) Athumb, 0.0, 1.0, 0.0); glutSolidSphere(3.0,10,10); gluCylinder(cone, 3.0, 3.0, 5.0, 20, 20); glTranslatef(0.0,0.0,5.0); glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Bthumb, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 9.0, 20, 20); glTranslatef(0.0,0.0,9.0); glutSolidSphere(3.0,10,10); glRotatef( (GLfloat) Cthumb, 0.0, 1.0, 0.0); gluCylinder(cone, 3.0, 3.0, 6.0, 20, 20); glTranslatef(0.0,0.0,6.0); glutSolidSphere(3.0,10,10); } glPopMatrix(); glPopMatrix(); } glPopMatrix(); glutSwapBuffers(); } void reshape (int w, int h) { glViewport (0, 0, (GLsizei) w, (GLsizei) h); glMatrixMode (GL_PROJECTION); glLoadIdentity (); glFrustum(-14.0, 14.0, -14.0, 14.0, 30.0, 400.0); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); glTranslatef (0.0, 0.0, -50.0); } void keyboard (unsigned char key, int x, int y) { switch (key) { case 'a': Afirst = (Afirst + 5) % 360; break; case 's': Bfirst = (Bfirst + 5) % 360; break; case 'd': Cfirst = (Cfirst + 5) % 360; break; case 'f': Asecond = (Asecond + 5) % 360; break; case 'g': Bsecond = (Bsecond + 5) % 360; break; case 'h': Csecond = (Csecond + 5) % 360; break; case 'z': Athird = (Athird + 5) % 360; break; case 'x': Bthird = (Bthird + 5) % 360; break; case 'c': Cthird = (Cthird + 5) % 360; break; case 'v': Afourth = (Afourth + 5) % 360; break; case 'b': Bfourth = (Bfourth + 5) % 360; break; case 'n': Cfourth = (Cfourth + 5) % 360; break; case 'w': wrist = (wrist + 10) % 360; break; case 't': Athumb = (Athumb - 10) % 360; break; case 'y': Bthumb = (Bthumb - 10) % 360; break; case 'u': Cthumb = (Cthumb - 10) % 360; break; case 'p': position = (position + 10) % 360; break; case 'o': updown += 1; break; case 'i': updown -= 1; break; case 'q': case 'Q': exit(0); break; case 27: exit(0); break; default: break; } glutPostRedisplay(); } int main(int argc, char** argv) { GLenum type; glutInit(&argc, argv); initialize_hand(); type = GLUT_RGB | GLUT_DEPTH; type |= (doubleBuffer) ? GLUT_DOUBLE : GLUT_SINGLE; glutInitDisplayMode(type); /* glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB); */ glutInitWindowSize (500, 500); glutInitWindowPosition (100, 100); glutCreateWindow (argv[0]); init(); glutDisplayFunc(display); glutReshapeFunc(reshape); glutKeyboardFunc(keyboard); glutIdleFunc(idle); glutMainLoop(); return 0; }