#include <Servo.h>
//Servos pegados al cuerpo (hombro)
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
//servos secundarios (codo)
Servo servo5;
Servo servo6;
Servo servo7;
Servo servo8;

int pos1, pos2, pos3, pos4;
int tiemp; // si quiero que usen un delay siempre de X valor pongo tiempo(tiemp);
int sube,baja; // Para poner directamente valores configurados a los servos


//--------------------------------------------------------------------------------------------------------------

void setup() {  
    // DEFINIMOS EL PIN DE LA PLACA
    //el servo 1 esta en el pin 2 (todos estan en el pin siguiente)
    //HOMBROS 
    servo1.attach(2);
    servo2.attach(3);
    servo3.attach(4);
    servo4.attach(5);
    //CODOS
    servo5.attach(6);
    servo6.attach(7);
    servo7.attach(8);
    servo8.attach(9);

    //INICIALIZAMOS
   /* centrar(); //llamamos a la funcion centrar para que nos ponga todos los servos a 90º (modo inicio)
    delay(2000); //ponemos un delay para que tarde un poquito en inicializar (delay de 1000 es 1 segundo)
*/
/*
    prueba1();
    delay(1000);
    centrar(); //llamamos a la funcion centrar para que nos ponga todos los servos a 90º (modo inicio)
    delay(1000); //ponemos un delay para que tarde un poquito en inicializar (delay de 1000 es 1 segundo)
*/

/*
    prueba2();
    delay(1000);
    centrar(); //llamamos a la funcion centrar para que nos ponga todos los servos a 90º (modo inicio)
    delay(1000); //ponemos un delay para que tarde un poquito en inicializar (delay de 1000 es 1 segundo)
*/

/*
    prueba3();
    delay(1000);
    centrar(); //llamamos a la funcion centrar para que nos ponga todos los servos a 90º (modo inicio)
    delay(1000); //ponemos un delay para que tarde un poquito en inicializar (delay de 1000 es 1 segundo)
*/
/*
 prueba4();
    delay(1000);
    centrar(); //llamamos a la funcion centrar para que nos ponga todos los servos a 90º (modo inicio)
    delay(1000); //ponemos un delay para que tarde un poquito en inicializar (delay de 1000 es 1 segundo)
*/

}
//--------------------------------------------------------------------------------------------------------------


void loop() {  //MEGABUCLE
   pos1 = random(0, 180); // Generate random position between 0 and 180 degrees for servo1
  pos2 = random(0, 180); // Generate random position between 0 and 180 degrees for servo2
  pos3 = random(0, 180); // Generate random position between 0 and 180 degrees for servo3
  pos4 = random(0, 180); // Generate random position between 0 and 180 degrees for servo4

  int current_pos1 = servo1.read();
  int current_pos2 = servo2.read();
  int current_pos3 = servo3.read();
  int current_pos4 = servo4.read();

  for (int i = 0; i < abs(pos1 - current_pos1); i++) {
    servo1.write(current_pos1 + i * (pos1 - current_pos1) / abs(pos1 - current_pos1));
    servo2.write(current_pos2 + i * (pos2 - current_pos2) / abs(pos2 - current_pos2));
    servo3.write(current_pos3 + i * (pos3 - current_pos3) / abs(pos3 - current_pos3));
    servo4.write(current_pos4 + i * (pos4 - current_pos4) / abs(pos4 - current_pos4));
    delay(30);
  }
}
//--------------------------------------------------------------------------------------------------------------



void centrar () {
    // INICIALIZAMOS A 90º
    // hay que poner todos a 90 grados y colocarlos en la mitad fisicamente
    //HOMBROS   
    servo1.write(90);
    servo2.write(90);
    servo3.write(90);
    servo4.write(90);
    //CODOS
    servo5.write(160);
    servo6.write(160);
    servo7.write(160);
    servo8.write(160);
}
//--------------------------------------------------------------------------------------------------------------


void prueba1(){


//PATA 1
    tiemp=30;
    
    for(int i=90;i>20;i--) //movimiento pata delantera hacia abajo
    {
      servo1.write(i);   
      tiempo(tiemp);//puedo usar esto o un delay
    }


    for(int i=20;i<160;i++) //movimiento pata delantera hacia abajo
    {
      servo1.write(i);             
      tiempo(tiemp);//puedo usar esto o un delay
    }

    for(int i=160;i>90;i--) //movimiento pata delantera hacia abajo
    {
      servo1.write(i);   
  
      tiempo(tiemp);//puedo usar esto o un delay
    }

//servo2
    for(int i=90;i>20;i--) //movimiento pata delantera hacia abajo
    {
      servo2.write(i);   
      tiempo(tiemp);//puedo usar esto o un delay
    }


    for(int i=20;i<160;i++) //movimiento pata delantera hacia abajo
    {
      servo2.write(i);             
      tiempo(tiemp);//puedo usar esto o un delay
    }

    for(int i=160;i>90;i--) //movimiento pata delantera hacia abajo
    {
      servo2.write(i);   
  
      tiempo(tiemp);//puedo usar esto o un delay
    }

//servo3
    for(int i=90;i>20;i--) //movimiento pata delantera hacia abajo
    {
      servo3.write(i);   
      tiempo(tiemp);//puedo usar esto o un delay
    }


    for(int i=20;i<160;i++) //movimiento pata delantera hacia abajo
    {
      servo3.write(i);             
      tiempo(tiemp);//puedo usar esto o un delay
    }

    for(int i=160;i>90;i--) //movimiento pata delantera hacia abajo
    {
      servo3.write(i);   
      tiempo(tiemp);//puedo usar esto o un delay
    }

   //servo4
    for(int i=90;i>20;i--) //movimiento pata delantera hacia abajo
    {
      servo4.write(i);   
      tiempo(tiemp);//puedo usar esto o un delay
    }

    for(int i=20;i<160;i++) //movimiento pata delantera hacia abajo
    {
      servo4.write(i);             
      tiempo(tiemp);//puedo usar esto o un delay
    }

    for(int i=160;i>90;i--) //movimiento pata delantera hacia abajo
    {
      servo4.write(i);   
      tiempo(tiemp);//puedo usar esto o un delay
    } 
}

//--------------------------------------------------------------------------------------------------------------
void tiempo(int t) {
  delay(t);
}



void prueba2(){



    tiemp=30;
    for (int pos = 0; pos <= 180; pos += 1) {  // Gradually move servo1 from 0 to 180 degrees
      servo1.write(pos);                      // in 1 degree increments
      servo2.write(180 - pos);                // Gradually move servo2 from 180 to 0 degrees
      servo3.write(pos/2);                    // Gradually move servo3 from 0 to 90 degrees
      servo4.write(90 - pos/2);               // Gradually move servo4 from 90 to 0 degrees
      delay(30);                              // delay of 15 milliseconds
    }
}
 
void prueba3(){


  int pos1 = random(0, 180); // Generate random position between 0 and 180 for servo1
  int pos2 = random(0, 90);  // Generate random position between 0 and 90 for servo2
  int pos3 = random(90, 180); // Generate random position between 90 and 180 for servo3
  int pos4 = random(0, 45);  // Generate random position between 0 and 45 for servo4
  
  servo1.write(pos1); // Move servo1 to the random position
  servo2.write(pos2); // Move servo2 to the random position
  servo3.write(pos3); // Move servo3 to the random position
  servo4.write(pos4); // Move servo4 to the random position
  
  delay(1000); // delay of 1 second                          // delay of 15 milliseconds
}

void prueba4(){

   pos1 = random(1000, 2000); // Generate random position between 1000 and 2000 microseconds for servo1
   pos2 = random(1500, 2500);  // Generate random position between 1500 and 2500 microseconds for servo2
   pos3 = random(2000, 3000); // Generate random position between 2000 and 3000 microseconds for servo3
   pos4 = random(2500, 3500);  // Generate random position between 2500 and 3500 microseconds for servo4
  
  int current_pos1 = servo1.readMicroseconds();
  int current_pos2 = servo2.readMicroseconds();
  int current_pos3 = servo3.readMicroseconds();
  int current_pos4 = servo4.readMicroseconds();

  for (int i = 0; i < abs(pos1 - current_pos1); i++) {
    servo1.writeMicroseconds(current_pos1 + i * (pos1 - current_pos1) / abs(pos1 - current_pos1));
    servo2.writeMicroseconds(current_pos2 + i * (pos2 - current_pos2) / abs(pos2 - current_pos2));
    servo3.writeMicroseconds(current_pos3 + i * (pos3 - current_pos3) / abs(pos3 - current_pos3));
    servo4.writeMicroseconds(current_pos4 + i * (pos4 - current_pos4) / abs(pos4 - current_pos4));
    delay(2);
  }
}