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

}