maze-solving.pde

/*===============================================
 *                    ÀSTERIX
 *===============================================
 * This code intends to emulate a robot in a maze enviroment. This is just a
 * beta code and will be soon ported to Arduino in order to have its algorithm
 * used in a Rescue B robot.
 *
 * Rescue B Team (Raptor B): Italo Lelis de Carvalho, João Pedro Rosada Canesin
 * Instructor: Igor Araujo Dias Santos
 * Saturday. May 9th, 2015
 */

final int MAZE_X = 31; //Maze size X //Rows
final int MAZE_Y = 31; //Maze size Y //Columns

char[][] maze = {{'#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#'},
                 {'#','.','.','.','.','.','.','.','.','.','.','.','.','.','#','.','.','.','.','.','.','.','#','.','.','.','.','.','.','.','#'},
                 {'#','.','#','#','#','#','#','#','#','.','#','#','#','.','#','#','#','#','#','.','#','.','#','#','#','.','#','#','#','.','#'},
                 {'#','S','.','.','.','.','.','.','#','.','.','.','#','.','.','.','.','.','.','.','#','.','.','.','#','.','#','.','.','.','#'},
                 {'#','#','#','#','#','#','#','.','#','.','#','.','#','#','#','#','#','#','#','#','#','#','#','.','#','#','#','.','#','.','#'},
                 {'#','.','.','.','#','.','.','.','#','.','#','.','#','.','.','.','.','.','#','.','.','.','#','.','#','.','.','.','#','.','#'},
                 {'#','.','#','.','#','.','.','.','#','#','#','.','#','.','#','#','#','.','#','.','#','.','#','.','#','.','#','#','#','.','#'},
                 {'#','.','#','.','#','.','.','.','.','.','.','.','#','.','.','.','#','.','.','.','#','.','#','.','.','.','#','.','#','.','#'},
                 {'#','.','#','.','#','.','.','.','#','.','#','#','#','#','#','.','#','#','#','#','#','.','#','#','#','#','#','.','#','.','#'},
                 {'#','.','#','.','#','.','.','.','#','.','#','.','.','.','#','.','.','.','.','.','#','.','.','.','.','.','.','.','#','.','#'},
                 {'#','.','#','#','#','#','#','.','#','.','#','.','#','.','#','#','#','#','#','.','#','#','#','#','#','#','#','.','#','.','#'},
                 {'#','.','.','.','#','.','.','.','#','.','#','.','#','.','.','.','.','.','#','.','.','.','#','.','#','.','.','.','#','.','#'},
                 {'#','.','#','.','#','.','#','#','#','.','#','.','#','#','#','#','#','.','#','#','#','.','#','.','#','.','#','.','#','.','#'},
                 {'#','.','#','.','#','.','.','.','#','.','.','.','#','.','.','.','#','.','.','.','#','.','.','.','#','.','#','.','#','.','#'},
                 {'#','#','#','.','#','#','#','.','#','.','#','#','#','.','#','.','#','#','#','.','#','#','#','.','#','.','#','#','#','.','#'},
                 {'#','.','.','.','#','.','#','.','#','.','#','.','#','.','#','.','.','.','#','.','.','.','#','.','#','.','.','.','.','.','#'},
                 {'#','.','#','.','#','.','#','.','#','.','#','.','.','.','#','.','#','.','#','#','#','.','#','.','#','#','#','#','#','#','#'},
                 {'#','.','#','.','.','.','#','.','#','.','#','.','.','.','#','.','#','.','#','.','#','.','#','.','.','.','.','.','.','.','#'},
                 {'#','.','#','#','#','#','#','.','#','.','#','#','#','#','#','.','#','.','#','.','#','.','#','#','#','#','#','#','#','.','#'},
                 {'#','.','.','.','.','.','#','.','#','.','.','.','.','.','.','.','#','.','.','.','#','.','.','.','.','.','.','.','#','.','#'},
                 {'#','.','#','#','#','.','#','.','#','.','#','#','#','#','#','.','#','#','#','#','#','#','#','#','#','#','#','.','#','.','#'},
                 {'#','.','#','.','#','.','.','.','#','.','#','.','.','.','.','.','#','.','.','.','.','.','.','.','#','.','.','.','#','.','#'},
                 {'#','.','#','.','#','#','#','#','#','.','#','.','#','#','#','#','#','.','#','#','#','#','#','.','#','.','#','#','#','#','#'},
                 {'#','.','.','.','#','.','.','.','.','.','.','.','.','.','.','.','#','.','#','.','#','.','.','.','#','.','#','.','.','.','#'},
                 {'#','#','#','.','#','#','#','.','#','#','#','#','#','#','#','.','#','.','#','.','#','.','#','#','#','.','#','.','#','.','#'},
                 {'#','.','#','.','.','.','#','.','.','.','#','.','.','.','#','.','#','.','#','.','#','.','#','.','.','.','.','.','#','.','#'},
                 {'#','.','#','#','#','.','#','#','#','.','#','.','#','.','#','.','#','.','#','.','#','.','#','#','#','#','#','#','#','.','#'},
                 {'#','.','.','.','.','.','#','.','.','.','#','.','#','.','#','.','#','.','#','.','.','.','#','.','.','.','#','.','#','.','#'},
                 {'#','.','#','#','#','#','#','#','#','#','#','.','#','.','#','#','#','.','#','.','#','#','#','.','#','.','#','.','#','.','#'},
                 {'#','.','.','.','.','.','.','.','.','.','.','.','#','.','.','.','.','.','#','.','.','.','.','.','#','.','.','.','#','E','#'},
                 {'#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#','#'},};
                 //Create array for Maze //# - Walls; . - Open; S - Start; E - End; R - Robot

/*char[][] maze = {{'#','#','#','#','#','#','#'},
                 {'#','S','#','.','#','.','#'},
                 {'#','.','#','.','#','.','#'},
                 {'#','.','.','.','.','.','#'},
                 {'#','.','#','.','#','.','#'},
                 {'#','.','.','#','.','E','#'},
                 {'#','#','#','#','#','#','#'},};*/ //Create array for Maze //# - Walls; . - Open; S - Start; E - End; R - Robot

char[][] last_dir = new char[MAZE_Y][MAZE_X];

int[][] dist_to_start = new int[MAZE_Y][MAZE_X]; //Create array for Dist to Start
int[][] dist_to_end = new int[MAZE_Y][MAZE_X]; //Create array for Dist to End
int[][] way_traveled = new int[MAZE_Y][MAZE_X]; //Create array for Way travelled

int start_pos_X=2 -1, start_pos_Y=4 -1; //Start position of Maze //x=row, y=column
int end_pos_X=30 -1, end_pos_Y=30 -1; //Final position of Maze //x=row, y=column

int current_pos_X=start_pos_X, current_pos_Y=start_pos_Y; //Current position of Robot //x=row, y=column
boolean empty = false, start_delete = false; //Finish algorithm to find and delete endless route; Avoid algorithm to delete first square

void setup ()
{
  //Reset all values//
  for(int i=0; i<MAZE_Y; i++){
    for(int j=0; j<MAZE_X; j++){
      dist_to_start[i][j]=0;
      dist_to_end[i][j]=0;
      way_traveled[i][j]=0;
      last_dir[i][j]='X';
    }
  }
  
  //print_maze(); stop();
  
  calculate_return_end(); //Calculate distance to the end
  calculate_return_start(); //Calculate distance to the start
  
  //Algorithm to delete endless routes [on startup]//
  do{
    //print_maze();
    empty = false;
    calculate_empty_places();
  }while(empty == true);
  
  print_maze_values(); //Print logs of the maze
  //stop();
}

/////STOP CODE FOREVER/////
/*void stop () {
  while (true) {}
}*/

void draw ()
{
  //EXECUTE MAIN ALGORITHM//
  //The virtual robot will find the closest
  //way to the end of the maze
  do{
    asterix();
    delay(250);
  }while(current_pos_X != end_pos_X || current_pos_Y != end_pos_Y);
  
  print_maze(); println("\n\n\n");
  
  print_last_dir();
  
  print("\n The robot reached the finish!\n\tEND");
  exit(); //Close application
}

//Sometimes I believe compiler ignores all my comments