Colony

#define WHITE			0

#define GREY			1

#define BLACK	 		2

#define COLOR_BUFFER	10

#define CENTER			3

#define NOLINE			-42

int assignColor(int port, int analog);

int updateIR(int n);

void updateLine(int* values); 

int lineLocate(int* colors);

int straightLineFollow();

	straightLineFollow();

//Converts an analog color to the ternary black, grey, white system.

//If the value is between two codes, the port is read again and the function is called again

int assignColor(int port, int analog)

{

	int color;

		if(analog < 341-COLOR_BUFFER)

			color = WHITE;

		else if (analog < 341+COLOR_BUFFER)

			color = assignColor(port, updateIR(port));

		else if (analog < 682-COLOR_BUFFER)

			color = GREY; 

		else if (analog < 682+COLOR_BUFFER)

			color = assignColor(port, updateIR(port));

		else

			color = BLACK;

		return color;

}

//Gets the value of the nth IR sensor

int updateIR(int n)

{

	//maps the sensors to the analog input ports

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

	return analog10(ports[n]);

}

//Gets the value of the nth IR sensor

int updateBarCode()

{

	//maps the sensors to the analog input ports

	int ports[2] = {8,1};

	return analog10(ports[1]) | analog10(ports[2]);

}

// updates all of the values, and assigns them new colors

void updateLine(int* values)

{	

	int i;

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

		values[i] = assignColor(i, updateIR(i)); 

}

// given an array of ints this function will take the arrays weighted average. High numbers indicate that a left turn is needed. Low numberes indicate that a right turn is needed.

int lineLocate(int* colors)

{

	int i;

	int wsum = 0;

	int count=0;

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

	{

		count+=(colors[i]+1)/2;

		wsum += (i)*colors[i];

	}

	if(count==0)

			return NOLINE;	

	return (wsum/count)-4;

}

//The robot will follow a line until it is interrupted by a barcode.

int straightLineFollow()

{

	int colors[6];

	int location = 0;

	while(1)

	{	

		updateLine(colors);

		location = lineLocate(colors); 	

		//not on line

		if(location == NOLINE)

		{

			orb1_set_color(GREEN);

			orb2_set_color(GREEN);

			motors_off();

		}

		//turn left

		else if(location > 0)

		{

			orb1_set_color(GREEN);

			orb2_set_color(ORB_OFF);

			motor_l_set(FORWARD, 150);

			motor_r_set(FORWARD, 200);

		}

		//turn right

		else if(location < 0)

		{

			orb1_set_color(ORB_OFF);

			orb2_set_color(GREEN);

			motor_l_set(FORWARD, 200);

			motor_r_set(FORWARD, 150);

		}

		else

		{

			orb1_set_color(ORB_OFF);

			orb2_set_color(ORB_OFF);

			motor_l_set(FORWARD, 200);

			motor_r_set(FORWARD, 200);

		}

	}

	return 0;

}