#include <LiquidCrystal_I2C.h>  /*include LCD I2C Library*/
#include "Ultrasonic.h"

#include <Servo.h>

LiquidCrystal_I2C lcd(0x27,16,2);  /*I2C scanned address defined + I2C screen size*/
Ultrasonic ultrasonic(3, 2);
Servo servo;              // Create a "Servo" object called "arm"

int LED = 13;						// sets LED to pin 13
int DelayTime = 500;		// sets delay time in 1/1000ths of a second
const int servoPin = 5;

int distance;
int pos = 0;
int step = 6;
bool led = LOW ;

void setup() {
  pinMode(LED, OUTPUT);			  // sets LED pin as an output

  servo.attach(servoPin, 500, 2400);
  servo.write(pos);

  lcd.init();                 // LCD display initialized
  lcd.clear();                // Clear LCD Display
  lcd.backlight();            // Turn ON LCD Backlight
  lcd.setCursor(2,0);         // Set cursor to Row 1
  lcd.print("I2C LCD Nano");  // print text on LCD
  lcd.setCursor(2,1);         // set cursor on row 2
}

void loop() {

  distance = ultrasonic.read(CM);   // Get the distance

  pos=pos+step;                     // Move
  if (pos>=180) {
    pos=180;
    step=-step;
  }
  if (pos<=0) {
    pos=0;
    step=-step;
  }

  servo.write(pos);

  lcd.print("Dist: ");
  lcd.print(distance);

  led=not(led);
	digitalWrite(LED, led);		// change LED pin
  delay(DelayTime);			      // pauses for DelayTime in 1/1000s sececond

  lcd.clear();                  // Clear LCD Display
  lcd.setCursor(1,0);         // set cursor on row 2
}