// res3.c MR 06/12/99 // // Robot Environment Shell around robot brain program. // // Compile & link this (on LINUX) with // ``gcc res3.c -o res3 -lcurses''. // // // 07/12/99 : With name of brain program as res2 argument. // 16/12/99 : New communication protocol with brain program, and // with 'smell food' and without 'air' sensor. // 28/12/99 : Protocol "#n" (where n is a real > 0.0) instead of // "#Good"/"#Bad". Issue a "#n" with n near 0.95 or 0.99 // on a motor action not immmediately resulting in // bumping into a wall or food pellet. /* ---------------------------------------------------------------------- Copyright (C) 1999 Menno Rubingh This source code / program is free software; you are allowed to redistribute it and/or to modify it under the terms of the GNU Public Licence (GNU GPL) as published by the Free Software Foundation. See the file 'COPYING' which should have accompanied this source file. This program comes with ABSOLUTELY NO WARRANTY. ---------------------------------------------------------------------- */ #include #include #include typedef int _bool; // ------------------------------ screen output ----------------------------- // // The functions below provide a platform-independent API to do // output of characters to the screen, where characters are placed // at definite positions on the screen. (No scrolling, not ``stream''- // based). // // (The implementation below is for the linux/unix ``curses'' library.) // // Note: The keyboard INPUT function that is used in the code below // is the ``curses'' function // // char getch( void ); // // the DOS library contains exactly that same function, with // exactly the same name and prototype. #include void screenInit( void ) { initscr(); cbreak(); noecho(); } void screenEnd( void ) { endwin(); } int maxx( void ) { return COLS; } int maxy( void ) { return LINES; } void putCharAt( int x, int y, char c ) { mvaddch( y, x, c ); } void mvCursorTo( int x, int y ) { move( y, x ); } // ----- // // Some more handy screen output functions built upon the functions // defined above: // void msgPrintf( int y, const char * fmt, ... ) { //Print one line a la printf(). int x; char buf[80]; va_list valst; va_start( valst, fmt ); vsprintf( buf, fmt, valst ); x = 0; while ( buf[x] != '\0' ) { putCharAt( x++, y, buf[x] ); } while ( x < maxx() ) { putCharAt( x++, y, ' ' ); } va_end( valst ); } void clearscreen( void ) { int x, y; for ( y = 0; y < maxy(); y++ ) { for ( x = 0; x < maxx(); x++ ) { putCharAt( x, y, ' ' ); } } } void dumpText( int y1, int y2, const char * buf, int len ) //Print ascii text in buf[] to screen, starting at y-coordinate // y1 and printing up to and including y-coordinate y2. { int x, y; int i = 0; x = 0; y = y1; while ( y <= y2 ) { char c; if ( i < len ) { c = buf[i++]; } else { c = ' '; } if ( c == '\n' ) { while ( x < maxx() ) { putCharAt(x++,y,' '); } } else { putCharAt( x++, y, c ); } if ( x >= maxx() ) { x = 0; y++; } } } // --------------------------------------------------------------------------- // ----------------------- robot environment simulation ---------------------- typedef enum { NORTH, EAST, SOUTH, WEST, N_OR } orient_t; static const char rchar[N_OR] = { '^', '>', 'v', '<' }; static const int x_fwd[N_OR] = { 0, +1, 0, -1 }; static const int y_fwd[N_OR] = { -1, 0, +1, 0 }; typedef struct { int x, y; //Coordinates of position of robot in room. orient_t or; //Orientation of robot. } robot_t; #define ROOM_XMAX 8 #define ROOM_YMAX 8 //Translation of physical room coordinates to print positions on screen: #define YOFFS 1 #define XOFFS 1 #define XMAG 2 int xlx( int x ) { return ( (XOFFS + x) * XMAG ); } int xly( int y ) { return ( y + YOFFS ); } _bool inRoom( int x, int y ) { return ( 0 <= x && x < ROOM_XMAX && 0 <= y && y < ROOM_YMAX ); } void putRoomBorders(char c ) { int x, y; for ( x = -1; x <= ROOM_XMAX; x++ ) { for ( y = -1; y <= ROOM_XMAX; y++ ) { if ( ! inRoom( x, y ) ) { putCharAt( xlx(x), xly(y), c ); } } } } typedef enum { EMPTY, FOOD, WALL, N_SQCONT } sqcont_t; static const char sqcontchar[N_SQCONT] = { ' ', '&', '*' }; static const char * sqcontstr[N_SQCONT] = { "empty", "food", "wall" }; //static const char * aheadcmd[N_SQCONT] = { "aa\n\n", "af\n\n", "aw\n\n" }; static sqcont_t map[ROOM_XMAX][ROOM_YMAX]; void initmap( void ) { int x, y; for ( x = 0; x < ROOM_XMAX; x++ ) { for ( y = 0; y < ROOM_YMAX; y++ ) { map[x][y] = EMPTY; } } } void putItemIntoMap( int x, int y, sqcont_t what ) { //Use what==EMPTY to remove item. assert( inRoom( x, y ) ); map[x][y] = what; putCharAt( xlx(x), xly(y), sqcontchar[what] ); } //------ sensors in robot: ------ _bool wallahead( const robot_t * p ) //Return 1 if wall on square immediately in front of robot. { int x = p->x + x_fwd[p->or]; int y = p->y + y_fwd[p->or]; if ( ! inRoom( x, y ) || map[x][y] == WALL ) { return 1; } return 0; } _bool seefood( const robot_t * p ) //Return 1 if food on either of the 3 squares immediately in // front of the robot. { int x = p->x; int y = p->y; int i; for ( i = 0; i < 3; i++ ) { x += x_fwd[p->or]; y += y_fwd[p->or]; if ( inRoom(x,y) && map[x][y] == FOOD ) { return 1; } } return 0; } _bool smellfood( const robot_t * p ) //Return 1 if food on one of the squares horizontally or // vertically bordering the square the robot occupies. { int x, y; x = p->x + 1; y = p->y; if ( inRoom(x,y) && map[x][y] == FOOD ) { return 1; } x = p->x - 1; y = p->y; if ( inRoom(x,y) && map[x][y] == FOOD ) { return 1; } x = p->x; y = p->y + 1; if ( inRoom(x,y) && map[x][y] == FOOD ) { return 1; } x = p->x; y = p->y - 1; if ( inRoom(x,y) && map[x][y] == FOOD ) { return 1; } return 0; } //------ robot movement: ------ typedef enum { NONE, GOOD, BAD } bir_t; //built-in reaction bir_t move_FWD( robot_t * p ) { if ( wallahead( p ) ) { msgPrintf( maxy()-1, "BUMP !!! OUCH !!!" ); return BAD; } p->x += x_fwd[p->or]; p->y += y_fwd[p->or]; assert ( inRoom( p->x, p->y ) ); if ( map[p->x][p->y] == FOOD ) { msgPrintf( maxy()-1, "YUMMY !!!" ); //Remove food item putItemIntoMap( p->x, p->y, EMPTY ); return GOOD; } return NONE; } bir_t move_TURN( robot_t * p ) { p->or = ( p->or + 1 ) % N_OR; return NONE; } // ----------------------------------- main ---------------------------------- //Includes for fork(), pipe(), usleep(), execlp(): #include #include #include #include #include #include #include //------------- void sendCmd( const char * cmd, int * fifoTF, int * fifoFF, _bool * pFWD, _bool * pTURN, int y1, int y2 ) //y1,y2: y-coordinates where (between which) to print response // received from brain program on screen. { char c; int nread; char bufFF[1024]; /*To read input from Brain Program ('BP')*/ *pFWD = 0; *pTURN = 0; /* * Send line with cmd to 'BP' */ write( fifoTF[1], cmd, strlen(cmd) ); /* * Receive response from 'BP' */ while ( nread = read( fifoFF[0], bufFF, sizeof(bufFF)-1 ), nread == -1 && errno == EAGAIN ) { usleep(1000); /*usleep(): Arg in microseconds*/ } while ( nread > 0 ) { dumpText( y1, y2, /*10, maxy()-4,*/ bufFF, nread ); bufFF[nread] = '\0'; if ( strstr( bufFF, "MOTOR_fwd" ) ) { *pFWD = 1; } if ( strstr( bufFF, "MOTOR_turn" ) ) { *pTURN = 1; } nread = read( fifoFF[0], bufFF, sizeof(bufFF) ); } } int main( int argc, char ** argv ) { const char * pBrainProgName; char bufFU[80]; /*To read input from user (stdin)*/ char bufFF[1024]; /*To read input from 'Brain Program'*/ int fifoTF[2]; /*To Brain Program*/ int fifoFF[2]; /*From Brain Program*/ int pid; robot_t robot = { 0, 0, //Initial coordinates of position of robot in room. EAST //Initial orientation of robot. }; _bool dummy1, dummy2; // Do cmd line args if ( argc < 2 ) { fprintf( stderr, "Usage: res3 brainprog\n" ); return -1; } pBrainProgName = argv[1]; /*DOC fifoTF[] is channel from 'RES' to 'BP' (Brain Program): fifoTF[0] = file descriptor for read by 'BP' fifoTF[1] = file descriptor for write by 'RES' fifoFF[] is channel from 'BP' to 'RES': fifoFF[0] = file descriptor for read by 'RES' fifoFF[1] = file descriptor for write by 'BP'. */ if ( pipe(fifoTF) == -1 || pipe(fifoFF) == -1 ) { fprintf( stderr, "fifo create failed\n" ); _exit(2); } pid = fork(); if ( pid == -1 ) { fprintf( stderr, "fork failed\n" ); _exit(2); } if ( pid == 0 ) { /*Inside child process*/ /*''Pipe fitting'': Connect read-end of fifoTF to stdin of child process; Connect write-end of fifoFF to stdout of child process.*/ if ( dup2( fifoTF[0], STDIN_FILENO ) == -1 || dup2( fifoFF[1], STDOUT_FILENO ) == -1 ) { fprintf( stderr, "pipe fitting failed\n" ); _exit(2); } close( fifoTF[0] ); close( fifoTF[1] ); close( fifoFF[0] ); close( fifoFF[1] ); /* Replace child process with execution of 'BP' */ execlp( pBrainProgName, pBrainProgName, 0 ); /* If reached here, then execlp() failed */ fprintf( stderr, "execlp failed\n" ); _exit(2); } /*Parent process:*/ { /*Change flags for fifoFF[0] to make that read(fifoFF[0]) DOES NOT WAIT for data if there is none yet.*/ int curflags = fcntl( fifoFF[0], F_GETFL ); fcntl( fifoFF[0], F_SETFL, curflags | O_NONBLOCK ); } screenInit(); clearscreen(); putRoomBorders( '*' ); initmap(); //Put in a few food items. putItemIntoMap( 1, 7, FOOD ); putItemIntoMap( 6, 2, FOOD ); putItemIntoMap( 3, 4, FOOD ); // Send cmds to brain program to register sensors & motors. sendCmd( "xwall\n", fifoTF, fifoFF, &dummy1, &dummy2, 10, 10 ); sendCmd( "xsmell\n", fifoTF, fifoFF, &dummy1, &dummy2, 10, 10 ); sendCmd( "xsee\n", fifoTF, fifoFF, &dummy1, &dummy2, 10, 10 ); sendCmd( "mfwd\n", fifoTF, fifoFF, &dummy1, &dummy2, 10, 10 ); sendCmd( "mturn\n", fifoTF, fifoFF, &dummy1, &dummy2, 10, 10 ); //'smell' = Smell food //'see' = See food for(;;) { char c; int nread; _bool doFWD, doTURN; char * pCmd; // // Print (put) new situation on screen // putCharAt( xlx(robot.x), xly(robot.y), rchar[robot.or] ); msgPrintf( maxy()-2, "Sensors: %s %s %s", ( wallahead(&robot) ? "WALL" : "-" ), ( seefood(&robot) ? "SEEFOOD" : "-" ), ( smellfood(&robot) ? "SMELLFOOD" : "-" ) ); mvCursorTo( maxx()-1, maxy()-1 ); // // Get new user command // c = getch(); if ( c == 'q' ) { break; } // // Erase old situation on screen // putCharAt( xlx(robot.x), xly(robot.y), ' ' ); msgPrintf( maxy()-1, "" ); //Erase prev. msg. msgPrintf( maxy()-2, "" ); //Erase prev. msg. msgPrintf( maxy()-3, "" ); //Erase prev. msg. msgPrintf( maxy()-4, "" ); //Erase prev. msg. if ( c == '&' ) //Insert new food item { char c1, c2; int x, y; putCharAt( xlx(robot.x), xly(robot.y), rchar[robot.or] ); //Put back in! msgPrintf( maxy()-2, "Insert new food item" ); msgPrintf( maxy()-1, "Type x-coordinate" ); c1 = getch(); msgPrintf( maxy()-1, "Type y-coordinate" ); c2 = getch(); x = c1 - '0'; y = c2 - '0'; if ( inRoom( x, y ) && ! ( x == robot.x && y == robot.y ) ) { putItemIntoMap( x, y, FOOD ); } msgPrintf( maxy()-1, "" ); //Erase msgPrintf( maxy()-2, "" ); //Erase } else if ( c == '\n' ) { int i = 0; bir_t resp = NONE; //Loop and execute simulation until // a ''MOTOR-...'' response received from brain. do { //Send states of sensors to Brain // 1) Wall-ahead sensor sendCmd( ( wallahead(&robot) ? "awall\n" : "xwall\n" ), fifoTF, fifoFF, &doFWD, &doTURN, 10, 10 ); // 2) See-food sensor sendCmd( ( seefood(&robot) ? "asee\n" : "xsee\n" ), fifoTF, fifoFF, &doFWD, &doTURN, 10, 10 ); // 3) Smell-food sensor sendCmd( ( smellfood(&robot) ? "asmell\n" : "xsmell\n" ), fifoTF, fifoFF, &doFWD, &doTURN, 10, 10 ); //Send '\n'-command (= run Brain until // a motor fires) sendCmd( "\n", fifoTF, fifoFF, &doFWD, &doTURN, 11, maxy()-7 ); msgPrintf( maxy()-6, "%d", i++ ); } while ( !doFWD && !doTURN ); if ( doFWD ) { msgPrintf( maxy()-3, "FWD" ); resp = move_FWD( &robot ); } else if ( doTURN ) { msgPrintf( maxy()-3, "TURN" ); resp = move_TURN( &robot ); } //Send ''hard-wired'' pain/pleasure signal if ( resp == BAD ) { sendCmd( "#.5\n", fifoTF, fifoFF, &doFWD, &doTURN, maxy()-5, maxy()-4 ); } else if ( resp == GOOD ) { sendCmd( "#2\n", fifoTF, fifoFF, &doFWD, &doTURN, maxy()-5, maxy()-4 ); } else { sendCmd( "#0.999\n", fifoTF, fifoFF, &doFWD, &doTURN, maxy()-5, maxy()-4 ); } } else { //Send a single command pCmd = "\n"; switch ( c ) { case '\n': break; case 'd': pCmd = "d\n"; break; } sendCmd( pCmd, fifoTF, fifoFF, &doFWD, &doTURN, 10, maxy()-4 ); } }//for(;;) screenEnd(); close( fifoTF[0] ); close( fifoTF[1] ); close( fifoFF[0] ); close( fifoFF[1] ); return 0; }