/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*------------------------- [c]2020 - D. Ottensmeyer ------------------------*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*~
 ~ Target:    Arduino NANO mit RoboDuck (erschaffen von der "Fakultät für
 ~            Physik der Universität Regensburg", Deutschland, in 2019).
 ~            Link: go.ur.de/roboduck
 ~ Example:   RoboDuck_Demo_03_DE
 ~ Version:   0.90
 ~ Author(s): D. Ottensmeyer
 ~
 ~ Beschreibung:
 ~ In diesem Beispiel zeigen wir einen dritten Test für RoboDuck.
 ~ In diesem Test geht RoboDuck geradeaus und sucht nach Hindernissen mit
 ~ seinen Ultraschall-"Augen". Wenn kein Hindernis in Sicht ist, ist die
 ~ grüne LED an. Wenn ein Hindernis näher kommt als 10 cm, geht RoboDuck
 ~ nicht weiter, macht "Quaak-quaak" und die rote LED geht an.
 ~ Dann dreht RoboDuck seinen Kopf nach rechts, misst die Entfernung, dreht
 ~ den Kopf nach links und misst wieder die Entfernung. Wenn die Entfernung
 ~ nach rechts größer als nach links ist, dreht sich RoboDuck nach rechts
 ~ und geht weiter. Wenn die Entfernung nach links größer als nach rechts
 ~ ist, dreht sich RoboDuck nach links und geht weiter geradeaus.
*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Definitionen:

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Includes:
#include <RoboDuck.h>

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Konstanten:

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Variablen:

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

void setup() {

  // Initialisiere die serielle Ausgabe:
  Serial.begin(9600); 
  Serial.println("Arduino NANO RoboDuck Test 3!");
  Serial.println("");
  Serial.println("BITTE STELLE SICHER, DASS SICH ROBODUCK FREI BEWEGEN KANN!!!");
  Serial.println("");


// Weise den LEDs Arduino Nano Portpins zu:
  pinMode(LED_EINGEBAUT, OUTPUT);   // Pin f. eingebaute LED
  pinMode(LED_R_GRUEN_PIN, OUTPUT); // Pin 14 (A0, PC0)
  pinMode(LED_L_ROT_PIN, OUTPUT);   // Pin 15 (A1, PC1)

// Weise dem Beeper einen Arduino Nano Portpin zu:
  pinMode(BEEPER_PIN, OUTPUT);      // Pin 5 (PD5)


  ServoStart();                             // Starte die Servos
  Normalstellung();                         // Alle Servos: Standard Position
  delay(2000);                              // Pause 2 s
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

void loop() {

  LEDs(GRUEN);                              // Entfernung ok: grüne LED
  GeheVorwaerts_US(10, 30);                 // Gehe mit Entfernungskontrolle
  if (ZuNah()) {                            // Hindernis?
    LEDs(ROT);                              // Zu nah: rote LED
    Quaak();                                // "Quaak-quaak" Geräusch
    KopfRechts45();                         // Schaue nach rechts 45°
    while(BewegtSichServo(KOPF)) delay(30); // Warte, bis 45° erreicht
    int reEntf = Entfernung_cm();           // Messe die Entfernung rechts
    KopfLinks45();                          // Schaue nach links 45°
    while(BewegtSichServo(KOPF)) delay(30); // Warte, bis 45° erreicht
    int liEntf = Entfernung_cm();           // Messe die Entfernung links
    if ((reEntf > 0) && (liEntf > 0)) {     // Nicht außer Reichweite?
      if (reEntf > liEntf) DreheRechts(4);  // Weniger Platz li.: drehe re.!
      else DreheLinks(4);                   // Weniger Platz re.: drehe li.!
    }
    else DreheRechts(8);                    // Sollte nicht passieren ...
  }
                                            // ... und wieder 10 Schritte ...

}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
