Colony

#include <dragonfly_lib.h>

#ifndef _LINEFOLLOW_H_

#define _LINEFOLLOW_H_

#define LWHITE			0

#define LGREY			1

#define LBLACK	 		2

#define BAD_READING     3

#define CENTER			3

#define LINELOST		-1

#define NOBARCODE 		-2

#define INTERSECTION 	-25

#define NOLINE			-50

#define FULL_LINE 		-51

//! Number of consecutive barcode color readings for a significant reading.

#define MAX_DURATION    20

//! Number of consecutive white barcode readings before a barcode reset.

#define TIMEOUT_DURATION    1000

#define NUM_READINGS 20

#define LEFT_SENSOR     1

#define RIGHT_SENSOR    0

/**

 * @brief Initializes line following.

 * 

 * Must be called before line following will work.

 * Turns the analog loop off.

 */

void lineFollow_init(void);

/**	lineFollow

 *	Must call lineFollow_init first

 *	Must be called inside a loop

 */

int lineFollow(int speed);

/**	turnLeft turnRight mergeLeft mergeRight

 *	Must be called inside a loop

 *	returns 0 when complete

 */

int turnLeft(void);

int turnRight(void);

int mergeLeft(void);

int mergeRight(void);

void addToBuckets(int curColor, int i);

void printBuckets();

/**

 * @brief Updates the values stored in the array to white or black based on

 * current sensor readings.

 *

 * @param values The array of five integers to be updated. 

 */

void updateLine(int* values); 

/**

 * @brief Returns an index of the middle of the line based on line readings.

 *

 * Two special return values are possible:

 *   NOLINE if none of the sensors holds a black value, and

 *   FULL_LINE if all of the sensors see black.

 *

 * Otherwise, returns a value from -4 (farthest left) to 4 (farthest right), with

 * 0 the line being centered in the middle.

 *

 * @param colors The array of 5 readings from the line sensor.  Must be either

 *    LWHITE or LBLACK.

 * @return Either a special value or an index from -4 to 4.

 *

 */

int lineLocate(int* colors);

/**	updatebarCode

 *	Reads in and processes

 *	bar code data

 */

void updateBarCode(void);

/**

 * @brief Gets the completed value read by the barcode reader, or NOBARCODE.

 *

 * Returns a bar code if available (if at the end of a barcode) and resets the

 * barcodePosition to 0. Otherwise, return NOBARCODE.  *

 * @return The value of the barcode if a complete barcode, else NOBARCODE.

 */

int getBarCode(void);

//! A simple function to return the minimum of two integers.

int min(int x, int y);

//! A simple function to return the maximum of two integers.

int max(int x, int y);

/** @todo Alex: I hate these functions, but I'm keeping them so code will still work. But we should delete them sometime. */

/**	motorLeft

 *	Commands the left motor

 *	Cannot be used to stop

 *	0-126 are backward

 *	127-255 are forward

 */

void motorLeft(int speed);

/**	motorRight

 *	Commands the right motor

 *	Cannot be used to stop

 *	0-126 are backward

 *	127-255 are forward

 */

void motorRight(int speed);

/**	lost

 *	Internal counter to detect if the line was lost

 */

int lost;

int onLine(void);

#endif

/*********

 * Music Library!

 * Now with twice the fiber!

 * 

 *  Jeff Cooper, Fall 2010

 ***************/

#ifndef MUSIC_FUNCTIONS

#define MUSIC_FUNCTIONS

void c0(int dur){buzzer_chirp(dur, 16);} 

void db0(int dur){buzzer_chirp(dur, 17);} 

void d0(int dur){buzzer_chirp(dur, 18);} 

void eb0(int dur){buzzer_chirp(dur, 19);} 

void e0(int dur){buzzer_chirp(dur, 20);} 

void f0(int dur){buzzer_chirp(dur, 21);} 

void gb0(int dur){buzzer_chirp(dur, 23);} 

void g0(int dur){buzzer_chirp(dur, 24);} 

void ab0(int dur){buzzer_chirp(dur, 25);} 

void a0(int dur){buzzer_chirp(dur, 27);} 

void bb0(int dur){buzzer_chirp(dur, 29);} 

void b0(int dur){buzzer_chirp(dur, 30);} 

void c1(int dur){buzzer_chirp(dur, 32);} 

void db1(int dur){buzzer_chirp(dur, 34);} 

void d1(int dur){buzzer_chirp(dur, 36);} 

void eb1(int dur){buzzer_chirp(dur, 38);} 

void e1(int dur){buzzer_chirp(dur, 41);} 

void f1(int dur){buzzer_chirp(dur, 43);} 

void gb1(int dur){buzzer_chirp(dur, 46);} 

void g1(int dur){buzzer_chirp(dur, 49);} 

void ab1(int dur){buzzer_chirp(dur, 51);} 

void a1(int dur){buzzer_chirp(dur, 55);} 

void bb1(int dur){buzzer_chirp(dur, 58);} 

void b1(int dur){buzzer_chirp(dur, 61);} 

void c2(int dur){buzzer_chirp(dur, 65);} 

void db2(int dur){buzzer_chirp(dur, 69);} 

void d2(int dur){buzzer_chirp(dur, 73);} 

void eb2(int dur){buzzer_chirp(dur, 77);} 

void e2(int dur){buzzer_chirp(dur, 82);} 

void f2(int dur){buzzer_chirp(dur, 87);} 

void gb2(int dur){buzzer_chirp(dur, 92);} 

void g2(int dur){buzzer_chirp(dur, 98);} 

void ab2(int dur){buzzer_chirp(dur, 103);} 

void a2(int dur){buzzer_chirp(dur, 110);} 

void bb2(int dur){buzzer_chirp(dur, 116);} 

void b2(int dur){buzzer_chirp(dur, 123);} 

void c3(int dur){buzzer_chirp(dur, 130);} 

void db3(int dur){buzzer_chirp(dur, 138);} 

void d3(int dur){buzzer_chirp(dur, 146);} 

void eb3(int dur){buzzer_chirp(dur, 155);} 

void e3(int dur){buzzer_chirp(dur, 164);} 

void f3(int dur){buzzer_chirp(dur, 174);} 

void gb3(int dur){buzzer_chirp(dur, 185);} 

void g3(int dur){buzzer_chirp(dur, 196);} 

void ab3(int dur){buzzer_chirp(dur, 207);} 

void a3(int dur){buzzer_chirp(dur, 220);} 

void bb3(int dur){buzzer_chirp(dur, 233);} 

void b3(int dur){buzzer_chirp(dur, 246);} 

void c4(int dur){buzzer_chirp(dur, 261);} 

void db4(int dur){buzzer_chirp(dur, 277);} 

void d4(int dur){buzzer_chirp(dur, 293);} 

void eb4(int dur){buzzer_chirp(dur, 311);} 

void e4(int dur){buzzer_chirp(dur, 329);} 

void f4(int dur){buzzer_chirp(dur, 349);} 

void gb4(int dur){buzzer_chirp(dur, 369);} 

void g4(int dur){buzzer_chirp(dur, 392);} 

void ab4(int dur){buzzer_chirp(dur, 415);} 

void a4(int dur){buzzer_chirp(dur, 440);} 

void bb4(int dur){buzzer_chirp(dur, 466);} 

void b4(int dur){buzzer_chirp(dur, 493);} 

void c5(int dur){buzzer_chirp(dur, 523);} 

void db5(int dur){buzzer_chirp(dur, 554);} 

void d5(int dur){buzzer_chirp(dur, 587);} 

void eb5(int dur){buzzer_chirp(dur, 622);} 

void e5(int dur){buzzer_chirp(dur, 659);} 

void f5(int dur){buzzer_chirp(dur, 698);} 

void gb5(int dur){buzzer_chirp(dur, 739);} 

void g5(int dur){buzzer_chirp(dur, 783);} 

void ab5(int dur){buzzer_chirp(dur, 830);} 

void a5(int dur){buzzer_chirp(dur, 880);} 

void bb5(int dur){buzzer_chirp(dur, 932);} 

void b5(int dur){buzzer_chirp(dur, 987);} 

void c6(int dur){buzzer_chirp(dur, 1046);} 

void db6(int dur){buzzer_chirp(dur, 1108);} 

void d6(int dur){buzzer_chirp(dur, 1174);} 

void eb6(int dur){buzzer_chirp(dur, 1244);} 

void e6(int dur){buzzer_chirp(dur, 1318);} 

void f6(int dur){buzzer_chirp(dur, 1396);} 

void gb6(int dur){buzzer_chirp(dur, 1479);} 

void g6(int dur){buzzer_chirp(dur, 1567);} 

void ab6(int dur){buzzer_chirp(dur, 1661);} 

void a6(int dur){buzzer_chirp(dur, 1760);} 

void bb6(int dur){buzzer_chirp(dur, 1864);} 

void b6(int dur){buzzer_chirp(dur, 1975);} 

void c7(int dur){buzzer_chirp(dur, 2093);} 

void db7(int dur){buzzer_chirp(dur, 2217);} 

void d7(int dur){buzzer_chirp(dur, 2349);} 

void eb7(int dur){buzzer_chirp(dur, 2489);} 

void e7(int dur){buzzer_chirp(dur, 2637);} 

void f7(int dur){buzzer_chirp(dur, 2793);} 

void gb7(int dur){buzzer_chirp(dur, 2959);} 

void g7(int dur){buzzer_chirp(dur, 3135);} 

void ab7(int dur){buzzer_chirp(dur, 3322);} 

void a7(int dur){buzzer_chirp(dur, 3520);} 

void bb7(int dur){buzzer_chirp(dur, 3729);} 

void b7(int dur){buzzer_chirp(dur, 3951);} 

void c8(int dur){buzzer_chirp(dur, 4186);} 

void db8(int dur){buzzer_chirp(dur, 4434);} 

void d8(int dur){buzzer_chirp(dur, 4698);} 

void eb8(int dur){buzzer_chirp(dur, 4978);} 

#endif

/**

 * @file lineDrive.c

 *

 * Provides functions to implement line driving behavior.  This program extends

 * the behavior of the line-following program by following lines automatically

 * and implementing behaviors to deal with commands passed to lineDrive.

 *

 * @author Dan Jacobs

 * @date 11-1-2010

 */

#include "lineDrive.h"

int state[5];       //! Stores a queue of sub-commands to be executed

int stateCounter;

int stateLength;

//! Whether lineDrive is currently paused. Set to 0 on initialization.

int stopped=1;

/**

 * Starts the line following procedure. Must be called before other

 * line-following functions will work.  This function essentially resets the

 * state of line-following.

 */

void lineDrive_init()

{

	lineFollow_init();

	for(int i=0; i<5; i++)state[i]=0;

	stateCounter=0;

	stateLength=0;

	stopped=0;

}

/**

 * Follows a line and executes whatever command is next on the queue.

 * @param speed The speed with which to drive along the line.

 */

int doDrive(int speed)

{

	if(stopped)

	{

		motor_l_set(FORWARD, 0);

		motor_r_set(FORWARD, 0);

		return NORMAL;

	}

	int code;

	switch(state[0])

	{	

	case ISTRAIGHT:

		code = lineFollow(speed);

		if(code==INTERSECTION)

		{

			for(int i=0; i<4; i++) state[i]=state[i+1];

			state[4]=0;

			if(state[0]==0)stateCounter++;

			break;

		}

		else if(code==NOBARCODE) return NORMAL;

		return code;

	case ILEFT:

		code = turnLeft();

		if(code==0)

		{

			state[0]=0;

			stateCounter++;

		}

		break;

        case IRIGHT:

                code = turnRight();

                if(code==0)

                {

			state[0]=0;

			stateCounter++;

		}

                break;

	case MERGELEFT:

		code = mergeLeft();

		if(code==0)

		{

			state[0]=0;

			stateLength=0;

			return FINISHED;

		}

		return NORMAL;

	case MERGERIGHT:

                code = mergeRight();

                if(code==0)

                {

                        state[0]=0;

                        stateLength=0;

			return FINISHED;

                }

                return NORMAL;

	default:

		return LOST;

	}

	if(stateCounter>=stateLength)

	{

		stateCounter=stateLength=0;

		return FINISHED;

	}

	return NORMAL;

}

/** Starts the line-drive process if paused. */

void start(void){stopped=0;}

/** Pauses the line-drive process. Default is started. */

void stop(void){stopped=1;}

/**

 * Defines a merge command in the direction specified.  A merge is a switch

 * of lanes.

 * @param dir Left or right, defined by ILEFT or IRIGHT

 */

int merge(int dir)

{

	if(stateLength!=0)return ERROR;

	stateLength++;

	state[0]=(dir==ILEFT ? MERGELEFT : MERGERIGHT);

	return NORMAL;

}

/**

 * Executes an intersection turn where the intersection type is specified by the

 * parameters. 

 * @param type A valid defined intersection type

 * @param dir The direction to turn at the intersection

 */

int turn(int type, int dir)

{

	if(stateLength!=0)return ERROR;

	if(dir==IRIGHT)

    {

        stateLength++;

        state[1]=IRIGHT;

        return NORMAL;

    }

	if(dir==IUTURN)

    {

        stateLength+=2;

        state[1]=state[2]=ILEFT;

        return NORMAL;

    }

	if(dir==ISTRAIGHT && type==SINGLE)

    {

        stateLength++;

        state[1]=ISTRAIGHT;

        return NORMAL;

    }

	if(dir==ISTRAIGHT)

    {

        stateLength+=2;

        state[1]=state[2]=ISTRAIGHT;

        return NORMAL;

    }

	// At this point, must be left turn

	if(type==SINGLE)

    {

        stateLength++;

        state[1]=ILEFT;

        return NORMAL;

    }

	if(type==DOUBLE_C || type==DOUBLE_T)

    {

        stateLength+=3;

        state[1]=state[3]=ISTRAIGHT;

        state[2]=ILEFT;

        return NORMAL;

    }

	if(type==ON_RAMP)

    {

        stateLength+=2;

        state[1]=ILEFT;

        state[2]=ISTRAIGHT;

        return NORMAL;

    }

	if(type==OFF_RAMP)

    {

        stateLength+=2;

        state[1]=ISTRAIGHT;

        state[2]=ILEFT;

        return NORMAL;

    }

	//Should never get here

	return ERROR;

}

/*

 * main.c for activities fair demo

 * 

 * Author: Colony Project, CMU Robotics Club

 */

#include <dragonfly_lib.h>

#include "../linefollowing/lineDrive.h"

#define LINESPEED 180

#define T 100

#define SPD 190

int main (void) {

	/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */

	dragonfly_init(ALL_ON);

	orb_init();

	if(button2_read()){

		lineDrive_init();

		while(true)

			doDrive(LINESPEED);

	} else {

		buzzer_init();

		orb1_set_color(YELLOW);

		a6(T/2);

		orb2_set_color(YELLOW);

		e7(T*3);

		delay_ms(T*3);

		while(1){

		  {	//i==1

					motor_l_set(FORWARD, SPD);

					motor_r_set(BACKWARD, SPD+10);

				orb1_set_color(GREEN);

			e6(T*2);

			b5(T);

			c6(T);

				orb2_set_color(GREEN);

			d6(T*2);

				orb2_set_color(CYAN);

			c6(T);

					motor_l_set(FORWARD, SPD-10);

					motor_r_set(FORWARD, SPD-10);;

			b5(T);

				orb1_set_color(CYAN);

			a5(T*2);

			a5(T);

			c6(T);

				orb1_set_color(ORANGE);

				orb2_set_color(ORANGE);

			e6(T*2);

				orb1_set_color(RED);

				orb2_set_color(RED);

			d6(T);

			c6(T);

				orb1_set_color(LIME);

				orb2_set_color(LIME);

			b5(T*3);

			c6(T);

			d6(T*2);

				orb1_set_color(BLUE);

		  }

		{	//i==2

					motor_l_set(FORWARD, SPD);

					motor_r_set(BACKWARD, SPD);

			e6(T*2);

				orb2_set_color(BLUE);

			c6(T*2);

				orb2_set_color(ORB_OFF);

			a5(T*2);

				orb1_set_color(ORB_OFF);

			a5(T);

		{			motors_off();

			a5(T*1);

			delay_ms(T*2);

		}	

					motor_l_set(FORWARD, SPD-10);

					motor_r_set(FORWARD, SPD-10);

			delay_ms(T);

				orb2_set_color(WHITE);

			d6(T);

			delay_ms(T);

				orb1_set_color(WHITE);

			f6(T);

				orb1_set_color(PINK);

				orb2_set_color(PINK);

			a6(T);

			c6(T/2);

			c6(T/2);

			g6(T);

			f6(T);

				orb1_set_color(GREEN);

				orb2_set_color(GREEN);

			e6(T*2);

			delay_ms(T);

				orb2_set_color(RED);

				orb1_set_color(RED);	

			c6(T);

			e6(T);

			a5(T/2);

			g5(T/2);

			d6(T);

		}				

		{	//i==3

					motor_l_set(FORWARD, SPD);

					motor_r_set(BACKWARD, SPD);

			c6(T);		

			b5(T*2);

			delay_ms(T);

				orb2_set_color(PURPLE);

				orb1_set_color(PURPLE);

			c6(T);

			d6(T*2);

				orb2_set_color(RED);		

					motor_r_set(FORWARD, SPD);

			e6(T*2);

				orb1_set_color(RED);

			c6(T*2);

				orb1_set_color(ORB_OFF);

			a5(T);

			a4(T);

				orb2_set_color(ORB_OFF);

			a5(T);

			a4(T);

		{

				motors_off();

			delay_ms(T*2);

				orb2_set_color(ORANGE);

				orb1_set_color(ORANGE);

			e6(T);

		}

					motor_l_set(FORWARD, SPD);

					motor_r_set(FORWARD, SPD);

			e6(T*3);

				orb2_set_color(RED);

				orb1_set_color(RED);

			c6(T*3);	

		}	

		{	//i==4

					motor_l_set(FORWARD, SPD);

					motor_r_set(BACKWARD, SPD);

			c6(T);

				orb2_set_color(MAGENTA);

				orb1_set_color(MAGENTA);

			d6(T*4);

				orb2_set_color(CYAN);

				orb1_set_color(CYAN);

			b5(T*2);	

					motor_r_set(FORWARD, SPD);

			b5(T*2);

				orb2_set_color(PURPLE);

				orb1_set_color(PURPLE);

			c6(T*4);

				orb2_set_color(PINK);

				orb1_set_color(PINK);

			a5(T*4);

				orb2_set_color(BLUE);

				orb1_set_color(GREEN);

			ab5(T*4);

				orb1_set_color(BLUE);

				orb2_set_color(GREEN);

			b5(T);

		}	

		{	//i==5

					motor_l_set(FORWARD, SPD);

					motor_r_set(BACKWARD, SPD);

			b5(T*2);

			delay_ms(T);

				orb2_set_color(ORANGE);

				orb1_set_color(ORANGE);

			e6(T*4);

				orb2_set_color(RED);

				orb1_set_color(RED);

					motor_r_set(FORWARD, SPD);

			c6(T*4);

				orb2_set_color(MAGENTA);

				orb1_set_color(MAGENTA);

			d6(T*4);

				orb2_set_color(CYAN);

				orb1_set_color(CYAN);

			b5(T*4);

			c6(T*2);

				orb1_set_color(WHITE);

			e6(T);

		}

					motors_off();

		{	//CIRCLE

				motor_l_set(FORWARD, SPD);

				motor_r_set(BACKWARD, SPD);

			e6(T);

				orb2_set_color(WHITE);

			a6(T*4);

		}

		{	//RESET

				motor_l_set(FORWARD, SPD+50);

				motor_r_set(FORWARD, SPD);

				orb1_set_color(LIME);

				orb2_set_color(LIME);

			ab6(T);

				orb1_set_color(ORB_OFF);

				orb2_set_color(ORB_OFF);

				delay_ms(T*13);

				motor_l_set(BACKWARD, SPD);

				motor_r_set(FORWARD, SPD);

				delay_ms(T*3);

		}

		motors_off();

		delay_ms(T*5);

		}

	}

	return 0;

}

#!/usr/bin/perl -W

my $line;

my $line2;

$line = `pwd`;

if($line =~ /linefollowing/igs) {

	print "Cannot modify linefollow code from linefollow\n";

} else {

	`rm lineFollow.*`;

	`rm lineDrive.*`;

	`cp ../linefollowing/lineFollow.* .`;

	`cp ../linefollowing/lineDrive.* .`;

}

if($line =~ /traffic_navigation/igs) {

	while(</*>) {

		if(/(.*?test)\.c/) {

			`mv $1.c $1.bak`;

		}

	}

}

#ifndef _LINE_DRIVE_

#define _LINE_DRIVE_

#include "lineFollow.h"

/* Old definitions, delete as soon as possible

#define DOUBLE   	0

#define SINGLE		1

#define ON_RAMP		2

#define OFF_RAMP	3

*/

#define SINGLE      0

#define ON_RAMP     1

#define OFF_RAMP    2

#define DOUBLE_C    3

#define DOUBLE_T    4

#define ISTRAIGHT	0

#define ILEFT		1

#define IRIGHT		2

#define IUTURN		3

#define MERGELEFT	4

#define MERGERIGHT	5

#define NORMAL		-1

#define FINISHED	-2

#define LOST		-3

#define ERROR		-4

void lineDrive_init(void);

int doDrive(int speed);

void start(void);

void stop(void);

int merge(int dir);

int turn(int type, int dir);

#endif

/**

 * @file lineFollow.c

 * @defgroup lineFollwing Line Following

 *

 * Takes care of following a line. Running this program is done by calling the

 * init() function and then the lineFollow(speed) command.  However, direct use

 * of this class is discouraged as its behavior is used by lineDrive.c, which

 * extends this class to provide behavior functionality.

 *

 * @author Dan Jacobs and the Colony Project

 * @date 11-1-2010

 */

#include "lineFollow.h"

//! The number of bits expected in a barcode

#define CODESIZE 5 

#define LINE_COLOR 200

/**

 * Helps with debugging.

 * 0 - no debug output.

 * 1 - print out buckets

 * 2 - print out line sensor readings

 */

#define DBG_LINEFOLLOW 2

//! Anything lower than this value is white

int GREY_THRESHOLD = 300;

//! Anything higher than this value is black

int BLACK_THRESHOLD = 750;

int countHi = 0;

int countLo = 0;

int maxAvg, avg;

// Everything has a dimension of 2 for left and right readings

int barCode[2][ CODESIZE ];

int barCodePosition[2]={0};

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;

int disableBarCode=0;

//! 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

void lineFollow_init()

{

    int i, j, curReading;

    int lowGrey = 1000, highGrey = 0, lowBlack = 1000,

        highBlack = 0;

	analog_init(0);

    encoders_init();

	lost = 0;

	intersectionFilter=0;

	disableBarCode=0;

    for(i=0; i<2; i++)

    {

        for(j=0; j<NUM_READINGS; j++)

        {

            lastReadings[i][j] = BAD_READING;

        }

        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 + highGrey) / 2;

    BLACK_THRESHOLD = (highBlack + 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);

    //numLast = { {0, 0, 0, NUM_READINGS}, {0, 0, 0, NUM_READINGS} };

}

/** 

 * Follows a line at the given speed.

 * @param speed The speed with which to follow the line.

 */

int lineFollow(int speed)

{

	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;

	}

    // If we're running over a line, stop reading barcodes for a sec

	if(disableBarCode-- > 0)

	{

        // Return intersection once we cross the line

		if(disableBarCode) return NOBARCODE;

		return INTERSECTION;

	}

	updateBarCode();

	return getBarCode();

}

/**

 * Implements the left merge, assuming a line exists to the left.  Works by

 * turning off the line at an increasing angle and waiting to hit another line

 * on the left.

 */

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;

}

/**

 * Implements the right merge, assuming a line exists to the right.  Works by

 * turning off the line at an increasing angle and waiting to hit another line

 * on the right.

 */

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;

}

/**

 * 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 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;*/

        motor_l_set(BACKWARD,200);

        motor_r_set(FORWARD,200);

        if(!encoderReset)

        {

            encoderStart = encoder_get_x(RIGHT);

            encoderReset = 1;

        }

        if(encoder_get_x(RIGHT) < encoderStart)

        {

            encoderStart = 0;

            // Temporary: display an "error message" in case of overflow.

            // Using this for debugging, take it out soon!

            motor_l_set(FORWARD,0);

            motor_r_set(FORWARD,0);

            //orb_set_color(WHITE);

            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 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[1] != CODESIZE)

        return NOBARCODE ;

    else

    {

        int temp = 0;

        for(int i=0; i<CODESIZE; i++)

            temp += (barCode[1][i] << i);

        barCodePosition[1] = 0;

        return temp;

    }

}

void updateLine(int* values)

{	

	for(int i = 0; i<5; i++)

		values[i] = (read_line(4-i) < LINE_COLOR ? LWHITE : LBLACK);

}

int 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 printBuckets(int i)

{

    int j;

    usb_puts("LP[");

    usb_puti(i);

    usb_puts("]: ");

    usb_puti(lastReadingsPtr[i]);

    usb_puts(", Totals: ");

    for(int j=0; j<=3; j++)

    {

        usb_puts("[");

        usb_puti(j);

        usb_puts("]: ");

        usb_puti(numLast[i][j]);

        usb_puts(" ");

    }

    usb_puts("\t ");

    usb_puts("\n");

}

void addToBuckets(int curColor, int i)

{

    int oldest = lastReadings[i][lastReadingsPtr[i]];

    numLast[i][oldest]--;

    lastReadings[i][lastReadingsPtr[i]] = curColor;

    lastReadingsPtr[i] = (lastReadingsPtr[i]+1) % NUM_READINGS;

    numLast[i][curColor]++;

    if(DBG_LINEFOLLOW == 1)

    {

        printBuckets(i);

    }

}

void updateBarCode()

{

    // USING THESE GLOBAL VARIABLES

    // global int duration = 0;

    // global int lastColor = 0;

    // global int barCodePosition = 0;

    // global char isReset = 0;

    // Just uses one sensor for now

    for(int i = /*RIGHT*/LEFT_SENSOR; i <= LEFT_SENSOR; i++)

    {

        // Add 6 to convert left (1) and right (0) to sensor 6 and 7

        int curReading = read_line(i + 6);

        int curColor;

        if(curReading > BLACK_THRESHOLD)

        {

            curColor = LBLACK;

        }

        else if(curReading < GREY_THRESHOLD)

        {

            curColor = LWHITE;

        }

        else

        {

            curColor = LGREY;

        }

        // Keep track of this reading

        addToBuckets(curColor, i);

        // Just an error check

        if(barCodePosition[i] > CODESIZE)

        {

            barCodePosition[i] = 0;

        }

        // We now edit curColor to use the majority of the last buckets.

        if(numLast[i][1] > NUM_READINGS / 2)

        {

            curColor = LGREY;

        }

        else if(numLast[i][2] > NUM_READINGS / 2)

        {

            curColor = LBLACK;

        }

        else if(numLast[i][0] > NUM_READINGS / 2)

        {

            curColor = LWHITE;

            duration[i]++;

        }

        else

        {

            curColor = BAD_READING;

        }

        // Print out the current reading and label, if in debug mode

        if(DBG_LINEFOLLOW == 2)

        {

            switch(curColor)

            {

                case LBLACK: usb_puts("LBLACK.\t"); break;

                case LGREY: usb_puts("LGREY.\t\t"); break;

                case LWHITE: usb_puts("LWHITE.\t\t\t"); break;

            }

            usb_puti(curReading);

            usb_puts("\n");

        }

        if(curColor != BAD_READING)

        {

            // Now we assume our reading is significant - a bit, or a white space

            if(bitColor[i] == LGREY || bitColor[i] == LWHITE)

                bitColor[i] = curColor;

            // Only read a value if we have read 0 first (isReset == 1)

            if(isReset[i] && (curColor == LBLACK || curColor == LGREY) )

            {

                isReset[i] = 0;

                duration[i] = 0;

            }

            else if(curColor == LWHITE)

            {

                if(!isReset[i])

                {

                    barCode[i][barCodePosition[i]++] = 

                        (bitColor[i] == LBLACK) ? 1 : 0;

                    usb_puts("Reset. Read bit: ");

                    usb_puts(((bitColor[i] == LBLACK) ? "BLACK" : "GREY"));

                    usb_puts("\t");

                    usb_puts(((curColor==LWHITE) ? "LWHITE" : "NOTWHITE"));

                    usb_puts("\n");

                    bitColor[i] = LWHITE;

                }

                isReset[i] = 1;

                orb_set(0, 0, 0);

            }

        }

        if(curColor == LWHITE && duration[i] > TIMEOUT_DURATION 

            && barCodePosition[i] != 0)

        {

            usb_puts("TIMED OUT. BARCODE READER RESET.\n");

            usb_puts("Encoders: ");

            usb_puti(encoder_get_dx(LEFT));

            usb_puts(", ");

            usb_puti(encoder_get_dx(RIGHT));

            usb_puts("\n");

            barCodePosition[i] = 0;

            duration[i] = 0;

            isReset[i] = 1;

        }

    }

}

int min(int x, int y){return x>y ? y : x;}

int max(int x, int y){return x<y ? y : x;}

void motorLeft(int speed){

	((speed-=127)>=0)?motor_l_set(FORWARD, 160+speed*95/128):motor_l_set(BACKWARD, 160-speed*95/127);

}

void motorRight(int speed){

        ((speed-=127)>=0)?motor_r_set(FORWARD, 160+speed*95/128):motor_r_set(BACKWARD, 160-speed*95/127);

}

# this is a local makefile special for traffic navigation

# Relative path to the root directory (containing lib directory)

ifndef COLONYROOT

COLONYROOT := ..

# Target file name (without extension).

TARGET = main

# Uncomment this to use the wireless library

USE_WIRELESS = 1

# com1 = serial port. Use lpt1 to connect to parallel port.

AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)

else

COLONYROOT := ../$(COLONYROOT)

endif

include $(COLONYROOT)/Makefile