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

const int trigPinA = 3, echoPinA = 2, trigPinB = 8, echoPinB = 9, Buzz = 7, LEDN = 10, LEDO = 11;
int distanceA, distanceB, DataA, DataB, Prxt;
long durationA, durationB;

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo ServoA;
Servo ServoB;
Servo ServoC;

void setup() {
  lcd.init();
  lcd.backlight();
  pinMode(trigPinA, OUTPUT);
  pinMode(echoPinA, INPUT);
  pinMode(trigPinB, OUTPUT);
  pinMode(echoPinB, INPUT);
  pinMode(Prxt, INPUT);
  pinMode(Buzz, OUTPUT);
  pinMode(LEDO, OUTPUT);
  pinMode(LEDN, OUTPUT);
  ServoA.attach(4);
  ServoB.attach(5);
  ServoC.attach(6);
  ServoA.write(2);
  ServoB.write(90);
  ServoC.write(90);
  //Serial.begin(9600);
}

void loop() {
  Prxt = analogRead(A0);
  digitalWrite(Buzz, LOW);
  digitalWrite(LEDO, LOW);
  digitalWrite(LEDN, LOW);
  DataA = UltsA();
  DataB = UltsB();

  if (DataA <= 40 && DataB < 8 && Prxt > 1000) {
    digitalWrite(Buzz, HIGH);
    digitalWrite(LEDO, HIGH);
    ServoB.write(10);
    ServoC.write(170);
    delay(1000);
  } else if (DataA <= 40 && DataB < 8 && Prxt < 200) {
    delay(500);
    digitalWrite(LEDN, HIGH);
    ServoB.write(170);
    ServoC.write(10);
    delay(1000);
  } else {
    ServoB.write(90);
    ServoC.write(90);
  }

  if (DataA <= 40) {
    ServoA.write(105);
  } else if (DataA >= 40 && DataB > 8 && Prxt < 200) {
    delay(500);
    ServoA.write(2);
  }

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("UR : ");
  lcd.setCursor(5, 0);
  lcd.print(DataA);
  lcd.setCursor(8, 0);
  lcd.print("UR : ");
  lcd.setCursor(13, 0);
  lcd.print(DataB);
  lcd.setCursor(0, 1);
  lcd.print("PX : ");
  lcd.setCursor(5, 1);
  lcd.print(Prxt);
  //  Serial.print("ULTR A : ");
  //  Serial.print(DataA);
  //  Serial.print("  |  ULTR B : ");
  //  Serial.print(DataB);
  //  Serial.print("  |  PRXT : ");
  //  Serial.println(Prxt);
  delay(100);
}

int UltsA() {
  digitalWrite(trigPinA, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinA, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinA, LOW);
  durationA = pulseIn(echoPinA, HIGH);
  distanceA = durationA * 0.034 / 2;
  return distanceA;
}

int UltsB() {
  digitalWrite(trigPinB, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinB, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinB, LOW);
  durationB = pulseIn(echoPinB, HIGH);
  distanceB = durationB * 0.034 / 2;
  return distanceB;
}