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

const int servo1Pin = 11;
const int trigPin = 7, echoPin = 6;            
int minAngle, minDistance;

LiquidCrystal_I2C lcd(0x27, 20, 4);
Servo servo1;

float measureDistance(int trigPin, int echoPin){
  digitalWrite(trigPin, HIGH);                 
  delayMicroseconds(10);                        
  digitalWrite(trigPin, LOW);                   
  long impulseTime = pulseIn(echoPin, HIGH);        
  return impulseTime/58.0;                     
}

void setup() {
  pinMode(servo1Pin, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  servo1.attach(servo1Pin);
  lcd.begin(16, 2);
  lcd.clear();
  showLcd();
}

void showLcd(){ 
  char buff[16];
  sprintf(buff, "%3dst    %3dcm", minAngle, minDistance);
  lcd.setCursor(0,0);
  lcd.print(buff);
  lcd.setCursor(0,1);
  lcd.print("Adam Scieszka");
}

void loop() {
  minAngle = 0;
  minDistance = 9999;
  for(int i = 0; i <= 180; i++){
    servo1.write(i); 
    int distance = measureDistance(trigPin, echoPin);
    distance = constrain(distance, 0, 400);
    delay(2);
    if(distance < minDistance){
      minAngle = i;
      minDistance = distance;   
    }
  }
  showLcd();
  servo1.write(minAngle); 
  delay(3000);
}