commit afa9104d1edad3de88368ce4e2a93c7c566a172a
Author: Priya <pdeo@Phlegethon.(none)>
Date:   Sun May 6 19:33:07 2012 -0400

    Ok Scheduler warehouse behaviour working! and linefollowing works better now! Onto better demo.

diff --git a/scout/libscout/src/behaviors/WH_Robot.cpp b/scout/libscout/src/behaviors/WH_Robot.cpp
index 814aa0e..025c677 100644
--- a/scout/libscout/src/behaviors/WH_Robot.cpp
+++ b/scout/libscout/src/behaviors/WH_Robot.cpp
@@ -34,6 +34,7 @@ void WH_Robot::follow_path(Path path_to_follow){
     switch(t)
     {
       case ISTRAIGHT:
+        turn_straight();
         follow_line();
         break;
       case ILEFT:
diff --git a/scout/libscout/src/behaviors/line_follow.cpp b/scout/libscout/src/behaviors/line_follow.cpp
index f117dff..eeebb17 100644
--- a/scout/libscout/src/behaviors/line_follow.cpp
+++ b/scout/libscout/src/behaviors/line_follow.cpp
@@ -30,9 +30,11 @@ using namespace std;
 static int motor_l;
 static int motor_r;
 
+Duration init_turn_time(0.5);
+
 void line_follow::follow_line()
 {
-    while(!linesensor->fullline())
+    do
     {
         double line_loc = linesensor->readline();
 
@@ -41,13 +43,27 @@ void line_follow::follow_line()
 
         motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
     }
+    while(!linesensor->fullline());
     halt();
     ROS_INFO("Intersection reached!");
 }
 
+void line_follow::turn_straight()
+{
+  do
+  {
+    motor_l = -MOTOR_BASE;
+    motor_r = -MOTOR_BASE;
+
+    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
+  }
+  while(linesensor->fullline());
+}
+
 void line_follow::turn_left()
 {
   bool first = true;
+  float line_loc;
   do
   {
     motor_l = -MOTOR_BASE;
@@ -57,13 +73,13 @@ void line_follow::turn_left()
 
     if(first)
     {
-        loop_rate->sleep();
-        loop_rate->sleep();
-        loop_rate->sleep();
+        init_turn_time.sleep();
         first = false;
     }
+
+    line_loc = linesensor->readline();
   }
-  while(linesensor->readline());
+  while(!(-.5 < line_loc && line_loc < .5));
 }
 
 void line_follow::halt()
@@ -71,9 +87,33 @@ void line_follow::halt()
     motors->set_sides(0, 0, MOTOR_ABSOLUTE);
 }
 
+void line_follow::spot_turn()
+{
+  bool first = true;
+  float line_loc;
+  do
+  {
+    motor_l = MOTOR_BASE;
+    motor_r = -MOTOR_BASE;
+
+    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
+
+    if(first)
+    {
+        while(line_loc < 2 && line_loc > -2)
+          line_loc = linesensor->readline();
+
+        first = false;
+    }
+    line_loc = linesensor->readline();
+  }
+  while(!(-1 < line_loc && line_loc < 1));
+}
+
 void line_follow::turn_right()
 {
   bool first = true;
+  float line_loc;
   do
   {
     motor_l = MOTOR_BASE/8;
@@ -83,13 +123,13 @@ void line_follow::turn_right()
 
     if(first)
     {
-        loop_rate->sleep();
-        loop_rate->sleep();
-        loop_rate->sleep();
+        init_turn_time.sleep();
         first = false;
     }
+
+    line_loc = linesensor->readline();
   }
-  while(linesensor->readline());
+  while(!(-1 < line_loc && line_loc < 1));
 }
 
 void line_follow::u_turn()
@@ -102,15 +142,12 @@ void line_follow::u_turn()
 void line_follow::run()
 {
   follow_line();
+  turn_right();
   follow_line();
-  /*
-  follow_line();
-  u_turn();
-  follow_line();
-  u_turn();
-  follow_line();
-  follow_line();
-  u_turn();
+  turn_straight();
+  motors->set_sides(-MOTOR_BASE, -MOTOR_BASE, MOTOR_ABSOLUTE);
+  Duration meow(2);
+  meow.sleep();
+  spot_turn();
   follow_line();
-  */
 }
diff --git a/scout/libscout/src/behaviors/line_follow.h b/scout/libscout/src/behaviors/line_follow.h
index a487523..fae7ace 100644
--- a/scout/libscout/src/behaviors/line_follow.h
+++ b/scout/libscout/src/behaviors/line_follow.h
@@ -42,6 +42,8 @@ class line_follow : public Behavior
     protected:
         void turn_left();
         void turn_right();
+        void turn_straight();
+        void spot_turn();
         void u_turn();
         void follow_line();
         void halt();
diff --git a/scout/libscout/src/behaviors/navigationMap.cpp b/scout/libscout/src/behaviors/navigationMap.cpp
index 44033c5..ef33c0b 100644
--- a/scout/libscout/src/behaviors/navigationMap.cpp
+++ b/scout/libscout/src/behaviors/navigationMap.cpp
@@ -94,7 +94,7 @@ navigationMap::navigationMap(string scoutname) : Behavior(scoutname, "navigation
     Edge* a7 = new Edge[ARRAY_SIZE];
     a7[0] = MAKE_EDGE(ISTRAIGHT, 6, 0);
     a7[1] = MAKE_EDGE(ILEFT, 11, 40);
-    a7[2] = MAKE_EDGE(IUTURN, 3, 10);
+    a7[2] = MAKE_EDGE(IUTURN, 4, 10);
 
     Edge* a8 = new Edge[ARRAY_SIZE];
     a8[0] = MAKE_EDGE(ISTRAIGHT, 7, 0);
diff --git a/scout/scoutsim/maps/warehouse.xcf b/scout/scoutsim/maps/warehouse.xcf
index ca2ff0b..95cbf7a 100644
Binary files a/scout/scoutsim/maps/warehouse.xcf and b/scout/scoutsim/maps/warehouse.xcf differ
diff --git a/scout/scoutsim/maps/warehouse_lines.bmp b/scout/scoutsim/maps/warehouse_lines.bmp
index e8fb404..ec29cc0 100644
Binary files a/scout/scoutsim/maps/warehouse_lines.bmp and b/scout/scoutsim/maps/warehouse_lines.bmp differ
