/*
Autor: Gregor Mausser
Datum: 02.03.2026
Titel: Projekt: Intelligente Abstandskontrolle einer Verladeeinheit
*/

//Bibliothek für Servo installieren
#include <Servo.h>

//Den Servo erstellen(für den Microkotroller sichtbar machen)
Servo servo;  //"servo" kann jede Bezeichnung haben

//Pins definieren
//Leds
const int LedPinRed = 13;
const int LedPinGreen = 11;
const int LedPinYellow = 12;
//Servo
const int ServoPin = 3;  //wichtig Pins mitPWM(Pulsweitenmodulation) nutzen
//Ultraschallsensor
const int EchoPin = 4;   //empfängt das Signal
const int TrigPin = 5;   //sendet das Singal

//Variablen definieren
const float air = 0.03432; //Konstante für die Schallgeschwindigkeit (cm/µs)
const int minDistance = 0;
const int maxDistance = 400;
long distance; 
int position;
int Servoposition;

void setup() {
  Serial.begin(115200);
  //Servo einen Pin zuweisen
  servo.attach(ServoPin);  
  //Pins eine Funktion zuweisen
  //Leds
  pinMode(LedPinRed, OUTPUT);
  pinMode(LedPinGreen, OUTPUT);
  pinMode(LedPinYellow, OUTPUT);
  //Ultraschallsensor
  pinMode(EchoPin, INPUT);  //Empfänger
  pinMode(TrigPin, OUTPUT); //Sender

  //Leds ausschalten
  digitalWrite(LedPinRed, LOW);
  digitalWrite(LedPinGreen, LOW);
  digitalWrite(LedPinYellow, LOW);
}


void measureDistance()  {
  //Sender kurz ausschalten(vehindert Störungen)
  digitalWrite(TrigPin, LOW);
  delay(5);
  //Signal für 10 Micrsekunden senden, danach wieder ausschalten
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPin, LOW);
  // pulseIn -> misst die Zeit bis das Signal zurückkommt
  long time = pulseIn(EchoPin, HIGH, 30000);  //300000 läst den Code 30ms warten
  time = 0;
  //Wenn kein Echo ankommt 
  if(time == 0) {
    return -1;    
  }
    //Berechnung der Entfernung in cm
  distance = (time/2) * air;

  //Wenn ein Wert ankommt der technisch nicht möglich ist
  if(distance < minDistance || distance > maxDistance)  {
    return -2;
  }
  
  return distance;

}

void ServoSetting(int Servoposition) {
  if(Servoposition >= position) {
    for(Servoposition; position <= Servoposition; position++) {
      //Serial.print("Position:");
      //Serial.println(position);
      servo.write(position);
      delay(10);
    }  
  }

  else  {
    for(Servoposition; position >= Servoposition; position--) {
      //Serial.print("Position:");
      //Serial.println(position);
      servo.write(position);
      delay(10);
    }  
  }

}

long mittelwert(int anzahlMessungen)  {
  long summe = 0;
  for(int i = 1; i <= anzahlMessungen; i++) {
    Serial.print(i);
    Serial.print("Summe");
    Serial.println(summe);
    summe += distance;
    delay(10);  //reduziert Messrauschen 
  }

  distance = summe/anzahlMessungen;
  Serial.print("Entfernung:");
  Serial.println(distance);
  return(distance);
}

void loop() {
  measureDistance();
  if(distance == -1) {
    Serial.println("Messfehler!!");
    digitalWrite(LedPinRed, HIGH);
    digitalWrite(LedPinGreen, HIGH);
    digitalWrite(LedPinYellow, HIGH);
    ServoSetting(145);
  }
  
  if (distance == -2) {
    Serial.println("Technischer Fehler: Grenzwerte errreicht");
    digitalWrite(LedPinRed, HIGH);
    digitalWrite(LedPinGreen, HIGH);
    digitalWrite(LedPinYellow, LOW);
    ServoSetting(45);
  }
  

  else{
    mittelwert(10);
    switch(distance)  {
      //<5; Kollision; Rotes Led leuchtet; Servo steht bei 0°
      case 0 ... 4:
        Serial.println("Stop!!! Sehr geringer Abstand!!!");
        digitalWrite(LedPinRed, HIGH);
        digitalWrite(LedPinGreen, LOW);
        digitalWrite(LedPinYellow, LOW);
        ServoSetting(0);
        break;

      //5 - 15; Warnbereich; Gelbes Led leuchtet; Servo steht bei 90°
      case 5 ... 15:
        Serial.println("Langsam! wenig Abstand!");
        digitalWrite(LedPinRed, LOW);
        digitalWrite(LedPinGreen, LOW);
        digitalWrite(LedPinYellow, HIGH);
        ServoSetting(90);
        break;

      //>5; Sicher; Grünes Led leuchtet; Servo steht bei 180°
      default:
        Serial.println("Keine Gafahr, ausreichend Abstand");
        digitalWrite(LedPinRed, LOW);
        digitalWrite(LedPinGreen, HIGH);
        digitalWrite(LedPinYellow, LOW);
        ServoSetting(180);
        break;
    }
  }


}