Index: trunk/code/projects/traffic_navigation/lineFollow.h
===================================================================
--- trunk/code/projects/traffic_navigation/lineFollow.h	(revision 1857)
+++ trunk/code/projects/traffic_navigation/lineFollow.h	(revision 1858)
@@ -3,34 +3,42 @@
  #ifndef _LINEFOLLOW_H_
  #define _LINEFOLLOW_H_
 
+#define LWHITE			0
+#define LGREY			1
+#define LBLACK	 		2
 #define CENTER			3
 #define NOLINE			-42
-#define SPEED			170
 #define LINELOST		-1
 
+#define NOBARCODE 		-2
+#define INTERSECTION 		-25
+#define FULL_LINE 		-26
+
+
+
 /* 	lineFollow_init
 	Must call before lineFollow
 	Turns analog loop off
 */ 
-void lineFollow_init();
+void lineFollow_init(void);
 
 /*	lineFollow
 	Must call lineFollow first
 	Must be called inside a loop
 */
-int lineFollow();
+int lineFollow(int speed);
 
-/*	assignColor
-	Converts an analog color to WHITE or BLACK
+/*	turnLeft turnRight mergeLeft mergeRight
+	Must be called inside a loop
+	returns 0 when complete
 */
-int assignColor(int port, int analog);
+int turnLeft(void);
+int turnRight(void);
+int mergeLeft(void);
+int mergeRight(void);
 
-/*	updateIR
-	Gets the value of the nth line sensor
-*/
-int updateIR(int n);
-
 /*	updateLine
+	Reads in the analog values
 	Fills the given array with WHITE
 	or BLACK representing the line
 */
@@ -43,13 +51,23 @@
 */
 int lineLocate(int* colors);
 
-//Not implemented yet, returns ???
-void updateBarCode();
+/*	updatebarCode
+	Reads in and processes
+	bar code data
+*/
+void updateBarCode(void);
 
-/*	min
-	returns the minimum of two values
+/*	getBarCode
+	returns a bar code, if
+	available, otherwise NOBARCODE
 */
+int getBarCode(void);
+
+/*	min max
+	returns the minimum/maximum of two values
+*/
 int min(int x, int y);
+int max(int x, int y);
 
 /*	motorLeft
 	Commands the left motor
Index: trunk/code/projects/traffic_navigation/lineFollow.c
===================================================================
--- trunk/code/projects/traffic_navigation/lineFollow.c	(revision 1857)
+++ trunk/code/projects/traffic_navigation/lineFollow.c	(revision 1858)
@@ -1,7 +1,5 @@
+#include "lineFollow.h"
 
-#include <lineFollow.h>
-
-#define NOBARCODE -2
 #define CODESIZE 5 
 
 int countHi = 0;
@@ -10,17 +8,24 @@
 int barCode[ CODESIZE ];
 int barCodePosition=0;
 
+int turnDistance=0;
+int intersectionFilter=0;
+int disableBarCode=0;
+
+
 void lineFollow_init()
 {
 	analog_init(0);
 	lost = 0;
+	intersectionFilter=0;
+	disableBarCode=0;
 }
 
 
 
-int lineFollow()
+int lineFollow(int speed)
 {
-	int colors[6];
+	int colors[5];
 	int position;
 	
 
@@ -37,20 +42,130 @@
 			return LINELOST;
 		}
 	}
+	else if(position == FULL_LINE)
+	{
+		if(intersectionFilter++>4)
+		{
+			orb2_set_color(RED);
+			barCodePosition=0;
+			disableBarCode=50;
+		}
+	}
 	//on line
 	else
 	{
 		position*=30;
 		orb2_set_color(ORB_OFF);
-		motorLeft(min(SPEED+position, 150));
-		motorRight(min(SPEED-position, 150));
+		motorLeft(min(speed+position, 255));
+		motorRight(min(speed-position, 255));
 		lost=0;
+		intersectionFilter=0;
 	}
+
+	if(disableBarCode--)
+	{
+		if(disableBarCode)return NOBARCODE;
+		return INTERSECTION;
+	}
 	updateBarCode();
 	return getBarCode();
 }
 
-int getBarCode(){
+
+int mergeLeft()
+{
+	motor_l_set(FORWARD, 200);
+	if(turnDistance!=21)motor_r_set(FORWARD, 230);
+	else motor_r_set(FORWARD, 210);
+	int colors[5];
+	updateLine(colors);
+	int position = lineLocate(colors);
+	if(position>3 || position<-3)turnDistance++;
+
+	if(turnDistance>20)
+	{
+	turnDistance=21;
+	
+		if(position<3 && position>-3)
+		{
+			turnDistance = 0;
+			return 0;
+		}	
+	}
+	return 1;
+}
+
+
+int mergeRight()
+{
+        motor_r_set(FORWARD, 200);
+        if(turnDistance!=21)motor_l_set(FORWARD, 230);
+        else motor_l_set(FORWARD, 210);
+        int colors[5];
+        updateLine(colors);
+        int position = lineLocate(colors);
+        if(position>3 || position<-3)turnDistance++;
+
+        if(turnDistance>20)
+        {
+        turnDistance=21;
+
+                if(position<3 && position>-3)
+                {
+                        turnDistance = 0;
+                        return 0;
+                } 
+        }
+        return 1;
+}
+
+
+
+int turnLeft()
+{
+        motor_l_set(BACKWARD, 200);
+        motor_r_set(FORWARD, 200);
+        int colors[5];
+        updateLine(colors);
+        int position = lineLocate(colors);
+        if(position>2 || position<-2)turnDistance++;
+        if(turnDistance>1)
+        {
+                if(position<3 && position>-3)
+                {
+                        turnDistance = 0;
+                         return 0;
+                }
+        }
+        return 1;
+}
+
+
+
+int turnRight()
+{
+        motor_r_set(BACKWARD, 200);
+        motor_l_set(FORWARD, 200);
+        int colors[5];
+        updateLine(colors);
+        int position = lineLocate(colors);
+        if(position>2 || position<-2)turnDistance++;
+        if(turnDistance>1) 
+	{
+		if(position<3 && position>-3)
+		{
+			turnDistance = 0;
+			 return 0;
+		}
+	}
+	return 1;
+}
+
+
+
+
+int getBarCode()
+{
 	if(barCodePosition!=CODESIZE) return NOBARCODE ;
 	int temp = 0;
 	int i;
@@ -64,30 +179,13 @@
 
 void updateLine(int* values)
 {	
+	int ports[5] = {13, 12, 3, 2, 9};
 	for(int i = 0; i<5; i++)
-		values[i] = assignColor(i, updateIR(i));
+		values[i] = analog_get10(ports[i])<150 ? LWHITE : LBLACK;
 }
 
 
-int updateIR(int n)
-{
-	//maps the sensors to the analog input ports
-	int ports[5] = {13, 12, 3, 2, 9};
 
-	return analog_get10(ports[n]);
-}
-
-
-
-int assignColor(int port, int analog)
-{
-	if (analog < 150)
-		return  0;
-	else
-		return  2;
-}
-
-
 int lineLocate(int* colors)
 {
 	int i;
@@ -100,8 +198,9 @@
 		wsum += (i)*colors[i];
 	}
 	if(count==0)
-			return NOLINE;	
-		
+		return NOLINE;	
+	if(count==5)
+		return FULL_LINE;
 	return (wsum/count)-4;
 }
 
@@ -108,15 +207,14 @@
 
 void updateBarCode()
 {
+
+	//NOTE: currently only uses one of the barcode sensors.
+
 	//maps the sensors to the analog input ports
 	int ports[2] = {8,1};
 	int current[2];
-//	current[0] = analog_get10(8);
-	current[1] = analog_get10(1);
-//	usb_puti(analog_get10(8));
-//	usb_putc('\t');
-//	usb_puti(analog_get10(1));
-//	usb_putc('\n');
+//	current[0] = analog_get10(ports[0]);
+	current[1] = analog_get10(ports[1]);
 
 	if(current[1]>500)
 	{
@@ -139,7 +237,7 @@
 			countHi=countLo=0;
 			if(maxAvg>825)orb1_set_color(RED);
 			else orb1_set_color(BLUE);
-			barCode[barCodePosition++] = (maxAvg > 825 ? 2:1)-1;
+			barCode[barCodePosition++] = maxAvg > 825 ? 1:0;
 		}
 	}
 	else countHi/=2;
