Colony

/**

 * @file lineFollow.c

 * @defgroup lineFollwing Line Following

 *

 * Takes care of following a line. Running this program is done by calling the

 * init() function and then the lineFollow(speed) command.  However, direct use

 * of this class is discouraged as its behavior is used by lineDrive.c, which

 * extends this class to provide behavior functionality.

 *

 * @author Dan Jacobs and the Colony Project

 * @date 11-1-2010

 */

#include "lineFollow.h"

//! The number of bits expected in a barcode

#define CODESIZE 5 

#define LINE_COLOR 200

/**

 * Helps with debugging.

 * 0 - no debug output.

 * 1 - print out buckets

 * 2 - print out line sensor readings

 */

#define DBG_LINEFOLLOW 2

//! Anything lower than this value is white

int GREY_THRESHOLD = 300;

//! Anything higher than this value is black

int BLACK_THRESHOLD = 750;

int countHi = 0;

int countLo = 0;

int maxAvg, avg;

// Everything has a dimension of 2 for left and right readings

int barCode[2][ CODESIZE ];

int barCodePosition[2]={0};

int duration[2] = {0};

int lastColor[2] = {0};

char isReset[2] = {1};

int lastReadings[2][ NUM_READINGS ] = {{0}};

int lastReadingsPtr[2] = {0};

int numLast[2][4] = { {0, 0, 0, NUM_READINGS}, {0, 0, 0, NUM_READINGS} };

int bitColor[2] = {0};

int turnDistance=0;

//! Counts the number of full line readings before we determine an intersection

int intersectionFilter=0;

int disableBarCode=0;

//! Keeps track of where the encoder of one motor started, for use in turns.

int encoderStart = -1;

int encoderReset = 0;   // 0 if encoderStart has no value set

void lineFollow_init()

{

    int i, j, curReading;

    int lowGrey = 1000, highGrey = 0, lowBlack = 1000,

        highBlack = 0;

        analog_init(0);

    encoders_init();

        lost = 0;

        intersectionFilter=0;

        disableBarCode=0;

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

    {

        for(j=0; j<NUM_READINGS; j++)

        {

            lastReadings[i][j] = BAD_READING;

        }

        isReset[i] = 1;

    }

    // Calibrate thresholds

    orb_set_color(YELLOW);

    delay_ms(2000);

    orb_set_color(BLUE);

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

    {

        curReading = read_line(LEFT_SENSOR + 6);

        if(curReading < lowGrey)

            lowGrey = curReading;

        if(curReading > highGrey)

            highGrey = curReading;

        delay_ms(20);

    }

    orb_set_color(YELLOW);

    delay_ms(2000);

    orb_set_color(GREEN);

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

    {

        curReading = read_line(LEFT_SENSOR + 6);

        if(curReading < lowBlack)

            lowBlack = curReading;

        if(curReading > highBlack)

            highBlack = curReading;

        delay_ms(20);

    }

    orbs_set(0,0,0,0,0,0);

    GREY_THRESHOLD = (lowGrey + highGrey) / 2;

    BLACK_THRESHOLD = (highBlack + lowBlack) / 2;

    usb_puts("Grey: ");

    usb_puti(lowGrey);

    usb_puts(", ");

    usb_puti(highGrey);

    usb_puts("\nBlack: ");

    usb_puti(lowBlack);

    usb_puts(", ");

    usb_puti(highBlack);

    usb_puts("\nThresholds: ");

    usb_puti(GREY_THRESHOLD);

    usb_puts(", ");

    usb_puti(BLACK_THRESHOLD);

    usb_puts("\n\n");

    delay_ms(1500);

    //numLast = { {0, 0, 0, NUM_READINGS}, {0, 0, 0, NUM_READINGS} };

}

/** 

 * Follows a line at the given speed.

 * @param speed The speed with which to follow the line.

 */

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]=0;

            barCodePosition[1]=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 we're running over a line, stop reading barcodes for a sec

        if(disableBarCode-- > 0)

        {

        // Return intersection once we cross the line

                if(disableBarCode) return NOBARCODE;

                return INTERSECTION;

        }

        updateBarCode();

        return getBarCode();

}

/**

 * Implements the left merge, assuming a line exists to the left.  Works by

 * turning off the line at an increasing angle and waiting to hit another line

 * on the left.

 */

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;

}

/**

 * Implements the right merge, assuming a line exists to the right.  Works by

 * turning off the line at an increasing angle and waiting to hit another line

 * on the right.

 */

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;

}

/**

 * Turns left at a cross of two lines.  Assumes that we are at lines in a cross

 * pattern, and turns until it sets straight on the new line.

 * @return 0 if turn finishes otherwise 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;*/

        motor_l_set(BACKWARD,200);

        motor_r_set(FORWARD,200);

        if(!encoderReset)

        {

            encoderStart = encoder_get_x(RIGHT);

            encoderReset = 1;

        }

        if(encoder_get_x(RIGHT) < encoderStart)

        {

            encoderStart = 0;

            // Temporary: display an "error message" in case of overflow.

            // Using this for debugging, take it out soon!

            motor_l_set(FORWARD,0);

            motor_r_set(FORWARD,0);

            //orb_set_color(WHITE);

            delay_ms(2000);

        }

        if(encoder_get_x(RIGHT) - encoderStart > 300)

        {

            encoderReset = 0;

            return 0;

        }

        return 1;

}

/**

 * Turns right at a cross of two lines.  Assumes that we are at lines in a cross

 * pattern, and turns until it sets straight on the new line.

 * @return 0 if the turn finishes otherwise 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[1] != CODESIZE)

        return NOBARCODE ;

    else

    {

        int temp = 0;

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

            temp += (barCode[1][i] << i);

        barCodePosition[1] = 0;

        return temp;

    }

}

void updateLine(int* values)

{        

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

                values[i] = (read_line(4-i) < LINE_COLOR ? 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; // Subtract 4 to center the index around the center.

}

void printBuckets(int i)

{

    int j;

    usb_puts("LP[");

    usb_puti(i);

    usb_puts("]: ");

    usb_puti(lastReadingsPtr[i]);

    usb_puts(", Totals: ");

    for(int j=0; j<=3; j++)

    {

        usb_puts("[");

        usb_puti(j);

        usb_puts("]: ");

        usb_puti(numLast[i][j]);

        usb_puts(" ");

    }

    usb_puts("\t ");

    usb_puts("\n");

}

void addToBuckets(int curColor, int i)

{

    int oldest = lastReadings[i][lastReadingsPtr[i]];

    numLast[i][oldest]--;

    lastReadings[i][lastReadingsPtr[i]] = curColor;

    lastReadingsPtr[i] = (lastReadingsPtr[i]+1) % NUM_READINGS;

    numLast[i][curColor]++;

    if(DBG_LINEFOLLOW == 1)

    {

        printBuckets(i);

    }

}

void updateBarCode()

{

    // USING THESE GLOBAL VARIABLES

    // global int duration = 0;

    // global int lastColor = 0;

    // global int barCodePosition = 0;

    // global char isReset = 0;

    // Just uses one sensor for now

    for(int i = /*RIGHT*/LEFT_SENSOR; i <= LEFT_SENSOR; i++)

    {

        // Add 6 to convert left (1) and right (0) to sensor 6 and 7

        int curReading = read_line(i + 6);

        int curColor;

        if(curReading > BLACK_THRESHOLD)

        {

            curColor = LBLACK;

        }

        else if(curReading < GREY_THRESHOLD)

        {

            curColor = LWHITE;

        }

        else

        {

            curColor = LGREY;

        }

        // Keep track of this reading

        addToBuckets(curColor, i);

        // Just an error check

        if(barCodePosition[i] > CODESIZE)

        {

            barCodePosition[i] = 0;

        }

        // We now edit curColor to use the majority of the last buckets.

        if(numLast[i][1] > NUM_READINGS / 2)

        {

            curColor = LGREY;

        }

        else if(numLast[i][2] > NUM_READINGS / 2)

        {

            curColor = LBLACK;

        }

        else if(numLast[i][0] > NUM_READINGS / 2)

        {

            curColor = LWHITE;

            duration[i]++;

        }

        else

        {

            curColor = BAD_READING;

        }

        // Print out the current reading and label, if in debug mode

        if(DBG_LINEFOLLOW == 2)

        {

            switch(curColor)

            {

                case LBLACK: usb_puts("LBLACK.\t"); break;

                case LGREY: usb_puts("LGREY.\t\t"); break;

                case LWHITE: usb_puts("LWHITE.\t\t\t"); break;

            }

            usb_puti(curReading);

            usb_puts("\n");

        }

        if(curColor != BAD_READING)

        {

            // Now we assume our reading is significant - a bit, or a white space

            if(bitColor[i] == LGREY || bitColor[i] == LWHITE)

                bitColor[i] = curColor;

            // Only read a value if we have read 0 first (isReset == 1)

            if(isReset[i] && (curColor == LBLACK || curColor == LGREY) )

            {

                isReset[i] = 0;

                duration[i] = 0;

            }

            else if(curColor == LWHITE)

            {

                if(!isReset[i])

                {

                    barCode[i][barCodePosition[i]++] = 

                        (bitColor[i] == LBLACK) ? 1 : 0;

                    usb_puts("Reset. Read bit: ");

                    usb_puts(((bitColor[i] == LBLACK) ? "BLACK" : "GREY"));

                    usb_puts("\t");

                    usb_puts(((curColor==LWHITE) ? "LWHITE" : "NOTWHITE"));

                    usb_puts("\n");

                    bitColor[i] = LWHITE;

                }

                isReset[i] = 1;

                orb_set(0, 0, 0);

            }

        }

        if(curColor == LWHITE && duration[i] > TIMEOUT_DURATION 

            && barCodePosition[i] != 0)

        {

            usb_puts("TIMED OUT. BARCODE READER RESET.\n");

            usb_puts("Encoders: ");

            usb_puti(encoder_get_dx(LEFT));

            usb_puts(", ");

            usb_puti(encoder_get_dx(RIGHT));

            usb_puts("\n");

            barCodePosition[i] = 0;

            duration[i] = 0;

            isReset[i] = 1;

        }

    }

}

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

}