/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*          SSS                                                              */
/*         SSSSS                                                             */
/*    SSSSSS SSSS                                                            */
/*     SSSSSSSSS                                                             */
/*          SSSS                                      ___   ___              */
/*          SSS         -----------------            /   \ /   \             */
/*         SSSA        | R o b o D u c k |          (  -  x  +  )            */
/*        SSSSSSN       -----------------            \___/ \___/             */
/*        SSSSSSSN                                     ARDUINO               */
/*       SSSSSSSSSSSA                                                        */
/*       SSSSSSSSSSSSSSSSSSS                                                 */
/*        SSSSSSSSSSSSSSSSS                                                  */
/*          SSSSSSSSSSS                                                      */
/*              S S                                                          */
/*            SSSSS                                                          */
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*------------------------- [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
 ~ Library:   RoboDuck Source
 ~ Version:   0.90
 ~ Author(s): D. Ottensmeyer
 ~
 ~ Description:
 ~ This is our new Library that contains basic routines and functions for
 ~ accessing the standard components of the RoboDuck.
 */
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// RoboDuck standard components:
// - Servos (Micro Servo SG90)
// - Ultrasonic Distance Sensor (HY-SRF05)
// - Beeper (CLW1027)
// - LEDs

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Includes:
#include "RoboDuck.h"

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Constants:
// ======== DEFAULT Positions: ===========
//                        HD,RL,RF,LL,LF
const int array_cal[5] = {84,79,84,90,86};
// =======================================

// Movement speed for center and default positioning functions:
const int vel = 10;

// Basic movements servo position arrays:
// (Internal library use only!)
// Walk ahead:
const int num_walkahead = 8;      
const int array_walkahead[num_walkahead][5] =
{   
  {-15,0,-40,0,-20},    // go ahead one step
  {-25,25,-40,18,-20},
  {0,25,0,18,0},
  {15,0,20,0,40},
  {25,-18,20,-25,40},
  {0,-18,0,-25,0},

  {0,-9,0,-12,0},       // zero pos 6-7
  {0,0,0,0,0},
};

// Walk back:
const int num_walkback = 8;      
const int array_walkback[num_walkback][5] =
{   
  {-15,0,20,0,40},      // go back one step
  {-25,18,20,25,40},
  {0,18,0,25,0},
  {15,0,-40,0,-20},
  {25,-25,-40,-18,-20},
  {0,-25,0,-18,0},

  {0,-12,0,-9,0},       // zero pos 6-7
  {0,0,0,0,0},
};

// Turn left:
const int num_turnleft = 4;      
const int array_turnleft[num_turnleft][5] =
{   
  {0,0,-40,0,-20},      // turn left one step
  {20,0,0,40,-20},
  {20,0,0,40,0},
  {0,0,0,0,0},
};

// Turn right:
const int num_turnright = 4;      
const int array_turnright[num_turnright][5] =
{   
  {0,0,20,0,40},        // turn right one step
  {-20,-40,20,0,0},
  {-20,-40,0,0,0},
  {0,0,0,0,0},
};

// Dance movements servo position arrays:
// (Internal library use only!)
// Dance 1:
const int num_dance1 = 10;
const int array_dance1[num_dance1][5] =
{ 
// slide to the left 0-4
  {0,0,-20,0,0},  
  {0,0,-40,0,20},
  {0,0,-20,0,40},
  {0,0,0,0,20},
  {0,0,0,0,0},
    
// slide to the right 5-9
  {0,0,0,0,20},  
  {0,0,-20,0,40},
  {0,0,-40,0,20},
  {0,0,-20,0,0},
  {0,0,0,0,0}, 
};

// Dance 2:
const int num_dance2 = 32;
const int array_dance2[num_dance2][5] =
{ 
// left foot support 0-15
  {0,20,0,40,0},
  {0,20,-30,40,-30}, 
  {0,20,-30,10,-30},
  {0,20,-30,40,-30}, 
  {0,20,-30,10,-30},

  {0,20,-30,40,-30}, 
  {0,20,0,40,-30},
  {0,20,80,40,-30},
  {0,20,0,40,-30},
  {0,20,-40,40,-30},
  {0,20,0,40,-30},
  {0,20,80,40,-30},
  {0,20,0,40,-30},
  {0,20,-30,40,-30},    
  {0,20,0,40,0},
  {0,0,0,0,0}, 
     
// right foot support 16-31
  {0,-40,0,-20,0},
  {0,-40,30,-20,30}, 
  {0,-10,30,-20,30}, 
  {0,-40,30,-20,30}, 
  {0,-10,30,-20,30}, 

  {0,-40,30,-20,30}, 
  {0,-40,30,-20,0},
  {0,-40,30,-20,-80},
  {0,-40,30,-20,0},
  {0,-40,30,-20,40},
  {0,-40,30,-20,0},
  {0,-40,30,-20,-80},
  {0,-40,30,-20,0},
  {0,-40,30,-20,30},    
  {0,-40,0,-20,0},
  {0,0,0,0,0},
};

// Dance 3:
const int num_dance3 = 8;       // split step
const int array_dance3[num_dance3][5] =
{ 
  {0,0,-40,0,0},
  {0,20,-30,20,20},
  {0,40,0,40,30},   
  {0,0,0,0,40},
  {0,-20,-20,-20,30},
  {0,-40,-30,-40,0},

  {0,0,-40,0,0},
  {0,0,0,0,0},
};

// Dance 4:
const int num_dance4 = 20;      // in place somersault inwards
                                //  and accelerate angles
const int array_dance4[num_dance4][5] =
{     
  {0,0,-20,0,20},   
  {0,0,0,0,0},
  {0,0,-20,0,20},
  {0,0,0,0,0},
  {0,0,-20,0,20},
  {0,0,0,0,0},
  {0,0,-20,0,20},
  {0,0,0,0,0},
    
  {0,0,-50,0,50},
  {0,0,0,0,0},    
  {0,-50,0,50},
  {0,0,0,0,0},    
  {0,0,-50,0,50},
  {0,0,0,0,0},    
  {0,0,-50,0,50},
  {0,0,0,0,0},
    
  {0,0,-40,0,40},
  {0,0,-50,0,50},
  {0,0,-60,0,60},
  {0,0,0,0,0,},
};

// Dance 5:
const int num_dance5 = 17;
const int array_dance5[num_dance5][5] =
{ 
  {0,35,0,15,0},     // let both legs touch together
  {0,35,30,15,30},   
  {0,-35,30,15,30},   
  {0,-20,0,15,0},
  {0,0,0,0,0},

  {0,0,-40,0,40},    // somersault inwards and rotate legs
  {0,-30,-40,-20,40},
  {0,0,-40,0,40},
  {0,20,-40,30,40},    

  {0,0,-40,0,40},    // go up and down in place and rotate legs
  {0,20,-40,-20,40},   
  {0,20,-20,-20,20},
  {0,20,0,-20,0},   
  {0,-20,-10,20,10},
  {0,-10,-30,10,30},

  {0,0,-40,0,40},
  {0,0,0,0,0},
};

// Dance 6:
const int num_dance6 = 32;      
const int array_dance6[num_dance6][5] =
{   
  {0,0,-40,0,-20},        // go ahead - toe out
  {0,25,-40,18,-20},
  {0,25,0,18,0},
  {0,0,20,0,40},
  {0,-18,20,-25,40},
  {0,-18,0,-25,0},
      
  {0,0,-40,0,-20},        // toe out - piaffe
  {0,25,-40,18,-20},
  {0,0,0,0,0},
  {0,0,20,0,40},
  {0,-18,20,-25,40},
  {0,0,0,0,0},

  {0,0,-40,0,-20},        // toe out - go backwards
  {0,-25,-40,-18,-20},
  {0,-25,0,-18,0},
  {0,0,20,0,40},
  {0,18,20,25,40},
  {0,18,0,25,0},

  {0,0,-40,0,-20},        // go ahead
  {0,30,-40,30,-20},
  {0,30,0,30,0},
  {0,0,20,0,40},
  {0,-30,20,-30,40},
  {0,-30,0,-30,0},
  
  {0,0,-40,0,-20},        // go backwards
  {0,-30,-40,-30,-20},
  {0,-30,0,-30,0},
  {0,0,20,0,40},
  {0,30,20,30,40},
  {0,30,0,30,0},

  {0,15,0,15,0},
  {0,0,0,0,0},
};

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Variables:
int vel_Dance1 = 30, vel_Dance2 = 25, vel_Dance3 = 40;     
int vel_Dance4 = 40, vel_Dance5 = 40, vel_Dance6 = 30;

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Servos (Micro Servo SG90):
// VarSpeedServo definitions:
VarSpeedServo headservo;      // Head
VarSpeedServo rightlegservo;  // Right Leg
VarSpeedServo rightfootservo; // Right Foot
VarSpeedServo leftlegservo;   // Left Leg
VarSpeedServo leftfootservo;  // Left Foot

// Functions:
/*~
 ~ Assigns Arduino Nano ports to the 5 servos.
 ~
 ~   PB0 -> Head servo
 ~   PB1 -> Right leg servo
 ~   PB2 -> Right foot servo
 ~   PB3 -> Left leg servo
 ~   PB4 -> Left foot servo
 ~
 ~ Hint: Be sure that the servos are connected
 ~       correctly!
 ~       This function is BLOCKING!
 ~
 */
void Servo_Init()
{                                            // Connect the ...
  rightfootservo.attach(RIGHTFOOTSERVO_PIN); //  right foot servo to pin 10 
  delay(100);
  leftfootservo.attach(LEFTFOOTSERVO_PIN);   //  left foot servo to pin 12 
  delay(100);
  headservo.attach(HEADSERVO_PIN);           //  head servo to pin 8
  delay(100);
  rightlegservo.attach(RIGHTLEGSERVO_PIN);   //  right leg servo to pin 9 
  delay(100);
  leftlegservo.attach(LEFTLEGSERVO_PIN);     //  left leg servo to pin 11 
  delay(250);
}

/*~
 ~ Moves all servos to their center positions.
 ~ This function can be used for mounting the
 ~ servos correctly centered.
 ~
 */
void AllServo_Center()
{
  headservo.slowmove(90, vel);
  rightlegservo.slowmove(90, vel);
  rightfootservo.slowmove(90, vel);
  leftlegservo.slowmove(90, vel);
  leftfootservo.slowmove(90, vel);
}

/*~
 ~ This function moves single servos to
 ~ their default positions stored in the
 ~ array_cal array.
 ~
 ~ Input:  Servo -> Servo number [0..4]
 ~
 ~ Hint: Servo: 0 or HEAD       -> Head servo
 ~              1 or RIGHT_LEG  -> Right leg servo
 ~              2 or RIGHT_FOOT -> Right foot servo
 ~              3 or LEFT_LEG   -> Left leg servo
 ~              4 or LEFT_FOOT  -> Left foot servo
 ~
 */
void Servo_Default(int Servo)
{
  switch (Servo) {
    case HEAD :
      headservo.slowmove(array_cal[0], vel);
      break;
    case RIGHT_LEG :
      rightlegservo.slowmove(array_cal[1], vel);
      break;
    case RIGHT_FOOT :
      rightfootservo.slowmove(array_cal[2], vel);
      break;
    case LEFT_LEG :
      leftlegservo.slowmove(array_cal[3], vel);
      break;
    case LEFT_FOOT :
      leftfootservo.slowmove(array_cal[4], vel);
  }
}

/*~
 ~ Moves all servos to their default positions
 ~ stored in the array_cal array.
 ~ This function can be used to test the
 ~ default servo positions.
 ~
 */
void AllServo_Default()
{
  Servo_Default(HEAD);
  Servo_Default(RIGHT_LEG);
  Servo_Default(RIGHT_FOOT);
  Servo_Default(LEFT_LEG);
  Servo_Default(LEFT_FOOT);
}

/*~
 ~ Moves the foot servos to their default positions
 ~ stored in the array_cal[2], [4] array.
 ~ This function can be used to set both feet to
 ~ their default positions without changing the
 ~ head and leg positions.
 ~
 */
void FeetServo_Default()
{
  Servo_Default(RIGHT_FOOT);
  Servo_Default(LEFT_FOOT);
}

/*~
 ~ This function returns the default position
 ~ of a servo stored in the array_cal array.
 ~
 ~ Input:  Servo  -> Servo number [0..4]
 ~ Output: Servo default position [75..105]
 ~
 ~ Hint: Servo: 0 or HEAD       -> Head servo
 ~              1 or RIGHT_LEG  -> Right leg servo
 ~              2 or RIGHT_FOOT -> Right foot servo
 ~              3 or LEFT_LEG   -> Left leg servo
 ~              4 or LEFT_FOOT  -> Left foot servo
 ~
 */
int defPos(int Servo)
{
  if(Servo > 4) return 90;
  else return array_cal[Servo];
}

/*~
 ~ Wait until the movements of the 5 servos
 ~ are finished.
 ~
 ~ Hint: This function is BLOCKING!
 ~
 */
void Waitwhilemoving()
{
  headservo.wait();
  rightlegservo.wait();
  rightfootservo.wait();
  leftlegservo.wait();
  leftfootservo.wait();
}

/*~
 ~ This function tests whether one of
 ~ the servos is still moving.
 ~
 ~ Input:  Servo    -> Servo number  [0..4]
 ~ Output: Servo is moving  -> true  [1]
 ~         Servo NOT moving -> false [0]
 ~
 ~ Hint: Servo: 0 or HEAD       -> Head servo
 ~              1 or RIGHT_LEG  -> Right leg servo
 ~              2 or RIGHT_FOOT -> Right foot servo
 ~              3 or LEFT_LEG   -> Left leg servo
 ~              4 or LEFT_FOOT  -> Left foot servo
 ~
 */
bool isServoMoving(int Servo)
{
  switch (Servo) {
    case HEAD :
      return (headservo.isMoving());
      break;
    case RIGHT_LEG :
      return (rightlegservo.isMoving());
      break;
    case RIGHT_FOOT :
      return (rightfootservo.isMoving());
      break;
    case LEFT_LEG :
      return (leftlegservo.isMoving());
      break;
    case LEFT_FOOT :
      return (leftfootservo.isMoving());
      break;
    default :
      return 0;
  }
}

/*~
 ~ Main servo control function.
 ~
 ~ Input: Servo    -> Servo number   [0..4]
 ~        Position -> Servo position [0..180]
 ~        Speed    -> Servo speed    [1..255]
 ~        Blocking -> Yes:  true     [1]
 ~                    No:   false    [0]
 ~
 ~ Hints:
 ~    1. Servo: 0 or HEAD       -> Head servo
 ~              1 or RIGHT_LEG  -> Right leg servo
 ~              2 or RIGHT_FOOT -> Right foot servo
 ~              3 or LEFT_LEG   -> Left leg servo
 ~              4 or LEFT_FOOT  -> Left foot servo
 ~   2. Position: Center position is 90.
 ~   3. Speed: Speed values > 127 will not work.
 ~   4. Blocking: With this parameter set to true,
 ~      the function is BLOCKING until the servo
 ~      movement is complete, else it is NOT
 ~      blocking.
 ~
 */
void ServoWrite(int Servo, int Position, int Speed, int Blocking)
{
  switch (Servo) {
    case HEAD :
      headservo.slowmove(Position, Speed);
      if (Blocking) headservo.wait();
      break;
    case RIGHT_LEG :
      rightlegservo.slowmove(Position, Speed);
      if (Blocking) rightlegservo.wait();
      break;
    case RIGHT_FOOT :
      rightfootservo.slowmove(Position, Speed);
      if (Blocking) rightfootservo.wait();
      break;
    case LEFT_LEG :
      leftlegservo.slowmove(Position, Speed);
      if (Blocking) leftlegservo.wait();
      break;
    case LEFT_FOOT :
      leftfootservo.slowmove(Position, Speed);
      if (Blocking) leftfootservo.wait();
  }
}

/*~
 ~ Servo stop function.
 ~ Stops a servo movement that was started with the
 ~ ServoWrite() function at it's current position.
 ~
 ~ Input: Servo -> Servo number [0..4]
 ~
 ~ Hint: Servo: 0 or HEAD       -> Head servo
 ~              1 or RIGHT_LEG  -> Right leg servo
 ~              2 or RIGHT_FOOT -> Right foot servo
 ~              3 or LEFT_LEG   -> Left leg servo
 ~              4 or LEFT_FOOT  -> Left foot servo
 ~
 */
void ServoStop(int Servo)
{
  switch (Servo) {
    case HEAD :
      headservo.stop();
      break;
    case RIGHT_LEG :
      rightlegservo.stop();
      break;
    case RIGHT_FOOT :
      rightfootservo.stop();
      break;
    case LEFT_LEG :
      leftlegservo.stop();
      break;
    case LEFT_FOOT :
      leftfootservo.stop();
  }
}

/*~
 ~ Servo Sequence Play function.
 ~ With this function you can play a sequence of
 ~ servo positions with a speed to reach each of
 ~ these positions. The sequences have to be
 ~ defined before in sequence point arrays.
 ~
 ~ Input: Servo    -> Servo number   [0..4]
 ~        PosArray -> Sequence point array
 ~        SequPos  -> Points in PosArray
 ~        Loop     -> Yes:  true     [1]
 ~                    No:   false    [0]
 ~        StartPos -> Starting point [0..SequPos-1]
 ~
 ~ Hints:
 ~    1. Servo: 0 or HEAD       -> Head servo
 ~              1 or RIGHT_LEG  -> Right leg servo
 ~              2 or RIGHT_FOOT -> Right foot servo
 ~              3 or LEFT_LEG   -> Left leg servo
 ~              4 or LEFT_FOOT  -> Left foot servo
 ~    2. PosArray: the servo sequence point array.
 ~                 Each point array has the type
 ~                 "servoSequencePoint", defined
 ~                 in VarSpeedServo.h!
 ~    3. SequPos:  number of sequence points [2..254]
 ~                 in PosArray
 ~    4. Loop:     true:  play the sequence in a loop
 ~                 false: play the sequence once
 ~    5. StartPos: first position = 0
 ~                 last position  = (SequPos - 1)
 ~    6. You can stop a sequence with the SequenceStop()
 ~       function
 ~
 */
void ServoSequence(int Servo, servoSequencePoint * PosArray, \
                   uint8_t SequPos, bool Loop = true, \
                   uint8_t StartPos = 0)
{
  switch (Servo) {
    case HEAD :
      headservo.sequencePlay(PosArray, SequPos, Loop, StartPos);
      break;
    case RIGHT_LEG :
      rightlegservo.sequencePlay(PosArray, SequPos, Loop, StartPos);
      break;
    case RIGHT_FOOT :
      rightfootservo.sequencePlay(PosArray, SequPos, Loop, StartPos);
      break;
    case LEFT_LEG :
      leftlegservo.sequencePlay(PosArray, SequPos, Loop, StartPos);
      break;
    case LEFT_FOOT :
      leftfootservo.sequencePlay(PosArray, SequPos, Loop, StartPos);
  }
}

/*~
 ~ Servo Sequence Play stop function.
 ~ Stops a movement (sequence) started before with
 ~ the ServoSequence() function.
 ~
 ~ Input: Servo -> Servo number [0..4]
 ~
 ~ Hint: Servo: 0 or HEAD       -> Head servo
 ~              1 or RIGHT_LEG  -> Right leg servo
 ~              2 or RIGHT_FOOT -> Right foot servo
 ~              3 or LEFT_LEG   -> Left leg servo
 ~              4 or LEFT_FOOT  -> Left foot servo
 ~
 */
void SequenceStop(int Servo)
{
  switch (Servo) {
    case HEAD :
      headservo.sequenceStop();
      break;
    case RIGHT_LEG :
      rightlegservo.sequenceStop();
      break;
    case RIGHT_FOOT :
      rightfootservo.sequenceStop();
      break;
    case LEFT_LEG :
      leftlegservo.sequenceStop();
      break;
    case LEFT_FOOT :
      leftfootservo.sequenceStop();
  }
}

// Basic moves:
/*~
 ~ Walk ahead.
 ~
 ~ Input: Steps -> Number of steps [1..32767]
 ~        Speed -> Walking speed   [1..255]
 ~
 ~ Hints: - This function is BLOCKING!
 ~        - Speed: The whole value range
 ~          [1..255] will not work for
 ~          walking!
 ~        - RoboDuck will stop walking,
 ~          if the number of steps is
 ~          reached.
 ~
 */
void Walkahead(int Steps, int Speed)
{   
  for(int x = 0; x < Steps; x++) { // walk ...
    for(int z = 0; z < 6; z++) {
      headservo.slowmove(array_cal[0] + array_walkahead[z][0], Speed);
      rightlegservo.slowmove(array_cal[1] + array_walkahead[z][1], Speed);   
      rightfootservo.slowmove(array_cal[2] + array_walkahead[z][2], Speed);
      leftlegservo.slowmove(array_cal[3] + array_walkahead[z][3], Speed);
      leftfootservo.slowmove(array_cal[4] + array_walkahead[z][4], Speed);
      Waitwhilemoving();
    }
  }
  for(int z = 6; z < 8; z++) { // default pos after last step
    headservo.slowmove(array_cal[0] + array_walkahead[z][0], Speed);
    rightlegservo.slowmove(array_cal[1] + array_walkahead[z][1], Speed);   
    rightfootservo.slowmove(array_cal[2] + array_walkahead[z][2], Speed);
    leftlegservo.slowmove(array_cal[3] + array_walkahead[z][3], Speed);
    leftfootservo.slowmove(array_cal[4] + array_walkahead[z][4], Speed);
    Waitwhilemoving();
  } 
}

/*~
 ~ Walk backwards.
 ~
 ~ Input: Steps -> Number of steps [1..32767]
 ~        Speed -> Walking speed   [1..255]
 ~
 ~ Hints: - This function is BLOCKING!
 ~        - Speed: The whole value range
 ~          [1..255] will not work for
 ~          walking!
 ~        - RoboDuck will stop walking,
 ~          if the number of steps is
 ~          reached.
 ~
 */
void Walkback(int Steps, int Speed)
{   
  for(int x = 0; x < Steps; x++) { // walk ...
    for(int z = 0; z < 6; z++) {
      headservo.slowmove(array_cal[0] + array_walkback[z][0], Speed);
      rightlegservo.slowmove(array_cal[1] + array_walkback[z][1], Speed);   
      rightfootservo.slowmove(array_cal[2] + array_walkback[z][2], Speed);
      leftlegservo.slowmove(array_cal[3] + array_walkback[z][3], Speed);
      leftfootservo.slowmove(array_cal[4] + array_walkback[z][4], Speed);
      Waitwhilemoving();
    } 
  }
  for(int z = 6; z < 8; z++) { // default pos after last step
    headservo.slowmove(array_cal[0] + array_walkback[z][0], Speed);
    rightlegservo.slowmove(array_cal[1] + array_walkback[z][1], Speed);   
    rightfootservo.slowmove(array_cal[2] + array_walkback[z][2], Speed);
    leftlegservo.slowmove(array_cal[3] + array_walkback[z][3], Speed);
    leftfootservo.slowmove(array_cal[4] + array_walkback[z][4], Speed);
    Waitwhilemoving();
  } 
}

/*~
 ~ Turn left.
 ~
 ~ Input: Steps -> Number of steps [1..32767]
 ~
 ~ Hint: This function is BLOCKING!
 ~
 */
void Turnleft(int Steps)
{
  for(int x = 0; x < Steps; x++) { // turn left ...
    for(int z = 0; z < 4; z++) {
      headservo.slowmove(array_cal[0] + array_turnleft[z][0], 30);
      rightlegservo.slowmove(array_cal[1] + array_turnleft[z][1], 30);   
      rightfootservo.slowmove(array_cal[2] + array_turnleft[z][2], 30);
      leftlegservo.slowmove(array_cal[3] + array_turnleft[z][3], 30);
      leftfootservo.slowmove(array_cal[4] + array_turnleft[z][4], 30);
      Waitwhilemoving();
    } 
  }
}

/*~
 ~ Turn right.
 ~
 ~ Input: Steps -> Number of steps [1..32767]
 ~
 ~ Hint: This function is BLOCKING!
 ~
 */
void Turnright(int Steps)
{
  for(int x = 0; x < Steps; x++) { // turn right
    for(int z = 0; z < 4; z++) {
      headservo.slowmove(array_cal[0] + array_turnright[z][0], 30);
      rightlegservo.slowmove(array_cal[1] + array_turnright[z][1], 30);   
      rightfootservo.slowmove(array_cal[2] + array_turnright[z][2], 30);
      leftlegservo.slowmove(array_cal[3] + array_turnright[z][3], 30);
      leftfootservo.slowmove(array_cal[4] + array_turnright[z][4], 30);
      Waitwhilemoving();
    } 
  }
}

// Dance moves:
// (Hint: all dances are BLOCKING!)
/*~
 ~ Wavy pro-/supination of both feet
 ~ with slightly moving left.
 ~
 ~ Input: times -> Number of moves [1..32767]
 ~
 */
void Slide_2_Left(int times)
{
  for(int time1 = 0; time1 < times; time1++) { 
    for(int z = 0; z < 5; z++) {                     
      rightlegservo.slowmove(array_cal[1] + array_dance1[z][1], vel_Dance1);   
      rightfootservo.slowmove(array_cal[2] + array_dance1[z][2], vel_Dance1);
      leftlegservo.slowmove(array_cal[3] + array_dance1[z][3], vel_Dance1);
      leftfootservo.slowmove(array_cal[4] + array_dance1[z][4], vel_Dance1);
      Waitwhilemoving();
    } 
  }
}

/*~
 ~ Wavy pro-/supination of both feet
 ~ with slightly moving right.
 ~
 ~ Input: times -> Number of moves [1..32767]
 ~
 */
void Slide_2_Right(int times)
{
  for(int time1 = 0; time1 < times; time1++) {
    for(int z = 5; z < 10; z++) {                     
      rightlegservo.slowmove(array_cal[1] + array_dance1[z][1], vel_Dance1);   
      rightfootservo.slowmove(array_cal[2] + array_dance1[z][2], vel_Dance1);
      leftlegservo.slowmove(array_cal[3] + array_dance1[z][3], vel_Dance1);
      leftfootservo.slowmove(array_cal[4] + array_dance1[z][4], vel_Dance1);
      Waitwhilemoving();
    }   
  }
}

/*~
 ~ Weight shift to the left, turning left leg
 ~ and right foot movements.
 ~
 */
void Left_Foot_Support()
{
  for(int z = 0; z < 16; z++) { // z < 12
    if (z > 5 && z < 14) {    // z(1, 10)
      vel_Dance2 = 50;
    }
    else { 
      vel_Dance2 = 25; 
    }

    rightlegservo.slowmove(array_cal[1] + array_dance2[z][1], vel_Dance2);   
    rightfootservo.slowmove(array_cal[2] + array_dance2[z][2], vel_Dance2);
    leftlegservo.slowmove(array_cal[3] + array_dance2[z][3], vel_Dance2);
    leftfootservo.slowmove(array_cal[4] + array_dance2[z][4], vel_Dance2);
    Waitwhilemoving();
  }
}

/*~
 ~ Weight shift to the right, turning right leg
 ~ and left foot movements.
 ~
 */
void Right_Foot_Support()
{
  for(int z = 16; z < 32; z++) {  // z < 24
    if (z > 21 && z < 30) {     // z(13, 22)
      vel_Dance2 = 50;
    }
    else { 
      vel_Dance2 = 25; 
    }

    rightlegservo.slowmove(array_cal[1] + array_dance2[z][1], vel_Dance2);   
    rightfootservo.slowmove(array_cal[2] + array_dance2[z][2], vel_Dance2);
    leftlegservo.slowmove(array_cal[3] + array_dance2[z][3], vel_Dance2);
    leftfootservo.slowmove(array_cal[4] + array_dance2[z][4], vel_Dance2);
    Waitwhilemoving();
  }
}

/*~
 ~ Wavy pro-/supination of both feet
 ~ with slightly moving left/right.
 ~ Weight shift to the left/right,
 ~ turning left/right leg and
 ~ right/left foot movements.
 ~
 */
void Dancing1_2()
{
  Slide_2_Left(2);
  Left_Foot_Support();

  Slide_2_Right(2);
  Right_Foot_Support();
}

/*~
 ~ Wavy turning of both legs and feet.
 ~
 ~ Input: Times -> Number of dances [1..32767]
 ~        Vel   -> Moving speed [1..255]
 ~
 */
void Dancing3(int Times = 1, int Vel = 40)
{   
  for(int time3 = 0; time3 < Times; time3++) {
    for(int z = 0; z < 6; z++) {
      if (time3 > 1 && time3 < 4) {
        vel_Dance3 = Vel;
      }
      else { 
        vel_Dance3 = 40; 
      }                     
            
      rightlegservo.slowmove(array_cal[1] + array_dance3[z][1], vel_Dance3);   
      rightfootservo.slowmove(array_cal[2] + array_dance3[z][2], vel_Dance3);
      leftlegservo.slowmove(array_cal[3] + array_dance3[z][3], vel_Dance3);
      leftfootservo.slowmove(array_cal[4] + array_dance3[z][4], vel_Dance3);
      Waitwhilemoving();
    } 
  }
  for(int z = 6; z < 8; z++) {                     
    rightlegservo.slowmove(array_cal[1] + array_dance3[z][1], vel_Dance3);   
    rightfootservo.slowmove(array_cal[2] + array_dance3[z][2], vel_Dance3);
    leftlegservo.slowmove(array_cal[3] + array_dance3[z][3], vel_Dance3);
    leftfootservo.slowmove(array_cal[4] + array_dance3[z][4], vel_Dance3);
    Waitwhilemoving();
  }       
}

/*~
 ~ Wavy pro-/supination of both feet with different
 ~ speed, one short internal rotation of both legs.
 ~
 */
void Dancing4()
{
  for(int z = 0; z < num_dance4; z++) {
    if (z > 17) {
      vel_Dance4 = 10;
    }
    else {
      vel_Dance4 = 40;
    }

    rightlegservo.slowmove(array_cal[1] + array_dance4[z][1], vel_Dance4);   
    rightfootservo.slowmove(array_cal[2] + array_dance4[z][2], vel_Dance4);
    leftlegservo.slowmove(array_cal[3] + array_dance4[z][3], vel_Dance4);
    leftfootservo.slowmove(array_cal[4] + array_dance4[z][4], vel_Dance4);
    Waitwhilemoving();
  } 
}

/*~
 ~ Weight shifts to the right with turning the right leg,
 ~ wavy pro-/supination of both feet and turning both legs
 ~ in different directions.
 ~
 */
void Dancing5()
{   
  for(int x = 0; x < 3; x++) {
    for(int z = 0; z < 5; z++) {                     
      rightlegservo.slowmove(array_cal[1] + array_dance5[z][1], vel_Dance5);   
      rightfootservo.slowmove(array_cal[2] + array_dance5[z][2], vel_Dance5);
      leftlegservo.slowmove(array_cal[3] + array_dance5[z][3], vel_Dance5);
      leftfootservo.slowmove(array_cal[4] + array_dance5[z][4], vel_Dance5);
      Waitwhilemoving();
    } 
  }
   
  for(int x = 0; x < 2; x++) {
    for(int z = 5; z < 9; z++) {                     
      rightlegservo.slowmove(array_cal[1] + array_dance5[z][1], 30);   
      rightfootservo.slowmove(array_cal[2] + array_dance5[z][2], 30);
      leftlegservo.slowmove(array_cal[3] + array_dance5[z][3], 30);
      leftfootservo.slowmove(array_cal[4] + array_dance5[z][4], 30);
      Waitwhilemoving();
    } 
  }
    
  for(int x = 0; x < 3; x++) {
    for(int z = 9; z < 15; z++) {                     
      rightlegservo.slowmove(array_cal[1] + array_dance5[z][1], vel_Dance5);   
      rightfootservo.slowmove(array_cal[2] + array_dance5[z][2], vel_Dance5);
      leftlegservo.slowmove(array_cal[3] + array_dance5[z][3], vel_Dance5);
      leftfootservo.slowmove(array_cal[4] + array_dance5[z][4], vel_Dance5);
      Waitwhilemoving();
    } 
  }    

  for(int z = 15; z < 17; z++) {                     
    rightlegservo.slowmove(array_cal[1] + array_dance5[z][1], 10);   
    rightfootservo.slowmove(array_cal[2] + array_dance5[z][2], 10);
    leftlegservo.slowmove(array_cal[3] + array_dance5[z][3], 10);
    leftfootservo.slowmove(array_cal[4] + array_dance5[z][4], 10);
    Waitwhilemoving();
  }   
}

/*~
 ~ Walking ahead and backwards in
 ~ two styles.
 ~
 */
void Dancing6()
{   
  const int array_cal_0 = array_cal[1] + 10 , array_cal_2 = array_cal[3] - 10;
    
  for(int x = 0; x < 3; x++) {
    for(int z = 0; z < 6; z++) {                     
      rightlegservo.slowmove(array_cal_0 + array_dance6[z][1], vel_Dance6);   
      rightfootservo.slowmove(array_cal[2] + array_dance6[z][2], vel_Dance6);
      leftlegservo.slowmove(array_cal_2 + array_dance6[z][3], vel_Dance6);
      leftfootservo.slowmove(array_cal[4] + array_dance6[z][4], vel_Dance6);
      Waitwhilemoving();
    } 
  }
   
  for(int x = 0; x < 3; x++) {
    for(int z = 6; z < 12; z++) {                     
      rightlegservo.slowmove(array_cal_0 + array_dance6[z][1], 40);   
      rightfootservo.slowmove(array_cal[2] + array_dance6[z][2], 40);
      leftlegservo.slowmove(array_cal_2 + array_dance6[z][3], 40);
      leftfootservo.slowmove(array_cal[4] + array_dance6[z][4], 40);
      Waitwhilemoving();
    } 
  }

  for(int x = 0; x < 3; x++) {
    for(int z = 12; z < 18; z++) {                     
      rightlegservo.slowmove(array_cal_0 + array_dance6[z][1], vel_Dance6);   
      rightfootservo.slowmove(array_cal[2] + array_dance6[z][2], vel_Dance6);
      leftlegservo.slowmove(array_cal_2 + array_dance6[z][3], vel_Dance6);
      leftfootservo.slowmove(array_cal[4] + array_dance6[z][4], vel_Dance6);
      Waitwhilemoving();
    } 
  }  

  for(int x = 0; x < 3; x++) {
    for(int z = 18; z < 24; z++) {                     
      rightlegservo.slowmove(array_cal[1] + array_dance6[z][1], vel_Dance6);   
      rightfootservo.slowmove(array_cal[2] + array_dance6[z][2], vel_Dance6);
      leftlegservo.slowmove(array_cal[3] + array_dance6[z][3], vel_Dance6);
      leftfootservo.slowmove(array_cal[4] + array_dance6[z][4], vel_Dance6);
      Waitwhilemoving();
    } 
  }

  for(int x = 0; x < 3; x++) {
    for(int z = 24; z < 30; z++) {                     
      rightlegservo.slowmove(array_cal[1] + array_dance6[z][1], vel_Dance6);   
      rightfootservo.slowmove(array_cal[2] + array_dance6[z][2], vel_Dance6);
      leftlegservo.slowmove(array_cal[3] + array_dance6[z][3], vel_Dance6);
      leftfootservo.slowmove(array_cal[4] + array_dance6[z][4], vel_Dance6);
      Waitwhilemoving();
    } 
  }

  for(int z = 30; z < 32; z++) {                     
    rightlegservo.slowmove(array_cal[1] + array_dance5[z][1], 10);   
    rightfootservo.slowmove(array_cal[2] + array_dance5[z][2], 10);
    leftlegservo.slowmove(array_cal[3] + array_dance5[z][3], 10);
    leftfootservo.slowmove(array_cal[4] + array_dance5[z][4], 10);
    Waitwhilemoving();
  }     
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Ultrasonic Distance Sensor (HY-SRF05):
// Ultrasonic setup:
/*~
 ~ Assigns Arduino Nano ports and the timeout
 ~ value [us] to the Ultrasonic Sensor. A
 ~ timeout value of 12000 us means a max.
 ~ measurable distance of up to 200 cm
 ~ [MAX_DISTANCE].
 ~
 ~   PD3 -> Echo
 ~   PD4 -> Trigger
 ~
 ~ Hint: Be sure that the sensor is connected
 ~       correctly! Otherwise it could be
 ~       destroyed!
 ~       If you want to use a MAX_DISTANCE,
 ~       that is higher than 200 cm, you MUST
 ~       increase the timeout value [us] of
 ~       this function!
 ~       Examples: 300 cm -> 18000UL
 ~                 400 cm -> 24000UL
 ~                 450 cm -> 27000UL
 ~       The MAX_DISTANCE definition can be
 ~       found in RoboDuck.h.
 ~
 */
Ultrasonic ultrasonic(TRIGGER_PIN, ECHO_PIN, 12000UL);

// Functions:
/*~
 ~ Measures and returns the distance to an
 ~ obstacle in centimeter (cm).
 ~
 ~ Output: Distance in cm  [1..MAX_DISTANCE]
 ~         Out of range -> [0]
 ~
 ~ Hint: This function is BLOCKING for about
 ~       12 ms! There must be a pause before
 ~       calling it again of AT LEAST 29 ms.
 ~
 */
int Distance_cm()
{
  int din = ultrasonic.read();
  if (din > MAX_DISTANCE) din = 0;
  return (din); 
}

/*~
 ~ Detects an obstacle that is too close.
 ~
 ~ Output: Too close     -> true  [1]
 ~         Not too close -> false [0]
 ~
 ~ Hint: This function is BLOCKING for
 ~       about 210 ms!
 ~       If an obstacle is nearer than
 ~       about 10 cm (4 inch), this
 ~       function will return true,
 ~       else it will return false.
 ~
 */
bool TooClose()
{
  int tooclose = 0;
  for(int a = 0; a < 5; a++) {  
    delay(30);
    int din = ultrasonic.read(INC);
    if (din < 5 && din > 0) tooclose++;
  }
  if (tooclose < 5) return 0;   
  return 1;
}

/*~
 ~ Walk ahead and stop if too close
 ~ to an obstacle.
 ~
 ~ Input: Steps -> Number of steps [1..32767]
 ~        Speed -> Walking speed   [1..255]
 ~
 ~ Hints: - This function is BLOCKING!
 ~        - Speed: The whole value range
 ~          [1..255] will not work for
 ~          walking!
 ~        - RoboDuck will stop walking,
 ~          if an obstacle is nearer
 ~          than about 10 cm (4 inch)
 ~          OR if the number of steps
 ~          is reached.
 ~
 */
void Walkahead_US(int Steps, int Speed)
{   
  int flag = 0;
  for(int x = 0; x < Steps; x++) { // walk ...
    for(int z = 0; z < 6; z++) {
      flag = 0;
      headservo.slowmove(array_cal[0] + array_walkahead[z][0], Speed);
      rightlegservo.slowmove(array_cal[1] + array_walkahead[z][1], Speed);   
      rightfootservo.slowmove(array_cal[2] + array_walkahead[z][2], Speed);
      leftlegservo.slowmove(array_cal[3] + array_walkahead[z][3], Speed);
      leftfootservo.slowmove(array_cal[4] + array_walkahead[z][4], Speed);
      if (TooClose()) {
        flag = 1;
        break; // stop, if an obstacle is nearer than 10 cm
      }
      Waitwhilemoving();
    }
    if (flag) break; // obstacle: no more steps!
  }
  for(int z = 6; z < 8; z++) { // default pos after last step
    headservo.slowmove(array_cal[0] + array_walkahead[z][0], Speed);
    rightlegservo.slowmove(array_cal[1] + array_walkahead[z][1], Speed);   
    rightfootservo.slowmove(array_cal[2] + array_walkahead[z][2], Speed);
    leftlegservo.slowmove(array_cal[3] + array_walkahead[z][3], Speed);
    leftfootservo.slowmove(array_cal[4] + array_walkahead[z][4], Speed);
    Waitwhilemoving();
  } 
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// Beeper (CLW1027):
/*~
 ~ "Beep" (or play notes) with the Beeper.
 ~
 ~ Input: Tone:     0  -> Tone off
 ~                  >0 -> Pitch [Hz]
 ~        Duration: 0  -> Tone on
 ~                  >0 -> Tone on for
 ~                        Duration [ms]
 ~
 ~ Hint: This function is BLOCKING for
 ~       Duration [ms]! If Duration is
 ~       zero (0), the function is NOT
 ~       blocking, but the tone stays
 ~       on permanently.
 ~       A tone can be switched off by
 ~       using: Beep(0, 0);
 ~
 */
void Beep(int Tone, int Duration)
{
  if (Tone == 0) noTone(BEEPER_PIN);
  else {
    tone(BEEPER_PIN, Tone);
    if (Duration > 0) {
      delay(Duration);
      noTone(BEEPER_PIN);
    }
  }
}

/*~
 ~ "Quack-quack" sound with the Beeper.
 ~
 ~ Hint: This function is BLOCKING for
 ~       about 400 ms!
 ~
 */
void Quack()
{
  for(int i = 0; i < 5; i++) {
    analogWrite(BEEPER_PIN, 50);
    delay(5);
    analogWrite(BEEPER_PIN, 0);
    delay(30);   
  }
  delay(50);
  for(int i = 0; i < 5; i++) {
    analogWrite(BEEPER_PIN, 50);
    delay(5);
    analogWrite(BEEPER_PIN, 0);
    delay(30);   
  }
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
// LEDs:
/*~
 ~ Switches RoboDuck's LEDs on and off.
 ~
 ~ Input: leds: 0 or ALL_OFF       -> all 3 LEDs off
 ~              1 or GREEN         -> green (right) LED on
 ~              2 or RED           -> red (left) LED on
 ~              3 or GREEN_RED     -> green AND red LEDs on
 ~              4 or BUILTIN       -> built-in LED on
 ~              5 or GREEN_BUILTIN -> green AND built-in LEDs on
 ~              6 or RED_BUILTIN   -> red AND built-in LEDs on
 ~              7 or ALL_ON        -> all 3 LEDs on
 ~
 ~ Hint: With LEDs() RoboDuck is able to display numbers from 0
 ~       to 7 with it's LEDs. So you can visualize results e.g.
 ~       from the distance sensor with the LEDs! 
 ~
 */
void LEDs(int leds)
{
  if (leds & 0b00000001) digitalWrite(LED_R_GREEN_PIN, HIGH); // green LED
  else digitalWrite(LED_R_GREEN_PIN, LOW);
  if (leds & 0b00000010) digitalWrite(LED_L_RED_PIN, HIGH);   // red LED
  else digitalWrite(LED_L_RED_PIN, LOW);
  if (leds & 0b00000100) digitalWrite(LED_BUILTIN, HIGH);
  else digitalWrite(LED_BUILTIN, LOW);
}

/*~
 ~ Switches RoboDuck's LED 1 on and off.
 ~ LED 1 is the green (right) LED.
 ~
 ~ Input: state: 0 or OFF -> LED off
 ~               1 or ON  -> LED on
 ~
 */
void LED1(int state)
{
  if (state) digitalWrite(LED_R_GREEN_PIN, HIGH); // green LED
  else digitalWrite(LED_R_GREEN_PIN, LOW);
}

/*~
 ~ Switches RoboDuck's LED 2 on and off.
 ~ LED 2 is the red (left) LED.
 ~
 ~ Input: state: 0 or OFF -> LED off
 ~               1 or ON  -> LED on
 ~
 */
void LED2(int state)
{
  if (state) digitalWrite(LED_L_RED_PIN, HIGH);   // red LED
  else digitalWrite(LED_L_RED_PIN, LOW);
}

/*~
 ~ Switches RoboDuck's LED 3 on and off.
 ~ LED 3 is the Arduino Nano builtin (normally red) LED.
 ~
 ~ Input: state: 0 or OFF -> LED off
 ~               1 or ON  -> LED on
 ~
 */
void LED3(int state)
{
  if (state) digitalWrite(LED_BUILTIN, HIGH);
  else digitalWrite(LED_BUILTIN, LOW);
}

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




/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 ~ Additional info
 ~ ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 ~ Changelog:
 ~ - v. 0.90 12.09.2020 by D. Ottensmeyer
 ~           Added:
 ~            - Default positioning of SINGLE servos (Servo_Default())
 ~            - Function for stopping single servos (ServoStop())
 ~            - Function for playing sequences of servo position
 ~              points (ServoSequence())
 ~            - Function for stopping the sequences started by the
 ~              ServoSequence() function (SequenceStop())
 ~            - Function defPos() returns the servo default value
 ~            - New demo 4 for showing moves with ServoSequence()
 ~            - Functions for switching each LED separately (LEDx())
 ~            - Some new German names for functions and constants
 ~            - Distance calculation improved in Ultrasonic-Lib
 ~            - Code cleaned up
 ~ - v. 0.85 29.08.2020 by D. Ottensmeyer
 ~           Bugfixes:
 ~            - Initial "jumping" to center servo position improved
 ~            - Slower default servo positioning
 ~            - Foot touches ground in Right_/Left_Foot_Support
 ~              functions fixed
 ~           Added:
 ~            - Now playing tones with the tone() function,
 ~              note pitch and duration constants defined
 ~            - NewPing- replaced by Ultrasonic-Library
 ~              (reason: tone() - NewPing-Lib conflict!)
 ~            - New main servo control function (ServoWrite())
 ~              (blocking OR NON-BLOCKING servo control!)
 ~            - New Walkahead_US() function for walking forward
 ~              with ultrasonic distance control (stops walking
 ~              if an obstacle is nearer than 10 cm)
 ~            - New isServoMoving() function
 ~            - New head moving function (MoveHead())
 ~            - 5 new demos / test programs
 ~ - v. 0.8  (initial release) 16.08.2020 by D. Ottensmeyer
 ~
 ~ ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 */

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