Colony

#include "lineFollow.h"

#define NOBARCODE -2

#define CODESIZE 5 

int countHi = 0;

int countLo = 0;

int maxAvg, avg;

int barCode[ CODESIZE ];

int barCodePosition=0;

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)

                {

                        orb2_set_color(GREEN);

                        motors_off();

                        return LINELOST;

                }

        }

        //on line

        else

        {

                position*=30;

                orb2_set_color(ORB_OFF);

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

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

                lost=0;

        }

        updateBarCode();

        return getBarCode();

}

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)

{        

        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]/2;

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

        }

        if(count==0)

                        return NOLINE;        

        return (wsum/count)-4;

}

void updateBarCode()

{

        //maps the sensors to the analog input ports

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

        int current[2];

//        current[0] = analog_get10(8);

        current[1] = analog_get10(1);

//        usb_puti(analog_get10(8));

//        usb_putc('\t');

//        usb_puti(analog_get10(1));

//        usb_putc('\n');

        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 ? 2:1)-1;

                }

        }

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

}