// JOHNNY5  16/03/16 by Eugenio Cosolo  
// routine animazione testa e occhi robot Johnny 5 x Arduino Uno tramite mini servocomandi
// www.missilistica.it 
// eugenio@missilistica.it

#include <Servo.h> // include la Libreria Servo.h
Servo servo0;  Servo servo1;  Servo servo2; Servo servo3; Servo servo4; Servo servo5; Servo servo6; Servo servo7;
int pos = 0;  int vtasto = 0; int tm =30; int lbianco = 9 ; int lrosso  = 8 ; int lverde  = 10;
int o1=0 ; int o2=0 ; int o3=-10 ; int o4=3 ; int o5=15 ; int o6=0 ; //offset posizione servi

void setup() 
{    
  servo7.attach(0);   // occhio sinistro          // connessioni ai servocomandi
  servo0.attach(1);   // occhio destro		  // filo nero=massa  filo rosso=5V  filo giallo=segnale di controllo
  servo1.attach(2);   // sopracciglia destra
  servo2.attach(3);   // palpebra destra
  servo3.attach(4);   // sopracciglia sinistra
  servo4.attach(5);   // palpebra sinistra
  servo5.attach(6);   // palpebra inferiore destra
  servo6.attach(7);   // palpebra inferiore sinistra
  pinMode(lbianco,OUTPUT);   // led bianchi
  pinMode(lrosso, OUTPUT);   // led led rossi
  pinMode(lverde, OUTPUT);   // led verdi
  delay (500);
} 
 
void loop() {
    
   neutro();   delay (5000);      // normale
   farfa();   delay (3000);       // sfarfalla
   lampeggio();delay (50);        // lampeggio led bianchi
   alverde();  delay (50);        // accende led verdi
   tasto3();   delay (2000);      // sbatte le ciglia sopra
   slverde();  delay (50);        // spegne led verdi
   tasto2();   delay (1500);      // occhi chiusi
   neutro();   delay (500);       // lampeggio led colorati
   tasto2();   delay (1000);      // occhi chiusi
   alverde();  delay (50);        // accende led verdi
   tasto1();   delay (3000);      // sorpreso
   neutro();   delay (500);       // normale
   farfa();   delay (3000);       // sfarfalla
   slverde();  delay (50);        // spegne led verdi
   albianco(); delay (50);        // accende led bianchi
   ammicca();   delay (1000);     // ammicca
   neutro();   delay (500);       // normale
   tasto3();   delay (1000);      // sbatte le ciglia sopra
   slbianco(); delay (50);        // spegne led bianchi
   alverde();  delay (50);        // accende led verdi  
   tasto1();   delay (1000);      // sorpreso
   neutro();   delay (3000);      // normale
   farfa();   delay (3000);       // sfarfalla
   slverde();  delay (50);        // spegne led verdi
   lampcolor();  delay (100);     // lampeggio led colorati
   slverde();  delay (50);        // spegne led verdi
   slbianco(); delay (50);        // spegne led bianchi
   tasto5();   delay (3000);      // arrabbiato
   slrosso();  delay (50);        // spegne led rossi
   tasto2();   delay (4000);      // occhi chiusi
}


// sequenze programmate
void albianco() {digitalWrite(lbianco,HIGH);  } // accende led bianchi
void slbianco() {digitalWrite(lbianco, LOW);  } // spegne led bianchi
void alrosso()  {digitalWrite(lrosso, HIGH);  } // accende led rossi
void slrosso()  {digitalWrite(lrosso,  LOW);  } // spegne led rossi
void alverde()  {digitalWrite(lverde, HIGH);  } // accende led verdi
void slverde()  {digitalWrite(lverde,  LOW);  } // spegne led verdi
void tasto7() { servo0.write(70);  delay(tm); servo7.write(100); delay(tm);  }  // abbassa occhi
void tasto8() { servo0.write(100); delay(tm); servo7.write(70); delay(tm);   }  // alza occhi
void tasto9() { servo0.write(85); delay(tm); servo7.write(85); delay(tm);    }  // occhi in centro

void farfa() {  // sfarfalla ciglia sopra
   servo1.write(150+o1);  delay(tm);
   servo2.write(135+o2); delay(tm);
   servo3.write(70+o3);  delay(tm);   
   servo4.write(80+o4);  delay(tm);  
   servo5.write(70+o5);  delay(tm);
   servo6.write(160+o6); delay(tm);
   servo2.write(65+o2);   delay(tm);
   servo4.write(150+o4);  delay(tm); delay(150);
   servo2.write(135+o2);  delay(tm);
   servo4.write(80+o4);   delay(tm); delay(150);
   servo2.write(65+o2);   delay(tm);    
   servo4.write(150+o4);  delay(tm);  delay(150);
   servo2.write(135+o2);  delay(tm);
   servo4.write(80+o4);   delay(tm); delay(150);
   servo2.write(65+o2);   delay(tm);
   servo4.write(150+o4);  delay(tm); delay(150);
   servo2.write(135+o2);  delay(tm);
   servo4.write(80+o4);   delay(tm); delay(150);
   servo2.write(65+o2);   delay(tm);    
   servo4.write(150+o4);  delay(tm);  delay(150);
   servo2.write(135+o2);  delay(tm);
   servo4.write(80+o4);   delay(tm); delay(150);   
 } 

void neutro() {  // posizione neutra
   servo1.write(150+o1);  delay(tm);
   servo2.write(135+o2); delay(tm);
   servo3.write(70+o3);  delay(tm);   
   servo4.write(80+o4);  delay(tm);  
   servo5.write(70+o5);  delay(tm);
   servo6.write(160+o6); delay(tm); } 
  
void tasto1() {  // sorpreso   OK
   servo1.write(170+o1); delay(tm);   
   servo2.write(120+o2); delay(tm);   
   servo3.write(40+o3);  delay(tm); 
   servo4.write(90+o4);  delay(tm);   
   servo5.write(60+o5);  delay(tm);   
   servo6.write(170+o6); delay(tm); 
  }
   
void tasto2() {   //chiude occhi
   servo5.write(185+o5); delay(tm);   
   servo6.write(45+o6);  delay(tm); 
   servo2.write(135+o2); delay(tm); 
   servo4.write(80+o4);  delay(tm);     
   servo3.write(160+o3); delay(tm); 
   servo1.write(55+o1);  delay(tm);  }

void tasto3() { // sbatte ciglia sopra
   servo6.write(130+o6); delay(tm);
   servo2.write(130+o2); delay(tm);   
   servo4.write(85+o4);  delay(tm);   
   servo5.write(80+o5); delay(tm);
   servo1.write(170+o1); delay(tm);   
   servo3.write(50+o3);  delay(tm);   delay (300);
   servo1.write(60+o1);  delay(tm);   
   servo3.write(145+o3); delay(tm);   delay (300);
   servo1.write(170+o1); delay(tm);   
   servo3.write(50+o3);  delay(tm);   delay (300); 
   servo1.write(50+o1);  delay(tm);   
   servo3.write(145+o3); delay(tm);   delay (300); 
   servo1.write(170+o1); delay(tm);   
   servo3.write(50+o3);  delay(tm);   delay (300);  }
 
 void tasto4()  {  // sbatte tutte le ciglia 
   servo1.write(60+o1);  delay(tm); 
   servo3.write(135+o3); delay(tm);   
   servo2.write(130+o2); delay(tm);   
   servo4.write(85+o4);  delay(tm);   
   servo5.write(180+o5); delay(tm);   
   servo6.write(70+o6);  delay(tm);   delay (30);
   servo1.write(180+o1); delay(tm);   
   servo2.write(125+o2); delay(tm);   
   servo3.write(30+o3);  delay(tm); 
   servo4.write(85+o4);  delay(tm);   
   servo5.write(70+o5);  delay(tm);   
   servo6.write(180+o6); delay(tm);   delay (30);
   servo1.write(60+o1);  delay(tm);   
   servo2.write(125+o2); delay(tm);   
   servo3.write(135+o3); delay(tm); 
   servo4.write(85+o4);  delay(tm);   
   servo5.write(180+o5); delay(tm);   
   servo6.write(70+o6);  delay(tm);   delay (30);
   servo1.write(180+o1); delay(tm);   
   servo2.write(125+o2); delay(tm);   
   servo3.write(30+o3);  delay(tm); 
   servo4.write(85+o4);  delay(tm);   
   servo5.write(70+o5);  delay(tm);   
   servo6.write(180+o6); delay(tm);   delay (30);
   servo1.write(60+o1);  delay(tm);   
   servo2.write(125+o2); delay(tm);   
   servo3.write(135+o3); delay(tm); 
   servo4.write(85+o4);  delay(tm);   
   servo5.write(180+o5); delay(tm);   
   servo6.write(70+o6);  delay(tm);   delay (30);
   servo1.write(180+o1); delay(tm);   
   servo2.write(125+o2); delay(tm);   
   servo3.write(30+o3);  delay(tm); 
   servo4.write(85+o4);  delay(tm);   
   servo5.write(70+o5);  delay(tm);   
   servo6.write(180+o6); delay(tm);   delay (30);   }
   
   
 
 void tasto5() {  //arrabbiato
   servo1.write(150+o1);  delay(tm);   
   servo2.write(120+o2); delay(tm);  
   servo3.write(50+o3);  delay(tm); 
   servo4.write(90+o4);  delay(tm);   
   servo5.write(60+o5);  delay(tm);   
   servo6.write(170+o6); delay(tm); } 
  

 void ammicca() {  //ammicca destro
   servo6.write(130+o6); delay(tm);
   servo3.write(70+o3);  delay(tm);   
   servo4.write(75+o4);  delay(tm);   
   servo2.write(135+o2); delay(tm);
   servo1.write(60+o1);   delay(tm);   
   servo5.write(180+o5);  delay(200); 
   servo1.write(150+o1);  delay(tm);   
   servo5.write(90+o5);   delay(200);
   servo1.write(60+o1);   delay(tm);   
   servo5.write(180+o5);  delay(200); 
   servo1.write(150+o1);  delay(tm);   
   servo5.write(90+o5);   delay(200);
   servo1.write(60+o1);   delay(tm);   
   servo5.write(180+o5);  delay(200); 
   servo1.write(150+o1);  delay(tm);   
   servo5.write(90+o5);   delay(200); }


  void lampeggio() {            // lampeggio led bianchi
  albianco();delay(50);slbianco();delay(40);albianco();delay(50);slbianco();delay(40);
  albianco();delay(50);slbianco();delay(40);albianco();delay(50);slbianco();delay(40);
  albianco();delay(50);slbianco();delay(40);albianco();delay(50);slbianco();delay(40);
  albianco();delay(50);slbianco();delay(40);albianco();delay(50);slbianco();delay(40); 
delay(10);  } 
  
   void lampcolor() {            // lampeggio led colorati
  albianco();delay(80);slbianco();delay(50);  alverde();delay(80);slverde();delay(50);  alrosso();delay(80);slrosso();delay(50);
  albianco();delay(80);slbianco();delay(50);  alverde();delay(80);slverde();delay(50);  alrosso();delay(80);slrosso();delay(50);
  albianco();delay(80);slbianco();delay(50);  alverde();delay(80);slverde();delay(50);  alrosso();delay(80);slrosso();delay(50);
  albianco();delay(80);slbianco();delay(50);  alverde();delay(80);slverde();delay(50);  alrosso();delay(80);slrosso();delay(50);
  albianco();delay(80);slbianco();delay(50);  alverde();delay(80);slverde();delay(50);  alrosso();delay(80);slrosso();delay(50);
  albianco();delay(80);slbianco();delay(50);  alverde();delay(80);slverde();delay(50);  alrosso();delay(80);slrosso();delay(50); delay(10);  }