/**************
Name: Ivan Ivanov
Datum: 07.05.2024
Übung: Servosteuerung mit Abstandssensor
**********/

#include <Servo.h>

#define TRIGGER_PIN 5
#define ECHO_PIN 6
#define SERVO_PIN 9

Servo servo; // Servo-Objekt erstellen

void setup() 
{
  pinMode(TRIGGER_PIN, OUTPUT); // Trigger-Pin als Ausgang konfigurieren
  pinMode(ECHO_PIN, INPUT); // Echo-Pin als Eingang konfigurieren
  Serial.begin(9600); // Serielle Kommunikation starten

  servo.attach(SERVO_PIN); // Servo an den entsprechenden Pin anschließen
}

void loop() 
{
  digitalWrite(TRIGGER_PIN, HIGH); // Ultraschall-Puls senden
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);

  long dauer = pulseIn(ECHO_PIN, HIGH); // Dauer des Echos messen
  long entfernung = dauer * 0.034 / 2; // Entfernung berechnen in cm

  // Servo steuern basierend auf der Entfernung
  
  int winkel = map(entfernung, 0, 400, 40, 130); // Winkel für den Servo berechnen
  servo.write(winkel); // Servo auf den berechneten Winkel setzen
  
  // Entfernungsinformationen ausgeben
  Serial.print("Entfernung in cm: ");
  Serial.println(entfernung);

  delay(100); // Kurze Pause für Stabilität
}