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

const int servo1Pin = 11, servo2Pin = 10;
const int trigPin = 7, echoPin = 6;               
const int potPin = A0, encPin1 = 2, encPin2 = 3; 
int servo1Val, servo2Val, distance;

LiquidCrystal_I2C lcd(0x27, 20, 4);
Servo servo1, servo2;  
Encoder enc(encPin1, encPin2);

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(servo2Pin, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);

  lcd.begin(16, 2);
  lcd.clear();
}

void showLcd(){ 
  char buff[16];
  sprintf(buff, "%3d  %3d  %3dcm", servo1Val, servo2Val, distance);
  lcd.setCursor(0,0);
  lcd.print(buff);
  lcd.setCursor(0,1);
  lcd.print("Adam Scieszka");
}

void loop() {
  distance = measureDistance(trigPin, echoPin);
  servo1Val = map(analogRead(potPin), 0, 1023, 0, 180);
  servo2Val = constrain(enc.read(), 0, 180);
  enc.write(servo2Val);
  servo1.write(servo1Val);  
  servo2.write(servo2Val);  
  showLcd();
  delay(20);
}