/* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ move4me.c ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ By: Matthew T. Beitler 349 North George Street 10 Scenic Acres Millersville, Pa 17551 Schnecksville, PA 18078 717/872 1719 215/799 2464 For: Dr. Roger W. Webster Artifical Intelligence - CS451 At: Robotic Vision and Artifical Intelligence Lab Millersville University 717/872 3860 717/872 3703 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ */ #include #include #include #include #include #include #include #include #define MOBILE_ROBOT_PORT "/dev/ttyd" #define BUFFSIZE 100 #include int mr_move(), mr_rotate(), mr_set_move_speed(), mr_set_rotate_speed(), mr_move_forward(), mr_move_backward(), movesmade[20], totalmoves = 0, result, fp; char whichtype[20]; struct sgttyb tab6; struct sgttyb *addr = &tab6; mr_port_default() { fp = open( (MOBILE_ROBOT_PORT), O_RDWR ); addr->sg_flags = O_ALLDELAY; addr->sg_ispeed = 13; addr->sg_ospeed = 13; } int mr_transmemory() { result = mr_transwhere(); whichtype[totalmoves] = 't'; movesmade[totalmoves++] = result; printf( "\nmr_transmemory() Int %d, Hex %x\n",result, result ); } int mr_rotatememory() { result = mr_rotatewhere(); whichtype[totalmoves] = 'r'; movesmade[totalmoves++] = result; printf( "\nmr_rotatememory() Int %d, Hex %x\n",result, result ); } mr_setup_port() { result = ioctl(fp, TIOCSETP, addr); } int mr_memorymove( distance ) int distance; { mr_transmemory(); mr_move( distance ); } int mr_move( distance ) int distance; { char arrow, c, buff[BUFFSIZE], buff_param[10]; int result, buff_len; if( distance < 0 ) /* T> pos, T< neg */ arrow = '<'; else arrow = '>'; distance = fabs( distance * 268.27 ); /* mr_setup_port(); */ sprintf( buff, "T%c %x\r\0", arrow, distance ); buff_len = strlen( buff ) + 1; result = (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); /* printf( "%c", c); */ } while( c != '\n' ); sprintf( buff, "WE 01\r\0" ); buff_len = strlen( buff ) + 1; result = result + (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); printf( "%c", c); } while( c != '\n' ); return( result ); } int mr_memoryrotate( degrees ) int degrees; { mr_rotatememory(); mr_rotate( degrees ); } int mr_rotate( degrees ) float degrees; { char buff[BUFFSIZE], c, arrow; int buff_len, result; if( degrees < 0 ) /* R> pos, R< neg */ arrow = '<'; else arrow = '>'; printf( "Here\n" ); degrees = fabs( degrees * 328.33 ); sprintf( buff, "R%c %x\r\0", arrow, degrees ); printf( buff ); buff_len = strlen( buff ) + 1; result = (buff_len==write( fp, buff, buff_len )); printf( "Here\n" ); do { read( fp, &c, 1 ); printf( "%c", c); } while( c != '\n' ); sprintf( buff, "WE 81\r\0" ); buff_len = strlen( buff ) + 1; result = result + (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); printf( "%c", c); } while( c != '\n' ); return( result ); } int mctoi( c ) char c; { if( ( c > 47 ) && ( c < 58 ) ) return( ( c-48 )); if( ( c > 64 ) && ( c < 71 ) ) return( ( c-55 )); if( ( c > 96 ) && ( c < 103 ) ) return( ( c-87 )); if( ( c >= 0 ) && ( c < 10 ) ) return( ( c )); return( 9999 ); } int mr_transwhere() { char buff[BUFFSIZE], where[BUFFSIZE], c; int buff_len, loop = 0, result, robotresponse; long wherent; /* mr_setup_port(); */ wherent = 0; sprintf( buff, "TW\r\0" ); buff_len = strlen( buff ) + 1; result = (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); } while( c != '\n' ); do { read( fp, &c, 1 ); loop = mctoi( c ); if( loop != 9999 ) wherent = ( 16 * wherent ) + loop; } while( c != '\n' ); /* result = atoi( where ); */ /* printf( "Int %d, Hex %x\n",wherent, wherent ); */ return( wherent ); } int mr_rotatewhere() { char buff[BUFFSIZE], c; int buff_len, loop = 0, result, robotresponse; long wherent; /* mr_setup_port(); */ sprintf( buff, "RW\r\0" ); buff_len = strlen( buff ) + 1; result = (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); printf( "%c", c); } while( c != '\n' ); do { read( fp, &c, 1 ); loop = mctoi( c ); if( loop != 9999 ) wherent = ( 16 * wherent ) + loop; printf( "%c", c); } while( c != '\n' ); return( wherent ); } int mr_set_move_speed( speed ) int speed; { char buff[BUFFSIZE], c, arrow; int result, buff_len; /* mr_setup_port(); */ sprintf( buff, "TV %d\r\0", speed ); buff_len = strlen( buff ) + 1; result = (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); printf( "%c", c); } while( c != '\n' ); printf( "\n" ); return( result ); } int mr_home() { char buff[BUFFSIZE], c, arrow; int result, buff_len; /* mr_setup_port(); */ sprintf( buff, "TP 80000000\r\0" ); buff_len = strlen( buff ) + 1; result = (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); /* printf( "%c", c); */ } while( c != '\n' ); /* printf( "\n" ); */ return( result ); } int mr_retracetrans( value ) long value; { char buff[BUFFSIZE], c, arrow; int result, buff_len; /* mr_setup_port(); */ sprintf( buff, "TP %x\r\0",value ); printf( "%s\n", buff ); buff_len = strlen( buff ) + 1; result = (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); /* printf( "%c", c); */ } while( c != '\n' ); printf( "trans\n" ); return( result ); } int mr_retracerotate( value ) long value; { char buff[BUFFSIZE], c, arrow; int result, buff_len; /* mr_setup_port(); */ sprintf( buff, "RP %x\r\0",value ); buff_len = strlen( buff ) + 1; result = (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); /* printf( "%c", c); */ } while( c != '\n' ); printf( "rotate\n" ); return( result ); } int mr_set_rotate_speed( speed ) int speed; { char buff[BUFFSIZE], c, arrow; int result, buff_len; /* mr_setup_port(); */ sprintf( buff, "RV %x\r\0", speed ); buff_len = strlen( buff ) + 1; result = (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); /* printf( "%c", c); */ } while( c != '\n' ); /* printf( "\n" ); */ sprintf( buff, "RA %x\r\0", speed ); buff_len = strlen( buff ) + 1; result = (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); /* printf( "%c", c); */ } while( c != '\n' ); return( result ); } int mr_move_forward() { char c, buff[BUFFSIZE]; int result; /* mr_setup_port(); */ sprintf( buff, "T+\r\0" ); result = (write( fp, buff, 4 )==4); do { read( fp, &c, 1 ); /* printf( "%c", c); */ } while( c != '\n' ); /* printf( "\n" ); */ return( result ); } int mr_move_backward() { char c, buff[BUFFSIZE]; int result; /* mr_setup_port(); */ sprintf( buff, "T-\r\0" ); result = (write( fp, buff, 4 )==4); do { read( fp, &c, 1 ); /* printf( "%c", c); */ } while( c != '\n' ); /* printf( "\n" ); */ return( result ); } int mr_halt() { char c, buff[BUFFSIZE]; int result; sprintf( buff, "TH\r\0" ); result = write( fp, buff, 4 ); do { read( fp, &c, 1 ); /* printf( "%c", c); */ } while( c != '\n' ); /* printf( "\n" ); printf( "\n" ); */ return( result ); } int mr_battery_voltage() { char c, buff[BUFFSIZE]; int result; /* mr_setup_port(); */ sprintf( buff, "\r\0" ); result = (write( fp, buff, 4 )==4); do { read( fp, &c, 1 ); printf( "%c", c); } while( c != '\n' ); printf( "\n" ); return( result ); } matty() { char watt, c, arrow, buff[BUFFSIZE], buff_param[10]; int distance, result, degree, dummy, buff_len; printf("\n Mobile Robot Driver Software \n" ); mr_port_default(); printf( "\n written by Matt Beitler\n" ); mr_setup_port(); printf( "\n for CS498 Independent Study \n" ); result = mr_set_move_speed( 4000 ); printf( "\nThe mr_set_move_speed( 4000 ) returned a %d\n",result ); result = mr_set_rotate_speed( 4000 ); printf( "\nThe mr_set_rotate_speed( 4000 ) returned a %d\n",result ); do { printf ("\nWhat is thy bidding my master: "); watt = getchar(); getchar(); if (watt != 'e') { switch (watt) { case 'm': { printf( "\nWhat distance do you desire (cm): " ); scanf( "%d", &distance ); /* printf( "\n%d distance\n",distance ); */ getchar(); result = mr_memorymove( distance ); printf( "\nThe mr_move( %d ) returned a %d\n",distance, result ); break; } case 'c': { result = mr_setup_port(); printf( "\nThe mr_setup_port() returned a %d\n",result ); break; } case 'r': { printf( "\nHow many degrees do you want to rotate: " ); scanf( "%d", °ree ); /* printf( "\n%d degree\n",degree ); */ getchar(); result = mr_memoryrotate( degree ); printf( "\nThe mr_rotate( %d ) returned a %d\n",degree, result ); break; } case 'f': { result = mr_move_forward(); printf( "\nThe mr_move_forward() returned a %d\n",result ); break; } case 'b': { result = mr_move_backward(); printf( "\nThe mr_move_backward() returned a %d\n",result ); break; } case 'W': { result = mr_transwhere(); whichtype[totalmoves] = 't'; movesmade[totalmoves++] = result; printf( "\nThe mr_transwhere() returned Int %d, Hex %x\n",result, result ); break; } case 'w': { result = mr_rotatewhere(); whichtype[totalmoves] = 'r'; movesmade[totalmoves++] = result; printf( "\nThe mr_rotatewhere() returned Int %d, Hex %x\n",result, result ); break; } case 'E': /* walk */ { result = mr_rotate( 45 ); printf( "\nThe mr_rotate( 45 ) returned a %d\n",result ); getchar(); result = mr_move( 150 ); printf( "\nThe mr_move( 150 ) returned a %d\n",result ); getchar(); result = mr_rotate( -90 ); printf( "\nThe mr_rotate( -90 ) returned a %d\n",result ); getchar(); result = mr_move( 150 ); printf( "\nThe mr_move( 150 ) returned a %d\n",result ); getchar(); result = mr_rotate( 90 ); printf( "\nThe mr_rotate( 90 ) returned a %d\n",result ); getchar(); result = mr_move( 300 ); printf( "\nThe mr_move( 300 ) returned a %d\n",result ); getchar(); break; } case 'x': { printf( "\nHow fast do you want to rotate: " ); scanf( "%d", &dummy ); getchar(); result = mr_set_rotate_speed( dummy ); printf( "\nmr_set_rotate_speed( %d ) returned a %d\n",dummy, result ); break; } case 's': { printf( "\nHow fast do you want to go: " ); scanf( "%d", &dummy ); getchar(); result = mr_set_move_speed( dummy ); printf( "\nThe mr_set_move_speed( %d ) returned a %d\n",dummy, result ); break; } case 'H': { /* mr_setup_port(); */ result = mr_home(); printf( "\nThe mr_home returned a %d\n",result ); break; } /* sprintf( buff, "WE 04\r\0" ); buff_len = strlen( buff ) + 1; result = (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); printf( "%c", c); } while( c != '\n' ); */ case 'T': { /* mr_setup_port(); */ for( --totalmoves; totalmoves>-1; --totalmoves ) { if( whichtype[totalmoves] == 't' ) { mr_retracetrans( movesmade[totalmoves] ); sprintf( buff, "WE 01\r\0" ); buff_len = strlen( buff ) + 1; result = result + (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); printf( "%c", c); } while( c != '\n' ); } else { mr_retracerotate( movesmade[totalmoves] ); sprintf( buff, "WE 81\r\0" ); buff_len = strlen( buff ) + 1; result = result + (buff_len==write( fp, buff, buff_len )); do { read( fp, &c, 1 ); printf( "%c", c); } while( c != '\n' ); } printf( "\nThe mr_retrace/wait returned a %d\n",result ); /* getchar(); */ } break; } case 'h': { /* mr_setup_port(); */ result = mr_halt(); printf( "\nThe mr_halt returned a %d\n",result ); break; } } } } while (watt != 'e'); close( fp ); }