#include <ESP32Servo.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

const int LED1_PIN = 2;
const int LED2_PIN = 4;
const int BUZZER_PIN = 15;
const int POT_PIN = 13;
int pos = 0;

Servo servo1;

void setup() {
  Serial.begin(115200);
  lcd.begin(16, 2);
  lcd.init();
  lcd.backlight();
  servo1.attach(5); 
  pinMode(POT_PIN, INPUT); 
  pinMode(LED1_PIN, OUTPUT);
  pinMode(LED2_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  noTone(BUZZER_PIN);
  lcd.setCursor(0,0);
  lcd.print("Lengan Robot");
  lcd.setCursor(0,1);
  lcd.print("Pemilah Barang");
  Serial.println("Selamat menggunakan Lengan Robot Pemilah Barang");
  delay(5000);
  lcd.clear();
}

void loop() {
  int weight = analogRead(13);
  if (weight <= 2000) {
    for (pos = 0; pos <= 180; pos += 1) {
      servo1.write(pos);
      delay(15);}
    for (pos = 180; pos >= 0; pos -= 1) {
      servo1.write(pos);
      delay(15);}
    digitalWrite(LED1_PIN, LOW);
    digitalWrite(LED2_PIN, HIGH);
    noTone(BUZZER_PIN);
    lcd.setCursor(0, 0);
    lcd.print("Beban: ");
    lcd.println(weight);
    lcd.println("gram");
    lcd.setCursor(0, 1);
    lcd.print("Pindahkan Barang");
    Serial.println("Berat Benda: " + String(weight) + "gram");
    Serial.println("Robot akan memindahkan barang");
    delay(3000);
    lcd.clear();
  }
  else {
    servo1.write(0);
    digitalWrite(LED1_PIN, HIGH); 
    digitalWrite(LED2_PIN, LOW);
    tone(BUZZER_PIN, 1000);
    lcd.setCursor(0, 0);
    lcd.print("Beban: ");
    lcd.println(weight);
    lcd.println("gram");
    lcd.setCursor(0, 1);
    lcd.print("Terlalu Berat!");
    Serial.println("Berat Benda: " + String(weight) + "gram");
    Serial.println("Berat beban melebihi set poin yang diharapkan!");
    delay(3000);
    lcd.clear();
  }
}