ARAÑA CUADRUPEDA (ARDUINO) * DitecnoMakers

#include <Servo.h>  
// Incluye la libreria Servo
Servo hombro1;
Servo pata1;
Servo hombro2;
Servo pata2;
Servo hombro3;
Servo pata3;
Servo hombro4;
Servo pata4;                   

int led1= 6; 
int led2= 7;        
int estado = 'c';         // inicia detenido

int pecho = 2;            // define el pin 2 como (pecho) para el Ultrasonido
int ptrig = 3;            // define el pin 3 como (ptrig) para el Ultrasonido
int duracion, distancia;  // para Calcular distacia

void setup()  { 
  Serial.begin(9600);    // inicia el puerto serial para comunicacion con el Bluetooth
  pinMode(pecho, INPUT);   // define el pin 2 como entrada (pecho) 
  pinMode(ptrig,OUTPUT);   // define el pin 3 como salida  (ptrig) 

  pinMode (led1, OUTPUT);
  pinMode (led2, OUTPUT);
  
  hombro1.attach(A0);//hombro 1
  pata1.attach(A1);//pata1
  
  hombro2.attach(A2);//hombro2
  pata2.attach(A3);//pata2
  
  hombro3.attach(A4);//hombro 3
  pata3.attach(A5);//pata3
  
  hombro4.attach(4);//hombro4
  pata4.attach(5);//pata4
  
  

  
} 

void loop()  { 

  if(Serial.available()>0){        // lee el bluetooth y almacena en estado
    estado = Serial.read();
    Serial.print("estado");
  }
 
  

/////////////////////////////////////////////////
//enciende leds
if (estado== 'h'){
digitalWrite(led1,LOW); 
digitalWrite(led2,LOW); 
}


//apaga leds
if (estado== 'i'){
digitalWrite(led1,HIGH);
digitalWrite(led2,HIGH); 
}

////////////////////////////////////////////////
  
  if(estado=='a'){           // Boton desplazar al Frente
  pata1.write(20);// pata1 20 grados
  delay (80);
  hombro1.write(150);//hombro 1150 grados
  delay (80);
  pata1.write(90);// pata1 --90 grados
  delay (80);

  pata3.write(20); //pata3 --20 grados
   delay (80);
  hombro3.write(40);//hombro3-- 40 grados 
   delay (80);
  pata3.write(90);// pata3-- 90 grados
  delay (80);
  
  hombro1.write(90);// hombros 1,3  regresan a 90 grados
  hombro3.write(90);
  delay (80);  
//////////////////////
  pata4.write(20);//pata4-- 20 grados
   delay (80);
  hombro4.write(40); //hombro4--40 grados
   delay (80);
  pata4.write(90);//pata4--90 grados
  delay (80);

  pata2.write(20);//pata2--20 grados
   delay (80);
  hombro2.write(150);//hombro2-- 150 grados
   delay (80);
  pata2.write(90);//pata2--90 grados
  delay (80);
  
  hombro2.write(90);//hombros 2,4 regresan a 90 grados
  hombro4.write(90);
  delay (80);
//////////////////////

  
  }  

if(estado=='b'){           // Boton desplazar a la iquierda
  pata1.write(20);
   delay (80);
  hombro1.write(150);
   delay (80);
  pata1.write(90);
  delay (80);

  pata2.write(20);
   delay (80);
  hombro2.write(150);
   delay (80);
  pata2.write(90);
  delay (80);
  
 
  pata3.write(20);
   delay (80);
  hombro3.write(150);
   delay (80);
  pata3.write(90);
  delay (80);

  pata4.write(20);
   delay (80);
  hombro4.write(150);
   delay (80);
  pata4.write(90);
  delay (80);
  
  hombro1.write(90);
  hombro2.write(90);
  hombro3.write(90);
  hombro4.write(90);
  delay (80);
//////////////////////
  
 }  

if(estado=='d'){           // Boton desplazar a la derecha
  pata1.write(20);
   delay (80);
  hombro1.write(40);
   delay (80);
  pata1.write(90);
  delay (80);

  pata4.write(20);
   delay (80);
  hombro4.write(40);
   delay (80);
  pata4.write(90);
  delay (80);
  
 
  pata3.write(20);
   delay (80);
  hombro3.write(40);
   delay (80);
  pata3.write(90);
  delay (80);

  pata2.write(20);
   delay (80);
  hombro2.write(40);
   delay (80);
  pata2.write(90);
  delay (80);
  
  hombro1.write(90);
  hombro2.write(90);
  hombro3.write(90);
  hombro4.write(90);
  delay (80);
 
 }  
 
//////////////////////////////////////////////////////////////////
 
if(estado=='e'){           // Boton desplazar atras
  pata1.write(20);
   delay (80);
  hombro1.write(40);
   delay (80);
  pata1.write(90);
  delay (80);

  pata3.write(20);
   delay (80);
  hombro3.write(150);
   delay (80);
  pata3.write(90);
  delay (80);
  
  hombro1.write(90);
  hombro3.write(90);
  delay (80);  
//////////////////////
  pata2.write(20);
   delay (80);
  hombro2.write(40);
   delay (80);
  pata2.write(90);
  delay (80);

  pata4.write(20);
   delay (80);
  hombro4.write(150);
   delay (80);
  pata4.write(90);
  delay (80);
  
  hombro2.write(90);
  hombro4.write(90);
  delay (80);
//////////////////////
   }     
// todos los servos a 90 grados
  if(estado=='c'){
  hombro1.write (90);
  pata1.write (90); 
    
  hombro2.write (90);
  pata2.write (90); 
  
  hombro3.write (90);
  pata3.write (90); 
   
  hombro4.write (90);
  pata4.write (90); 
  delay(80);
  }

//////////////////////////////////////////////////////////////////////////////////////////////////
if (estado =='f'){               // Boton ON,  se mueve sensando distancia 
  
     digitalWrite(ptrig, HIGH);   // genera el pulso de trigger por 10us
     delay(0.01);
     digitalWrite(ptrig, LOW);
     
     duracion = pulseIn(pecho, HIGH);          // Lee el tiempo del Echo
     distancia = (duracion/2) / 29;            // calcula la distancia en centimetros
     delay(10); 
     
     if (distancia <= 20 && distancia >=4){    // si la distancia es menor de 15cm
                        


 digitalWrite(led1,HIGH);
 digitalWrite(led2,HIGH);

        
  hombro1.write (90);     // Parar los motores por 200 mili segundos
  delay(80);
  pata1.write (90); 
    
  hombro2.write (90);
  delay(80);
  pata2.write (90); 
  delay(80);
  
  hombro3.write (90);
  delay(80);
  pata3.write (90); 
   delay(80);
   
  hombro4.write (90);
  delay(80);
  pata4.write (90); 
  delay(80);                     

// giro a la derecha
pata1.write(20);
   delay (80);
  hombro1.write(40);
   delay (80);
  pata1.write(90);
  delay (80);

  pata4.write(20);
   delay (80);
  hombro4.write(40);
   delay (80);
  pata4.write(90);
  delay (80);
  
 
  pata3.write(20);
   delay (80);
  hombro3.write(40);
   delay (80);
  pata3.write(90);
  delay (80);

  pata2.write(20);
   delay (80);
  hombro2.write(40);
   delay (200);
  pata2.write(90);
  delay (80);
  
  hombro1.write(90);
  hombro2.write(90);
  hombro3.write(90);
  hombro4.write(90);
  digitalWrite(led1,LOW);
  digitalWrite(led2,LOW);
  delay (80);

      
        
       
     }
     else{                             // Si no hay obstaculos se desplaza al frente  
      
  pata1.write(20);
  delay (80);
  hombro1.write(150);
  delay (80);
  pata1.write(90);
  delay (80);

  pata3.write(20);
   delay (80);
  hombro3.write(40);
   delay (80);
  pata3.write(90);
  delay (80);
  
  hombro1.write(90);
  hombro3.write(90);
  delay (80);  
//////////////////////
  pata4.write(20);
   delay (80);
  hombro4.write(40);
   delay (80);
  pata4.write(90);
  delay (80);

  pata2.write(20);
   delay (80);
  hombro2.write(150);
   delay (80);
  pata2.write(90);
  delay (200);
  
  hombro2.write(90);
  hombro4.write(90);
  delay (80);
     }
}

/////////////////////////////////////////////////////////////////////////////////////////
//saludo
  if(estado=='g'){
  hombro1.write (90);
  pata1.write (90); 
    
  hombro2.write (90);
  pata2.write (90); 
  
  hombro3.write (90);
  pata3.write (90); 
   
  hombro4.write (90);
  pata4.write (90); 
  delay(500);
  
  
  pata1.write (20);     
  
  pata2.write (20);   
  
  pata3.write (20);    

  pata4.write (20); 
  delay(500);

  pata1.write (90);     
  
  pata2.write (90);   
  
  pata3.write (90);    

  pata4.write (90); 
  delay(500);

  


   pata1.write (20); 
   pata2.write (100); 
   pata3.write (20); 
   pata4.write (20);
   delay (300);
   hombro3.write (170);
   delay (1000);
   hombro3.write (30);
   delay (1000);
   hombro3.write (170);
   delay (1000);
   hombro3.write (30);
   delay (1000);
   hombro3.write (170);
   delay (1000);
   hombro3.write (30);
   delay (1000);
  
  }
}