// JOHNNY5  16/03/16 by Eugenio Cosolo  
// routine animazione robot Johnny 5 x Arduino Uno e scheda controllo motori esterna
// www.missilistica.it 
// eugenio@missilistica.it


int P1 = 0; int P2 = 0; int P3 = 0; int P4 = 0; int bz=0;   // buzzer per procedura test, 0 per spento
// connessioni in uscita al driver motori:
// pin 1 : solleva tronco inferiore
// pin 2 : abbassa tronco inferiore
// pin 3 : solleva tronco superiore
// pin 4 : abbassa tronco speriore
// pin 5 : solleva testa

// connessioni in entrata dal ricevitore radiocomando
// A1 : tasto B
// A2 : tasto D
// A3 : tasto C
// A4 : tasto A

void setup() {
//Serial.begin(9600); /// non usare se viene utilizzato il pin 1
pinMode(1, OUTPUT); pinMode(2, OUTPUT);pinMode(3, OUTPUT);pinMode(4, OUTPUT);pinMode(5, OUTPUT);pinMode(6, OUTPUT);
pinMode(7, OUTPUT);pinMode(8, OUTPUT);pinMode(9, OUTPUT);pinMode(10, OUTPUT);pinMode(11, OUTPUT);pinMode(12, OUTPUT);
pinMode(A1, INPUT); pinMode(A2, INPUT); pinMode(A3, INPUT); pinMode(A4, INPUT); 
}

void loop() {
digitalWrite(1, LOW);digitalWrite(2, LOW);digitalWrite(3, LOW);digitalWrite(4, LOW);digitalWrite(5, LOW);digitalWrite(6, LOW);
digitalWrite(7, LOW);digitalWrite(8, LOW);digitalWrite(9, LOW);digitalWrite(10, LOW);digitalWrite(11, LOW);digitalWrite(12, LOW);

// controllo ingressi dal ricevitore a 4 canali 
// premendo il tasto 1 (A4) inizia la sequenza di movimenti programmata, poi rimane in stand by
P1 = digitalRead(A1); P2 = digitalRead(A2);P3 = digitalRead(A3);P4 = digitalRead(A4);
if (digitalRead(A4) == HIGH) {seq1(); } // tasto A   (mix movimenti)
if (digitalRead(A1) == HIGH) {seq4(); } // tasto B   (alza tutto e poi abbassa tutto)
if (digitalRead(A3) == HIGH) {seq3(); } // tasto C
if (digitalRead(A2) == HIGH) {seq2(); } // tasto D   ()
}

void seq1()  {     // tasto A
    abtrinf();     // abbassa tronco inferiore
    abtrsup();     // abbassa tronco superiore
    altesta();     // alza testa
    gtdx();        // gira testa destra
    abtesta();     // abbassa testa
    gtsx();        // gira testa sinistra
    altrinf();     // alza tronco inferiore
    altrsup();     // alza tronco superiore
    altrinf();     // alza tronco inferiore
    altrsup();     // alza tronco superiore
    altesta();     // alza testa
    delay(500);
    abtesta();     // abbassa testa  
    gtdx();        // gira testa destra
    abtrinf();     // abbassa tronco inferiore
    abtrsup();     // abbassa tronco superiore
    gtdx();        // gira testa destra
    altesta();     // alza testa
    delay(1000);
    abtesta();     // abbassa testa  
    altrsup();     // alza tronco superiore
    abtrinf();     // abbassa tronco inferiore
    gtdx();        // gira testa destra
    buzzer();      // buzzer
   }
     

void seq2() {   }  //tasto B   ( accende laser, al momento disabilitato )
  
void seq3() {   }   //tasto C   ( al momento non usato    )

void seq4()  {      //tasto D   (alza e poi abbassa tutto)
   {digitalWrite(1, HIGH); }  delay(5000);  {digitalWrite(1, LOW); } delay(10);             // ALZA TRONCO INFERIORE
   {digitalWrite(3, HIGH); }  delay(7000); {digitalWrite(3, LOW); } delay(5000);            // ALZA TRONCO SUPERIORE
   {digitalWrite(4, HIGH); }  delay(7000); {digitalWrite(4, LOW); } delay(10);              // ABBASSA TRONCO SUPERIORE
   {digitalWrite(2, HIGH); }  delay(7000); {digitalWrite(2, LOW); } delay(10);   }          // ABBASSA TRONCO INFERIORE 
                  
void buzzer()  {digitalWrite(13, HIGH); delay(600); digitalWrite(13, LOW); delay(20);}      // attiva buzzer per 600 msec

void gtdx() { for(int z=0;z<25;z++) { {digitalWrite(10, HIGH); } delay(15);{digitalWrite(10, LOW); } delay(60); }   delay(100); } // GIRA TESTA DX   

void gtsx() { for(int z=0;z<25;z++) { {digitalWrite(9, HIGH); } delay(15);{digitalWrite(9, LOW); } delay(60); }  delay(100); }    // GIRA TESTA SX

void altesta() {digitalWrite(5, HIGH); }                          // ALZA TESTA 

void abtesta() {digitalWrite(5, LOW); }                           // ABBASSA TESTA

void altrinf() {digitalWrite(1, HIGH);   delay(2000); {digitalWrite(2, LOW); } delay(10);}         // ALZA TRONCO INFERIORE x 2 sec

void abtrinf() {digitalWrite(2, HIGH);   delay(2000); {digitalWrite(2, LOW); } delay(10);}         // ABBASSA TRONCO INFERIORE x 2 sec

void altrsup() {digitalWrite(3, HIGH);   delay(2000); {digitalWrite(4, LOW); } delay(10);}         // ALZA TRONCO SUPERIORE x 2 sec

void abtrsup() {digitalWrite(4, HIGH);   delay(2000); {digitalWrite(4, LOW); } delay(10);}         // ABBASSA TRONCO SUPERIORE x 2 sec