#include <Servo.h>
#include <Keypad.h>

#define trigPin 9
#define echoPin 10
#define servoPin 11
#define ledPin 8

int afstand = 0;
int afstandDrempel = 20; // min afstand om slagboom te openenen
bool slagboomOpen = false;

Servo slagboomServo;


void openSlagboom();
void sluitSlagboom();
void meetAfstand(int &afstand);


void setup() {
  Serial.begin(9600);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ledPin, OUTPUT);

  slagboomServo.attach(servoPin);
  sluitSlagboom(); // Startpositie: slagboom dicht
}

void loop() {
  //afstand = meetAfstand();

  meetAfstand(afstand);

  String afstandStr = "Afstand: " + String(afstand) + " cm";

  /*Serial.print("Afstand: ");
  Serial.print(afstand);
  Serial.println(" cm");*/
  Serial.println("Afstand: " + String(afstand) + " cm");

  if (afstand > 0 && afstand < afstandDrempel) {
    if (!slagboomOpen) {
      openSlagboom();
    }
  } else {
    if (slagboomOpen) {
      sluitSlagboom();
    }
  }
  delay(200);
}

/*int meetAfstand() {
  long duur;
  int afstand;

  // Ultrasoon signaal sturen
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Duur van de echo meten
  duur = pulseIn(echoPin, HIGH);

  // Omrekenen naar afstand in cm
  afstand = duur * 0.034 / 2;

  return afstand;
}*/

void meetAfstand(int &afstand) {
  long duur;

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duur = pulseIn(echoPin, HIGH);
  afstand = duur * 0.034 / 2;
}

void openSlagboom() {
  slagboomServo.write(90);         // Slagboom open
  digitalWrite(ledPin, HIGH);      // LED aan
  slagboomOpen = true;
  Serial.println("Slagboom geopend");
}

void sluitSlagboom() {
  slagboomServo.write(0);          // Slagboom dicht
  digitalWrite(ledPin, LOW);       // LED uit
  slagboomOpen = false;
  Serial.println("Slagboom gesloten");
}