/************************************************************************/ /************************************************************************/ /* */ /* Program: ncs.unix.c */ /* */ /* Version: Unix */ /* */ /* Compatible Libraries: 2.1 */ /* */ /* */ /* Description: This program is to make the nomad */ /* avoid, wander_explore */ /* */ /* */ /* Author: Brian J. McCullough */ /* School: Wichita State University */ /* Date: September 15, 1995 */ /* */ /* Language: C */ /* */ /* Revisions: 1.1 */ /* */ /************************************************************************/ /************************************************************************/ /**********************************************/ #define UNIX /* UNIX or DOS program? -- DOS for PC version */ /**********************************************/ #ifdef UNIX /* for unix version */ #include "Nclient.h" #endif #include #include #include "mapcode.h" #define NOIR -1 /* same as below, but for IR */ #define NOSONAR -1 /* Sonar No. returned from */ /* shortest_front if no obstacles */ #define TRUE 1 #define debug2 0 /***********************************************************************/ /* */ /* InitSonar(); */ /* This function sets the firing order of the sonars */ /* and sets which sonars are used by setting Smask[]. */ /* */ /* NON Library function calls */ /* Called by : InitRobot() */ /* Calls : None */ /* */ /***********************************************************************/ void InitSonar() { int i; int order[16]; for (i = 0; i <= 15; i++) /* set sonar fire order */ order[i] = i; conf_sn(2,order); for (i = 17; i <= 32; i++) /* Setup Mask for all sonar */ Smask[i] = 1; /* Smask 17 - 32 == 0 - 15 sonar */ ct(); /* sends Smask to robot */ } /* end of InitSonar */ /***********************************************************************/ /* */ /* InitIR(); */ /* This function sets the firing order of the IR sensors */ /* and sets which IR sensors are used by setting Smask[]. */ /* */ /* NON Library function calls */ /* Called by : InitRobot */ /* Calls : None */ /* */ /***********************************************************************/ void InitIR() { int i; int order[16]; for (i = 0; i <= 15; i++) /* set IR fire order */ order[i] = i; conf_sn(2,order); for (i = 1; i <= 16; i++) /* Setup Smask for all sonar */ Smask[i] = 1; /* Smask 1 - 16 == 0 - 15 IR */ ct(); /* sends Mask to robot */ } /* end InitIR */ /***********************************************************************/ /* */ /* InitRobot(); */ /* This function is responable for configuring the robot */ /* setting up the connection between the client - robot, */ /* and the server - on which the robot runs. */ /* */ /* NON Library function calls */ /* Called by : main() */ /* Calls : InitSonar() */ /* InitIR() */ /* */ /***********************************************************************/ void InitRobot() { int index; if (server_is_running() == 1) printf("The server is running\n"); else{ printf("The server is not running\n"); exit(0); } connect_robot(1); /* must do */ sleep(1); zr(); /* reset all motors and sensors */ ac(100,450,450); /* set accelerations */ /* ac(50,225,225) for dos */ conf_tm(3); /* set time out */ /* set return info flags */ for (index = 34; index <= 43; index ++) Smask[index] = 1; Smask[42] = 0; /* not avaible */ Smask[33] = 0; /* not avaible */ InitSonar(); /* set-up sonar sensors */ sleep(1); InitIR(); /* set-up IR sensors */ sleep(1); /* function in tree.c */ /*printf("building decision tree. \n"); init_tree();*/ /* build decision tree */ ct(); gs(); /* update State vector */ } /* end of InitRobot */ /***********************************************************************/ /* */ /* SN_violtions(); */ /* This funciton updates the State vector and then */ /* checks the front five sensors 2,1,0,15,14, to */ /* see if any are in violation with min_safe_dist */ /* a min threshold value. if so, then return the */ /* senor number in violation, else return -1, a */ /* default all clear value. Sonar */ /* */ /* */ /* NON Library function calls */ /* Called by : Avoid() */ /* Calls : None */ /* */ /***********************************************************************/ int SN_violations() { int index; int shortest; /* shortest distance */ int shortest_sonar; /* shortest sonar number */ int min_safe_dist; /* min Threshold value */ min_safe_dist = 17; shortest = 255; shortest_sonar = -1; get_sn(); /* update sonar readings */ for(index = 17; index <= 19; index++) { if (State[index] <= shortest) /* check sonars 0, 1, 2 */ { shortest = (short)State[index]; shortest_sonar = index - 17; /* State[17] is sonar 0 */ } } /* end for loop */ for(index = 31; index <= 32; index++) /* check sonars 14, 15 */ { if (State[index] <= shortest) { shortest = (short)State[index]; shortest_sonar = index - 17; } } /* end for loop */ if (shortest > min_safe_dist) /* if shortest is greater than */ shortest_sonar = NOSONAR; /* min_safe_dist then set */ /* shortest_sonar to NOSONAR */ else st(); /* else stop and */ return(shortest_sonar); /* return shortest_sonar number */ } /* end of funciton SN_violations */ /***********************************************************************/ /* */ /* Shortest IR */ /* This funciton updates the State vector and then */ /* checks the front five sensors 2,1,0,15,14, to */ /* see if any are in violation with min_safe_dist */ /* a min threshold value. if so, then return the */ /* senor number in violation, else return -1, a */ /* default all clear value. */ /* */ /* NON Library function calls */ /* Called by : None */ /* Calls : None */ /* */ /***********************************************************************/ int shortest_IR() { int index; int min_safe_dist; int shortest; /* shortest distance */ int shortest_IR; /* shortest IR number */ min_safe_dist = 16; /* IR threshold */ shortest = 255; shortest_IR = 0; get_ir(); /* update IR readings */ for(index = 1; index <= 16; index++) { if (State[index] <= shortest) /* check all IRs */ { shortest = (short)State[index]; shortest_IR = index - 1; /* State[1] is IR 0 */ } } /* end for loop */ if (shortest > min_safe_dist) /* if shortest is greater than */ shortest_IR = NOIR; /* min_safe_dist then set */ /* shortest_IR to NOIR (-1) */ return(shortest_IR); /* return shortest_IR number */ } /* end of funciton shortest_IR */ /***********************************************************************/ /* */ /* Calc_Safe_Dir() */ /* This function is called when a new direction to travel */ /* is needed. It checks all sonar sensors in the following */ /* left three (1,2,3) then if not clear then right three */ /* (13,14,15) then if not clear check the remaing uncheck */ /* sensors for a possible path. Then turn to the wheels */ /* and turret to direction of new path. Sonar */ /* */ /* NON Library function calls */ /* Called by : avoid(); */ /* wander_explore(); */ /* Calls : None */ /* */ /***********************************************************************/ int calc_safe_dir() { int min_safe_dist; int turn_deg; /* hold amout of turn */ int loop; /* # of times needed to turn */ int index; int shortest; /* shortest distance */ int sonar_number; /* shortest sonar number */ min_safe_dist = 17; /* sonar theshold min */ shortest = 255; sonar_number = NOSONAR; /* default flag */ get_sn(); /* update sonar readings */ if((State[17] > min_safe_dist) && /* else check sensors #s */ (State[18] > min_safe_dist) && /* 14, 15, 0, 1, 2 */ (State[19] > min_safe_dist) && /* if all > 17 then want to */ (State[32] > min_safe_dist) && /* do not turn and exit fucition*/ (State[31] > min_safe_dist)) return; else if((State[18] > min_safe_dist) && /* check sonar sensors 1, 2, 3 */ (State[19] > min_safe_dist) && /* if all > 17 then want to */ (State[20] > min_safe_dist)) /* turn left toward */ sonar_number = 2; /* sensor number 2, new heading */ else if((State[30] > min_safe_dist) && /* else check sensor 13, 14, 15 */ (State[31] > min_safe_dist) && /* if all > 17 then want to */ (State[32] > min_safe_dist)) /* turn right toward */ sonar_number = 14; /* sensor number 14, newheading */ else for(index = 21; index <= 29; index++) /* else check the remaing */ { /* sensors for a new heading */ if((State[index] > min_safe_dist) && (State[index + 1] > min_safe_dist)) sonar_number = index - 17; } if(sonar_number <= 8) /* if sensor number < 8 */ { /* turn left shortest dist. */ turn_deg = 225; loop = sonar_number; } else /* else sensor number > 8 */ { /* turn right */ turn_deg = -225; loop = 16 - sonar_number; } for(index = 1; index < loop; index++) /* used to turn robot in 22.5 */ { /* degree increments */ pr(0, turn_deg, turn_deg); ws(0, 1, 1, 2); /* wait until motors are done */ } } /* end of funciton calc_safe_dir */ /***********************************************************************/ /* */ /* avoid(); */ /* This function provides the Nomad robot the ability to avoid */ /* objects. The function first checks if there are any threshold */ /* violations, if not any then exit the funciton, else find a */ /* safe direction to travel until Nomad is in a safe state then */ /* stop. */ /* */ /* NON Library function calls */ /* Called by : main(); */ /* Calls : SN_violations(); */ /* calc_safe_dir(); */ /* */ /***********************************************************************/ void avoid() { int thres_viol; /* contains the sensor number in */ /* violation or error flag of -1 */ gs(); /* update sensor readings */ thres_viol = SN_violations(); /* check if there is a violation */ if(thres_viol != -1) /* of the threshold of the robot */ { /* if so, then stop and */ if(debug2) printf("We are in Avoid\n"); st(); while (thres_viol != -1) /* while threshold violation */ { calc_safe_dir(); /* find safest direction and */ pr(10,0,0); /* move in that direction, */ ws(1,0,0,3); /* one inch at a time */ gs(); thres_viol = SN_violations(); } st(); /* in a safe state now stop moving */ } } /* end of function avoid */ /***********************************************************************/ /* */ /* Wander and Explore */ /* This is the second level of control of the nomad robot. */ /* The function's purpose is if the robot is not moving */ /* then calculate a safe direction and move in that direction */ /* */ /* NON Library function calls */ /* Called by : main(); */ /* Calls : calc_safe_dir(); */ /* */ /***********************************************************************/ void wander_explore() { if(State[38] == 0) /* translational speed in State vector */ { /* check if not moving State[38] == 0 */ /* printf("We are in Explore\n"); */ calc_safe_dir(); /* find safest direction and move in */ vm(200,0,0); /* that direction. */ } /* need to put in find_dir(min) to check if there is any openings */ /* to either side of the current direction and then a random gen */ /* to turn another direction bsides current, 1 in 4 , check when ? */ } /* end of function wander_explore */ /***********************************************************************/ /* situation representation 1 */ /* function prints sonar readings to a file specified in main */ /* */ /***********************************************************************/ void sitrep(outptr) FILE *outptr ; { int i; if(State[38] == 0) { gs(); for (i = 17; i <= 32; i++) fprintf(outptr," %3d",State[i]); fprintf(outptr,"t\n"); } } /***********************************************************************/ /* */ /* The Main Strain - - - Program Control */ /* */ /* NON Library function calls */ /* Called by : None */ /* Calls : InitRobot(); */ /* avoid(); */ /* wander_explore(); */ /* */ /***********************************************************************/ void main() { int i,j,k; /* TCP/IP port number must be the same as the */ SERV_TCP_PORT = 7019; /* port number in the "world.setup" file */ /* initialize the robot */ InitRobot(); printf("Press enter to begin."); getchar(); while ( TRUE ) { /* avoid and explore function calls */ fflush(stdout); avoid(); /* if (decision()) random(); */ /* external in tree.c */ wander_explore(); map_area(); print_map(); } /* end while loop */ #ifdef DOS /* DOS power down */ st(); disconnect_robot(1); return(1); #endif } /* end of main */