//Script for bot to move around and avoid obstacles by turning in new direction

// bot will turn around when he thinks it's too dark

#include <Servo.h> 
#include <AFMotor.h>


Servo servo2;  // create servo object to control a servo 
AF_DCMotor motor1(1, MOTOR12_64KHZ); // create motor #1, 64KHz pwm
AF_DCMotor motor2(2, MOTOR12_64KHZ); // create motor #2, 64KHz pwm
               
 
int pos = 0;    // variable to store the servo position 
int IRpin = 4;  // analog pin for reading the IR sensor
int LDR = 0; // analog pin for reading the LDR (Light Detecting Resistor)
int LED = 13; //digital output pin for LED

int VLeft = 1;
int VRight = 1;
int VCenter = 1;
int DLeft = 300;
int DRight = 300;
int DCenter = 300;
int Dis = 40; // change to change IR distance
float Light = 400;

unsigned long startTime = 0;

void setup() 
{ 
  servo2.attach(9);  // attaches the servo on pin 9 to the servo object 
  servo2.write(90); //center servo
  pinMode(LDR, INPUT);
  pinMode(LED, OUTPUT);
  Serial.begin(9600); // start the serial port (communication)
  
  motor1.setSpeed(52);     // set the speed to 52/255
  motor2.setSpeed(50);     // set the speed to 50/255
  
  delay(3000);
} 
 
 
void loop() 
{ 
Light = analogRead(LDR);

if (Light < 800) {
   digitalWrite(LED,LOW);
   startTime = 0;
  for(pos = 15; pos < 165; pos += 1)  // goes from 0 degrees to 180 degrees 
  {                                  // in steps of 1 degree 
    servo2.write(pos);      // tell servo to go to position in variable 'pos' 
    delay(10);                       // waits 15ms for the servo to reach the position 
    float volts = analogRead(IRpin)*0.0048828125;   // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
    float distance = 65*pow(volts, -1.10)/2;          // worked out from graph 65 = theretical distance / (1/Volts)S 
    Serial.println(distance);                       // print the distance

    // Will store a go (1) or no go (0) for each direction
   if ((pos < 65 && pos > 15) && int(distance) < Dis) {
     VLeft = 0;
   } else if ((pos < 65 && pos > 15) && int(distance) > Dis) {
     VLeft = 1;
   }
   if ((pos < 115 && pos > 65) && int(distance) < Dis) {
     VCenter = 0;
   } else if ((pos < 115 && pos > 65) && int(distance) > Dis) {
     VCenter = 1;
   }
   if ((pos < 165 && pos > 115) && int(distance) < Dis) {
     VRight = 0;
   } else if ((pos < 165 && pos > 115) && int(distance) > Dis) {
     VRight = 1;
   }
  
 // Will store actual distance on each direction
   if (pos == 40) {
     DLeft = int(distance);
   } else if (pos == 90) {
     DCenter = int(distance);
   } else if (pos == 140) {
     DRight = int(distance);
   }
   
   
  
   if (VCenter == 1) {
     if (int(distance) > Dis && int(distance) < (Dis*1.25)) {
       motor1.setSpeed(52);     
       motor2.setSpeed(50);
       motor1.run(FORWARD);      // turn it on going forward
       motor2.run(FORWARD);      // turn it on going forward
     } else if (int(distance) > (Dis*1.25)) {
       motor1.setSpeed(73);     
       motor2.setSpeed(70);
       motor1.run(FORWARD);      // turn it on going forward
       motor2.run(FORWARD); 
     } else if (int(distance) > (Dis*1.5)) {
       motor1.setSpeed(106);     
       motor2.setSpeed(100);
       motor1.run(FORWARD);      // turn it on going forward
       motor2.run(FORWARD); 
     } else if (int(distance) > (Dis*2)) {
       motor1.setSpeed(210);     
       motor2.setSpeed(200);
       motor1.run(FORWARD);      // turn it on going forward
       motor2.run(FORWARD); 
     }
   } else if (VCenter == 0 && VRight == 1 && VLeft == 0) {
     motor1.setSpeed(52);     
     motor2.setSpeed(50);
     motor1.run(FORWARD);    // turn it on going forward
     motor2.run(BACKWARD);      // stop motor to turn right
   } else if (VCenter == 0 && VRight == 0 && VLeft == 1) {
     motor1.setSpeed(52);     
     motor2.setSpeed(50);
     motor1.run(BACKWARD);    // stop motor to turn left
     motor2.run(FORWARD);      // turn it on going forward
   } else if (VCenter == 0 && VRight == 1 && VLeft == 1) {
     if (DLeft > DRight) {
      motor1.setSpeed(52);     
      motor2.setSpeed(50);
      motor1.run(BACKWARD);    // stop motor to turn left
      motor2.run(FORWARD);      // turn it on going forward
     } else if (DRight > DLeft) {
       motor1.setSpeed(52);     
       motor2.setSpeed(50);
       motor1.run(FORWARD);    // turn it on going forward
       motor2.run(BACKWARD);      // stop motor to turn right
     }
   } else if (VCenter == 0 && VRight == 0 && VLeft == 0) {
       motor1.setSpeed(52);     
       motor2.setSpeed(50); 
       motor1.run(FORWARD);    // turn it on going forward
       motor2.run(BACKWARD);      // stop motor to turn right
   }
  } 

  for(pos = 165; pos>=15; pos-=1)     // goes from 180 degrees to 0 degrees 
  {                                
    servo2.write(pos);              // tell servo to go to position in variable 'pos' 
    delay(10);                       // waits 15ms for the servo to reach the position
    float volts = analogRead(IRpin)*0.0048828125;   // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
    float distance = (65*pow(volts, -1.10))/2;          // worked out from graph 65 = theretical distance / (1/Volts)S  
    Serial.println(distance);                       // print the distance
      
   // Will store a go (1) or no go (0) for each direction
   if ((pos < 65 && pos > 15) && int(distance) < Dis) {
     VLeft = 0;
   } else if ((pos < 65 && pos > 15) && int(distance) > Dis) {
     VLeft = 1;
   }
   if ((pos < 115 && pos > 65) && int(distance) < Dis) {
     VCenter = 0;
   } else if ((pos < 115 && pos > 65) && int(distance) > Dis) {
     VCenter = 1;
   }
   if ((pos < 165 && pos > 115) && int(distance) < Dis) {
     VRight = 0;
   } else if ((pos < 165 && pos > 115) && int(distance) > Dis) {
     VRight = 1;
   }
  
 // Will store actual distance on each direction
   if (pos == 40) {
     DLeft = int(distance);
   } else if (pos == 90) {
     DCenter = int(distance);
   } else if (pos == 140) {
     DRight = int(distance);
   }
   
   
     if (VCenter == 1) {
     if (int(distance) > Dis && int(distance) < (Dis*1.25)) {
       motor1.setSpeed(52);     
       motor2.setSpeed(50);
       motor1.run(FORWARD);      // turn it on going forward
       motor2.run(FORWARD);      // turn it on going forward
     } else if (int(distance) > (Dis*1.25)) {
       motor1.setSpeed(73);     
       motor2.setSpeed(70);
       motor1.run(FORWARD);      // turn it on going forward
       motor2.run(FORWARD); 
     } else if (int(distance) > (Dis*1.5)) {
       motor1.setSpeed(106);     
       motor2.setSpeed(100);
       motor1.run(FORWARD);      // turn it on going forward
       motor2.run(FORWARD); 
     } else if (int(distance) > (Dis*2)) {
       motor1.setSpeed(210);     
       motor2.setSpeed(200);
       motor1.run(FORWARD);      // turn it on going forward
       motor2.run(FORWARD); 
     }
   } else if (VCenter == 0 && VRight == 1 && VLeft == 0) {
     motor1.setSpeed(52);     
     motor2.setSpeed(50);
     motor1.run(FORWARD);    // turn it on going forward
     motor2.run(BACKWARD);      // stop motor to turn right
   } else if (VCenter == 0 && VRight == 0 && VLeft == 1) {
     motor1.setSpeed(52);     
     motor2.setSpeed(50);
     motor1.run(BACKWARD);    // stop motor to turn left
     motor2.run(FORWARD);      // turn it on going forward
   } else if (VCenter == 0 && VRight == 1 && VLeft == 1) {
     if (DLeft > DRight) {
      motor1.setSpeed(52);     
      motor2.setSpeed(50);
      motor1.run(BACKWARD);    // stop motor to turn left
      motor2.run(FORWARD);      // turn it on going forward
     } else if (DRight > DLeft) {
       motor1.setSpeed(52);     
       motor2.setSpeed(50);
       motor1.run(FORWARD);    // turn it on going forward
       motor2.run(BACKWARD);      // stop motor to turn right
     }
   } else if (VCenter == 0 && VRight == 0 && VLeft == 0) {
       motor1.setSpeed(52);     
       motor2.setSpeed(50); 
       motor1.run(FORWARD);    // turn it on going forward
       motor2.run(BACKWARD);      // stop motor to turn right
   }
  } 
  
} else if (Light > 800) {
  if (startTime == 0) {
  startTime = millis();
  }
  if ((millis() - startTime) < 10000) {
   digitalWrite(LED, HIGH); 
  //Set all directions to clear
   VLeft = 1;
   VRight = 1;
   VCenter = 1;
   motor1.setSpeed(52);     
   motor2.setSpeed(50); 
   motor1.run(BACKWARD);
   motor2.run(BACKWARD); 
   delay(1500); 
   motor1.run(BACKWARD);
   motor2.run(FORWARD);
   delay(2500); 
   
   
  } else if ((millis() - startTime) > 10000) {
    motor1.run(BACKWARD);
    motor2.run(BACKWARD);
    delay(5000);
    motor1.run(RELEASE);
    motor2.run(RELEASE);
  }
  
  
}
}