OS Project of Fall 2017: "Cartography"

Upon receiving the start signal, the robot starts by creating a thread that will send the position to the server every 2 second through the use of pthread_create(&positioning, NULL, thSendPosition, NULL) and a second thread to receive any secial instructions sent from the server : pthread_create(&receiving, NULL, thReceiveFromServer, NULL)
Then, the initial map is generated! char* map = get_new_local_map(width, height); The map's width and height is given as arguments when launching the program.
Each character represents a tile -- or pixel -- for wether it is a free tile, a wall or a yet unknown space in the arena.

/* Code for the rest of the program on the large arena */
// position the robot at one extremity of the map
start_position = tCoord_new(width/2, height - 300*MM_TO_PIX_SIZE); //This automaticaly creates a virtual wall behind the robot

// scan a first time to get the surroundings :
scan(robotPosition, start_position, width, height, map);
tCoord previousSpots[4*width*height/(DIST_MIN_FROM_WALLS*DIST_MIN_FROM_WALLS)]; //A list of all the spots ou robot is going to go
size_t size_previousSpots = 0;
while(!mapComplete(map))
{   // determine a new spot in order to maximise the new area to explore
    tCoord spot = getNewSpot(Point_to_tCoord(fPoint_to_Point(robotPosition), start_position),
                             width,
                             height,
                             map);
    previousSpots[size_previousSpots++] = spot;
    if(getPathTo(spot))  // writes into a file the checkpoints we have to go by in order to avoid obstacles
    {
        printf("failed to find the path to the new coordinates");
        exit(EXIT_FAILURE);
    }
    FILE *path = fopen(PATH_PATH, "r"); //PATH_PATH is the path to the fie containing the checkpoints
    int pathLen = countlines(path);
    fclose(path);
    int i;
    for(i=0; i<pathLen; i++)
    {
        tCoord checkPoint = getCheckpoint(i, pathLen); //pops the next node from the file.
        if(moveTo(checkPoint, start_position, map))
        {
            printf("failed to move to the next coordinates");
            exit(EXIT_FAILURE);
        }
    }
    scan(robotPosition, start_position, width, height, map); //finally, when we get to the right spot, scan and start over
}
DONE_EXPLORING = 1; //signals the threads to stop
sendMap();

Here we show the core functions that makes the robot do it's magic.

int refresh_distance() refreshes the global variable distance with the output of the sonar.

int refresh_angle() refreshes the global variable angle with the output of the gyroscope. In order to increase presision, angle takes the average value of several consecutive measures.

int init_mov_motors() loads the pointers to use the motors into some global variables.

void start_straight(int speed_increment) makes the robot go forward or backward, depending on the sign of the increment.

void start_turn(int speed_increment) makes the robot turn on itself one way or another, depending on the sign of the increment.

void stop_mov_motors() stops the robot.

int turn_to_angle(float ang) makes the robot turn until angle == ang (obviously we don't really test if those floats are equal).

void continue_until(float goal) robot advances until distance < goal

int grab(int instruct) uses the front mechanism in one way or the other depending on the instruction.

During the scan, the robot turns on itself and continuously asks for the output of the gyroscope and sonar. Then it modifies the map according to some rules :

• Do not scan further than the distance SCANNING_MAX_DISTANCE
• If the distance measured from sonar is under that specific value, then assign a wall to the measured position on the map
• For the first position you measure, mark all of the pixels between that position and the robot as free
• For the following measures, consider the triangle defined by the previous point, the current measured point and the position of the robot. Mark all the tiles inside it as free
• Do not overwrite known pixels if they are outside your precision radius

For the sake of comprehension, the next snippet of code is a simplified (yet functional) version of the scan function. It does not necessarily respond to all of the previous criterias

int scan(tCoord robot_coord_on_map, char* map, int width_n, int height)
{
    Point O = Point_new(0,0);
    int mm_to_pixel_size = 10*PIXEL_SIZE; //can convert a size in mm to the length of a pixel
    float distance_before_setting_a_new_wall = SCANNING_MAX_DISTANCE - .4; //-.4 because of rounding errors
    Point measured_point; //coordinates of the tile/pixel the sonar hits into.
    // last_point will take the value of previously measured point.
    Point last_point =  Point_new(0,0); //initialised like O so as to know if nothing has been measured before

    refresh_angle(); //refreshes the global variable angle
    float starting_scan_angle = angle;
    start_turn(SCANNING_SPEED); //The robot starts turning
    while(abs(starting_scan_angle-angle)<FULL_TURN_ANGLE)
    {
        refresh_angle();
        refresh_distance();
        distance = min(SCANNING_MAX_DISTANCE, distance);
        fPoint fmeasured_point = fPoint_new(distance*cos(2*pi*angle/FULL_TURN_ANGLE)/mm_to_pixel_size,
                                            distance*sin(2*pi*angle/FULL_TURN_ANGLE)/mm_to_pixel_size);
        measured_point = fPoint_to_Point(fmeasured_point); //Converts a point's coordinates from floats to integers.                                     '
        tCoord coord = Point_to_tCoord(measured_point, robot_coord_on_map);
        if( Point_eq(last_point,O) )
        {
            free_pixels_between(O, measured_point, map);
        } else
        {
            free_pixels_in_trigon(O,last_point, measured_point, map);
        }
        // And set the furthest pixel to become a wall if appropriate.
        if(distance < distance_before_setting_a_new_wall)
        {
            set_char(coord,width_n,height,WALL_PIXEL,map);
        }
        last_point = measured_point;
    }
    stop_mov_motors(); //stops the robot from turning.
    return 0;
}

Between two rounds of scanning, the robot needs to get to it's next viewpoint, preferably without hitting anything. This is why we need to find the right path using a "greedy" version of the Dijkstra algorithm (some people might say that we cut corners...)

To communicate with the game server, we have implemented the protocol described in the project repository. We used threads to let the robot listen for server messages and periodically send its position.

ssize_t read_from_server(int sock, char *msg, size_t maxSize) {
    printf("meh\n");
    ssize_t bytes_read = read(sock, msg, maxSize);
    printf("meh?\n");
    if (bytes_read <= 0) {
        fprintf(stderr, "Server unexpectedly closed connection...\n");
        close(sock);
        exit(EXIT_FAILURE);
    }
    printf("[DEBUG] received %d bytes\n", bytes_read);
    return bytes_read;
}

int send_to_server(int sock, char *data, size_t size) {
    return write(sock, data, size);
}

int send_POSITION(int sock, int16_t x, int16_t y){
    char string[9];
    *((uint16_t *) string) = msgId++;
    string[2] = TEAM_ID;
    string[3] = 0xFF;
    string[4] = MSG_POSITION;
    string[5] = x;
    string[6] = 0x00;
    string[7] = y;
    string[8] = 0x00;
    return send_to_server(sock, string, 9);
}

int send_MAPDATA(int sock, int16_t x, int16_t y, uint8_t R, uint8_t G, uint8_t B){
    char string[12];
    *((uint16_t *) string) = msgId++;
    string[2] = TEAM_ID;
    string[3] = 0xFF;
    string[4] = MSG_MAPDATA;
    string[5] = x;
    string[6] = 0x00;
    string[7] = y;
    string[8] = 0x00;
    string[9] = R; // red value of pixel
    string[10] = G; // green value of pixel
    string[11] = B; // blue value of pixel
    return send_to_server(sock, string, 12);
}

int send_MAPDONE(int sock) {
    char string[5];
    *((uint16_t *) string) = msgId++;
    string[2] = TEAM_ID;
    string[3] = 0xFF;
    string[4] = MSG_MAPDONE;
    return send_to_server(sock, string, 5);
}

void *thSendPosition() {
    while(!DONE_EXPLORING) {
        Sleep(2000);
        setPosition();
        send_POSITION(s, posX, posY);
    }
    pthread_exit(NULL);
}