 
 /***********************************************************************
  * Adapted from Exp6_2_LineFollowing_IRSensors -- RedBot Experiment 6
- *
- * This code reads the three line following sensors on A3, A6, and A7
- * and prints them out to the Serial Monitor. Upload this example to your
- * RedBot and open up the Serial Monitor by clicking the magnifying glass
- * in the upper-right hand corner.
- *
- * This is a real simple example of a line following algorithm. It has 
- * a lot of room for improvement, but works fairly well for a curved track. 
- * It does not handle right angles reliably -- maybe you can come up with a 
- * better solution?
- * 
- * This sketch was written by SparkFun Electronics,with lots of help from
- * the Arduino community. This code is completely free for any use.
- *
- * 18 Reb 2015 B. Huang
  ***********************************************************************/
 
 #include <RedBot.h>


void loop()
{
  Serial.print(left.read());
  Serial.print("\t");  // tab character
  Serial.print(center.read());
  Serial.print("\t");  // tab character
  Serial.print(right.read());
  Serial.println();

  // if on the line drive left and right at the same speed (left is CCW / right is CW)
  if(center.read() < LINETHRESHOLD)
  {
    leftSpeed = -SPEED; 
    rightSpeed = SPEED;
  }
   
  // if the line is under the right sensor, 
  // adjust relative speeds to turn to the right
  else if(right.read() < LINETHRESHOLD)
  {
    leftSpeed = -(SPEED + 50);
    rightSpeed = SPEED - 50;
  }

  // if the line is under the left sensor, adjust relative speeds to turn to the left
  else if(left.read() < LINETHRESHOLD)
  {
    leftSpeed = -(SPEED - 50);
    rightSpeed = SPEED + 50;
  }
    
  // if all sensors are on black or up in the air, stop the motors.
  // otherwise, run motors given the control speeds above.
  if((left.read() < LINETHRESHOLD) && 
     (center.read() < LINETHRESHOLD) && (right.read() < LINETHRESHOLD) )
  {
    motors.stop();
  }
  else
  {
    motors.leftMotor(leftSpeed);
    motors.rightMotor(rightSpeed);   
  }
  delay(0);  // add a delay to decrease sensitivity.
}

void calibrate () {
  double l_accum;
  double c_accum;
  double r_accum;
  double avg;

  for (int i = 0; i<5000; i++){
    l_accum += left.read();
    c_accum += center.read();
    r_accum += right.read();
  }

  avg = (l_accum + c_accum + r_accum)/15000;
  Serial.println(avg);
  LINETHRESHOLD = avg * 1/2;
  Serial.println(LINETHRESHOLD);
  motors.drive(255);
  delay(10);
  motors.stop();
}

RedBotSensor center = RedBotSensor(A6); // initialize a center sensor object on A6
RedBotSensor right = RedBotSensor(A7);  // initialize a right sensor object on A7

#define SPEED 120  // sets the nominal speed. Set to any number from 0 - 255.

#define SPEED_ADJUSTMENT 30

#define COUNTER_MAX 20

// These thresholds are manually configured.
#define TAPE_THRESHOLD  600 // below this value, we are looking at the tape
#define BLUE_THRESHOLD  750 // above the TAPE_THRESHOLD and below this threshold, we are looking at blue carpet
#define BLACK_THRESHOLD 850 // above this threshold, we are looking at black carpet

#define NOT_SET 0
#define TAPE_STATE 1
#define NOT_TAPE_STATE 2

RedBotMotors motors;
int leftSpeed;   // variable used to store the leftMotor speed
int rightSpeed;  // variable used to store the rightMotor speed

bool driftingRight = false;
bool driftingLeft = false;

void setup()
{
  Serial.begin(9600);
  Serial.println("Line Following Startup");  
  Serial.println("------------------------------------------");
  delay(2000);
  Serial.println("IR Sensor Readings: ");
