#include <SPI.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
#include <Servo.h> 

const int trigPin = 7;
const int echoPin = 6;
int Ymax = 128;
int Xmax = 128;
int base = 8;
int pos = base+6;
int deg=0;
int x;
int val =200;
int j = 2;
Servo myServo; 
LiquidCrystal_I2C lcd(0x27,  16, 2);

long duration;
int distance;
int angle;

void setup(void)
{
  delay(1000);
  myServo.write(80);\
  lcd.init(); //initialize the lcd
  lcd.backlight(); //open the backlight
  
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  Serial.begin(9600);
  myServo.attach(12); // Defines on which pin is the servo motor attached

}

void loop(void)
{
  
 fix(); 
   for ( x=80; x >= 10; x--){  
     distance = calculateDistance();
     Serial.println(distance); 
     angle = map(x, 80, 10, 15,165); 
     myServo.write(angle);

  if (distance < 40){
    int f = x+6;

    lcd.setCursor(0,0);
    lcd.print("OBJECT AT: ");
    lcd.print(distance); 
    lcd.print("cm  "); 
    delay(10);
    lcd.setCursor(0,1);
    lcd.print("Angle : ");
    lcd.print(deg);
    lcd.print((char)223);
    lcd.print("  ");
    delay(2000);
  }
  else{
    lcd.setCursor(0,0);
    lcd.print("NOTHING IN RANGE");
    delay(10);
    lcd.setCursor(0,1);
    lcd.print("Angle : ");
    lcd.print(deg);
    lcd.print((char)223);
    lcd.print("  ");
    delay(80);
  }

  int d = x+1;
  int c = x+2;
  int b = x+3;
  int a = x+4;
  int e = x+5;
  deg = map (x, 80, 10 , 0, 180); 
  }
   
  fix();
  for ( x=10; x <= 80; x++){  
    distance = calculateDistance();
    Serial.println(distance); 
    angle = map(x, 10, 80, 165,15); 
    myServo.write(angle);
  if (distance < 20){
    int e = x-5;
    lcd.setCursor(0,0);
    lcd.print("OBJECT AT: ");
    lcd.print(distance); 
    lcd.print("cm  "); 
    delay(10);
    lcd.setCursor(0,1);
    lcd.print("Angle : ");
    lcd.print(deg);
    lcd.print((char)223);
    lcd.print("  ");
    delay(80);
  }

  else{
    lcd.setCursor(0,0);
    lcd.print("NOTHING IN RANGE");
    delay(10);
    lcd.setCursor(0,1);
    lcd.print("Angle: ");
    lcd.print(deg);
    lcd.print((char)223);
    lcd.print("  ");
    delay(80);
  }

   int a = x-1;
   int b = x-2;
   int c = x-3;
   int d = x-4;
 
  deg = map (x, 10, 80 , 0, 180); 
  }  
}

void fix(){
}

int calculateDistance(){ 
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH); 
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH); // Reads the echoPin, returns the sound wave travel time in microseconds
  distance= duration*0.034/2;
  return distance;
}