#include <NewPing.h>                //Ultrasonic Sensor          
#include <LiquidCrystal_I2C.h>      //I2C LCD
#include <Servo.h>                  //Servo 


//I2C LCD parameters
#define I2C_ADDR  0x27              //I2C address of LCD
#define LCD_COLUMNS 16
#define LCD_LINES 2

//initialize the liquid crystal library
//the first parameter is  the I2C address
//the second parameter is how many rows are on your screen
//the  third parameter is how many columns are on your screen
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);


//Ultrasonic parameters
#define TRIG_PIN  9              //trigger pin of Ultrasonic module
#define ECHO_PIN 10              //echo pin of Ultrasonic module
#define MAX_DISTANCE 180        //Maximum distance we want to ping for (in centimeters).

NewPing sonar_1(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);    //Object : Sonar sensor 1

int distance;                   //Variable for store measured distance

//Servo parameters
int servo1Pin = 3;              //PWM pin for servo motor 1

Servo Servo1;                   //Object : Servo motor 1


int angle = 0;


void setup() {
  // put your setup code here, to run once:

  pinMode(LED_BUILTIN, OUTPUT);                   // Builtin LED mode

  //Setting up LCD Screen
  lcd.init();
  lcd.clear();
  lcd.backlight();


  //Setting up pins for Ultrasound module


  // Print something
  lcd.setCursor(0, 0);
  lcd.print("Ultra Sonic");
  lcd.setCursor(2, 1);
  lcd.print("Line 2");

  delay(2000);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Distance in cm");


  //Attaching servo 1 to the pin number
  Servo1.attach(servo1Pin);

  //Servo1.write(90);

  for (angle =0 ; angle <= 180 ; angle++){
    Servo1.write(angle);
    lcd.clear();
     lcd.print(angle);

    delay(50);

    for (angle = 180 ; angle >= 0; angle--){
     // Servo1.write(angle);
     /*
     lcd.clear();
     lcd.print(angle);
     delay(20);
     */
    }
  }

}

void loop() {
  // put your main code here, to run repeatedly:

  
  //Test code with LED
  digitalWrite(LED_BUILTIN, HIGH);
  delay(500);
 
  digitalWrite(LED_BUILTIN, LOW);
  delay(500);
  //EOF Test code


  //Measure the response from the HS-SR04 Echo pin
  distance = sonar_1.ping_cm();
  
  
  //Display  the measured length
  lcd.clear();
  lcd.println("Distance in cm  ");
  lcd.setCursor(0, 1);
  lcd.print(distance);
  lcd.setCursor(4,1);
  lcd.print("cm");

  Servo1.write(distance);



}