Colony

#include "lineFollow.h"

void lineFollow_init()

{

        analog_init(0);

        lost = 0;

}

int lineFollow()

{

        int colors[6];

        int position;

        updateLine(colors);

        position = lineLocate(colors);         

        //not on line

        if(position == NOLINE)

        {

                if(lost++>20)

                {

                        orb1_set_color(GREEN);

                        orb2_set_color(GREEN);

                        motors_off();

                        return LINELOST;

                }

        }

        //on line

        else

        {

                position*=30;

                orb1_set_color(GREEN);

                orb2_set_color(ORB_OFF);

                motorLeft(min(SPEED+position, 255));

                motorRight(min(SPEED-position, 255));

                lost=0;

        }

        return 0;

}

void updateLine(int* values)

{        

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

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

}

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

        else

                return  BLACK;

}

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;

}

int updateBarCode()

{

        //maps the sensors to the analog input ports

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

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

}

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

void motorLeft(int speed){

        speed-=127;

        if(speed>=0)motor_l_set(FORWARD, 160+speed*95/128);

        else motor_l_set(BACKWARD, 160-speed*95/127);

}

void motorRight(int speed){

        speed-=127;

        if(speed>=0)motor_r_set(FORWARD, 160+speed*95/128);

        else motor_r_set(BACKWARD, 160-speed*95/127);

}