Colony

#include "lineFollow.h"

#define CODESIZE 5 

int countHi = 0;

int countLo = 0;

int maxAvg, avg;

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

			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(disableBarCode--)

	{

		if(disableBarCode)return NOBARCODE;

		return INTERSECTION;

	}

	updateBarCode();

	return 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;

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

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

	barCodePosition = 0;

	return temp;

}

void updateLine(int* values)

{	

	int ports[5] = {13, 12, 3, 2, 9};

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

		values[i] = analog_get10(ports[i])<150 ? 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;

}

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(ports[0]);

	current[1] = analog_get10(ports[1]);

	if(current[1]>500)

	{

		if(countHi++==0) 

		{

			avg = 500;

			maxAvg = 500;

		}

		else

		{

			avg = 3*avg + current[1];

			avg/=4;

			maxAvg = max(maxAvg, avg);

		}

	}

	else if(countHi>5)

	{

		if(countLo++>15)

		{

			countHi=countLo=0;

			if(maxAvg>825)orb1_set_color(RED);

			else orb1_set_color(BLUE);

			barCode[barCodePosition++] = maxAvg > 825 ? 1:0;

		}

	}

	else countHi/=2;

	if(countHi==0)countLo=0; 

}

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);

}

#include <dragonfly_lib.h>

 #ifndef _LINEFOLLOW_H_

 #define _LINEFOLLOW_H_

#define LWHITE			0

#define LGREY			1

#define LBLACK	 		2

#define CENTER			3

#define NOLINE			-42

#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

	Must call lineFollow 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);

/*	updateLine

	Reads in the analog values

	Fills the given array with WHITE

	or BLACK representing the line

*/

void updateLine(int* values); 

/*	lineLocate

	Finds the location of the line

	Outputs positive for right side

	Negative for left, or NOLINE if a line is not found

*/

int lineLocate(int* colors);

/*	updatebarCode

	Reads in and processes

	bar code data

*/

void updateBarCode(void);

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

	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;

#endif

#include "lineFollow.h"