commit 2fd5122af2833aeb70eec7f91f9233d6a74653d5
Author: viki <zeh@andrew.cmu.edu>
Date:   Thu Feb 28 16:49:35 2013 -0500

    Add smart runaround behaviors.

diff --git a/scout/libscout/src/behaviors/smart_runaround.cpp b/scout/libscout/src/behaviors/smart_runaround.cpp
new file mode 100644
index 0000000..b437451
--- /dev/null
+++ b/scout/libscout/src/behaviors/smart_runaround.cpp
@@ -0,0 +1,458 @@
+/**
+ * Copyright (c) 2011 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include "maze_solve.h"
+
+using namespace std;
+
+
+// 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 on map, NOT on robot
+#define UP 0
+#define RIGHT 1
+#define DOWN 2
+#define LEFT 3
+// number of pixels on one row/column of map block
+#define BLOCK_LENGTH 13
+
+// robot control variables
+#define BASESPEED 50
+
+// robots row and column on map, starting at center
+int row = MAP_LENGTH/2+1; // add 1 because we know MAP_LENGTH is odd
+int col = MAP_LENGTH/2+1;
+
+// utility functions
+// pixels to meters
+float pix_to_m(int pixels)
+{
+    return pixels/200.0;
+}
+
+// meters to pixels
+int m_to_idx(float meters)
+{
+    float pixels = meters*200.0;
+    float idx = pixels/BLOCK_LENGTH;
+    return floor(idx+0.5);
+}
+
+/* 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 maze_solve::choose_direc(int row, int col, int info)
+{
+    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;
+}
+
+Duration sonar_update_time(1.5);
+
+// THIS VERSION MODIFIED BY ZANE
+void maze_solve::run(){
+    
+    ROS_INFO("Starting to solve the maze");
+    // Go up to the first line.
+    //follow_line();
+    // Turn the sonar on.
+    sonar->set_on();
+    sonar->set_range(0, 23);
+    // initialize map to all UNSEEN
+    for(int r = 0; r < MAP_LENGTH; r++) {
+	for(int c = 0; c < MAP_LENGTH; c++)
+	    map[r][c] = UNSEEN;
+    }
+
+    // Wait for the sonar to initialize.
+    while(!look_around(25, 25, RIGHT) && ok())
+    {
+      spinOnce();
+    }
+
+    /* Assumptions:
+     * Grid has a fixed size
+     * Start location unknown
+     * When robot moves forward, it moves to exact center
+     * of the block in front.
+     */
+
+
+    int dir = RIGHT; // current direction
+    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);
+	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.
+    if (success)
+        ROS_INFO("YAY! I have solved the maze");
+    else
+        ROS_INFO("NO! The maze is unsolvable");
+}
+
+// NOT CURRENTLY USED!!!
+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);
+        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,
+ * and turns the scout to the intended direction
+ */
+void maze_solve::turn_from_to(int current_dir, int intended_dir) {
+    switch ((4 + intended_dir - current_dir) % 4) // TODO: Try without "4 +" at start
+    {
+        case 0:
+	    turn_straight(intended_dir);
+            break;
+        case 1:
+            turn_right();
+            break;
+        case 2:
+            spot_turn();
+            break;
+        case 3:
+            turn_left();
+            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();
+
+    // Assumption: readings are given in millimeters - Zane
+
+    // distances with respect to robot, NOT map
+    // Look to the left.
+    float left_distance = readings[0]/1000.0;
+    int left_idx = m_to_idx(left_distance);
+    // Look to the front.
+    float front_distance = readings[36]/1000.0;
+    int front_idx = m_to_idx(front_distance);
+    // Look to the right.
+    float right_distance = readings[24]/1000.0;
+    int right_idx = m_to_idx(right_distance);
+
+    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;
+
+    // determine relative distances on map, based on robot position
+    int up_d, right_d, down_d, left_d;
+    // determine upward distance
+    switch (dir)
+    {
+	case UP:
+	    up_d = front_idx;
+	    right_d = right_idx;
+	    down_d = 0; // unknown
+	    left_d = left_idx;
+	    break;
+	case RIGHT:
+	    up_d = left_idx;
+	    right_d = front_idx;
+	    down_d = right_idx;
+	    left_d = 0; // unknown
+	    break;
+	case DOWN:
+	    up_d = 0; // unknown
+	    right_d = left_idx;
+	    down_d = front_idx;
+	    left_d = right_idx;
+	    break;
+	case LEFT:
+	    up_d = right_idx;
+	    right_d = 0; // unknown
+	    down_d = left_idx;
+	    left_d = front_idx;
+	    break;
+    }
+
+    // change map until wall index, or until reading < 500
+    // reading < 500 <=> left_idx < 8 (approx.)
+
+    // map blocks above robot (on map)
+    for(int u = 0; u < 8; u++)
+    {
+	if(u = up_d) {
+	    map[row-u][col] = (up_d)?WALL:SEEN;
+            break;
+	}
+	map[row-u][col] = SEEN;
+    }
+
+    // map blocks to right of robot
+    for(int r = 0; r < 8; r++)
+    {
+	if(r = right_d) {
+	    map[row][col+r] = (right_d)?WALL:SEEN;
+            break;
+	}
+	map[row][col+r] = SEEN;
+    }
+
+    // map blocks under robot (on map)
+    for(int d = 0; d < 8; d++)
+    {
+	if(d = down_d) {
+	    map[row+d][col] = (down_d)?WALL:SEEN;
+            break;
+	}
+	map[row+d][col] = SEEN;
+    }
+
+    // map blocks to left of robot
+    for(int l = 0; l < 8; l++)
+    {
+	if(l = left_d) {
+	    map[row][col-l] = (left_d)?WALL:SEEN;
+            break;
+	}
+	map[row][col-l] = SEEN;
+    }
+
+    return true;
+}
+
+// TODO: test this function
+// return true iff the map has been completely explored
+// and only true if it "returned" to home destination after solving
+bool maze_solve::at_destination() 
+{
+    // check whether there is a square portion with
+    // only SEEN or WALL blocks, of the size of the map.
+    int start_row = -1;
+    int start_col = -1;
+    int seen_width = 0;
+    int seen_height = 0;
+    for(int r = 0; r < MAP_LENGTH; r++) {
+	for(int c = 0; c < MAP_LENGTH; c++) {
+	    if(map[r][c] == UNSEEN) {
+		start_row = -1;
+		start_col = -1;
+		seen_width = 0;
+		seen_height = 0;
+	    }
+	    else {
+		start_row = r;
+		start_col = c;
+		seen_width++;
+		seen_height++;
+	    }
+	    if(seen_width >= 15)
+		return true;
+	}
+    }
+    return false;
+}
+
+// TODO: test these functions to make sure robot moves well
+// move forward one block in direction "dir"
+void maze_solve::turn_straight(int dir)
+{
+    // TODO: try to use motor encoder values to move forward enough
+    motors->set_sides(BASESPEED, BASESPEED, MOTOR_ABSOLUTE);
+    spinOnce();
+    Duration moveTime(1.0);
+    moveTime.sleep();
+
+    // update robot's position on map
+    switch(dir)
+    {
+	case UP:
+	  row--;
+	  break;
+	case RIGHT:
+	  col++;
+	  break;
+	case DOWN:
+	  row++;
+	  break;
+	case LEFT:
+	  col--;
+	  break;
+    }
+}
+
+// turn clockwise (right)
+void maze_solve::turn_right()
+{
+    motors->set_sides(BASESPEED, -BASESPEED, MOTOR_ABSOLUTE);
+    spinOnce();
+    Duration moveTime(3.3);
+    moveTime.sleep();
+}
+
+// do a 180 deg turn, NOT a barrel roll
+void maze_solve::spot_turn()
+{
+    motors->set_sides(BASESPEED, -BASESPEED, MOTOR_ABSOLUTE);
+    spinOnce();
+    Duration moveTime(6.6);
+    moveTime.sleep();
+}
+
+// turn counter-clockwise (left)
+void maze_solve::turn_left()
+{
+    motors->set_sides(-BASESPEED, BASESPEED, MOTOR_ABSOLUTE);
+    spinOnce();
+    Duration moveTime(3.3);
+    moveTime.sleep();
+}
diff --git a/scout/libscout/src/behaviors/smart_runaround.h b/scout/libscout/src/behaviors/smart_runaround.h
new file mode 100644
index 0000000..33f934b
--- /dev/null
+++ b/scout/libscout/src/behaviors/smart_runaround.h
@@ -0,0 +1,67 @@
+/**
+ * Copyright (c) 2011 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef _MAZE_SOLVE_H_
+#define _MAZE_SOLVE_H_
+
+#include "line_follow.h"
+/* Details about map:
+ * 1 meter = 200 pixels
+ * 1 block is a 13x13 pixel section
+ * actual meter x meter map has 15 blocks in one row/column
+ * # blocks per robot map row = 2*(block per map's row)-1
+ */
+
+// number of blocks in one row/column of robot's map
+#define MAP_LENGTH 29
+
+class maze_solve: public line_follow
+{
+    public:
+        maze_solve(std::string scoutname, Sensors* sensors):
+            line_follow(scoutname, "maze_solve", sensors) {};
+        void run();
+    private:
+	int choose_direc(int row, int col, int info);
+        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);
+        bool at_destination();
+	// functions for changing direction or moving
+        void turn_straight(int dir);
+        void turn_right();
+        void spot_turn();
+	void turn_left();
+
+	/* Initialize map so that boundaries MAP_LENGTH away
+	 * from the center. Then car won't go outside of
+	 * map even if it starts at a boundary and goes to
+	 * the other side.
+         * Rows top to bottom, and columns left to right
+	*/
+        int map[MAP_LENGTH][MAP_LENGTH];
+};
+#endif
+
