#include <Servo.h>

//Declaración de servos
Servo cadera_d;
Servo rodilla_d;
Servo tobillo_d;
Servo cadera_i;
Servo rodilla_i;
Servo tobillo_i;

int pos_cd = 68;
int pos_rd = 160;
int pos_td = 95;
int pos_ci = 135;
int pos_ri = 52;
int pos_ti = 140;

void setup()
{
  //Pines donde se conecta cada servo
  cadera_d.attach(9);
  rodilla_d.attach(10);
  tobillo_d.attach(11);
  cadera_i.attach(3);
  rodilla_i.attach(5);
  tobillo_i.attach(6);
}

void loop()
{
  for(int i=0;i<=25;i++)
  {
    delay(15);
    pos_cd ++;
    pos_ci ++;
    pos_rd --;
    pos_ri --;
    pos_td ++;
    pos_ti ++;
    cadera_d.write(pos_cd);
    cadera_i.write(pos_ci);
    rodilla_d.write(pos_rd);
    rodilla_i.write(pos_ri);
    tobillo_d.write(pos_td);
    tobillo_i.write(pos_ti);
  }
  for(int i=0;i<=25;i++)
  {
    delay(15);
    pos_cd --;
    pos_ci --;
    pos_rd ++;
    pos_ri ++;
    pos_td --;
    pos_ti --;
    cadera_d.write(pos_cd);
    cadera_i.write(pos_ci);
    rodilla_d.write(pos_rd);
    rodilla_i.write(pos_ri);
    tobillo_d.write(pos_td);
    tobillo_i.write(pos_ti);
  }
  for(int i=0;i<=25;i++)
  {
    delay(15);
    pos_cd --;
    pos_ci --;
    pos_rd ++;
    pos_ri ++;
    pos_td --;
    pos_ti --;
    cadera_d.write(pos_cd);
    cadera_i.write(pos_ci);
    rodilla_d.write(pos_rd);
    rodilla_i.write(pos_ri);
    tobillo_d.write(pos_td);
    tobillo_i.write(pos_ti);
  }
  for(int i=0;i<=25;i++)
  {
    delay(15);
    pos_cd ++;
    pos_ci ++;
    pos_rd --;
    pos_ri --;
    pos_td ++;
    pos_ti ++;
    cadera_d.write(pos_cd);
    cadera_i.write(pos_ci);
    rodilla_d.write(pos_rd);
    rodilla_i.write(pos_ri);
    tobillo_d.write(pos_td);
    tobillo_i.write(pos_ti);
  }
  pos_cd = 68;
  pos_rd = 160;
  pos_td = 95;
  pos_ci = 135;
  pos_ri = 52;
  pos_ti = 140;

}