/*
 Controlling a servo position using a potentiometer (variable resistor)
 by Michal Rinott <http://people.interaction-ivrea.it/m.rinott>

 modified on 8 Nov 2013
 by Scott Fitzgerald
 http://www.arduino.cc/en/Tutorial/Knob
*/

#include <Servo.h>
#include <LiquidCrystal.h>
#define LDR_PIN 2
#include <LiquidCrystal_I2C.h>



LiquidCrystal_I2C lcd2(0x27, 20, 4);

LiquidCrystal lcd(12, 11, 10, 9, 8, 7);


Servo myservo,myservo1;  // create servo object to control a servo

int potpin = 0;  // analog pin used to connect the potentiometer
int potpin1 = 1;  // analog pin used to connect the potentiometer
int val,val1;    // variable to read the value from the analog pin
float cm,inches;

void setup() {
  pinMode(LDR_PIN, INPUT); // set pin 2 as input from the light sensor

  lcd2.init();
  lcd2.backlight();

  lcd2.print("Hello from 2nd LCD");
  
  myservo1.attach(5);  // attaches the servo on pin 9 to the servo object
  myservo.attach(6);  // attaches the servo on pin 9 to the servo object
  lcd.begin(16, 2);
  // you can now interact with the LCD, e.g.:
  lcd.setCursor(1, 0);
  lcd.print("Checking System");
  delay(15);  
  
  
  lcd.setCursor(0, 2);
  String message = "Performing Diagnostics";
  for (byte i = 0; i < message.length(); i++) {
    lcd.print(message[i]);
    delay(100);
  }
  
  lcd.clear();   
  lcd.setCursor(1, 0);
  lcd.print("Welcome Zhelo");
  
  
  lcd.setCursor(0, 4);
  lcd.print(" I'm Ready.");
  delay(550);     
  
}

long readUltrasonicDistance(int triggerPin, int echoPin)
{
	pinMode(triggerPin, OUTPUT);  // Clear the trigger
	digitalWrite(triggerPin, LOW);
	delayMicroseconds(2);
	// Sets the trigger pin to HIGH state for 10 microseconds
	digitalWrite(triggerPin, HIGH);
	delayMicroseconds(10);
	digitalWrite(triggerPin, LOW);
	pinMode(echoPin, INPUT);
	// Reads the echo pin, and returns
	// the sound wave travel time in microseconds
	return pulseIn(echoPin, HIGH);
}

void loop() {
                        // waits for the servo to get there
  val1 = analogRead(potpin1);            // reads the value of the potentiometer (value between 0 and 1023)
  val1 = map(val1, 0, 1023, 0, 160);     // scale it to use it with the servo (value between 0 and 180)
  myservo1.write(val1);                  // sets the servo position according to the scaled value
  

  val = analogRead(potpin);            // reads the value of the potentiometer (value between 0 and 1023)
  val = map(val, 0, 500, 0, 90);     // scale it to use it with the servo (value between 0 and 180)
  myservo.write(val);                  // sets the servo position according to the scaled value
  lcd.clear();

  lcd.setCursor(0, 0);
  if (digitalRead(LDR_PIN) == LOW) {
    lcd.print("Light!");      
  } else {
    lcd.print("Dark!");       
  }   
  delay(30);  
  lcd.setCursor(0, 0);
  lcd.print("          ");      

  lcd.clear();
  cm = 0.0344/2 * readUltrasonicDistance(3, 2);
	inches = (cm / 2.54);
  lcd.setCursor(1,0);
  lcd.print(cm);
  lcd.setCursor(1, 5);
  lcd.print(inches);      
  
}