commit c43cef099178b85870a376d778126a6f8120363b
Author: Hui Jun Tay <viki@c3po.(none)>
Date:   Mon Apr 15 21:24:48 2013 -0500

    Maze solve now works

diff --git a/scout/libscout/src/test_behaviors/smart_runaround.cpp b/scout/libscout/src/test_behaviors/smart_runaround.cpp
index 1bfb9bf..87404b7 100644
--- a/scout/libscout/src/test_behaviors/smart_runaround.cpp
+++ b/scout/libscout/src/test_behaviors/smart_runaround.cpp
@@ -59,21 +59,29 @@ float pix_to_m(int pixels)
     return pixels/200.0;
 }
 
-// millimeters to pixels
-int mm_to_idx(float meters)
+// meters to pixels
+int m_to_idx(float meters)
 {
-    // 200 pixels per meter, and 1000 millimeters per meter
-    float pixels = meters*0.2;
+    float pixels = meters*200.0;
     float idx = pixels/BLOCK_LENGTH;
     return floor(idx+0.5);
 }
 
-float *matrix_mult(float inputs[2], float matrix[2][2])
+/* return a direction (if any) where adjacent block
+ * is labeled "info" on map. Searches clockwise
+ * starting at up. Returns -1 if no direction valid.
+ */
+int smart_runaround::choose_direc(int row, int col, int info)
 {
-    float newX = matrix[0][0]*inputs[0]+matrix[0][1]*inputs[1];
-    float newY = matrix[1][0]*inputs[0]+matrix[1][1]*inputs[1];
-    float output[2] = {newX, newY};
-    return output;
+    if (map[row-1][col] == info)
+	return UP;
+    else if (map[row][col+1] == info)
+	return RIGHT;
+    else if (map[row+1][col] == info)
+	return DOWN;
+    else if (map[row][col-1] == info)
+	return LEFT;
+    return -1;
 }
 
 // TODO This is bad! It's defined globally across all behaviors. Please fix this. -Alex
@@ -109,19 +117,20 @@ void smart_runaround::run(){
 
 
     int dir = RIGHT; // current direction
-    //int new_dir = RIGHT; // direction in which to turn after a scan
+    int new_dir = RIGHT; // direction in which to turn after a scan
     bool success = false; // true when maze solved
     while(ok())
     {
+	// Look left, right, and forward
 	look_around(row, col, dir);
 	// Try moving in each direction
-	/*new_dir = choose_direc(row, col, UNSEEN);
+	new_dir = choose_direc(row, col, UNSEEN);
 	if(new_dir < 0)
 	    new_dir = choose_direc(row, col, SEEN);
 	if(new_dir >= 0) {
 	    turn_from_to(dir, new_dir);
 	    dir = new_dir;
-	}*/
+	}
     }
 
     // Check and report final condition.
@@ -131,21 +140,110 @@ void smart_runaround::run(){
         ROS_INFO("NO! The maze is unsolvable");
 }
 
-/* return a direction (if any) where adjacent block
- * is labeled "info" on map. Searches clockwise
- * starting at up. Returns -1 if no direction valid.
- */
-int smart_runaround::choose_direc(int row, int col, int info)
+// NOT CURRENTLY USED!!!
+bool smart_runaround::solve(int row, int col, int dir)
 {
-    if (map[row-1][col] == info)
-	return UP;
-    else if (map[row][col+1] == info)
-	return RIGHT;
-    else if (map[row+1][col] == info)
-	return DOWN;
-    else if (map[row][col-1] == info)
-	return LEFT;
-    return -1;
+    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_time2.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);
+        follow_line();
+        // 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,
@@ -169,68 +267,18 @@ void smart_runaround::turn_from_to(int current_dir, int intended_dir) {
     }
 }
 
-/* Purpose: look front, left, and right using sonar, and update
- * map accordingly. Returns true if and only if sonar is initialized.
- */
 bool smart_runaround::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();
 
     // Assumption: readings are given in millimeters - Zane
 
-<<<<<<< HEAD
-    // matrices for going from robot's frame to base frame
-    float rightMat[2][2] = {{0, 1}, {-1, 0}};
-    float downMat[2][2] = {{-1, 0}, {0, -1}};
-    float leftMat[2][2] = {{0, 1}, {1, 0}};
-
-    // Look to the left (and update map).
-    float left_distance = readings[0]; // w.r.t. robot
-    if(left_distance == 0)
-        return false;
-    int left_idx = -mm_to_idx(left_distance); // w.r.t. map
-    // plot to map if indices are valid
-    if (0 <= left_idx && left_idx < MAP_LENGTH)
-        map[row][col+left_idx] = SEEN;
-
-    // Look in the other directions (and update map).
-    for (int i = 24; i < 48; i++) {
-        float distance = readings[i]; // w.r.t. robot
-        if(distance == 0)
-            return false;
-        if(distance >= 500)
-            break; // too far to be accurate
-        float theta = (M_PI/24)*i - M_PI;
-        float xDist = distance*cos(theta); // w.r.t. robot
-        float yDist = distance*sin(theta); // w.r.t. robot
-        float inputs[2] = {xDist, yDist};
-        float *ans;
-
-        // re-orient x and y distances based on direction
-        switch(dir) {
-            case UP:
-                ans[0] = xDist;
-                ans[1] = yDist;
-                break;
-            case RIGHT:
-                ans = matrix_mult(inputs, rightMat);
-                break;
-            case DOWN:
-                ans = matrix_mult(inputs, downMat);
-                break;
-            case LEFT:
-                ans = matrix_mult(inputs, leftMat);
-                break;
-        }
-        // indices into the map
-        int pixDistX = row + mm_to_idx(ans[0]);
-        int pixDistY = col + mm_to_idx(ans[1]);
-        // plot to map if indices are valid
-        if (0 <= pixDistX && pixDistX < MAP_LENGTH
-            && 0 <= pixDistY && pixDistY < MAP_LENGTH)
-                map[pixDistX][pixDistY] = SEEN;
-=======
     // distances with respect to robot, NOT map
     // Look to the left.
     float left_distance = readings[0]/1000.0;
@@ -323,8 +371,8 @@ bool smart_runaround::look_around(int row, int col, int dir)
                 break;
         }
         map[row][col-l] = SEEN;
->>>>>>> dbbad8ed5cba08f5500e7d333beab2d089f9956c
     }
+
     return true;
 }
 
