#include <Servo.

h>

// Creating servo objects

Servo leftAnkle;
Servo rightAnkle;
Servo leftHip;
Servo rightHip;
// Setting neutral position values for each motor
int leftAnklePos = 90;
int rightAnklePos = 90;
int leftHipPos = 93;
int rightHipPos = 105;

int forDelay = 15; //delay of movement of each single motor

int delayOne = 15; //transistion delay from one motor to the next
boolean firstTime = true;
//Function prototypes
void centerPosition();
void forward();

void setup()
{
//setting output for motors
leftAnkle.attach(3); // attaches the servo on pin 3 to the servo
object
rightAnkle.attach(5); // attaches the servo on pin 5 to the servo
object
leftHip.attach(6); // attaches the servo on pin 6 to the servo
object
rightHip.attach(9);// attaches the servo on pin 9 to the servo

centerPosition(); //neutral position

//Move Forward
for( int i = 0; i<3 ; i++ )
{forward();
}
delay(500);

// back to neutral
centerPosition();
}

ankle

/********ANKLE FUNCTIONS******************************************/

/******First Time********************************************/
void leftAnkleOut ()
{
for (int i = 0; i < 15 ; i++ )
 { leftAnkle.write(leftAnklePos-i); // left ankle out
delay(forDelay);}
}

void rightAnkleOut()
{for (int i = 0; i < 10 ; i++ )
{rightAnkle.write(rightAnklePos-i); //right ankle out
delay(forDelay);}
}

void ankleOutFirst()
{
leftAnkleOut();
delay(delayOne);
rightAnkleOut();
delay(delayOne);
}

/******LOOP*****************************************************/
void ankleInLoop()
{
for (int i = 0; i <25 ; i++)
{
leftAnkle.write((leftAnklePos -15)+i);
rightAnkle.write((rightAnklePos-10)+i);
delay(forDelay); }
}

void ankleOutLoop()
{
for (int i = 0; i <25 ; i++ )
{
rightAnkle.write((rightAnklePos+15)-i); //right foot
if (i <= 20)
{leftAnkle.write((leftAnklePos+10)-i);}
delay(forDelay); }
}

hip
********HIP
FUNCTIONs*************************************************************
********/

// First Time
void leftForwardFirst()
{
for (int i = 0; i < 15 ; i++ ) //
{
leftHip.write(leftHipPos+i); // left hip up
rightHip.write(rightHipPos+i); //right hip down
 delay(forDelay);}
}

/*******LOOP************************************************/
// Loop
void leftForwardLoop()
{
for (int i = 0; i < 30 ; i++ )
{
leftHip.write((leftHipPos-15)+i); //left hip up
rightHip.write((rightHipPos-15)+i); //right up down

delay(forDelay);}
}

void rightForwardLoop()
{
for (int i = 0; i < 30 ; i++ )
{
leftHip.write((leftHipPos+15)-i); // left hip up
rightHip.write((rightHipPos+15)-i); // right hip up
delay(forDelay);
}
}

////WALKING FORWARD
FUNCTION /////////////////////////////////////////////////////////////
///////////

//Fuction to walk forward

void forward()
{
if(firstTime)
{
ankleOutFirst(); // left leg forward
leftForwardFirst();
}

else
{
leftForwardLoop(); //left leg forward
}

ankleInLoop();
delay(delayOne);

//Right Leg Forward

rightForwardLoop();
delay(delayOne);

ankleOutLoop();
delay(delayOne);

firstTime = false;
}//end forward()

// Function to write all servos to its neutral position

void centerPosition()
{
leftAnkle.write(leftAnklePos);
rightAnkle.write(rightAnklePos);
leftHip.write(leftHipPos);
rightHip.write(rightHipPos);
leftShoulder.write(leftShoulderPos);
rightShoulder.write(rightShoulderPos);
delay(1000);
}//end centerPosition();

//Function to write only HIP and ANKLE servos to center position

void lowerCenter()
{
leftAnkle.write(leftAnklePos);
rightAnkle.write(rightAnklePos);
leftHip.write(leftHipPos);
rightHip.write(rightHipPos);
}//end lowerCenter()