Milestone 3: Robot Detection and Maze Navigation

Detecting other Robots

To detect other robots, we implemented an IR sensing circuit that uses a phototransistor's amplified output to determine the proximity of another robot emitting infrared light. There is a flag called "pt" to determine if the phototransistor output is a 0 or 1, and if it is 1, then a robot has been detected and our robot makes a 180 degree turn.

The command to turn 180 degrees upon detection of a robot can be seen in the code below.

if(pt == 1) {
   turn(TURN_RIGHT, DEG_180, left_line_val, mid_line_val, right_line_val);
}

The video below shows a demonstration of our robot detecting another robot. We used a stationary IR LED powered by a 9V battery instead of another robot.

Maze Navigation

The maze mapping algorithm uses an iterative depth first search process, continuously selecting to visit nodes that haven't been visited, before reaching a logical or physical dead end and retracing its steps until it finds another path to travel that explores more unvisited nodes. The way we implement this is by defining an 2-dimensional mxn array of structs containing node information such as our backtrack orientation, whether the node's neighbors have been visited and whether the node itself has been visited. Globally, we also keep track of the robot's orientation and position within the maze and the total number of nodes we still haven't visited. We continue exploring the maze until that global number of nodes is 0, at which point we stop and turn the green LED on. The orientation and position of the robot help us make decisions about turning and ensuring we know how to retrace our steps effectively. With this method, we guarantee that each path is only explored once (so no repeats), but we don't necessarily take the shortest path through the maze.

The code below shows how the maze mapping algorithm is implemented with the where_to_go() function. The definitions for the node struct and orientation and direction lookup tables are also shown below.

void where_to_go() {
  // print_robot_coordinates();
  if (maze[robot_x][robot_y].visited == false){
    update_node();
  }

  // stop at end of maze
  // no more nodes are unvisited
  if (nodes_to_visit == 0) { 
    while(true) {
      servo_L.write(STOPPED);
      servo_R.write(STOPPED);
      digitalWrite(LED_GREEN, HIGH);
      digitalWrite(LED_BLUE, LOW);
      digitalWrite(LED_RED, LOW);
    }
  }

  // set default orientation to backtrack in case this node
  // has no neighbors that haven't been visited
  ORIENTATION path_or = maze[robot_x][robot_y].or_backtrack;
  // check neighbors array
  for(int o = OR_NORTH; o < OR_NUM; o++) 
  {
    if (maze[robot_x][robot_y].neighbor[o] == false){
      // neighbor in orientation o hasn't been visited, we should go there
      path_or = (ORIENTATION) o;
      break;
    }
  }

  // path_or is the desired orientation to travel in
  DIRECTION dir = or_or[robot_orientation][path_or];

  // print which direction we are going
  // Serial.print("going in orientation ");
  // Serial.print(path_or);
  // Serial.print("\t we are turning ");
  // Serial.println(dir);
   
  // call turning function based off direction
  switch (dir) {
    case DIR_LEFT: 
      turn(DIR_LEFT, DEG_90, left_line_val, mid_line_val, right_line_val);
      turn_executed = true;
      break; 
    case DIR_FORWARD:
      // do nothing, line following should take us to next intersection
      break;
    case DIR_RIGHT:
      turn(DIR_RIGHT, DEG_90, left_line_val, mid_line_val, right_line_val);
      turn_executed = true;
      break;
    case DIR_BACKWARD:
      turn(DIR_LEFT, DEG_180, left_line_val, mid_line_val, right_line_val);
      turn_executed = true;
      break;
    default:
      Serial.println("DIRECTION OUT OF BOUNDS");
  }
}

Below is the struct definition for a node.

typedef struct{
  bool neighbor[OR_NUM] = {0, 0, 0, 0}; // this array keeps track of whether or not a neighbor has been visited, one entry for each possible neighbor orientation
  ORIENTATION or_backtrack;              // if we have no where else to go, we bail this way
  bool visited = false;                  // have we been to this node before??
} node_t;

Below are the definitions for the lookup tables used to determine orientation and direction.

ORIENTATION or_dir[OR_NUM][DIR_NUM] = {
  //                        L         F         R         B
  /* NORTH ORIENTATION */  {OR_WEST,  OR_NORTH, OR_EAST,  OR_SOUTH},
  /* EAST ORIENTATION  */  {OR_NORTH, OR_EAST,  OR_SOUTH, OR_WEST} ,
  /* SOUTH ORIENTATION */  {OR_EAST,  OR_SOUTH, OR_WEST,  OR_NORTH},
  /* WEST ORIENTATION  */  {OR_SOUTH, OR_WEST,  OR_NORTH, OR_EAST} ,
};

// LUT of sorts to figure out which direction to turn based on current vs. desired orientation
DIRECTION or_or[OR_NUM][OR_NUM] = {
  //                         N            E             S              W
  /* NORTH ORIENTATION */  {DIR_FORWARD,  DIR_RIGHT,    DIR_BACKWARD,  DIR_LEFT},
  /* EAST ORIENTATION  */  {DIR_LEFT,     DIR_FORWARD,  DIR_RIGHT,     DIR_BACKWARD} ,
  /* SOUTH ORIENTATION */  {DIR_BACKWARD, DIR_LEFT,     DIR_FORWARD,   DIR_RIGHT},
  /* WEST ORIENTATION  */  {DIR_RIGHT,    DIR_BACKWARD, DIR_LEFT,      DIR_FORWARD} ,
};

The videos below show the robot implementing maze mapping on multiple mazes.

5 x 6 Grid

3 x 3 Grid