/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*------------------------- [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: http://www.physik.uni-regensburg.de/rsl/roboduck.html
 ~ Example:   RoboDuck_FindDefaultServos
 ~ Version:   0.90
 ~ Author(s): D. Ottensmeyer
 ~
 ~ Description:
 ~ In this program we try to find and define the default positions of all
 ~ servos of the RoboDuck.
 ~ This program assumes, that all servos are mounted correctly in center
 ~ position by using the CenterServos program!
 ~
 ~ Guide for using this program:
 ~ If you have mounted all servos correctly in center position by using the
 ~ CenterServos program, you can define DEFAULT positions of all servos.
 ~ The default position can be used to reach a better center position,
 ~ because also if you mount a servo correctly in center position, this
 ~ mounting position really may be off base by +- 15 degrees.
 ~ Another possibility for using default positions would be to define
 ~ other not centered starting positions for the servos, but this is not
 ~ used with RoboDuck.
 ~ After starting this program with your RoboDuck connected to a PC or
 ~ laptop you'll see an initial message on the Serial Monitor.
 ~ Then the first servo (head servo) will be moved slowly to the left side
 ~ with increasing position values. Then it will be moved to the right
 ~ side again. During this movement the servo position crosses the center
 ~ position TWICE. So you have the chance to watch this movement and
 ~ write down the last displayed position value of the centered head.
 ~ You can do it like this: "HD = 85", if 85 is the right value.
 ~ Do the same with the other servos: right leg, right foot, left leg,
 ~ left foot. For the foot servos you should hold the RoboDuck in your hand
 ~ because they should be able to move freely.
 ~ In the end you should have 5 new default values: HD, RL, RF, LL, LF. 
 ~ Now that you found the default positions for your RoboDuck, you have to
 ~ write them ONCE into your RoboDuck Library:
 ~ 1. Find the RoboDuck.cpp file in \[Sketchbook]\libraries\RoboDuck.
 ~ 2. Make a copy of the original file (if anything goes wrong ...)
 ~ 3. Open RoboDuck.cpp with the Windows or another text editor.
 ~ 4. In the "Constants" section locate these lines:
 ~    // ======== DEFAULT Positions: ===========
 ~    //                        HD,RL,RF,LL,LF
 ~    const int array_cal[5] = {90,90,90,90,90};
 ~    // =======================================
 ~ 5. Replace the values (90) in the array_cal array with your own default
 ~    values for the head (HD), right leg (RL), right foot (RF), left leg
 ~    (LL), left foot (LF) servo. The "const..." line should then look like
 ~    this:
 ~    const int array_cal[5] = {82,96,85,80,97};
 ~    ... where 82,96,85,80,97 are your five new default values.
 ~    All values should be in the range 75..105.
 ~ 6. Don't change anything else and save the RoboDuck.cpp file.
 ~ 7. Test your new default positions with the program DefaultServos.
 ~ 8. If you don't like these default positions, start this program again!
 ~ 9. Else: Done!
 */
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Defines:
#define SERVO_STARTPOS 75
#define SERVO_ENDPOS   105

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

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

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

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

void setup() {

  // Initialize serial:
  Serial.begin(9600); 
  Serial.println("Arduino NANO RoboDuck Find Default Servos!");
  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)
}

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

void loop() {
  Servo_Init();
  AllServo_Center();
  delay(3000);
  Serial.println("Now please watch the HEAD moving!");
  Serial.println("If the center position is reached,");
  Serial.println("write down the last displayed position");
  Serial.println("value for the head servo!");
  Serial.println("");
  for(int pos = SERVO_STARTPOS; pos < SERVO_ENDPOS; pos++) {
    ServoWrite(HEAD, pos, 50, 1);
    Serial.print("Position: ");
    Serial.print(pos);
    if (pos == 90) Serial.print(" <== CENTER");
    if (pos == defPos(HEAD)) Serial.print(" <== DEFAULT");
    Serial.println("");
    delay(1000);
  }
  for(int pos = SERVO_ENDPOS; pos >= SERVO_STARTPOS; pos--) {
    ServoWrite(HEAD, pos, 50, 1);
    Serial.print("Position: ");
    Serial.print(pos);
    if (pos == 90) Serial.print(" <== CENTER");
    if (pos == defPos(HEAD)) Serial.print(" <== DEFAULT");
    Serial.println("");
    delay(1000);
  }
  Serial.println("--------------------------------------");
  AllServo_Center();
  delay(2000);
  Serial.println("Now please watch the RIGHT LEG moving!");
  Serial.println("If the center position is reached,");
  Serial.println("write down the last displayed position");
  Serial.println("value for the right leg servo!");
  Serial.println("");
  for(int pos = SERVO_STARTPOS; pos < SERVO_ENDPOS; pos++) {
    ServoWrite(RIGHT_LEG, pos, 50, 1);
    Serial.print("Position: ");
    Serial.print(pos);
    if (pos == 90) Serial.print(" <== CENTER");
    if (pos == defPos(RIGHT_LEG)) Serial.print(" <== DEFAULT");
    Serial.println("");
    delay(1000);
  }
  for(int pos = SERVO_ENDPOS; pos >= SERVO_STARTPOS; pos--) {
    ServoWrite(RIGHT_LEG, pos, 50, 1);
    Serial.print("Position: ");
    Serial.print(pos);
    if (pos == 90) Serial.print(" <== CENTER");
    if (pos == defPos(RIGHT_LEG)) Serial.print(" <== DEFAULT");
    Serial.println("");
    delay(1000);
  }
  Serial.println("--------------------------------------");
  AllServo_Center();
  delay(2000);
  Serial.println("Now please watch the RIGHT FOOT moving!");
  Serial.println("If the center position is reached,");
  Serial.println("write down the last displayed position");
  Serial.println("value for the right foot servo!");
  Serial.println("");
  for(int pos = SERVO_STARTPOS; pos < SERVO_ENDPOS; pos++) {
    ServoWrite(RIGHT_FOOT, pos, 50, 1);
    Serial.print("Position: ");
    Serial.print(pos);
    if (pos == 90) Serial.print(" <== CENTER");
    if (pos == defPos(RIGHT_FOOT)) Serial.print(" <== DEFAULT");
    Serial.println("");
    delay(1000);
  }
  for(int pos = SERVO_ENDPOS; pos >= SERVO_STARTPOS; pos--) {
    ServoWrite(RIGHT_FOOT, pos, 50, 1);
    Serial.print("Position: ");
    Serial.print(pos);
    if (pos == 90) Serial.print(" <== CENTER");
    if (pos == defPos(RIGHT_FOOT)) Serial.print(" <== DEFAULT");
    Serial.println("");
    delay(1000);
  }
  Serial.println("--------------------------------------");
  AllServo_Center();
  delay(2000);
  Serial.println("Now please watch the LEFT LEG moving!");
  Serial.println("If the center position is reached,");
  Serial.println("write down the last displayed position");
  Serial.println("value for the left leg servo!");
  Serial.println("");
  for(int pos = SERVO_STARTPOS; pos < SERVO_ENDPOS; pos++) {
    ServoWrite(LEFT_LEG, pos, 50, 1);
    Serial.print("Position: ");
    Serial.print(pos);
    if (pos == 90) Serial.print(" <== CENTER");
    if (pos == defPos(LEFT_LEG)) Serial.print(" <== DEFAULT");
    Serial.println("");
    delay(1000);
  }
  for(int pos = SERVO_ENDPOS; pos >= SERVO_STARTPOS; pos--) {
    ServoWrite(LEFT_LEG, pos, 50, 1);
    Serial.print("Position: ");
    Serial.print(pos);
    if (pos == 90) Serial.print(" <== CENTER");
    if (pos == defPos(LEFT_LEG)) Serial.print(" <== DEFAULT");
    Serial.println("");
    delay(1000);
  }
  Serial.println("--------------------------------------");
  AllServo_Center();
  delay(2000);
  Serial.println("Now please watch the LEFT FOOT moving!");
  Serial.println("If the center position is reached,");
  Serial.println("write down the last displayed position");
  Serial.println("value for the left foot servo!");
  Serial.println("");
  for(int pos = SERVO_STARTPOS; pos < SERVO_ENDPOS; pos++) {
    ServoWrite(LEFT_FOOT, pos, 50, 1);
    Serial.print("Position: ");
    Serial.print(pos);
    if (pos == 90) Serial.print(" <== CENTER");
    if (pos == defPos(LEFT_FOOT)) Serial.print(" <== DEFAULT");
    Serial.println("");
    delay(1000);
  }
  for(int pos = SERVO_ENDPOS; pos >= SERVO_STARTPOS; pos--) {
    ServoWrite(LEFT_FOOT, pos, 50, 1);
    Serial.print("Position: ");
    Serial.print(pos);
    if (pos == 90) Serial.print(" <== CENTER");
    if (pos == defPos(LEFT_FOOT)) Serial.print(" <== DEFAULT");
    Serial.println("");
    delay(1000);
  }
  Serial.println("--------------------------------------");
  AllServo_Center();
  Serial.println("Done!!");
  Serial.println("");
  Serial.println("Now please test your new default");
  Serial.println("values with the program DefaultServos!");

  while(1);
}

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