commit 5837143311f6de5068f1724a2d1dc8377610237d
Author: Priya <pdeo@Phlegethon.(none)>
Date:   Wed Apr 25 23:45:28 2012 -0400

    Renaming to line follow and getting rid of old linefollowing files.

diff --git a/scout/libscout/src/behaviors/line_follow.cpp b/scout/libscout/src/behaviors/line_follow.cpp
index 49f1537..1a3072a 100644
--- a/scout/libscout/src/behaviors/line_follow.cpp
+++ b/scout/libscout/src/behaviors/line_follow.cpp
@@ -1,281 +1,109 @@
 /**
- * @file line_follow.cpp
- * @defgroup lineFollwing Line Following
- *
- * Takes care of following a line. Running this program is done by calling the
- * init() function and then the line_follow(speed) command.  However, direct use
- * of this class is discouraged as its behavior is used by lineDrive.cpp, which
- * extends this class to provide behavior functionality.
- *
- * @author Dan Jacobs and the Colony Project
- * @date 11-1-2010
+ * 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 "line_follow.h"
 
-//! Anything lower than this value is white
-int GREY_THRESHOLD = 300;
-//! Anything higher than this value is black
-int BLACK_THRESHOLD = 750;
+using namespace std;
 
-int countHi = 0;
-int countLo = 0;
-int maxAvg, avg;
+static int motor_l;
+static int motor_r;
 
-int duration[2] = {0};
-int lastColor[2] = {0};
-char isReset[2] = {1};
-int lastReadings[2][ NUM_READINGS ] = {{0}};
-int lastReadingsPtr[2] = {0};
-int numLast[2][4] = { {0, 0, 0, NUM_READINGS}, {0, 0, 0, NUM_READINGS} };
-int bitColor[2] = {0};
-
-int turnDistance=0;
-//! Counts the number of full line readings before we determine an intersection
-int intersectionFilter=0;
+void line_follow::follow_line()
+{
+    while(!linesensor->fullline())
+    {
+        double line_loc = linesensor->readline();
 
-//! Keeps track of where the encoder of one motor started, for use in turns.
-int encoderStart = -1;
-int encoderReset = 0;   // 0 if encoderStart has no value set
+        cout << "line_loc: " << line_loc << endl;
 
-int min(int x, int y){return x>y ? y : x;}
-int max(int x, int y){return x<y ? y : x;}
+        motor_l = -MOTOR_BASE + SCALE * line_loc;
+        motor_r = -MOTOR_BASE - SCALE * line_loc;
 
+        motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
+    }
+    halt();
+}
 
-void line_follow::init()
+void line_follow::turn_left()
 {
-  int i, j, curReading;
-  int lowGrey = 1000, highGrey = 0, lowBlack = 1000,
-      highBlack = 0;
+  bool first = true;
+  do
+  {
+    motor_l = -MOTOR_BASE;
+    motor_r = MOTOR_BASE/8;
 
-  analog_init(0);
-  encoders_init();
-  lost = 0;
-  intersectionFilter=0;
+    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
 
-  for(i=0; i<2; i++)
-  {
-    for(j=0; j<NUM_READINGS; j++)
+    double line_loc = linesensor->readline();
+    cout << "line_loc: " << line_loc << endl;
+    if(first)
     {
-      lastReadings[i][j] = BAD_READING;
+        loop_rate->sleep();
+        loop_rate->sleep();
+        loop_rate->sleep();
+        first = false;
     }
-    isReset[i] = 1;
-  }
-
-  // Calibrate thresholds
-  orb_set_color(YELLOW);
-  delay_ms(2000);
-
-  orb_set_color(BLUE);
-
-  for(i=0; i<100; i++)
-  {
-    curReading = read_line(LEFT_SENSOR + 6);
-    if(curReading < lowGrey)
-      lowGrey = curReading;
-    if(curReading > highGrey)
-      highGrey = curReading;
-
-    delay_ms(20);
   }
-
-  orb_set_color(YELLOW);
-  delay_ms(2000);
-
-  orb_set_color(GREEN);
-
-  for(i=0; i<100; i++)
-  {
-    curReading = read_line(LEFT_SENSOR + 6);
-    if(curReading < lowBlack)
-      lowBlack = curReading;
-    if(curReading > highBlack)
-      highBlack = curReading;
-
-    delay_ms(20);
-  }
-
-  orbs_set(0,0,0,0,0,0);
-
-  GREY_THRESHOLD = lowGrey / 2;
-  BLACK_THRESHOLD = (highGrey + lowBlack) / 2;
-
-  usb_puts("Grey: ");
-  usb_puti(lowGrey);
-  usb_puts(", ");
-  usb_puti(highGrey);
-  usb_puts("\nBlack: ");
-  usb_puti(lowBlack);
-  usb_puts(", ");
-  usb_puti(highBlack);
-  usb_puts("\nThresholds: ");
-  usb_puti(GREY_THRESHOLD);
-  usb_puts(", ");
-  usb_puti(BLACK_THRESHOLD);
-  usb_puts("\n\n");
-
-  delay_ms(1500);
+  while(linesensor->readline());
 }
 
-
-/** 
- * Follows a line at the given speed.
- * @param speed The speed with which to follow the line.
- */
-int line_follow::follow_line(int speed)
+void line_follow::halt()
 {
-	int colors[5];
-	int position;
-	
-
-	updateLine(colors);
-	position = lineLocate(colors); 	
-	
-	//not on line
-	if(position == NOLINE)
-	{
-		if(lost++ > 20)
-		{
-			orb2_set_color(GREEN);
-			motors_off();
-			return LINELOST;
-		}
-	}
-	else if(position == FULL_LINE)
-	{
-		if(intersectionFilter++ > 4)
-		{
-			orb2_set_color(RED);
-			barCodePosition[0]=0;
-            barCodePosition[1]=0;
-			disableBarCode=50;
-		}
-	}
-	//on line
-	else
-	{
-		position*=30;
-		orb2_set_color(ORB_OFF);
-		motorLeft(min(speed+position, 255));
-		motorRight(min(speed-position, 255));
-		lost=0;
-		intersectionFilter=0;
-	}
+    motors->set_sides(0, 0, MOTOR_ABSOLUTE);
 }
 
-
-/**
- * Turns left at a cross of two lines.  Assumes that we are at lines in a cross
- * pattern, and turns until it sets straight on the new line.
- * @return 0 if turn finishes otherwise return 1
- */
-int line_follow::turnLeft()
+void line_follow::turn_right()
 {
-        motors->set_sides(-20, 20, MOTOR_ABSOLUTE);
-		
-        if(!encoderReset)
-        {
-            encoderStart = encoder_get_x(RIGHT);
-            encoderReset = 1;
-        }
-
-        if(encoder_get_x(RIGHT) < encoderStart)
-        {
-            encoderStart = 0;
-            motors->set_sides(0, 0, MOTOR_ABSOLUTE);
-            delay_ms(2000);
-        }
-
-        if(encoder_get_x(RIGHT) - encoderStart > 300)
-        {
-            encoderReset = 0;
-            return 0;
-        }
-        return 1;
-}
-
-
-
-/**
- * Turns right at a cross of two lines.  Assumes that we are at lines in a cross
- * pattern, and turns until it sets straight on the new line.
- * @return 0 if the turn finishes otherwise return 1
- */
-int line_follow::turnRight()
-{
-  /// Set motors using new motors functions
-  motors->set_sides(20, -20, MOTOR_ABSOLUTE);
+  bool first = true;
+  do
+  {
+    motor_l = MOTOR_BASE/8;
+    motor_r = -MOTOR_BASE;
 
-  int colors[5];
-  updateLine(colors);
-  int position = lineLocate(colors);
-  if(position>2 || position<-2)
-    turnDistance++;
+    motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
 
-  if(turnDistance>1) 
-  {
-    if(position<3 && position>-3)
+    double line_loc = linesensor->readline();
+    cout << "line_loc: " << line_loc << endl;
+    if(first)
     {
-      turnDistance = 0;
-      return 0;
+        loop_rate->sleep();
+        loop_rate->sleep();
+        loop_rate->sleep();
+        first = false;
     }
   }
-  return 1;
-}
-
-
-void line_follow::updateLine(int* values)
-{	
-	for(int i = 0; i<5; i++)
-		values[i] = (read_line(4-i) < LINE_COLOR ? LWHITE : LBLACK);
-}
-
-
-int line_follow::lineLocate(int* colors)
-{
-	int i;
-	int wsum = 0;
-	int count=0;
-
-	for(i = 0; i<5; i++)
-	{
-		count += colors[i]/2;
-		wsum += (i)*colors[i];
-	}
-	if(count==0)
-		return NOLINE;	
-	if(count==5)
-		return FULL_LINE;
-	return (wsum/count)-4; /// Subtract 4 to center the index around the center.
-}
-
-void line_follow::motorLeft(int speed)
-{
-	int tempspeed = 0;
-	if ((speed-=127)>=0)
-	{	
-		tempspeed = 160+speed*95/128;
-		motors->set(MOTOR_LEFT, tempspeed, MOTOR_ABSOLUTE);
-	}
-	else
-	{
-		tempspeed = 160+speed*95/128;
-		motors->set(MOTOR_LEFT, -tempspeed, MOTOR_ABSOLUTE);
-	}
+  while(linesensor->readline());
 }
 
-void line_follow::motorRight(int speed)
+void line_follow::run()
 {
-	int tempspeed = 0;
-	if ((speed-=127)>=0)
-	{	
-		tempspeed = 160+speed*95/128;
-		motors->set(MOTOR_RIGHT, tempspeed, MOTOR_ABSOLUTE);
-	}
-	else
-	{
-		tempspeed = 160+speed*95/128;
-		motors->set(MOTOR_RIGHT, -tempspeed, MOTOR_ABSOLUTE);
-	}
+  follow_line();
+  turn_right();
+  follow_line();
+  turn_left();
+  follow_line();
+  turn_right();
+  follow_line();
 }
-	
