// Linus the Line-bot
// Follows a Black line on a White surface (poster-board and electrical tape).
// Code by JDW 2010 – feel free to modify.
// Adapted Dec 2012. R. Beatty, K. MacMillan, J. Arbesman, M. Kruk & C. D'Arcy

#include <L293D.h>  // this includes the Afmotor library for the motor-controller

L293D right(9,8,7);  // enable, input1, input2
L293D left(11,12,13);// enable, input1, input2

// Create variables for sensor readings
int sensors[] ={
  0,0,0,0,0};
// You can change the min/max values below to fine tune each sensor on your bot
int mins[] = {
  180,330,244,294,250};
int maxs[] = {
  524,560,600,690,620};
// Create variables for adjusted readings
int adjs[5];

// this threshold defines when the sensor is reading the black line
int lower_threshold = 0; 
// value to define a middle threshold (half of the total 255 value range)
int threshold = 128; 
// this threshold defines when the sensor is reading the white poster board
int upper_threshold = 230;

// this value sets the maximum speed of linus (255 = max). 
int speed_value = 255  ;  // 255
// end of changeable variables

void setup()  
{
  //  left.forward(speed_value);
  //  right.forward(speed_value);
  Serial.begin(9600); // start serial monitor to see sensor readings
  //  Serial.println("Test:");
}

void update_sensors(){
  for(int i=0; i<5; i++){
    sensors[i] = analogRead(4-i); 
    adjs[i] = map(sensors[i], mins[i], maxs[i], 0, 255);
    adjs[i] = constrain(adjs[i], 0, 255);
  }
}

void loop(){

  update_sensors(); // update sensors 

    // first, check the value of the center sensor
  if (adjs[2] < lower_threshold){

    // if center sensor value is below threshold, check surrounding sensors
    if (adjs[1] > threshold && adjs[3] > threshold){

      // if all sensors check out, drive forward	
      left.forward(speed_value);
      right.forward(speed_value);
    }
    // you want the bot to stop when it reaches the black box.

    else if (adjs[0] < 1){
      if (adjs[1] < 1){
        if (adjs[2] < 1){
          if (adjs[3] < 1){
            if (adjs[4] < 1){

              //  if all sensors are reading black, stop Linus.
              left.stop();
              right.stop();

            }
          }
        }
      }
    }

  }

  // otherwise, the center sensor is above the threshold
  // so we need to check what sensor is above the black line 

  else {


    // first check sensors 0
    if (adjs[0] < upper_threshold && adjs[4] > upper_threshold){
      left.stop();
      right.forward(speed_value);
    }  

    // then check sensor 4
    else if (adjs[0] > upper_threshold && adjs[4] < upper_threshold){
      left.forward(speed_value);
      right.stop();
    }  

    // if not sensor 0 or 4, then check sensor 1
    else if (adjs[1] < upper_threshold && adjs[3] > upper_threshold){
      left.stop();
      right.forward(speed_value);
    }

    // if not sensor 1, then check sensor 3
    else if (adjs[1] > upper_threshold && adjs[3] < upper_threshold){
      left.forward(speed_value);
      right.stop();
    }
  }

  // displaySensors();
  //displayAdjusted();

}


void displaySensors(){
  ///// Print raw values for each sensor
  for(int i=0; i<5; i++){
    Serial.print("sensor ");
    Serial.print(i);
    Serial.print(": ");
    Serial.print(sensors[i]);
    Serial.print("  -  ");
  }
  Serial.println();
}

void displayAdjusted(){
  ///// Print adjusted values for each sensor
  for(int i=0; i<5; i++){
    Serial.print("adj ");
    Serial.print(i);
    Serial.print(": ");
    Serial.print(adjs[i]);
    Serial.print("  -  ");
  }
  Serial.println();

}
// end of code