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

// Pin STM32 (gunakan pin PWM yang umum di STM32)
#define SERVO0_PIN PA8
#define SERVO1_PIN PA9
#define SERVO2_PIN PA10
#define BUTTON1_PIN PB1
#define BUTTON2_PIN PB10

// Konfigurasi LCD I2C
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Servo
Servo servo0;
Servo servo1;
Servo servo2;


void setup() {
  // LCD
  lcd.init();
  lcd.backlight();

  // Tombol
  pinMode(BUTTON1_PIN, INPUT);
  pinMode(BUTTON2_PIN, INPUT);

  // Servo setup
  servo0.attach(SERVO0_PIN);
  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);

  servo0.write(90);
  servo1.write(90);
  servo2.write(90);
   

}

void loop() {
  if (digitalRead(BUTTON1_PIN) == LOW) {
    // LOLOS: buka A1 dan A2
    servo1.write(0);
    servo2.write(0);
    servo0.write(0);

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("LOLOS");

    delay(100);
  } else if (digitalRead(BUTTON2_PIN) == LOW) {
    // TIDAK LOLOS: buka A1 saja
    servo1.write(0);
    servo2.write(90);
    servo1.write(0);

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("TIDAK LOLOS");

    delay(100);
  }

  
}