//line follower 10/12/10

#include <LiquidCrystal.h>
int backLight = 37;    // pin 37 will control the backlight
int s1, s2, s3, s4, s5, s6;
int motorls, motorrs;       //motor speed
int dir;
float stavg;                // sensor average
float st;
int motordir1a= 24;          //motor1 direction digital pin 24
int motordir1b= 25;          //motor1 direction digital pin 25
int motordir2a= 22;          //motor2 direction digital pin 22
int motordir2b= 23;          //motor2 direction digital pin 23
int motorspd1= 8;            //motor1 speed on PWM pin 8
int motorspd2= 2;            //motor2 speed on PWM pin 2
int chk = 0;

LiquidCrystal lcd(30, 31, 32, 33, 34, 35, 36);

void setup()   {                
  Serial.begin(9600); 
  analogWrite(motorspd1, 0);  //set inital speed to 0 
  analogWrite(motorspd2, 0);  
  digitalWrite(motordir1a, LOW);                 //set direction forward 
  digitalWrite(motordir1b, HIGH);                //set direction forward
  digitalWrite(motordir2a, LOW);                 //set direction forward 
  digitalWrite(motordir2b, HIGH);                //set direction forward
  pinMode(backLight, OUTPUT);
  digitalWrite(backLight, HIGH); // turn backlight on. Replace 'HIGH' with 'LOW' to turn it off.
  lcd.begin(16,2);              // columns, rows.  use 16,2 for a 16x2 LCD, etc.
  lcd.clear();    

}

void loop() {

  pinMode(42, OUTPUT);     //set sensor pins as digital pins and as output
  digitalWrite(42, HIGH);  //write a 1 to each sensor
  delayMicroseconds(10);  // delay for 10 us
  pinMode(42, INPUT);      //set digital pins as input to read sensors
  delay(2);
  s1 = digitalRead(42);

  pinMode(44, OUTPUT);    
  digitalWrite(44, HIGH); 
  delayMicroseconds(10);  // delay for 10 us
  pinMode(44, INPUT);       
  delay(2);
  s2 = digitalRead(44); 

  pinMode(46, OUTPUT);    
  digitalWrite(46, HIGH);
  delayMicroseconds(10);  // delay for 10 us
  pinMode(46, INPUT);
  delay(2);
  s3 = digitalRead(46);

  pinMode(48, OUTPUT);    
  digitalWrite(48, HIGH);
  delayMicroseconds(10);  // delay for 10 us
  pinMode(48, INPUT);    
  delay(2);
  s4 = digitalRead(48);

  pinMode(50, OUTPUT);    
  digitalWrite(50, HIGH);  
  delayMicroseconds(10);  // delay for 10 us
  pinMode(50, INPUT);    
  delay(2);
  s5 = digitalRead(50);

  pinMode(52, OUTPUT); 
  digitalWrite(52, HIGH);
  delayMicroseconds(10);  // delay for 10 us
  pinMode(52, INPUT);
  delay(2);               //wait 2ms before reading
  s6 = digitalRead(52);

  //Serial.print(s1);
  //Serial.print(" ");
  //Serial.print(s2);
  //Serial.print(" ");
  //Serial.print(s3);
  //Serial.print(" ");
  //Serial.print(s4);
  //Serial.print(" ");
  //Serial.print(s5);
  //Serial.print(" ");
  //Serial.print(s6);
  //Serial.print(" ");
  //Serial.println();

  lcd.setCursor(0,0); 
  lcd.print(s1);
  lcd.setCursor(2,0);
  lcd.print(s2);
  lcd.setCursor(4,0);
  lcd.print(s3);
  lcd.setCursor(6,0);
  lcd.print(s4);
  lcd.setCursor(8,0); 
  lcd.print(s5);
  lcd.setCursor(10,0);
  lcd.print(s6);


  if ((s1==1) && (s2==0) && (s3==0) && (s4==0) && (s5==0) && (s6==0)) {  //way to the left
    dir = 1;
    digitalWrite(motordir1a, HIGH);                 //set direction forward 
    digitalWrite(motordir1b, LOW);                //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);                //set direction forward
    motorls = 25;
    motorrs = 30; 
  }

  if ((s1==1) && (s2==1) && (s3==0) && (s4==0) && (s5==0) &&(s6==0)) {  //way to the left
    dir = 1;
    digitalWrite(motordir1a, HIGH);                 //set direction forward 
    digitalWrite(motordir1b, LOW);                    //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);                //set direction forward
    motorls = 25;
    motorrs = 30; 
  } 

  if ((s1==0) && (s2==1) && (s3==0) && (s4==0) && (s5==0) && (s6==0)) { //more to the left
    dir = 2;
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);                //set direction forward
    motorls = 25;
    motorrs = 35; 
  }

  if ((s1==0) && (s2==1) && (s3==1) && (s4==0) && (s5==0) && (s6==0)) { //more to the left
    dir = 2;
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);                //set direction forward
    motorls = 25;
    motorrs = 35; 
  }

  if ((s1==0) && (s2==0) && (s3==1) && (s4==0) && (s5==0) && (s6==0)) { //dead on
    dir = 3;
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);                //set direction forward
    motorls = 30;
    motorrs = 35; 
  } 

  if ((s1==0) && (s2==0) && (s3==1) && (s4==1) && (s5==0) && (s6==0)) { //dead on
    dir = 3;
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);                //set direction forward
    motorls = 35;
    motorrs = 35; 
  } 

  if ((s1==0) && (s2==0) && (s3==0) && (s4==1) && (s5==0) && (s6==0)) { //little to the right
    dir = 3;
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);                //set direction forward 
    motorls = 35;
    motorrs = 30; 
  }  

  if ((s1==0) && (s2==0) && (s3==0) && (s4==1) && (s5==1) && (s6==0)) { //little to the right
    dir = 4;
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);                //set direction forward 
    motorls = 35;
    motorrs = 25; 
  }  

  if ((s1==0) && (s2==0) && (s3==0) && (s4==0) && (s5==1) && (s6==0)) { //way to the right
    dir = 4;
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, HIGH);                //revsere motor 2 
    digitalWrite(motordir2b, LOW);                 //revsere motor 2 
    motorls = 35;
    motorrs = 25; 
  } 

  if ((s1==0) && (s2==0) && (s3==0) && (s4==0) && (s5==1) && (s6==1)) { //way to the right
    dir = 5;
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, HIGH);                //revsere motor 2 
    digitalWrite(motordir2b, LOW);                  //set direction forward
    motorls = 30;
    motorrs = 25; 
  } 

  if ((s1==0) && (s2==0) && (s3==0) && (s4==0) && (s5==0) && (s6==1)) { //way to the right
    dir = 5;
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, HIGH);                //revsere motor 2 
    digitalWrite(motordir2b, LOW);                 //revsere motor 2 
    motorls = 30;
    motorrs = 25; 
  } 

  //=======================================================================================================


  if ((s1==0) && (s2==0) && (s3==0) && (s4==0) && (s5==0) && (s6==0)) { //at end of line
    chk = chk + 1;
    if (chk==2) {
      digitalWrite(motordir1a, LOW);                 //set direction forward 
      digitalWrite(motordir1b, HIGH);                //set direction forward
      digitalWrite(motordir2a, HIGH);                //reverse motor2 
      digitalWrite(motordir2b, LOW);                 //reverse motor2
      analogWrite(motorspd2, 60);   
      analogWrite(motorspd1, 60); 
      delay(3000);
      digitalWrite(motordir1a, LOW);                 //set direction forward 
      digitalWrite(motordir1b, HIGH);                //set direction forward
      digitalWrite(motordir2a, LOW);                 //set direction forward 
      digitalWrite(motordir2b, HIGH);                //set direction forward
      motorls = 35;
      motorrs = 35;
      chk = 0; 
    } 
    /*}
     else if (dir == 1) {            //went to far left? go right
     digitalWrite(motordir1a, LOW);                 //set direction forward 
     digitalWrite(motordir1b, HIGH);                //set direction forward
     digitalWrite(motordir2a, HIGH);                //revsere motor 2 
     digitalWrite(motordir2b, LOW);                 //revsere motor 2 
     motorls = 35;
     motorrs = 20; 
     }
     else if (dir == 2) {          //went to far left?, go right
     digitalWrite(motordir1a, LOW);                 //set direction forward 
     digitalWrite(motordir1b, HIGH);                //set direction forward
     digitalWrite(motordir2a, LOW);                 //set direction forward 
     digitalWrite(motordir2b, HIGH);                //set direction forward 
     motorls = 45;
     motorrs = 30; 
     }
     else if (dir == 4) {        //went to far right?, go left
     digitalWrite(motordir1a, LOW);                 //set direction forward 
     digitalWrite(motordir1b, HIGH);                //set direction forward
     digitalWrite(motordir2a, LOW);                 //set direction forward 
     digitalWrite(motordir2b, HIGH);                //set direction forward
     motorls = 30;
     motorrs = 45; 
     }
     else if ( dir == 5) {        //went to far right?, go left
     digitalWrite(motordir1a, LOW);                 //set direction forward 
     digitalWrite(motordir1b, HIGH);                //set direction forward
     digitalWrite(motordir2a, LOW);                 //set direction forward 
     digitalWrite(motordir2b, HIGH);                //set direction forward
     motorls = 20;
     motorrs = 35; }
     */
  }

  if ((s1==0) && (s4==1) && (s5==1) && (s6==1)) { //at right turn, turn right
    analogWrite(motorspd1, 0);         //STOP MOTORS
    analogWrite(motorspd2, 0);  
    digitalWrite(motordir1a, HIGH);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, HIGH);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);

    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);  
    analogWrite(motorspd1, 25); 
    analogWrite(motorspd2, 25);   
    delay(500);
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, HIGH);                 //set direction forward 
    digitalWrite(motordir2b, LOW);                //set direction forward
    analogWrite(motorspd1, 45); 
    analogWrite(motorspd2, 45);   
    delay(2300);
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);
    motorls = 35;
    motorrs = 35; 
  }

  if ((s1==1) && (s2==1) && (s3==1) && (s4==1) && (s5==1) && (s6==1)) { //at INTERESCTION, turn right
    
    digitalWrite(motordir1a, LOW);                 //go forward 1 inch
    digitalWrite(motordir1b, HIGH);                 
    digitalWrite(motordir2a, LOW);                 
    digitalWrite(motordir2b, HIGH);  
    analogWrite(motorspd1, 25); 
    analogWrite(motorspd2, 25);   
    delay(500);
    
    analogWrite(motorspd1, 0);         //STOP MOTORS
    analogWrite(motorspd2, 0);  
    digitalWrite(motordir1a, HIGH);                 
    digitalWrite(motordir1b, HIGH);                
    digitalWrite(motordir2a, HIGH);                  
    digitalWrite(motordir2b, HIGH);  

    pinMode(42, OUTPUT);     //set sensor pins as digital pins and as output
    digitalWrite(42, HIGH);  //write a 1 to each sensor
    delayMicroseconds(10);  // delay for 10 us
    pinMode(42, INPUT);      //set digital pins as input to read sensors
    delay(2);
    s1 = digitalRead(42);

    pinMode(44, OUTPUT);    
    digitalWrite(44, HIGH); 
    delayMicroseconds(10);  // delay for 10 us
    pinMode(44, INPUT);       
    delay(2);
    s2 = digitalRead(44); 

    pinMode(46, OUTPUT);    
    digitalWrite(46, HIGH);
    delayMicroseconds(10);  // delay for 10 us
    pinMode(46, INPUT);
    delay(2);
    s3 = digitalRead(46);

    pinMode(48, OUTPUT);    
    digitalWrite(48, HIGH);
    delayMicroseconds(10);  // delay for 10 us
    pinMode(48, INPUT);    
    delay(2);
    s4 = digitalRead(48);

    pinMode(50, OUTPUT);    
    digitalWrite(50, HIGH);  
    delayMicroseconds(10);  // delay for 10 us
    pinMode(50, INPUT);    
    delay(2);
    s5 = digitalRead(50);

    pinMode(52, OUTPUT); 
    digitalWrite(52, HIGH);
    delayMicroseconds(10);  // delay for 10 us
    pinMode(52, INPUT);
    delay(2);               //wait 2ms before reading
    s6 = digitalRead(52);
    
    if ((s1==1) && (s2==1) && (s3==1) && (s4==1) && (s5==1) && (s6==1)) //read again to see if at end
      delay(30000);  //at end, stop for 30 secs
    else {                              //not at end, backup and proceed
    //digitalWrite(motordir1a, HIGH);                 //backup 
    //digitalWrite(motordir1b, LOW);                  //
    //digitalWrite(motordir2a, HIGH);                 // 
    //digitalWrite(motordir2b, LOW);  
    //analogWrite(motorspd1, 35); 
    //analogWrite(motorspd2, 35);   
    //delay(700);
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, HIGH);                 //set direction forward 
    digitalWrite(motordir2b, LOW);                //set direction forward
    analogWrite(motorspd1, 45); 
    analogWrite(motorspd2, 45);   
    delay(2300);
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);
    motorls = 35;
    motorrs = 35; }
  }

  if ((s1==1) && (s2==1) && (s3==1) && (s6==0)) { //at left, turn left
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);  
    analogWrite(motorspd1, 25); 
    analogWrite(motorspd2, 25);   
    delay(500);

    pinMode(42, OUTPUT);     //set sensor pins as digital pins and as output
    digitalWrite(42, HIGH);  //write a 1 to each sensor
    delayMicroseconds(10);  // delay for 10 us
    pinMode(42, INPUT);      //set digital pins as input to read sensors
    delay(2);
    s1 = digitalRead(42);

    pinMode(44, OUTPUT);    
    digitalWrite(44, HIGH); 
    delayMicroseconds(10);  // delay for 10 us
    pinMode(44, INPUT);       
    delay(2);
    s2 = digitalRead(44); 

    pinMode(46, OUTPUT);    
    digitalWrite(46, HIGH);
    delayMicroseconds(10);  // delay for 10 us
    pinMode(46, INPUT);
    delay(2);
    s3 = digitalRead(46);

    pinMode(48, OUTPUT);    
    digitalWrite(48, HIGH);
    delayMicroseconds(10);  // delay for 10 us
    pinMode(48, INPUT);    
    delay(2);
    s4 = digitalRead(48);

    pinMode(50, OUTPUT);    
    digitalWrite(50, HIGH);  
    delayMicroseconds(10);  // delay for 10 us
    pinMode(50, INPUT);    
    delay(2);
    s5 = digitalRead(50);

    pinMode(52, OUTPUT); 
    digitalWrite(52, HIGH);
    delayMicroseconds(10);  // delay for 10 us
    pinMode(52, INPUT);
    delay(2);               //wait 2ms before reading
    s6 = digitalRead(52);

  if ((s3==1) || (s4==1) || (s5==1)) {   //continue on straight
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);
    motorls = 35;
    motorrs = 35; }
  else {
    analogWrite(motorspd1, 0);         //STOP MOTORS
    analogWrite(motorspd2, 0);  
    digitalWrite(motordir1a, HIGH);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, HIGH);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);

    digitalWrite(motordir1a, HIGH);                 //backup 
    digitalWrite(motordir1b, LOW);                //
    digitalWrite(motordir2a, HIGH);                 // 
    digitalWrite(motordir2b, LOW);  
    analogWrite(motorspd1, 35); 
    analogWrite(motorspd2, 35);   
    delay(700);
    
    digitalWrite(motordir1a, HIGH);                 //TURN LEFT
    digitalWrite(motordir1b, LOW);                 //TURN LEFT
    digitalWrite(motordir2a, LOW);                 //TURN LEFT 
    digitalWrite(motordir2b, HIGH);                //TURN LEFT
    analogWrite(motorspd1, 45); 
    analogWrite(motorspd2, 45);   
    delay(2300);
    digitalWrite(motordir1a, LOW);                 //set direction forward 
    digitalWrite(motordir1b, HIGH);                //set direction forward
    digitalWrite(motordir2a, LOW);                 //set direction forward 
    digitalWrite(motordir2b, HIGH);
    motorls = 35;
    motorrs = 35; } 
  }

  analogWrite(motorspd1, motorrs); 
  analogWrite(motorspd2, motorls);   
  delay(100);
}