Colony

#include <dragonfly_lib.h>

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

int main(void)

{

        /* initialize components, set wireless channel */

        dragonfly_init(ALL_ON);

        straightLineFollow();

        return 0;

}

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

                }

                //turn left

                else if(location > 0)

                {

                        orb1_set_color(GREEN);

                        orb2_set_color(ORB_OFF);

                }

                //turn right

                else if(location < 0)

                {

                        orb1_set_color(ORB_OFF);

                        orb2_set_color(GREEN);

                }

                else

                {

                        orb1_set_color(ORB_OFF);

                        orb2_set_color(ORB_OFF);

                }

        }

        return 0;

}