/* --------------------------------------------------------------------- */ /* - ROBOT PROGRAMMERS INTERFACE - */ /* - ROBOT.C - */ /* --------------------------------------------------------------------- */ /* Filename: ROBOT.C - */ /* Description: Contains the robot calls for Alpha II - */ /* Microbot Five Axis Robot - */ /* Written by: - */ /* Roger W. Webster, Ph.D. - */ /* Robot Vision and Artificial Intelligence Lab - */ /* Department of Computer Science - */ /* Millersville University - */ /* Millersville, PA 17551 - */ /* (717) 872-3539 or (717) 872-3860 - */ /* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */ /* - necessary defines for this file: Robot.c - */ /* --------------------------------------------------------------------- */ #include #include /* --------------------------------------------------------------------- */ /* - Global Defines in ROBOT.C */ /* --------------------------------------------------------------------- */ #define STRLN 80 #define ERROR 0 #define NOERROR 1 #define ROBO_PORT "/dev/ttya" #define SPEECH_PORT "/dev/ttyc" #define ROBOT$SGN( x ) (( x<0.0 ) ? (-1) : (( x>0.0 ) ? (1) : (0) )) #define HEIGHT_L 8.50 /* Shoulder Height Above Table */ #define UPPERARM_L 7.0 /* Shoulder to Elbow */ #define FOREARM_L 7.0 /* Elbow to Wrist */ #define HAND_L 4.52 /* Wrist to Fingertip ( Gripper Closed ) */ #define DEG_PER_RAD 57.2957795 #define ANG_CONV( x ) ( x/DEG_PER_RAD ) #define S1 1291.02 /* was 1301.02 7/25/90 */ /* Base - Motor Steps per Radian */ #define S2 -1160.18 /* was -1120.18 7/25/90 */ /* Shoulder - Motor Steps per Radian */ #define S3 -679.497 /* was -699.497 7/25/90 */ /* Elbow - Motor Steps per Radian */ #define S4 326.505 /* Right Wrist - Motor Steps per Radian */ #define S5 326.505 /* Left Wrist - Motor Steps per Radian */ #define S6 384.548 /* Hand - Motor Steps per Inch */ /* #define R1 1.0 */ #define P1 0.0 #define P2 2417.0 #define P3 -193.0 #define P4 1539.0 #define P5 -1539.0 #define P6 1261.0 #define PI 3.14159265 /* #define NOTCALIBRATED 1 */ #define STEPS_OFF 62 /* ******************** End of Global Defines *********************** */ /* ******************************************************************** * Global Variables in ROBOT.C * ******************************************************************** */ FILE *speechfp; int position_reg[11], gripper_width = 0, robotfp, visfp, result; int xvals[1000], yvals[1000]; char robotline[900][STRLN]; /* used to hold up to 900 steps from AlphaII */ /* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */ /* - ROBOT$OPEN() - */ /* - - */ /* - This procedure opens the device and returns YES - */ /* --------------------------------------------------------------------- */ ROBOT$OPEN() { robotfp = open( (ROBO_PORT),2 ); return (robotfp); } /* end function */ /* ******************************************************************** * * * FUNCTION ROBOT$SEND * * * * This function is passed a char pointer to a string. It sends * * this string out to the COMM PORT a byte at a time until it * * encounters the standard C string terminator (0). * ******************************************************************** */ ROBOT$SEND( command ) char *command; { int result, nbytes; char buf[STRLN]; sprintf(buf,"%s\r",command); nbytes = strlen(buf); result = write(robotfp,buf,nbytes) ; #ifdef ROGER printf("*************************************************\n"); printf("sent: %d bytes to robot in ROBOT$SEND\n", result); printf("*************************************************\n"); #endif result = ROBOT$CHECK_ERROR(); #ifdef ROGER printf("*************************************************\n"); printf("result of CHECK_FOR_ERROR: %d\n", result); printf("*************************************************\n"); #endif return (result); } /* end function */ /* ******************************************************************** * * * FUNCTION ROBOT$GET * * * * This function reads the COMM PORT and assembles a string. It * * terminates upon receiving a carriage return. The value of the * * parameter is a pointer to this string. * ******************************************************************** */ ROBOT$GET( message ) char message[]; { int x = 0; x = read(robotfp,message,STRLN); message[ x ] = '\0'; return (x); } /* end function */ /* ******************************************************************** * * * FUNCTION ROBOT$CHECK_ERROR * * * * This procedure gets the command acknoweledgement from robbie. * * This will indicate if the robot had an error when trying to per- * * form the command. This function will return TRUE if the robot has* * encountered an error. If the error was of type 2, then the robot * * will be returned to ROBOT$HOME and the program aborted. * * * ******************************************************************** */ ROBOT$CHECK_ERROR() { int x; char ack[ STRLN ]; char checkcode; x = ROBOT$GET( ack ); checkcode = ack[0]; x = ROBOT$GET( ack ); #ifdef ROGER printf("ack[0] %d ack[1] %d len: %d\n", ack[0], ack[1], x); printf("ack[0] %d ack[1] %d ack[2] %d\n", ack[0], ack[1], ack[2]); if (ack[1] != '\0') printf("line feed not in second position\n"); else /* rww don't care anymore this was debug junk */ #endif switch (checkcode) { /* switch to ack's first character */ case '0': return( 0 ); break; case '1': return( 1 ); break; case '2': return( 2 ); break; default: printf("illegal ack returned by robot!\n"); break; } /* end of switch */ } /* end function */ /* ******************************************************************** * FUNCTION ROBOT$SET * * This procedure builds and sends the microbot set commmand * ******************************************************************** */ ROBOT$SET () { return ( ROBOT$SEND( "@SET 200" ) ); } /* end function */ /* ******************************************************************** * FUNCTION ROBOT$RESET * * This procedure builds and sends the microbot reset commmand * ******************************************************************** */ ROBOT$RESET() { return ( ROBOT$SEND( "@RESET 200" ) ); } /* end function */ /* ******************************************************************** * FUNCTION ROBOT$READ * * * * This function reads robbie's current position in motor steps. * * The current position will then be placed into the position_reg * * directly. This means that the current position is NOT passed out * * as parameters. * ******************************************************************** */ ROBOT$READ() { int r, errorcode, matt; char ack[ STRLN ], robotline[ STRLN ], readline[ STRLN ]; errorcode = ROBOT$SEND( "@READ" ); if (errorcode != NOERROR) printf ("Error in the ROBOT$READ command. \n"); r= ROBOT$GET( readline ); r = ROBOT$GET( ack ); extract(readline, position_reg ); /* Function extract included from UTIL.C */ return (errorcode); } /* end function */ /* ******************************************************************** * ROBOT$POSITION : Using the position registers, the current loc- * * ation of the robot in X-Y-Z-P-R space is returned. No para- * * meters are passed into the routine, only out. * ******************************************************************** */ int ROBOT$POSITION( x, y, z, p, r ) float *x, *y, *z, *p, *r; { int error, cnt; float Theta[5], rr, t1, t2; error = ROBOT$READ(); /* load the position registers */ if (error == ERROR) { printf ("Error in read in ROBOT$POSITION.\n"); return ( ERROR ); } Theta[1] = (position_reg[1]-P1) / S1; Theta[2] = (position_reg[2]-P2) / S2; Theta[3] = (position_reg[3]- (P3) ) / S3; Theta[4] = (position_reg[4]- (P4) ) / S4; Theta[5] = (position_reg[5]- (P5) ) / S5; t1 = -((Theta[5] + Theta[4])/2); *r = floor(((Theta[5]-Theta[4])/2)*DEG_PER_RAD*100.0+0.5)/100.0; rr = UPPERARM_L*cos(Theta[2]) + FOREARM_L*cos(Theta[3]) + HAND_L*cos(t1); *x = floor((rr*cos(Theta[1]) - 2)*100.0+0.5)/100.0; *y = floor(rr*sin(Theta[1])*100.0+0.5)/100.0; t2=HEIGHT_L+UPPERARM_L*sin(Theta[2])+FOREARM_L*sin(Theta[3])+HAND_L*sin(t1); *z = floor(t2*100.0+0.5)/100.0; *p = floor(t1*DEG_PER_RAD*100.0+0.5)/100.0; } /* end function */ /* ******************************************************************** * * * FUNCTION ROBOT$STACK_DEMO * * * * This function demonstrates the robot control functions by * * stacking the blocks that are postioned at (8,3) and (8,-3) at * * (8,0). The function calls the ROBOT$MOVE, ROBOT$OPEN_GRIPPER, * * ROBOT$CLOSE_GRIPPER, and ROBOT$HOME functions. * ******************************************************************** */ ROBOT$STACK_DEMO() { ROBOT$HOME(); ROBOT$MOVE(250.0,8.0,0.0,5.0,-90.0,0.0) ; ROBOT$MOVE(250.0,8.0,5.0,2.5,-90.0,0.0) ; ROBOT$OPEN_GRIPPER(2.5) ; ROBOT$MOVE(230.0,8.0,5.0,0.7,-90.0,0.0) ; ROBOT$CLOSE_GRIPPER() ; ROBOT$MOVE(250.0,8.0,5.0,1.0,-90.0,90.0) ; ROBOT$MOVE(250.0,8.0,0.0,1.0,-90.0,90.0) ; ROBOT$MOVE(230.0,8.0,0.0,0.7,-90.0,90.0) ; ROBOT$OPEN_GRIPPER(3.0) ; ROBOT$MOVE(250.0,8.0,0.0,2.5,-90.0,0.0) ; ROBOT$MOVE(250.0,8.0,-5.0,2.5,-90.0,0.0) ; ROBOT$MOVE(230.0,8.0,-5.0,0.7,-90.0,0.0) ; ROBOT$CLOSE_GRIPPER() ; ROBOT$MOVE(250.0,8.0,0.0,4.0,-90.0,90.0) ; ROBOT$MOVE(230.0,8.0,0.0,2.2,-90.0,90.0) ; ROBOT$OPEN_GRIPPER(2.5) ; ROBOT$MOVE(250.0,8.0,0.0,5.0,-90.0,0.0) ; ROBOT$CLOSE_GRIPPER() ; ROBOT$MOVE(250.0,5.0,0.0,5.0,-90.0,0.0) ; ROBOT$HOME(); ROBOT$MOVE(250.0,5.0,0.0,5.0,-90.0,0.0) ; ROBOT$MOVE(250.0,8.0,0.0,5.0,-90.0,0.0) ; ROBOT$OPEN_GRIPPER(2.5); ROBOT$MOVE(230.0,8.0,0.0,2.2,-90.0,90.0) ; ROBOT$CLOSE_GRIPPER(); ROBOT$MOVE(250.0,8.0,0.0,4.0,-90.0,90.0) ; ROBOT$MOVE(250.0,8.0,-5.0,0.7,-90.0,0.0) ; ROBOT$OPEN_GRIPPER(2.0); ROBOT$MOVE(250.0,8.0,-5.0,2.5,-90.0,0.0) ; ROBOT$CLOSE_GRIPPER(); ROBOT$MOVE(250.0,8.0,0.0,2.5,-90.0,0.0) ; ROBOT$OPEN_GRIPPER(3.2); ROBOT$MOVE(230.0,8.0,0.0,0.7,-90.0,90.0) ; ROBOT$CLOSE_GRIPPER(); ROBOT$MOVE(250.0,8.0,0.0,1.0,-90.0,90.0) ; ROBOT$MOVE(250.0,8.0,5.0,1.0,-90.0,0.0) ; ROBOT$MOVE(230.0,8.0,5.0,0.7,-90.0,0.0) ; ROBOT$OPEN_GRIPPER(2.5); ROBOT$MOVE(250.0,8.0,5.0,2.5,-90.0,0.0) ; ROBOT$CLOSE_GRIPPER(); ROBOT$MOVE(250.0,8.0,0.0,5.0,-90.0,0.0) ; ROBOT$HOME(); } /* end function */ /* ******************************************************************** * FUNCTION ROBOT$HOME * * This function returns robbie to his ROBOT$HOME position * ******************************************************************** */ ROBOT$HOME() { int i; result = ROBOT$SEND( "@GOHOME 205" ); result = ROBOT$SEND( "@RESET" ); result = ROBOT$READ(); gripper_width = position_reg[6] - position_reg[3] ; #ifdef NOTCALIBRATED ROBOT$SEND("@STEP 200,0,0,0,-62, 62,0"); ROBOT$SEND( "@RESET" ); #endif NOTCALIBRATED result = ROBOT$READ(); #ifdef NOTCALIBRATED for (i=1;i<=8;i++) printf("pos_reg[%d]: %d\n", i, position_reg[i]); #endif NOTCALIBRATED gripper_width = position_reg[6] - position_reg[3] ; return(result); } /* end function */ /* ******************************************************************** * FUNCTION ROBOT$INIT * * This function initializes the global variables and instructs the * * user to ROBOT$HOME the robot. * ******************************************************************** */ ROBOT$INIT() { int errorcode; char str[STRLN]; errorcode = NOERROR ; system("/work/users/stuff/setup.robot"); /* open the port file pointer for the Robot */ errorcode = ROBOT$OPEN() ; printf ("fp in OPEN %d \n", errorcode); if (errorcode > 0) { errorcode = ROBOT$HOME(); if (errorcode != NOERROR) { printf ("Error in HOME for Robot.\n") ; return (errorcode) ; } #ifdef ROGER while(( line[x] != ',' ) && ( line[x] != '\0' )) #endif #ifdef ROGER errorcode = ROBOT$DELAY(5); printf ("errorcode in DELAY is %d \n", errorcode); if (errorcode != NOERROR) { printf ("Error in DELAY set for Robot. \n") ; return (errorcode) ; } #endif errorcode = ROBOT$READ(); if (errorcode != NOERROR) { printf ("Error in READ for Robot.\n") ; return (errorcode) ; } errorcode = ROBOT$SPEECH_INIT(); if (errorcode != NOERROR) { printf ("Error in SPEECH INITIALIZATION.\n"); return (errorcode) ; } } /* endif */ else { printf ("The robot did not open properly.\n") ; return(-1) ; } return (NOERROR) ; } /* end function */ /* ******************************************************************** * FUNCTION ROBOT$CLOSE_GRIPPER * * This function closes the gripper and then steps the motor in 60 * * more steps. * ******************************************************************** */ ROBOT$CLOSE_GRIPPER() { char str[STRLN], ch; int i, error ; result = ROBOT$SEND("@CLOSE 255"); result = ROBOT$SEND("@STEP 255,0,0,0,0,0,-60"); error = ROBOT$READ() ; gripper_width = position_reg[6] - position_reg[3] ; return( result ) ; } /* end function */ /* ******************************************************************** * FUNCTION ROBOT$OPEN_GRIPPER * * This function opens the gripper to x inches * ******************************************************************** */ ROBOT$OPEN_GRIPPER( x ) float x; { char str1[STRLN], str2[STRLN], ch; int i, j; x = (( x <= 3.2 ) ? ( x ) : ( 3.2 )); i = floor(x * S6 + 0.5) ; j = i - gripper_width ; sprintf(str1,"@STEP 245,0,0,0,0,0,%d",j); result = ROBOT$SEND(str1) ; result = ROBOT$READ() ; gripper_width = i ; return(result); } /* end function */ ROBOT$GRIPPER_WIDTH() { return (gripper_width); } ROBOT$GRIP_WIDTH(width) float width; { width = 0; if (gripper_width > 0) width = (float)(gripper_width / 385); /* width at one inch in steps */ return (gripper_width); } /* end function */ /* ******************************************************************** * FUNCTION ROBOT$MOVE * * * * This function moves the robot arm to the x,y,z,p,r position * * specified by the parameters passed to it. The function calls * * ROBOT$CART_TO_STEP to convert the cartesian coordinates to motor * * steps. It then calls ROBOT$SEND which sends the command to the * * robot. * * * ******************************************************************** */ ROBOT$MOVE( sp, x, y, z, p, r ) float sp, x, y, z, p, r; { static int j[7], k[7]; int s, i, temp, dd; char str1[STRLN], str2[STRLN]; /* result = ROBOT$READ(); */ x = x + 2.0; /* add 2 inches because represents center of rotation */ str1[0] = 0; str2[0] = 0; for (i=1;i<=7;i++) { j[i] = 0; k[i] = 0; } result = ROBOT$CART_TO_STEP( x, y, z, p, r, j ); for (i=1;i<=6;i++) { k[i] = j[i] - position_reg[i]; } s = floor(sp); sprintf (str1, "@STEP %d", s); for (dd=1;dd<=6;dd++) sprintf (str1,"%s,%d",str1,k[dd]); #ifdef ROGER for (i=1;i<=8;i++) printf("Before MOVE pos_reg[%d]: %d\n", i, position_reg[i]); #endif if (result == NOERROR) { result = ROBOT$SEND(str1); for (i=1;i<=5;i++) { position_reg[i] = j[i]; } position_reg[6] = position_reg[3] + gripper_width; } ROBOT$READ(); /* RWW */ #ifdef ROGER printf ("sending: %s\n",str1); for (i=1;i<=8;i++) printf("after MOVE pos_reg[%d]: %d\n", i, position_reg[i]); #endif return(result); } /* end function */ /* ******************************************************************** * FUNCTION ROBOT$CART_TO_STEP * * This function converts cartesian coordinates to motor steps to * * be sent to the Microbot Teachmover. Input is the x,y,z,p,r * * position of the robot arm. Output is the motor steps from the * * ROBOT$HOME position. * ******************************************************************** */ ROBOT$CART_TO_STEP ( x, y, z, pitch, roll, j ) float x, y, z, pitch, roll; int j[7]; { int i, error = NOERROR; float t1, t2, t3, t4, t5, rr, ro, zo, b, a, R1; float pitch2; pitch2 = pitch; pitch = pitch / DEG_PER_RAD; roll = roll / DEG_PER_RAD; if (x == 0.0) t1 = ROBOT$SGN(y)*PI/2.0; else t1 = atan(y/x); if (x<0.0) { error = ERROR ; /* printf("\nerror - x < 0"); */ } /* printf ("taking the sqrt of %d \n", x*x + y*y ); */ rr = sqrt(x*x+y*y); /* rww */ if (( rr<2.25 ) && ( z<15.0 )) { error = ERROR ; /* printf("error - hand too close to body rr=%f\n",rr); */ } if ( rr>18.1 ) { error = ERROR; /* printf("error - reach out of range"); */ } ro = rr - HAND_L * cos(pitch); if ((( x<2.25 ) && ( z<2.25 ) && ( ro<5.0 )) && ( pitch < ( -90.0/DEG_PER_RAD ))) { error = ERROR; /* printf("error - hand interface with base\n"); */ } zo = z - HAND_L*sin(pitch) - HEIGHT_L; if (ro == 0.0) b = ROBOT$SGN(zo)*PI/2.0; else b = atan(zo/ro); a = ro * ro + zo * zo; a = (4.0* (UPPERARM_L * UPPERARM_L)) / a; a = a - 1.0; if (a < 0.0) { error = ERROR; /* printf("error - reach out of range for shoulder & elbow\n"); */ } /* printf ("taking the sqrt of %d \n", a); */ a = sqrt(a); /* rww */ a = atan(a); t2 = a+b; t3 = b-a; if (( t2>( 133.0/DEG_PER_RAD )) || ( t2<( -18.0/DEG_PER_RAD ))) { error = ERROR; /* printf("error - shoulder out of range\n"); */ } if ((( t2-t3 )<0.0 ) || (( t2-t3 )>(140.0/DEG_PER_RAD))) { error = ERROR; /* printf("error - elbow out of range\n"); */ } if ( ( roll> (180.0/DEG_PER_RAD) ) || ( roll< (-180.0/DEG_PER_RAD) )) if (( pitch> ((90.0/DEG_PER_RAD+t3) - (roll+180.0/DEG_PER_RAD)) ) || ( pitch< ((-90.0/DEG_PER_RAD+t3) + (roll-180.0/DEG_PER_RAD)) )) { error = ERROR; /* printf("error - pitch out of range\n"); */ } if (( pitch>( 90/DEG_PER_RAD+t3 )) || ( pitch<( -90/DEG_PER_RAD+t3 ))) { error = ERROR; /* printf("error - pitch out of range\n"); */ } if (( roll>( 270.0/DEG_PER_RAD - fabs( pitch-t3 ))) || ( roll <( -270.0/DEG_PER_RAD + fabs( pitch-t3 )))) { error = ERROR; /* printf("error - roll out of range\n"); */ } if (((pitch2 >= 89.0) && (pitch2 <= 91.0)) || ((pitch2 <= -89.0) && (pitch2 >= -91.0))) R1 = 1.0; else R1 = 0.0; t4 = -pitch - roll + R1 * t1; t5 = -pitch + roll - R1 * t1; j[1] = floor(S1*t1+0.5)+P1 ; j[2] = floor(S2*t2+0.5)+P2 ; j[3] = floor(S3*t3+0.5)+P3 ; j[4] = floor(S4*t4+0.5)+P4 ; j[5] = floor(S5*t5+0.5)+P5 ; j[6] = j[3] + gripper_width ; return ( error ); } /* end function */ /* ******************************************************************** * ROBOT$QDUMP (steps) * * * * This function does the QDUMP utility * ******************************************************************** */ ROBOT$QDUMP(steps) int steps; { char buffer[ STRLN ]; int x,errorcode; sprintf( buffer, "@QDUMP 0 %d", steps); errorcode = ROBOT$SEND( buffer ); if (errorcode == NOERROR) { for (x=0; x < steps; x++) { ROBOT$GET( robotline[x] ); } } return(errorcode); } /* end function */ /* ******************************************************************** * * * ROBOT$QWRITE(steps) * * * * This function does the QWRITE utility * ******************************************************************** */ ROBOT$QWRITE (steps) int steps; { char buffer[ STRLN ]; int x; for (x=0; x < steps; x++) { sprintf(buffer, "@QWRITE %s",robotline[x] ); result = ROBOT$SEND(buffer); } return(result); } /* end function */ /* ******************************************************************** * ROBOT$QDUMP_LOAD (filename) * * * * This function does the QDUMP utility * ******************************************************************** */ ROBOT$QDUMP_LOAD ( filename ) char *filename; { char buffer[ STRLN ]; int x=0, qwrite_fp, nbytes = 80, steps; qwrite_fp = open(filename,0); if (qwrite_fp != -1) { while(( result = read (qwrite_fp, robotline[x++], nbytes)) != 0) printf("numbytes => %d \n TEXT: %s \n", result, robotline[x-1] ); close(qwrite_fp); } steps = x; if (x == 0) return(0); else return(steps); } /* end function ROBOT$QDUMP_LOAD */ /* ******************************************************************** * ROBOT$QDUMP_SAVE (steps, filename) * * * * This function does the QDUMP_SAVE utility * ******************************************************************** */ ROBOT$QDUMP_SAVE (steps, filename) int steps; char *filename; { char buffer[ STRLN ]; int x, qwrite_fp, nbytes; qwrite_fp = creat(filename,0644); if (qwrite_fp != -1) { for (x=0; x < steps; x++) { /* sprintf(buffer, "@QWRITE %s",robotline[x] ); */ nbytes = strlen(robotline[x]); result = write( qwrite_fp, robotline[x], STRLN); printf("numbytes => %d \n TEXT: %s \n", result, robotline[x] ); } close(qwrite_fp); } return(qwrite_fp); } /* end function */ /* ******************************************************************** * FUNCTION ROBOT$DELAY * ******************************************************************** */ ROBOT$DELAY (delay) int delay; { int errorcode; char buf[STRLN]; int nbytes; sprintf(buf,"@DELAY %d",delay) ; /* rww */ nbytes = strlen(buf) ; errorcode = ROBOT$SEND(buf); if (errorcode != NOERROR ) { printf ("Error reading @DELAY of robot.\n") ; } return (errorcode) ; } /* ******************************************************************** * ROBOT$SOUND_PLAY(rsystem, sound_file * ******************************************************************** */ int ROBOT$SOUND_PLAY (rsystem, sound_file ) char *rsystem, *sound_file; { char c[100]; sprintf (c,"rsh %s /usr/demo/SOUND/play -v 80 %s&\n", rsystem, sound_file); system ( c ); /* rww */ /* printf ("%s\n", c); */ } /* ******************************************************************** * FUNCTION ROBOT$SPEECH_INIT * ******************************************************************** */ int ROBOT$SPEECH_INIT ( ) { if ((speechfp=fopen( SPEECH_PORT,"w" )) == NULL ) return ( ERROR ); else return ( NOERROR ); } /* ******************************************************************** * FUNCTION ROBOT$SPEECH_CLOSE * ******************************************************************** */ int ROBOT$SPEECH_CLOSE ( ) { if ( speechfp != NULL ) { fclose ( speechfp ); return ( NOERROR ); } else return ( ERROR ); } /* ****************************************************************** */ /* ******************************************************************** * FUNCTION ROBOT$SPEAK * ******************************************************************** */ int ROBOT$SPEAK ( sentence ) char *sentence; { char local_sentence[259]; local_sentence[0] = 0; if ( speechfp == NULL ) return ( ERROR ); sprintf ( local_sentence, "%s\r", sentence ); /* sprintf ( local_sentence, "%s\r\n", sentence ); */ fputs ( local_sentence, speechfp ); fflush ( speechfp ); return ( NOERROR ); } /* ****************************************************************** */ #define SKIP_PIX 1 /* step SKIP_PIX and read */ #define BLACK 69 /*don't know what to declare black as */ #define PIX_TO_INCH 1.0 /* was 0.7287234 was .75 */ #define DIST_FROM_HOME 6.00 /* was 5.41 dist from gripper to base plate */ /* ****************************************************************** */ /* ******************************************************************** * VIS$ROBOT_CALIBRATE(pix_per_inch, origin_x, origin_y, make_file) * ******************************************************************** */ VIS$ROBOT_CALIBRATE(ppi, origin_x, origin_y, make_file) float *ppi; int *origin_x, *origin_y, make_file; { int minx=40, miny=40, maxx=560, maxy=430, length, fp, n, values[256], thresh=68; if (make_file) { /* system("rm /work/users/stuff/ppi"); fp = creat("/work/users/stuff/ppi",0777); */ system("rm ppi"); fp = creat("ppi",0777); ROBOT$MOVE(245.0,0.0,0.0,26.0,90.0,0.0); visfp = VIS$OPEN(); VIS$INIT_FB(); VIS$INIT_LUTS(); VIS$VIEW(); VIS$FREEZE(); thresh = VIS$OPTIMAL_THRESHOLD(minx,miny,maxx,maxy,0, 255, 5); printf ("threshold is %d\n", thresh); for (n=0; n<256; n++) { if (n < thresh) values[n] = 1; else values[n] = 140; } VIS$LOAD_LUT(0, 0, values); VIS$FREEZE(); VIS$DRAW_BOX (minx, miny, maxx, maxy, 255, 5); VIS$BORDER_FOLLOW (&length, xvals, yvals, 1, 255, minx, miny, maxx, maxy, 5); VIS$GET_MAX_XY (xvals, yvals, length, &minx, &miny, &maxx, &maxy); VIS$DRAW_BOX (minx, miny, maxx, maxy, 255, 9); /* printf ("maxx: %d minx: %d maxy: %d miny: %d\n", maxx, minx, maxy, miny); */ *ppi = (float)(((maxx - minx) + (maxy - miny))/2 + 1); *origin_x = minx + ((*ppi)/2); *origin_y = miny - ( (*ppi) * 8.0); write (fp, ppi, sizeof(*ppi)); write (fp, origin_x, 4); write (fp, origin_y, 4); close (fp); /* system("chmod a+rwx /work/users/stuff/ppi"); */ } else { fp = open( "ppi", 0); n = read (fp, ppi, sizeof(*ppi)); n = read (fp, origin_x, 4); n = read (fp, origin_y, 4); close (fp); } /* printf ("ppi: %f origx: %d origy: %d\n", *ppi, *origin_x, *origin_y); */ } /* end function */ /* ******************************************************************** * * * ROBOT$XY (v_o_x, vx, v_o_y, vy, robot_x, robot_y, ppi) * * * ******************************************************************** */ ROBOT$XY (v_o_x, vx, v_o_y, vy, robot_x, robot_y, ppi) int v_o_x, vx, v_o_y, vy; float *robot_x, *robot_y, ppi; { *robot_x = (float)(vy - v_o_y) / ppi; *robot_y = (float)(vx - v_o_x) / ppi; } /* ********************************************************************* * * * Function itoa * * * * This procedure is taken from KERNIGHAN & RITCHIE'S C * * programming guide version 2, p. 64 * * * ********************************************************************* */ itoa( n, s ) char s[]; int n; { int i, sign; if (( sign = n ) < 0 ) /* record sign */ n = -n; /* make n positive */ i = 0; do { /* generate digits in reverse order */ s[i++] = n % 10 + '0'; /* get next digit */ } while (( n /= 10 ) > 0 ); /* delete it */ if ( sign < 0 ) s[i++] = '-'; s[i] = '\0'; reverse(s); /* put string in correct order */ } /* ******************* end of Function itoa ***************************** */ /* ************************************************************************ * * * Function reverse * * * * This procedure is taken from KERNIGHAN & RICHIE'S C * * PROGRAMMING GUIDE Version 2.0, p. 62, with slight alterations * * * ************************************************************************ */ reverse( s ) char s[]; { int i, j; for ( i=0, j=strlen(s)-1; i18.1 ) { error = ERROR; /* printf("error - reach out of range"); */ } ro = rr - HAND_L * cos(pitch); if ((( x<2.25 ) && ( z<2.25 ) && ( ro<5.0 )) && ( pitch < ( -90.0/DEG_PER_RAD ))) { error = ERROR; /* printf("error - hand interface with base\n"); */ } zo = z - HAND_L*sin(pitch) - HEIGHT_L; if (ro == 0.0) b = ROBOT$SGN(zo)*PI/2.0; else b = atan(zo/ro); a = ro * ro + zo * zo; a = (4.0* (UPPERARM_L * UPPERARM_L)) / a; a = a - 1.0; if (a < 0.0) { error = ERROR; /* printf("error - reach out of range for shoulder & elbow\n"); */ } /* printf ("taking the sqrt of %d \n", a); */ a = sqrt(a); /* rww */ a = atan(a); t2 = a+b; t3 = b-a; if (( t2>( 133.0/DEG_PER_RAD )) || ( t2<( -18.0/DEG_PER_RAD ))) { error = ERROR; /* printf("error - shoulder out of range\n"); */ } if ((( t2-t3 )<0.0 ) || (( t2-t3 )>(140.0/DEG_PER_RAD))) { error = ERROR; /* printf("error - elbow out of range\n"); */ } if ( ( roll> (180.0/DEG_PER_RAD) ) || ( roll< (-180.0/DEG_PER_RAD) )) if (( pitch> ((90.0/DEG_PER_RAD+t3) - (roll+180.0/DEG_PER_RAD)) ) || ( pitch< ((-90.0/DEG_PER_RAD+t3) + (roll-180.0/DEG_PER_RAD)) )) { error = ERROR; /* printf("error - pitch out of range\n"); */ } if (( pitch>( 90/DEG_PER_RAD+t3 )) || ( pitch<( -90/DEG_PER_RAD+t3 ))) { error = ERROR; /* printf("error - pitch out of range\n"); */ } if (( roll>( 270.0/DEG_PER_RAD - fabs( pitch-t3 ))) || ( roll <( -270.0/DEG_PER_RAD + fabs( pitch-t3 )))) { error = ERROR; /* printf("error - roll out of range\n"); */ } #ifdef ROGER RWW 3/9/93 commented this out to allow R1 to be passed into this Function .... if (((pitch2 >= 89.0) && (pitch2 <= 91.0)) || ((pitch2 <= -89.0) && (pitch2 >= -91.0))) R1 = 1.0; /* R1 of 1.0 means relative to the plane*/ else R1 = 0.0; /* R1 of 0.0 means relative to the arm*/ #endif /* R1 of 1.0 means relative to the cartesian plane (workspace) */ /* R1 of 0.0 means relative to the arm (end effector) */ t4 = -pitch - roll - R1 * t1; t5 = -pitch + roll + R1 * t1; j[1] = floor(S1*t1+0.5)+P1 ; j[2] = floor(S2*t2+0.5)+P2 ; j[3] = floor(S3*t3+0.5)+P3 ; j[4] = floor(S4*t4+0.5)+P4 ; j[5] = floor(S5*t5+0.5)+P5 ; j[6] = j[3] + gripper_width ; return ( error ); } /* end function */ #ifdef ROGER /* ******************************************************************** * * * VIS$ROBOT_FIND_GRIP(pix_per_inch, origin_x, origin_y) * * * ******************************************************************** */ VIS$ROBOT_FIND_GRIP(pix_per_inch, origin_x, origin_y) float *pix_per_inch; int *origin_x, *origin_y; { int x = 220, y = 200, dx = 200, /* 0? */ dy = 200, /* 0? */ values[630], /* 20? */ i = 0, found_grip = 0, stop_scan = 0, max_x = 640, /* 0? */ max_y = 480, /* 0? */ first_x = 0, /* 0? */ second_x = 0, /* 0? */ center_grip, ppi[5], ctr; ROBOT$MOVE(240.0,0.5,0.0,25.0,90.0,0.0); do { VIS$READ_HORIZONTAL_LINE( x, y, dx, values ); VIS$H_WRITE_LINE( x, y, dx, 255 ); for( i=0; (( values[i] > BLACK ) && ( i < dx )); ++i ); ( i != dx ) ? ( found_grip = 1 ) : ( y -= SKIP_PIX ); } while( (!found_grip) && ( y <= max_y ) ); if ( found_grip ) { for (ctr=1; ctr<=5; ctr++) { VIS$MAP(); VIS$INIT_LUTS(); VIS$INIT_FB(); VIS$FREEZE(); VIS$DRAW_BOX( x, y-dy, x+dx, y, 255, 10 ); VIS$READ_HORIZONTAL_LINE( x, y-4, dx, values ); VIS$H_WRITE_LINE( x, y-4, dx, 0 ); i=0; do { } while ( (values[++i] > BLACK) && (i < max_x) ); first_x = x+i; do { } while ( (values[++i] < BLACK) && (i < max_x) ); second_x = x+i; /* *pix_per_inch = ( second_x - first_x ) * PIX_TO_INCH; */ ppi[ctr] = second_x - first_x; center_grip = first_x + ( second_x - first_x ) / 2; printf( "Pixels per inch %f", *pix_per_inch ); printf( "Center Grip at X: %d Y: %d\n", center_grip, y); printf( "second_x: %d first_x: %d diff: %d\n", second_x, first_x, second_x - first_x); } *pix_per_inch = 0; for (ctr=1;ctr<=5;ctr++) *pix_per_inch += ppi[ctr]; *pix_per_inch /= 5; VIS$FREEZE(); VIS$CROSS_HAIR ( center_grip, y, 255, 10); for (i = first_x; i<= second_x; i++) VIS$SET_PIXEL(i, y-4, 255); *origin_x = center_grip; *origin_y = y - (DIST_FROM_HOME * (*pix_per_inch) ); printf("average pix per inch = %d\n",*pix_per_inch); return(1); } else return(0); } /* ROBOT$SEND( "@STEP 243,0,557,-197,1539,-1539,-193,0,0" );*/ for (i=1;i<=8;i++) printf("pos_reg[%d]: %d\n", i, position_reg[i]); #endif