/*
Quadraped gait file
*/


#include <Servo.h> 

// Define servo objects for the four ankle joints  
Servo ankleFL; 
Servo ankleFR;
Servo ankleBL;
Servo ankleBR;

// Define servo objects for the four hip joints
Servo hipFL;
Servo hipFR;
Servo hipBL;
Servo hipBR;
  
// Define variables
int flex1 = 45;  // Determines the length of each stride
int delayTime = 500; // Delay between limb movements
int startPause = 5000; // Delay to allow robot placement before movement
int pos = 0;  // Loop counter
int smoothnessDelay = 15;  
  
void setup() 
{ 
  
   hipFL.attach(10);
   ankleFL.attach(11);

   hipFR.attach(4);
   ankleFR.attach(5);
  
   hipBL.attach(6);
   ankleBL.attach(7);  
  
   hipBR.attach(8);
   ankleBR.attach(9);
   
// Put legs in starting position with 
// front left and back right legs in forward position and
// front right and back left legs in rearward position
   hipFL.write(90-flex1);
   ankleFL.write(90-flex1);
   
   hipFR.write(90+flex1);
   ankleFR.write(90+flex1);
   
   hipBL.write(90+flex1);
   ankleBL.write(90+flex1);
   
   hipBR.write(90-flex1);
   ankleBR.write(90-flex1);  
   
   delay(startPause);
} 
  
  
void loop() 
{
  
// Straighten front left and back right legs
  delay(delayTime);
  for(pos = 0; pos < flex1; pos +=  1)
  {
    hipFL.write(90-flex1+pos);  
    ankleFL.write(90-flex1+pos);
  
    hipBR.write(90-flex1+pos); 
    ankleBR.write(90-flex1+pos);
    
    delay(smoothnessDelay);
  }

// Bring front right and back left legs forward in three steps
// First flex ankles forward
  delay(delayTime);
  ankleFR.write(180);
  
  ankleBL.write(180);
  
// Second, swing the front right and back left legs forward
  delay(delayTime);
  hipFR.write(90-flex1);
  
  hipBL.write(90-flex1);
  
// Third, reset front right and back left ankles
  delay(delayTime);
  ankleFR.write(90-flex1);
  
  ankleBL.write(90-flex1);

// Bring front left and back right legs rearward
  delay(delayTime);
  for(pos = 0; pos < flex1; pos += 1)
  {
    hipFL.write(90+pos);
    ankleFL.write(90+pos);
  
    hipBR.write(90+pos);
    ankleBR.write(90+pos);
    
    delay(smoothnessDelay);
  }

// Straighten front right and back left legs
  delay(delayTime);
  for(pos = 0; pos < flex1; pos += 1)
  {
    hipFR.write(90-flex1+pos);
    ankleFR.write(90-flex1+pos);
  
    hipBL.write(90-flex1+pos);
    ankleBL.write(90-flex1+pos);
    
    delay(smoothnessDelay);
  }

// Bring front left and back right legs forward in three steps
// First flex ankles forward
  delay(delayTime);
//  ankleFL.write(180);
  hipFL.write(180);
  
  ankleBR.write(180);
  
// Second, swing the front left and back right legs forward
  delay(delayTime);
//  hipFL.write(90-flex1);
  ankleFL.write(90-flex1);
  
  hipBR.write(90-flex1);
  
// Third, reset front left and back right ankles
  delay(delayTime);
//  ankleFL.write(90-flex1);
  hipFL.write(90-flex1);
  
  
  
  ankleBR.write(90-flex1);

// Bring front right and back left legs rearward
  delay(delayTime);
  for(pos = 0; pos < flex1; pos += 1)
  {
    hipFR.write(90+pos);
    ankleFR.write(90+pos);
  
    hipBL.write(90+pos);
    ankleBL.write(90+pos);
    
    delay(smoothnessDelay);
  }
}