/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*------------------------- [c]2020 - D. Ottensmeyer ------------------------*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*~
 ~ Target:    Arduino NANO with RoboDuck (created by the "Fakultät für Physik
 ~            der Universität Regensburg", Germany, in 2019).
 ~            Link: go.ur.de/roboduck
 ~ Example:   RoboDuck_Demo_03
 ~ Version:   0.90
 ~ Author(s): D. Ottensmeyer
 ~
 ~ Description:
 ~ In this example we show a 3rd test for the RoboDuck.
 ~ In this test RoboDuck walks ahead and scans for obstacles with it's
 ~ ultrasonic sensor "eyes". If no obstacle is in sight, the green LED is
 ~ on. If an obstacle comes nearer than about 10 cm, RoboDuck stops walking,
 ~ makes "Quack-quack" and the red LED goes on.
 ~ Then RoboDuck turns it's head to the right, measures the distance, turns
 ~ it's head to the leaft and again measures the distance. If the distance
 ~ to the right is higher than to the left, RoboDuck turns right and walks
 ~ ahead. If the distance to the left is higher than to the right, RoboDuck
 ~ turns left and walks ahead.
 */
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Defines:

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

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Constants:

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Variables:

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

void setup() {

  // Initialize serial:
  Serial.begin(9600); 
  Serial.println("Arduino NANO RoboDuck Test 3!");
  Serial.println("");
  Serial.println("PLEASE ENSURE THAT ROBODUCK CAN MOVE FREELY!!!");
  Serial.println("");


// Assign Arduino Nano ports to the LEDs:
  pinMode(LED_BUILTIN, OUTPUT);     // built-in LED pin
  pinMode(LED_R_GREEN_PIN, OUTPUT); // pin 14 (A0, PC0)
  pinMode(LED_L_RED_PIN, OUTPUT);   // pin 15 (A1, PC1)

// Assign an Arduino Nano port to the Beeper:
  pinMode(BEEPER_PIN, OUTPUT);      // pin 5 (PD5)


  Servo_Init();                             // Start servos
  AllServo_Default();                       // All servos: default position
  delay(2000);                              // Pause 2 s
}

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

void loop() {

  LEDs(GREEN);                              // Distance ok: green LED
  Walkahead_US(10, 30);                     // Walk with distance control
  if (TooClose()) {                         // Obstacle?
    LEDs(RED);                              // Too close: red LED
    Quack();                                // "Quack-quack" sound
    HeadRight45();                          // Look right 45°
    while(isServoMoving(HEAD)) delay(30);   // Wait until 45° reached
    int rightDin = Distance_cm();           // Measure distance to the right
    HeadLeft45();                           // Look left 45°
    while(isServoMoving(HEAD)) delay(30);   // Wait until 45° reached
    int leftDin = Distance_cm();            // Measure distance to the left
    if ((rightDin > 0) && (leftDin > 0)) {  // Not out of range?
      if (rightDin > leftDin) Turnright(4); // Less space left: turn right!
      else Turnleft(4);                     // Less space right: turn left!
    }
    else Turnright(8);                      // Should not happen ...
  }
                                            // ... and another 10 steps ...

}

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