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

// Instantieer het LCD-scherm (adres 0x27 kan variëren, controleer dit indien nodig)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Servo en ultrasone sensor pinnen
Servo servo;
const int trigPin = 9;
const int echoPin = 10;

// Servo instellingen
const int minAngle = 0;
const int maxAngle = 180;
const int stepSize = 10;

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

  // Servo initialisatie
  servo.attach(6); // Servo aangesloten op pin 6

  // Ultrasone sensor initialisatie
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Welkomstbericht op LCD
  lcd.setCursor(0, 0);
  lcd.print("Sonar gestart");
  delay(2000);
  lcd.clear();
}

void loop() {
  for (int angle = minAngle; angle <= maxAngle; angle += stepSize) {
    servo.write(angle);
    delay(200); // Wacht tot de servo op positie is

    long distance = measureDistance();

    // Toon de gegevens op het LCD
    lcd.setCursor(0, 0);
    lcd.print("Hoek:");
    lcd.print(angle);
    lcd.print((char)223); // Graden symbool

    lcd.setCursor(0, 1);
    lcd.print("Afstand:");
    lcd.print(distance);
    lcd.print(" cm");

    delay(500); // Wacht voor leesbaarheid
  }

  for (int angle = maxAngle; angle >= minAngle; angle -= stepSize) {
    servo.write(angle);
    delay(200); // Wacht tot de servo op positie is

    long distance = measureDistance();

    // Toon de gegevens op het LCD
    lcd.setCursor(0, 0);
    lcd.print("Hoek:");
    lcd.print(angle);
    lcd.print((char)223); // Graden symbool

    lcd.setCursor(0, 1);
    lcd.print("Afstand:");
    lcd.print(distance);
    lcd.print(" cm");

    delay(500); // Wacht voor leesbaarheid
  }
}

long measureDistance() {
  // Stuur een korte puls op de trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Meet de duur van de echo-puls
  long duration = pulseIn(echoPin, HIGH);

  // Bereken de afstand in cm
  long distance = duration * 0.034 / 2;
  return distance;
}