/*************************************************************************** * * * Spherical Map code for Nomad 200 * * Written By : Brian Hand, Stephen Hutchins, and Simon Evelyn * * Changes by J.Zytkow * ***************************************************************************/ /* WARNING: perhaps some of the (map_size / 2) should be changed to */ /* ((map_size - 1) / 2) */ /* preprocessor statements */ #include "Nclient.h" /* for Nomad functions */ #include "mapcode.h" /* header for map function */ #include #include #include #if 0 #include #include #include #endif #define ST_SONAR_SIZE 16 /* constant for the size of the sonar array */ #define MAP_CELL_SIZE 4 /* square size will be 4 inches */ #define N_LENGTH_INCH 10 /* nomad's representation of 1 inch */ /* replace by a straightforward unit conversion */ #define PI 3.1415926535 /* the value of pi */ #define MOTORSON /* according to Brian: even if defined in */ /* client.h problems appear if not repeated here */ #define ST_SONAR_BASE 17 /* base index for sonar index from the state variable */ #define SURE_DIST 60 /* cut off distance for accurate reading */ #define ST_NOMAD_STEER 36 /* index from state variable for steering angle */ #define ST_NOMAD_X 34 /* nomad's x coordinate from the state variable */ #define ST_NOMAD_Y 35 /* nomad's y coordinate from the state variable */ /* global declarations */ int map[map_size][map_size]; /* this array will hold all of the sonar readings */ int sonar_array[ST_SONAR_SIZE]; /* used to hold the raw sonar readings */ int robotx = (map_size - 1) / 2; /* robot's initial coordinate on map */ int roboty = (map_size - 1) / 2; /* robot's initial coordinate on map */ FILE *outfile; /* file pointer to output file */ double tot_disp = 0.0; /* total displacement used to determine when to dump to file */ void map_area() { st(); /* ensure stop */ ws( 1, 1, 1, 2); /* wait for the stop */ get_sonar_data(); /* get sonar data */ mapper(); /* map the data to the spherical array */ } void get_sonar_data() { int count; /* index variable for looping */ /* outfile = fopen ("MAP.DAT", "a"); */ gs(); /* update the global sonar state */ for ( count = 0 ; count < ST_SONAR_SIZE ; count++ ) { sonar_array[count] = State[ST_SONAR_BASE + count]; /* get sonar readings */ } /* fclose(outfile); */ } /* mapper maintains the spherical array and nomad location in that array */ void mapper () { double steering = (State[ST_NOMAD_STEER] / 10.0) * PI / 180.0; /* steering angle */ /* from tenths of degree to radians */ double angle = steering; /* for polar to rect. conversion */ int count; /* index variable for looping */ int x, y; /* index for placement in map */ int dx, dy; /* displacement values */ int x1, y1, x2, y2; /* variables for itteration */ double displacement; /* displacement value to keep track of motion */ double slope; /* slope of a line between the robot and the detected point. Used to decrement points between the robot and the point */ /* update nomad's position in the map */ /* Have to do this FIRST, since the sonar readings are taken from the NEW position, not the old. Otherwise the map is displaced. - Martian */ displacement = sqrt((State[ST_NOMAD_X] * State[ST_NOMAD_X]) + (State[ST_NOMAD_Y] * State[ST_NOMAD_Y])); tot_disp = tot_disp + displacement; dx = (int) ((displacement / (N_LENGTH_INCH * MAP_CELL_SIZE)) * cos(steering)); dy = (int) ((displacement / (N_LENGTH_INCH * MAP_CELL_SIZE)) * sin(steering)); robotx += dx; robotx = robotx % map_size; if(robotx < 0) robotx += map_size; roboty =+ dy; roboty = roboty % map_size; if(roboty < 0) roboty += map_size; /* erase data */ if ( State[ST_NOMAD_STEER] >= 900 && State[ST_NOMAD_STEER] <= 2700 ) { x1 = robotx - (map_size / 2) + dx; x2 = robotx - (map_size / 2) - 1; } else { x1 = robotx + (map_size / 2) - dx; x2 = robotx + (map_size / 2) + 1; } if ( State[ST_NOMAD_STEER] >= 0 && State[ST_NOMAD_STEER] <= 1800 ) { y1 = roboty + (map_size / 2) - dy; y2 = roboty + (map_size / 2) + 1; } else { y1 = roboty - (map_size / 2) + dy; y2 = roboty - (map_size / 2) - 1; } if(dx) for(; x1 <= x2; x1++) for(count = 0; count < map_size; count++ ) { x = x1 % map_size; if(x < 0) x += map_size; map[x][count] = 0; } if(dy) for(; y1 <= y2; y1++) for(count = 0; count < map_size; count++ ) { y = y1 % map_size; if(y < 0) y += map_size; map[count][y] = 0; } for ( count = 0; count < ST_SONAR_SIZE; count++ ) { if ( sonar_array[count] < SURE_DIST ) { x = (int) (robotx + ((sonar_array[count] * (cos(angle))) / MAP_CELL_SIZE)); y = (int) (roboty + ((sonar_array[count] * (sin(angle))) / MAP_CELL_SIZE)); x = x % map_size; if(x < 0) x += map_size; y = y % map_size; if(y < 0) y += map_size; map[x][y]++; /* Increment detected point */ slope = (double)(y - roboty) / (double)(x - robotx); if( x < robotx ) { x1 = x; x2 = robotx; y1 = y; } else { x1 = robotx; x2 = x; y1 = roboty; } for(x = x1; x < x2; x++) { y = (int)(slope * (double)x) + (int)(slope * (double)x1) + y1; map[x][y]--; /* Decrement points between */ } } angle += (2 * PI / ST_SONAR_SIZE); /* add angle for next sonar reading */ } if (tot_disp >= 720 ) { tk("print map"); print_map(); tot_disp = 0.0; } if ( displacement >= 40.0 ) { displacement = 0.0; dp(0, 0); } } void print_map() { int x, y, x1, x2, y1, y2; /* indices for traversing through array */ x1 = robotx-(map_size/2); y1 = roboty-(map_size/2); x2 = ((x1 - 1) + map_size); y2 = ((y1 - 1) + map_size); outfile = fopen( "World.map", "a" ); fprintf( outfile, "\n\n"); for ( ; y1 <= y2; y1++) { x1 = robotx-(map_size/2); for ( ; x1 <= x2; x1++ ) { x = (x1+map_size) % map_size; y = (y1+map_size) % map_size; if(x == robotx && y == roboty) { fprintf( outfile, "RR" ); } else if(map[x][y] > 0 ) { fprintf( outfile, "%2.2d", map[x][y] ); } else { fprintf(outfile, ".."); } } fprintf(outfile, "\n"); } fprintf(outfile, "\n\n"); fclose(outfile); }