#include <Servo.h> 
 
Servo ho;
Servo hu;
Servo hr;
Servo hl;
const int bo=0;
const int bu=0;
const int br=0;
const int bl=0;
const int llro=3;
const int llge=10;
const int llgr=2;
const int lrro=11;
const int lrge=12;
const int lrgr=13;

int incomingByte;
int servoNr=9;
int servoDegree=0;
int atSerial=0;
int serialServo=0;
int serialDegree=0;
const int seperator=44;
int lastmillis = 0;

void setup() 
{ 
  ho.attach(5);
  hu.attach(3);
  hr.attach(6);
  hl.attach(9);
  pinMode(llro,OUTPUT);
  pinMode(llge,OUTPUT);
  pinMode(llgr,OUTPUT);
  pinMode(lrro,OUTPUT);
  pinMode(lrge,OUTPUT);
  pinMode(lrgr,OUTPUT);
} 

void myinit()
{
  ho.write(90+bo);
  hu.write(90+bu);
  hl.write(90+bl);
  hr.write(90+br);
}

void so(int v)
{
  ho.write(v+bo);
}
void su(int v)
{
  hu.write(v+bu);
}
void sl(int v)
{
  hl.write(v+bl);
}
void sr(int v)
{
  hr.write(v+br);
}

void feet(int l,int r)
{
  for(int i=0;i<30;i++) {
    sl(90+i*l);
    sr(90+i*r);
    delay(50);
  } 
  for(int i=30;i>0;i--) {
    sl(90+i*l);
    sr(90+i*r);
    delay(50);
  } 
}

void bodylr(int sto,int eo,int stu,int eu)
{
  for(int i=0;i<30;i++) {
    so(90+map(i,0,30,sto,eo));
    su(90+map(i,0,30,stu,eu));
    delay(50);
  }
}

void llrot()
{
  for(int i=0;i<10;i++) {
    digitalWrite(llro,HIGH);
    delay(50);
    digitalWrite(llro,LOW);
    digitalWrite(llgr,HIGH);
    delay(50);
    digitalWrite(llgr,LOW);
    digitalWrite(llge,HIGH);
    delay(50);
    digitalWrite(llge,LOW);
  }
}
void lrrot()
{
  for(int i=0;i<10;i++) {
    digitalWrite(lrro,HIGH);
    delay(50);
    digitalWrite(lrro,LOW);
    digitalWrite(lrgr,HIGH);
    delay(50);
    digitalWrite(lrgr,LOW);
    digitalWrite(lrge,HIGH);
    delay(50);
    digitalWrite(lrge,LOW);
  }
}
void loop() 
{ 
    //myservo.write(90);              // tell servo to go to position in variable 'pos' 
  myinit();
  bodylr(0,30,0,-10);
  llrot();
  feet(0,-2);
  bodylr(30,0,-10,0);
  bodylr(0,-30,0,-10);
  lrrot();
  feet(2,0);
  bodylr(-30,0,-10,0);
  
  myinit();

  delay(1000);
    
}