Colony » Colony Scout

// want to have a minimal working thing, use a big enough 

// static array and start in the middle

// we assume we are facing right, that affects where we store

// wall information

// -1 for wall, 0 for unseen, 1 for traveled, 2 for critical

#define WALL -1

#define UNSEEN 0

#define SEEN 1

#define CRITICAL 2

// facings

#define UP 0

#define RIGHT 1

#define DOWN 2

#define LEFT 3

// TODO This is bad! It's defined globally across all files. Please put it inside a good scope. -Alex

Duration sonar_update_time(1.5);

    // TODO:first initialize map to all 0's

    // Wait for the sonar to initialize.

    while(!look_around(25, 25, RIGHT) && ok())

    {

      spinOnce();      

    }

    // Solve the maze

    bool finished = solve(25,25, RIGHT);

    // Check and report final condition.

    if (finished)

        ROS_INFO("YAY! I have solved the maze");

    else

        ROS_INFO("NO! The maze is unsolvable");

}

bool maze_solve::solve(int row, int col, int dir)

{

    int initial_dir = dir;

    ROS_INFO("I am at direction %d", dir);

    // use backtracking to solve the maze

    if (at_destination())

        return true;

    // Wait for sonar to update.

    sonar_update_time.sleep();

    // this function should fill the adjacent cells around me with

    // wall's or paths

    while(!look_around(row, col, dir) && ok())

        spinOnce();

    }

    /* try go up */

    if (map[row-1][col] != WALL && initial_dir != UP)

    {

    ROS_INFO("GOING UP!");

        // Turn up.

        turn_from_to(dir, UP);

        follow_line();

        // Solve recursively.

        bool solved = solve(row-1, col, DOWN);

        if (solved)

            return solved;

        else

            //Update where we are.

            dir = UP;

    }

    /* try right */

    if (map[row][col+1] != WALL && initial_dir != RIGHT)

    {

    ROS_INFO("GOING RIGHT!");

        // Turn right.

        turn_from_to(dir, RIGHT);

        follow_line();

        // Solve recursively.

        bool solved = solve(row, col+1, LEFT);

        if (solved)

            return solved;

        else

            //Update where we are.

            dir = RIGHT;

    }

    /* try down */

    if (map[row+1][col] != WALL && initial_dir != DOWN)

    {

    ROS_INFO("GOING DOWN!");

        // Turn down.

        turn_from_to(dir, DOWN);

        follow_line();

        // Solve recursively.

        bool solved = solve(row+1, col, UP);

        if (solved)

            return solved;

        else

            //Update where we are.

            dir = DOWN;

    }

    /* try left */

    if (map[row][col-1] != WALL && initial_dir != LEFT)

    {

    ROS_INFO("GOING LEFT!");

        // Turn down.

        turn_from_to(dir, LEFT);

        // Solve recursively.

        bool solved = solve(row, col-1, RIGHT);

        if (solved)

        {

            return solved;

        }

        else

        {

            //Update where we are.

            dir = LEFT;

        }

    }

    ROS_INFO("DEAD END FOUND, TURNING BACK.");

    // we have exhausted all the options. This path is clearly a

    // dead end. go back to where we come from and return false.

    turn_from_to(dir, initial_dir);

    follow_line();

    return false;

}

// this function takes in the current direction and turns the scout

// into it intended direction

void maze_solve::turn_from_to(int current_dir, int intended_dir) {

    switch ((4 + intended_dir - current_dir) % 4) 

    {

        case 0:

            spot_turn();

            break;

        case 1:

            turn_left();

            break;

        case 2:

            turn_straight();

            break;

        case 3:

            turn_right();

            break;

bool maze_solve::look_around(int row, int col, int dir)

{

    // look around current place using sonar

    // store whether or not

    // there is a wall into the map

    // stores at row col 2 if point is critical, 1 otherwise

    int* readings = sonar->get_sonar_readings();

    spinOnce();

/*

    // Look to the left.

    int left_distance = (readings[1] + readings[0] + readings[47])/3;

    // Look to the front.

    int front_distance = (readings[35] + readings[36] + readings[37])/3;

    // Look to the right.

    int right_distance = (readings[23] + readings[24] + readings[25])/3;

*/

    // Look to the left.

    int left_distance = readings[0];

    // Look to the front.

    int front_distance = readings[36];

    // Look to the right.

    int right_distance = readings[24];

    ROS_INFO("front: %d  left: %d  right: %d", front_distance, left_distance, right_distance);

    if (right_distance == 0 || front_distance == 0 || left_distance == 0)

      return false;

    switch (dir)

    {

        case UP:

            // If the distance is less than 500, mark the area as a wall otherwise

            // mark it as seen.

            map[row][col+1] = (left_distance < 500)?WALL:SEEN;

            map[row+1][col] = (front_distance < 500)?WALL:SEEN;

            map[row][col-1] = (right_distance < 500)?WALL:SEEN;

            break;

        case RIGHT:

            // If the distance is less than 500, mark the area as a wall otherwise

            // mark it as seen.

            map[row+1][col] = (left_distance < 500)?WALL:SEEN;

            map[row][col-1] = (front_distance < 500)?WALL:SEEN;

            map[row-1][col] = (right_distance < 500)?WALL:SEEN;

            break;

        case DOWN:

            // If the distance is less than 500, mark the area as a wall otherwise

            // mark it as seen.

            map[row][col-1] = (left_distance < 500)?WALL:SEEN;

            map[row-1][col] = (front_distance < 500)?WALL:SEEN;

            map[row][col+1] = (right_distance < 500)?WALL:SEEN;

            break;

        case LEFT:

            // If the distance is less than 500, mark the area as a wall otherwise

            // mark it as seen.

            map[row-1][col] = (left_distance < 500)?WALL:SEEN;

            map[row][col+1] = (front_distance < 500)?WALL:SEEN;

            map[row+1][col] = (right_distance < 500)?WALL:SEEN;

            break;

    }

    return true;

}

        bool solve(int row, int col, int dir);

        void turn_from_to(int current_dir, int intended_dir);

        bool look_around(int row, int col, int dir);