/*
distance muss in jeder for schleifen wiederholung ausgerechnet werden
--> weniger iterations für weniger lag wahrscheinlich

*/
#include <NewPing.h>  // NewPing-Library einbinden, muss auch installiert sein
#include <Servo.h>    // Servo-Library einbinden,   muss auch

#define TRIGGER_PIN 12    // Hier überall beliebige Pins 
#define ECHO_PIN 13       //
#define TRIGGER_PIN_2 7   //
#define ECHO_PIN_2 8      //
#define MAX_DISTANCE 400
int a, i;
int red = 3;
int green = 5;
int blue = 6;

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
NewPing sonar2(TRIGGER_PIN_2, ECHO_PIN_2, MAX_DISTANCE);
Servo mr_servo;

float distance, duration, distance2, duration2;
int iterations = 2;

void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
mr_servo.attach(9);
}

void loop() {
  // put your main code here, to run repeatedly:


// 1. Berechnung der beiden Distanzen
duration = sonar.ping_median(iterations); 
duration2 = sonar2.ping_median(iterations);
distance = (duration/2)* 0.0343;
distance2 = (duration2/2)* 0.0343;

// 4 For-Schleifen, jeweils 90°
// 1. For-Schleife - 0°-90°
for( i = 0; i<=90; i = i + 5)
{
  mr_servo.write(i);
  
  if(distance < 100 || distance2 < 100 )
  {
    digitalWrite(red, HIGH);
    digitalWrite(green, LOW);
    digitalWrite(blue, LOW);
    
  }
  else
  {
    digitalWrite(red, LOW);
    digitalWrite(green, HIGH);
    digitalWrite(blue, LOW);

  }
  delay(10);
}

// 2. Berechnung
duration = sonar.ping_median(iterations); 
duration2 = sonar2.ping_median(iterations);
distance = (duration/2)* 0.0343;
distance2 = (duration2/2)* 0.0343;


// 2. For-Schleife - 90°-180°
for( i = 90; i<=180; i = i + 5)
{
  mr_servo.write(i);
  
  if(distance < 100 || distance2 < 100 )
  {
    digitalWrite(red, HIGH);
    digitalWrite(green, LOW);
    digitalWrite(blue, LOW);
    
  }
  else
  {
    digitalWrite(red, LOW);
    digitalWrite(green, HIGH);
    digitalWrite(blue, LOW);

  }
  delay(10);
}

// 3. Berechnung
duration = sonar.ping_median(iterations); 
duration2 = sonar2.ping_median(iterations);
distance = (duration/2)* 0.0343;
distance2 = (duration2/2)* 0.0343;

// 3. For-Schleife 180°-90°
for( a = 180; a>=90; a=a-5)
{
  mr_servo.write(a);
  if(distance < 100 || distance2 < 100 )
  {
    digitalWrite(red, HIGH);
    digitalWrite(green, LOW);
    digitalWrite(blue, LOW);
    
  }
  else
  {
    digitalWrite(red, LOW);
    digitalWrite(green, HIGH);
    digitalWrite(blue, LOW);

  }
  delay(10);
}

// 4. Berechnung
duration = sonar.ping_median(iterations); 
duration2 = sonar2.ping_median(iterations);
distance = (duration/2)* 0.0343;
distance2 = (duration2/2)* 0.0343;

// 4. For-Schleife - 90°-0°
for( a = 90; a>=9; a=a-5)
{
  mr_servo.write(a);
  if(distance < 100 || distance2 < 100 )
  {
    digitalWrite(red, HIGH);
    digitalWrite(green, LOW);
    digitalWrite(blue, LOW);
    
  }
  else
  {
    digitalWrite(red, LOW);
    digitalWrite(green, HIGH);
    digitalWrite(blue, LOW);

  }
  delay(10);
}

}