Colony » Colony Scout

/**

 * @file line_follow.cpp

 * @defgroup lineFollwing Line Following

 *

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

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

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

 * extends this class to provide behavior functionality.

 *

 * @author Dan Jacobs and the Colony Project

 * @date 11-1-2010

 */

#include "line_follow.h"

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

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;

//! 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

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

int max(int x, int y){return x<y ? y : x;}

void line_follow::init()

{

  int i, j, curReading;

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

      highBlack = 0;

  analog_init(0);

  encoders_init();

  lost = 0;

  intersectionFilter=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 / 2;

  BLACK_THRESHOLD = (highGrey + 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);

}

/** 

 * Follows a line at the given speed.

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

 */

int line_follow::follow_line(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;

        }

}

/**

 * 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 line_follow::turnLeft()

{

        motors->set_sides(-20, 20, MOTOR_ABSOLUTE);

        if(!encoderReset)

        {

            encoderStart = encoder_get_x(RIGHT);

            encoderReset = 1;

        }

        if(encoder_get_x(RIGHT) < encoderStart)

        {

            encoderStart = 0;

            motors->set_sides(0, 0, MOTOR_ABSOLUTE);

            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 line_follow::turnRight()

{

  /// Set motors using new motors functions

  motors->set_sides(20, -20, MOTOR_ABSOLUTE);

  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;

}

void line_follow::updateLine(int* values)

{        

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

                values[i] = (read_line(4-i) < LINE_COLOR ? LWHITE : LBLACK);

}

int line_follow::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 line_follow::motorLeft(int speed)

{

        int tempspeed = 0;

        if ((speed-=127)>=0)

        {        

                tempspeed = 160+speed*95/128;

                motors->set(MOTOR_LEFT, tempspeed, MOTOR_ABSOLUTE);

        }

        else

        {

                tempspeed = 160+speed*95/128;

                motors->set(MOTOR_LEFT, -tempspeed, MOTOR_ABSOLUTE);

        }

}

void line_follow::motorRight(int speed)

{

        int tempspeed = 0;

        if ((speed-=127)>=0)

        {        

                tempspeed = 160+speed*95/128;

                motors->set(MOTOR_RIGHT, tempspeed, MOTOR_ABSOLUTE);

        }

        else

        {

                tempspeed = 160+speed*95/128;

                motors->set(MOTOR_RIGHT, -tempspeed, MOTOR_ABSOLUTE);

        }

}