//        __    __________     ___    ____
//       / /   / ____/ __ \   /   |  /  _/
//      / /   / __/ / / / /  / /| |  / /  
//     / /___/ /___/ /_/ /  / ___ |_/ /   
//    /_____/_____/\____/  /_/  |_/___/   v 1.1 Final 25/01/2014
                                    



#include <Servo.h>
//____________________
//Servos
Servo servoCenter; 
Servo servoRight;
Servo servoLeft;
Servo servoHead;
Servo servoGripper;
//____________________
//Servo variables
int pos1;      
int pos2;
int pos3;
int pos4;
int pos5;
int pos6;
int pos7;
int pos8;
int pos9;
int pos10;
int posheadTG;
//____________________
//IR sensor
int sensorPin = 0;
int distance1 = 630;
int distance2 = 480;
int distance3 = 344;
int distance4 = 272;
int distance5 = 217;
int distance6 = 187;
int distance7 = 166;
int distance8 = 146;
int distance9 = 135;
int distance10 = 123;
int distance11 = 115;
int distance12 = 107;
int distance13 = 99;
int distance14 = 90;
int distance15 = 87;
int distance16 = 82;
//____________________
//LEDS 
int led1 = 13;
int led2 = 6;
const int led3 = 12;
const int led4 = 2;
//____________________
//Micro switches
int inPinLegs = 7;   
int val1 = 0;
int inPinGripper = 4;   
int val2 = 0;     
//____________________
//Blink
int ledState1 = LOW;
int ledState2 = HIGH;
long previousMillisBlink = 0;
long interval = 100;
//____________________
//Fade1
int value, value2 ;
long time=0;
int periode = 2000;
int displace = 500;
//____________________
//Fade2
unsigned long fadeMillis = 0;
boolean fadeUp = true;
int fadeValue = 0;
//____________________
//Strove
int valstrove = 0;
//____________________
//Search Timer
long previousMillisSearch = 0;        
long intervalSearch = 20000; 
//Sleep Timer
long previousMillisSleep = 0;        
long intervalSleep = 80000;
//____________________
//Standby
int buttonPushCounterLeg = 0;   
int buttonStateLeg = 0;         
int lastButtonStateLeg = 0;
//
int buttonStateGrip = 0;         
//____________________
//Random
long rndNumber1;
long rndNumber2;
long rndNumber3;
long rndNumber4;
long rndNumber5;
long rndNumber6;
//____________________
//Sound System
const int tonePin = 8;  

#define OCTAVE_OFFSET 0

int notes[] = { 0,
262, 277, 294, 311, 330, 349, 370, 392, 415, 440, 466, 494,
523, 554, 587, 622, 659, 698, 740, 784, 831, 880, 932, 988,
1047, 1109, 1175, 1245, 1319, 1397, 1480, 1568, 1661, 1760, 1865, 1976,
2093, 2217, 2349, 2489, 2637, 2794, 2960, 3136, 3322, 3520, 3729, 3951
};

char *song1 = "roar1:d=4,o=5,b=200:32e,32f,32g,32a,32b,32c6,32d6,32e6,32f6";
char *song2 = "roar2:d=4,o=5,b=200:32e,32f,32g,32a,32b,32a,32g,32f,32e";
char *song3 = "roar3:d=4,o=5,b=200:32e,32f6,32a,32d6,32f,32c6,32a,32c6,32e";
char *song4 = "roar4:d=4,o=5,b=200:32e6,32b,32e6,32b,32c6,32f,32f,32f,32d6";
char *song5 = "roar5:d=4,o=5,b=200:32f,32f,32d6,32e6,32c6,32d6,32d6,32c6,32d6";
char *song6 = "roar6:d=4,o=5,b=200:32c6,32f,32e,32c,32e,32b6,32b6,32c6,32g";
char *song7 = "Sonar:d=32,o=6,b=200:16e4";
char *song8 = "WolfWhis:d=4,o=5,b=900:8a4,16a#4,16b4,16c,16c#,16d,16d#,16e,16f,16f#,16g,16g#,16a,16a#,16b,16c6,8c#6,16d6,16d#6,16e6,16f6,p,p,16a4,16a#4,16b4,16c,16c#,16d,16d#,16e,16f,16f#,16g,16g#,16a,16a#,16b,16a#,16a,16g#,16g,16f#,16f,16e,16d#,16d,16c#,16c,16b4,16a#4,16a4";
char *song9 = "Inwards:d=4,o=5,b=160:32p,32c#7,32p,32f#,32p,32c7,32p,32g,32p,32b6,32p,32g#,32p,32a#6,32p,32a,32p,32a6,32p,32a#,32p,32g6,32p,32b,32p,32f#6,32p,32c6,32p,32f6,32p,32c#6,32p,32e6,32p,32d6,32d#6,32d6,32d#6,32d6,32d#6,32d6,32c#6,32c6,32b,32b6,32f#6,32b,32f#,32e";
char *song10 = "Status:d=4,o=5,b=63:32e,32e";

//#########################################
void setup()
{
  //Serial
  Serial.begin(9600);
  
  //Servos
  servoCenter.attach(11);  
  servoRight.attach(3);  
  servoLeft.attach(10);
  servoHead.attach(9);  
  servoGripper.attach(5);
  
  //LEDS
  pinMode(led3, OUTPUT);
  pinMode(led4, OUTPUT);
  
  //Micro switches
  pinMode(inPinLegs, INPUT); 
  pinMode(inPinGripper, INPUT);
  
  //Random
  randomSeed(analogRead(1));
  
}

//#########################################

             

void loop()
{
 A_ttach();
 delay(1000); //+
 Center();
 delay(1000); //+
 De_tach();
 Standby(); 
}

//#########################################
//++++++++++++++++++++++++++++++++++++++++++++++++++++++ STANDBY ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void Standby()
{
  Serial.println("Standby");
  Serial.println(millis());
  Serial.println("{");
  
  //
  play_rtttl(song10);
  //
  
  unsigned long currentMillisDelay = millis();
  
  while(millis() < currentMillisDelay + 70000 && analogRead(0) <= distance1 ) {
 
    
  valstrove = 100; 
  digitalWrite(led3, HIGH);  
  delay(valstrove);                  
  digitalWrite(led3, LOW);   
  delay(valstrove); 
    
    
  buttonStateLeg = digitalRead(inPinLegs);
  buttonStateGrip = digitalRead(inPinGripper);
  
  if (buttonStateGrip == HIGH) {
    
    
    GripperPlay();
  }
  
  if (buttonStateLeg != lastButtonStateLeg) {
    
    if (buttonStateLeg == HIGH) {
      
      buttonPushCounterLeg++;
      //
      play_rtttl(song1);
      //
      
      Serial.println("on");
      Serial.print("number of button pushes:  ");
      Serial.println(buttonPushCounterLeg);
    }
    else {
      Serial.println("off");
    }
  }
  
  lastButtonStateLeg = buttonStateLeg;
 
  }
  //__
  
  if(buttonPushCounterLeg  == 2 ) {
    
    buttonPushCounterLeg = 0;
    NavigateViaIR();
   
    
  }
  if(buttonPushCounterLeg  == 4 ) {
    
      buttonPushCounterLeg = 0;
      TrackObject();
  }
  
  if(buttonPushCounterLeg  == 1 ) {
    
      buttonPushCounterLeg = 0;
      Animate();
  
  }
  
  //__
  Serial.println("}");
  }
//++++++++++++++++++++++++++++++++++++++++++++++++++ NAVIGATE VIA IR ++++++++++++++++++++++++++++++++++++++++++++++++++++
void NavigateViaIR()
{
  Serial.println("NavigateViaIR");
  Serial.println(millis());
  Serial.println("{");
  //
  play_rtttl(song8);
  //
  A_ttach();
  Center();
  
  unsigned long currentMillisDelay = millis();
  
  while(millis() < currentMillisDelay + 70000  ) {
    
    if (analogRead(0) <= distance8)
    {
      Forward();  
    }
     
    if (analogRead(0) > distance8 &&  analogRead(0)< distance1) 
    {
      Search();
    }
    
  //____________________
  
     unsigned long currentMillisSearch = millis();
    
    if(currentMillisSearch - previousMillisSearch > intervalSearch) {
      
    previousMillisSearch = currentMillisSearch, Search ();
  }
  //____________________
  
     unsigned long currentMillisSleep = millis();
    
    if(currentMillisSleep - previousMillisSleep > intervalSleep) {
      
    previousMillisSleep = currentMillisSleep, Sleep ();
  }
  
  //____________________
  
  }  
  Center();
  Serial.println("}");
}

//_________________________________________
//++++++++++++++++++++++++++++++++++++++++++++++++++++ TRACK OBJECT +++++++++++++++++++++++++++++++++++++++++++++++++++++
void TrackObject()
{
  Serial.println("TrackObject");
  Serial.println(millis());
  Serial.println("{"); 
  
  //
  play_rtttl(song9);
  //
  
  A_ttach();
  Center();
  
  unsigned long currentMillisDelay = millis();
  
  while(millis() < currentMillisDelay + 70000  ) {
    
  TrackGo(); 
  
  }
  Center();
  Serial.println("}");
}
//_________________________________________
//++++++++++++++++++++++++++++++++++++++++++++++++++++++ GRIPPER PLAY +++++++++++++++++++++++++++++++++++++++++++++++++++
void GripperPlay()
{
  Serial.println("GripperPlay");
  Serial.println(millis());
  Serial.println("{");
  
  //
  play_rtttl(song2);
 
  //
  
  A_ttach();
  Center();
  De_tachMain();
  
  
  unsigned long currentMillisDelay = millis();
  
  while(millis() < currentMillisDelay + 40000  ) {
    
  Gripper();
  }
  Serial.println("}");
}
//_________________________________________
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ANIMATE ++++++++++++++++++++++++++++++++++++++++++++++++++++

void Animate()

{
  Serial.println("Animate");
  Serial.println(millis());
  Serial.println("{");

  A_ttach();
  Center();
      //
      play_rtttl(song5);
      //
  
  
  unsigned long currentMillisDelay = millis();
  
  while(millis() < currentMillisDelay + 70000  ) {
  
  ////////////////////////////////////// 
  //////////////////////////////////////
  A_ttach();
   
  if (analogRead(0) > distance2 &&  analogRead(0)< distance1) 
    {
  A_ttach();    
  rndNumber1= random(1,5);
  
  if (rndNumber1 == 1){
    
    EnergyAnim();
    EnergyAnim();
    EnergyAnim();
    EnergyAnim();
    EnergyAnim();
  }
  if (rndNumber1 == 2){
    
    GreetingAnim();
  }
  if (rndNumber1 == 3){
    
    AttackAnim();
  }
  if (rndNumber1 == 4){
    
    WakeUpAnim();
  }
  if (rndNumber1 == 5){
    
   StandBy1Anim (); 
  }
 }
 else {
  StandBy2Anim (); 
 }
  
    
  //////////////////////////////////////
    
  }
  A_ttach();
  Center();
 Serial.println("}"); 
}
  
//====================================================LEDS FADE & BLINK==================================================

//_________________________________________
void Blink()
{
 unsigned long currentMillisBlink = millis();
 
  if(currentMillisBlink - previousMillisBlink > interval) {
    previousMillisBlink = currentMillisBlink;  

    if (ledState1 == LOW)
      ledState1 = HIGH;
    else
      ledState1 = LOW;

    digitalWrite(led3, ledState1);
    
      //______
      
       if (ledState2 == HIGH)
      ledState2 = LOW;
    else
      ledState2 = HIGH;

    digitalWrite(led4, ledState2);
    
  }
  
}

//_________________________________________
void Fade1 ()

{
  time = millis();
  value = 128+127*cos(2*PI/periode*time);
  value2 = 128+127*cos(2*PI/periode*(displace-time));
  analogWrite(led1, value);           
  analogWrite(led2, value2);
  
    
}
//_________________________________________

void Fade2()

{
  if (millis() - fadeMillis >= 5)
	  {
	    fadeMillis = millis();
	 
	    if (fadeUp == true)
	    {
	      if (fadeValue < 255)
	      {
	        fadeValue++;
	      }
	      else
	      {
	        fadeUp = false;
	        fadeValue--;
	      }
	    }
	    else
	    {
	      if (fadeValue > 0)
	      {
	        fadeValue--;
	      }
	      else
	      {
	        fadeUp = true;
	        fadeValue++;
	      }
	    }
	    analogWrite(led1, fadeValue);
	    analogWrite(led2, fadeValue);

	  }
 
	}
//_________________________________________
//===================================================FORWARD,BACKWARD,TURN===============================================
//_________________________________________
  


  void Forward ()
 { 
    Serial.println("Forward");
    Serial.println(millis());
    Serial.println("{");  
   
  Fade1 ();
  Blink();
  
  servoCenter.write(140);
  
  Fade1 ();
  Blink();
  
  delay(400);
  
  Fade1 ();
  Blink(); 
  
  servoRight.write(70);
  
  Fade1 ();
  Blink(); 
  
  servoLeft.write(70);
  
  Fade1 ();
  Blink();
  
  delay(400);
  
  Fade1 ();
  Blink();
  
  servoCenter.write(60);
  
  Fade1 ();
  Blink();
  
  delay(400);
  
  Fade1 ();
  Blink();
  
  servoRight.write(110);
  
  Fade1 ();
  Blink();
  
  servoLeft.write(110);
  
  Fade1 ();
  Blink();
  
  delay(400);
  
  Fade1 ();
  Blink();
  
Serial.println("}");

}
//_________________________________________
void Backward()
{
    Serial.println("Backward");
    Serial.println(millis());  
    Serial.println("{");
  
  Fade1 ();
  Blink();
  
  servoCenter.write(140);
  delay(400);
  servoRight.write(110);
  servoLeft.write(110);
  delay(400);
  servoCenter.write(60);
  delay(400);
  servoRight.write(70);
  servoLeft.write(70);
  delay(400);
 
 Serial.println("}");
}
//_________________________________________


 void Turn_Right()
{
  Serial.println("Turn_Right");
  Serial.println(millis());  
  
  Fade1 ();
  Blink();
  
  servoCenter.write(60);
  delay(400);
  servoRight.write(70);
  servoLeft.write(110);
  delay(400);
  servoCenter.write(140);
  delay(400);
  servoRight.write(110);
  servoLeft.write(70);
  delay(400);

}
//_________________________________________

void Turn_Left()
{
  Serial.println("Turn_Left");
  Serial.println(millis());
  Serial.println("{");
  
  Fade1 ();
  Blink();
  
  servoCenter.write(60);
  delay(400);
  servoRight.write(110);
  servoLeft.write(70);
  delay(400);
  servoCenter.write(140);
  delay(400);
  servoRight.write(70);
  servoLeft.write(110);
  delay(400);

Serial.println("}");
}

//_________________________________________
//========================================================SLEEP============================================================

void Sleep()
{
  Serial.println("Sleep");
  Serial.println(millis());  
  Serial.println("{");
  
 Center(); 
 De_tach();
 
 digitalWrite(led1, HIGH);
 digitalWrite(led2, HIGH);
 digitalWrite(led3, HIGH);
 digitalWrite(led4, HIGH);
 
 play_rtttl(song3);
 
 unsigned long currentMillisDelay = millis();
  
 while(millis() < currentMillisDelay + 70000 && analogRead(0) <= distance1 ) {
 
  Fade2();
  }
  
  
  A_ttach();
  
  WakeUpAnim();
  delay(1000);
  play_rtttl(song4);
  Center();
  
  Serial.println("}");
 }

//_________________________________________

//========================================================SEARCH==========================================================

void Search()
{
  Serial.println("Search");
  Serial.println(millis());
  Serial.println("{");  
  
  A_ttach();
  Center();
  
   servoHead.write(35);
   play_rtttl(song7);
   
   delay(1000);
   if (analogRead(0) > distance8 &&  analogRead(0)< distance1) 
    {
    Backward();
    Turn_Left();
    Turn_Left();
    }
    delay(1000);
    
   servoHead.write(135);
   play_rtttl(song7);
   
   delay(1000);
   if (analogRead(0) > distance8 &&  analogRead(0)< distance1) 
    {
    Backward();
    Turn_Right();
    Turn_Right();
    }
   delay(1000);
   
   Center();

   
   Serial.println("}");
 }
 
//_________________________________________
//========================================================GRIPPER========================================================
//_________________________________________

void Opened ()
{
  servoGripper.write(110);
}

void Closed()
{
  
  servoGripper.write(70);
  
}
void Middle()
{
  servoGripper.write(90);
}

//_________________________________________
void Gripper()


{ 
  Serial.println("Gripper");
  Serial.println(millis());
  Serial.println("{");
    
  {

    if (analogRead(0) <= distance2)
    {
      Closed();
      
      
    }
   
    if (analogRead(0) > distance1)
    {
      Closed();
            
    }
    if (analogRead(0) > distance2 &&  analogRead(0)< distance1) 
    {
     Opened();
     
     delay(2000);
     Closed();
     play_rtttl(song4);
     delay(2000);
    }
  }
  
  Serial.println("}");
}
//_________________________________________
//=========================================================TRACK & GO====================================================
//_________________________________________

void TrackGo ()
{
  Serial.println("TrackGo");
  Serial.println(millis());
  Serial.println("{");
  
  
  for (int i=0; i<100; i += 5) {
    posheadTG = 35 + i;              
              
    servoHead.write(posheadTG);
   play_rtttl(song7); 
    delay(200);
    //
    if (analogRead(0) > distance11)
    {
      delay(500);
      
    if (analogRead(0) > distance11)
    {
      //
      Center2();
      delay(2000);
       
     if (analogRead(0) > distance11 && 70 <= (posheadTG) && (posheadTG)<= 100)
    {
     Forward();
     Forward();
     Center2();
    }
    if (analogRead(0) > distance11 && 35<=(posheadTG) && (posheadTG) <70)
    {
     //Turn_Right();
     Turn_Right();
     Center2();
    }
    
    if (analogRead(0) > distance11 && 100<(posheadTG) && (posheadTG) <=135)
    {
     //Turn_Left();
     Turn_Left();
     Center2();
    }
   // 
    }  
   //  
  }
  }  
  
  
  for (int i=0; i<100; i += 5) {
    posheadTG = 135 - i;              
              
    servoHead.write(posheadTG);
    play_rtttl(song7); 
    delay(200);
    //
    if (analogRead(0) > distance11)
    {
      delay(500);
      
    if (analogRead(0) > distance11)
    {
      //
      Center2();
      delay(2000);
      
     if (analogRead(0) > distance11 && 70<=(posheadTG) && (posheadTG)<=100)
    {
     Forward();
     Forward();
     Center2();
    }
    if (analogRead(0) > distance11 && 35<=(posheadTG) && (posheadTG)<70)
    {
     Turn_Right();
     Center2();
    }
    
    if (analogRead(0) > distance11 && 100<(posheadTG)&& (posheadTG)<=135)
    {
     Turn_Left();
     Center2();
    }
   // 
    }  
   //  
  }  
  
  }
    Serial.println("}");
 }
 
//_________________________________________

//=======================================================ANIMATIONS======================================================

//_________________________________________
void GreetingAnim()
{
  Serial.println("GreetingAnim");
  Serial.println(millis());
  Serial.println("{");
      //
      play_rtttl(song6);
      //  
  servoCenter.write(70);
  delay(500);
  servoRight.write(110);
  delay(500);
  servoRight.write(70);
  delay(500);
  servoRight.write(110);
  delay(500);
  servoRight.write(70);
  delay(500);
  servoRight.write(90);
  delay(500);
  servoCenter.write(100);
  delay(500);
  
 Serial.println("}");
}
 

//_________________________________________
void WakeUpAnim()
{
  Serial.println("WakeUpAnim");
  Serial.println(millis());
  Serial.println("{");
  
  servoCenter.write(70);
  delay(500);
  servoRight.write(110);
  delay(500);
  servoRight.write(70);
  delay(500);
  servoRight.write(110);
  delay(500);
  servoRight.write(70);
  delay(500);
  servoRight.write(90);
  delay(500);
  servoCenter.write(130);
  delay(500);
  servoLeft.write(110);
  delay(500);
  servoLeft.write(70);
  delay(500);
  servoLeft.write(110);
  delay(500);
  servoLeft.write(70);
  delay(500);
  servoLeft.write(90);
  delay(500);
  servoCenter.write(100);
  delay(500);
  servoGripper.write(60);
  delay(500);
  servoGripper.write(110);
  delay(500);
  servoGripper.write(60);
  delay(500);
  servoGripper.write(90);
  delay(500);  
  servoHead.write(115);
  delay(500);
  servoHead.write(55);
  delay(500);
  servoHead.write(115);
  delay(500);
  servoHead.write(55);
  delay(500);
  servoHead.write(85);
  delay(500);
  
  Serial.println("}");
  }
//_________________________________________
void StandBy1Anim ()
{
  
  Serial.println("StandBy1Anim");
  Serial.println(millis());
  Serial.println("{");
  
  for (int i=0; i<40; i += 1) {
    pos1 = 110 - i;              
    pos2 = 70 + i;          
    
    servoRight.write(pos1);
    servoLeft.write(pos2); 
    delay(15);                           
  }
  
  for (int i=0; i<40; i += 1) {
    pos1 = 110 - i;              
    pos2 = 70 + i;          
    
    servoRight.write(pos2);
    servoLeft.write(pos1); 
    delay(15);                           
  }
  for (int i=0; i<40; i += 1) {
    pos1 = 110 - i;              
    pos2 = 70 + i;          
    
    servoRight.write(pos1);
    servoLeft.write(pos2); 
    delay(15);                           
  }
  
  for (int i=0; i<40; i += 1) {
    pos1 = 110 - i;              
    pos2 = 70 + i;          
    
    servoRight.write(pos2);
    servoLeft.write(pos1); 
    delay(15);                           
  }
  servoRight.write(90);
  servoLeft.write(90); 
  
  
  Serial.println("}");
  
}
//_________________________________________
void AttackAnim()
{
  Serial.println("AttackAnim");
  Serial.println(millis());
  Serial.println("{");
  
  Forward();
  delay(1000);
  Center();
  delay(6000);
  //
  for (int i=0; i<40; i += 1) {
    pos1 = 110 - i;              
    pos2 = 70 + i;          
    
    servoRight.write(pos1);
    servoLeft.write(pos2); 
    delay(5);                           
  }
  
  for (int i=0; i<40; i += 1) {
    pos1 = 110 - i;              
    pos2 = 70 + i;          
    
    servoRight.write(pos2);
    servoLeft.write(pos1); 
    delay(5);                           
  }
  //
  
  delay(500);
  servoGripper.write(60);
  delay(200);
  servoGripper.write(110);
  delay(200);
  servoGripper.write(60);
  delay(200);
  servoGripper.write(110);
  delay(200);
  servoGripper.write(60);
  delay(200);
  servoGripper.write(110);
  delay(200);
  servoGripper.write(60);
  delay(200);
  servoGripper.write(90);
  delay(200);  
  
  Serial.println("}");
}
//_________________________________________
void EnergyAnim()
{
  Serial.println("EnergyAnim");
  Serial.println(millis());
  Serial.println("{");
  //
  rndNumber2 = random(75,135);
  rndNumber3 = random(35,95);
  rndNumber4 = random(100,110);
  rndNumber5 = random(60,70);
  
  for (int i=0; i<40; i += 1) {
    
    pos5 = (rndNumber2) - i; //rnd head (105->65)
    pos6 = 120 - i; //120->80 center
    pos9 = (rndNumber4) -i; //gripper rnd
   
    servoHead.write(pos5);
    servoGripper.write(pos9);
    servoCenter.write(pos6); 

    
    delay(10);                           
  }
  
  for (int i=0; i<40; i += 1) {
    
    pos7 = (rndNumber3) + i; //rnd head (65->105)
    pos8 = 80 + i; //80->120 center
    pos10 = (rndNumber5) +i; ////gripper rnd
  
    servoHead.write(pos7);
    servoGripper.write(pos10);
    servoCenter.write(pos6);
    
    delay(10);                           
  }
  //
  
  Serial.println("}");
}
//_________________________________________

void StandBy2Anim()
{
  Serial.println("StandByAnim");
  Serial.println(millis());
  Serial.println("{");
  
  rndNumber2 = random(75,135);
  rndNumber3 = random(35,95);
  rndNumber4 = random(100,110);
  rndNumber5 = random(60,70);
  rndNumber6 = random(1,3);
  
 //__________ 
 for (int i=0; i<40; i += 1) {
    pos3 = 110 - i; //110->70 leg, grip           
    pos4 = 70 + i;  //70->110 leg, grip
    pos5 = (rndNumber2) - i; //rnd head (105->65)
    pos6 = 120 - i; //120->80 center
    pos9 = (rndNumber4) -i; //gripper rnd
   
    servoHead.write(pos5);
   
    if (rndNumber6 == 2){ 
    servoGripper.write(pos9); 
    }
    
    delay(50);                           
  }
  
  for (int i=0; i<40; i += 1) {
    pos3 = 110 - i; //110->70 leg, grip           
    pos4 = 70 + i;  //70->110 leg, grip
    pos7 = (rndNumber3) + i; //rnd head (65->105)
    pos8 = 80 + i; //80->120 center
    pos10 = (rndNumber5) +i; ////gripper rnd
  
    servoHead.write(pos7);
    
    if (rndNumber6 == 2){
    servoGripper.write(pos10);
    }
    
    delay(50);                           
  }
  
 //__________  
  
 if (rndNumber6 == 1){
   
   delay(500);
   A_ttach();
   delay(500);
   
  for (int i=0; i<40; i += 1) {
    pos3 = 110 - i; //110->70 leg, grip           
    pos4 = 70 + i;  //70->110 leg, grip
    pos6 = 120 - i; //120->80 center
    
    
    
    servoCenter.write(pos6);
    servoRight.write(pos4);
    servoLeft.write(pos4);
    
    delay(50);                           
  }
  
  for (int i=0; i<40; i += 1) {
    pos3 = 110 - i; //110->70 leg, grip           
    pos4 = 70 + i;  //70->110 leg, grip
    pos8 = 80 + i; //80->120 center
    
    
    servoCenter.write(pos8);
    servoRight.write(pos3);
    servoLeft.write(pos3);
    
    delay(50);                           
  }
  delay(500);
   De_tachMain();
  delay(500);
 }
 
 
  Serial.println("}");
}
//_________________________________________
//==================================================SOUND SYSTEM=========================================================

// This plays RTTTL (RingTone Text Transfer Language) songs using the
// now built-in tone() command in Wiring and Arduino.
// Written by Brett Hagman
// http://www.roguerobotics.com/

// To play the output on a small speaker (i.e. 8 Ohms or higher), simply use
// a 1K Ohm resistor from the output pin to the speaker, and connect the other
// side of the speaker to ground.

// You can get more RTTTL songs from
// http://code.google.com/p/rogue-code/wiki/ToneLibraryDocumentation

//_________________________________________
#define isdigit(n) (n >= '0' && n <= '9')

void play_rtttl(char *p)
{
  // Absolutely no error checking in here

  byte default_dur = 4;
  byte default_oct = 6;
  int bpm = 63;
  int num;
  long wholenote;
  long duration;
  byte note;
  byte scale;

  // format: d=N,o=N,b=NNN:
  // find the start (skip name, etc)

  while(*p != ':') p++;    // ignore name
  p++;                     // skip ':'

  // get default duration
  if(*p == 'd')
  {
    p++; p++;              // skip "d="
    num = 0;
    while(isdigit(*p))
    {
      num = (num * 10) + (*p++ - '0');
    }
    if(num > 0) default_dur = num;
    p++;                   // skip comma
  }

  Serial.print("ddur: "); Serial.println(default_dur, 10);

  // get default octave
  if(*p == 'o')
  {
    p++; p++;              // skip "o="
    num = *p++ - '0';
    if(num >= 3 && num <=7) default_oct = num;
    p++;                   // skip comma
  }

  Serial.print("doct: "); Serial.println(default_oct, 10);

  // get BPM
  if(*p == 'b')
  {
    p++; p++;              // skip "b="
    num = 0;
    while(isdigit(*p))
    {
      num = (num * 10) + (*p++ - '0');
    }
    bpm = num;
    p++;                   // skip colon
  }

  Serial.print("bpm: "); Serial.println(bpm, 10);

  // BPM usually expresses the number of quarter notes per minute
  wholenote = (60 * 1000L / bpm) * 4;  // this is the time for whole note (in milliseconds)

  Serial.print("wn: "); Serial.println(wholenote, 10);


  // now begin note loop
  while(*p)
  {
    // first, get note duration, if available
    num = 0;
    while(isdigit(*p))
    {
      num = (num * 10) + (*p++ - '0');
    }
    
    if(num) duration = wholenote / num;
    else duration = wholenote / default_dur;  // we will need to check if we are a dotted note after

    // now get the note
    note = 0;

    switch(*p)
    {
      case 'c':
        note = 1;
        break;
      case 'd':
        note = 3;
        break;
      case 'e':
        note = 5;
        break;
      case 'f':
        note = 6;
        break;
      case 'g':
        note = 8;
        break;
      case 'a':
        note = 10;
        break;
      case 'b':
        note = 12;
        break;
      case 'p':
      default:
        note = 0;
    }
    p++;

    // now, get optional '#' sharp
    if(*p == '#')
    {
      note++;
      p++;
    }

    // now, get optional '.' dotted note
    if(*p == '.')
    {
      duration += duration/2;
      p++;
    }
  
    // now, get scale
    if(isdigit(*p))
    {
      scale = *p - '0';
      p++;
    }
    else
    {
      scale = default_oct;
    }

    scale += OCTAVE_OFFSET;

    if(*p == ',')
      p++;       // skip comma for next note (or we may be at the end)

    // now play the note

    if(note)
    {
      Serial.print("Playing: ");
      Serial.print(scale, 10); Serial.print(' ');
      Serial.print(note, 10); Serial.print(" (");
      Serial.print(notes[(scale - 4) * 12 + note], 10);
      Serial.print(") ");
      Serial.println(duration, 10);
      tone(tonePin, notes[(scale - 4) * 12 + note]);
      delay(duration);
      noTone(tonePin);
    }
    else
    {
      Serial.print("Pausing: ");
      Serial.println(duration, 10);
      delay(duration);
    }
  }
}
//=================================================SERVOS ATTACH & DETACH==========================================================
//_________________________________________
void A_ttach()
{
  Serial.println("Attach");
  Serial.println(millis());
  Serial.println("{");
  
  servoCenter.attach(11);  
  servoRight.attach(3);  
  servoLeft.attach(10);
  servoHead.attach(9);  
  servoGripper.attach(5); 
  
  Serial.println("}");
}  


void De_tach()
{
  Serial.println("Detach");
  Serial.println(millis());
  Serial.println("{");
  
  servoCenter.detach();  
  servoRight.detach();  
  servoLeft.detach();
  servoHead.detach();  
  servoGripper.detach(); 
  
  Serial.println("}");
}  

void De_tachMain()
{
  Serial.println("DetachMain");
  Serial.println(millis());
  Serial.println("{");
  
  servoCenter.detach();  
  servoRight.detach();  
  servoLeft.detach();
  
  Serial.println("}");
}  


//=======================================================CENTER SERVOS===================================================
//_________________________________________
 void Center()
 {
  Serial.println("Center");
  Serial.println(millis());
  Serial.println("{");
  
  servoCenter.write(100);
  servoRight.write(90);
  servoLeft.write(90);
  servoHead.write(85);
  servoGripper.write(90);
  
   Serial.println("}");
 }
 //_________________________________________
 void Center2()
 {
  Serial.println("Center2");
  Serial.println(millis());
  Serial.println("{");
  
  servoCenter.write(100);
  servoRight.write(90);
  servoLeft.write(90);
  servoGripper.write(90);
  
   Serial.println("}");
 }