/*
Autor: Gregor Mausser
Datum: 04.03.2026
Titel: 1. SMÜ, 2. Semester Intelligente Abstandskontrolle einer Verladeeinheit
*/

/*
  TEMPLATE – Abstandsmessung mit Ultraschallsensor

  Aufgabe:
  - Abstand messen
  - Mittelwert aus mehreren Messungen berechnen
  - Servo je nach Abstand bewegen

  Hardware:
  TRIG -> Pin 2
  ECHO -> Pin 3
  SERVO -> Pin 5
*/
//Bibliothek für Servo einfügen
#include <Servo.h>

//Pins definieren
int trigPin = 2;
int echoPin = 3;
int servoPin = 5;

Servo servoMotor;   //Den Servo erstellen

void setup() {
  //Serielen Monitor starten
  Serial.begin(9600);

  //Den Pins von Ultraschallsensor eine Funktion(In-,Output) zuweisen
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  servoMotor.attach(servoPin);  //Servo einen Pin zuweisen

}

void loop() {
  
  long mittelwert = mittelwertBerechnen(10);  //Der Variable "mittelwert" den Berechneten Mittelwert von der Funktion geben

  int position = servoPosition(mittelwert); // Die Variable "position" bekommt den wert aus der Funktion übergeben die Funktion bekommt davor den "mittelwert übergeben"

  servoMotor.write(position);   //Dem Servo eine Position geben an die er sich stellen soll

  //Mittelwert ausgeben
  Serial.print("Mittelwert: ");
  Serial.println(mittelwert);

  delay(200);

}


// ----------------------------
// Funktion: Abstand messen
// ----------------------------

long distanceCm() {
  //Sender kurz ausschalten(vehindert Störungen)   
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  //Signal für 10 Micrsekunden senden, danach wieder ausschalten
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  //Zeit messen bis das Signal zurück kommt
  long t = pulseIn(echoPin, HIGH);

  long cm = t / 2 * 0.03432;       //Berechnung für die Zeit / 2 * Schallgeschwindigkeit (cm/µs)
  //Die Distanz(cm) zurückgeben
  return cm;
}


// ----------------------------
// Funktion: Mittelwert berechnen
// ----------------------------

long mittelwertBerechnen(int n) {

  long sum = 0;   //Summe auf 0 setzen

  for(int i = 0; i < n; i++) {

    long wert = distanceCm(); //wert wurde nur definiert nie bunutzt

    sum = sum + wert;   //den wert/distanz mit der summe addiern
    Serial.print("Summe:");
    Serial.println(sum);
    delay(20);
  }

  return sum / n; //Summe durch die anzahl der Messungen dividieren

}


// ----------------------------
// Funktion: Servo Position
// ----------------------------

int servoPosition(long cm) {
  //Wenn der abstand kleiner als 5cm ist den Servo auf 0° stellen
  if(cm < 5)
    return 0;
  //Wenn der abstand kleiner als 20cm ist den Servo auf 90° stellen
  if(cm <= 20)
    return 90;
  
  //Wenn kein Abfrage passt(also >20 cm ist) den Servo auf 180° stellen
  return 180;

}