Marching code

#include <Servo.h>
 
Servo left;  
Servo right;
Servo flag;

//servo offsets to stand robot upright at zero
int LOffset = 0;
int ROffset =  155;
int BOffset = 55;
int FOffset = 68;

int delayFactor = 15;

//locations of the three servos (flag, right, left)
int fAt;
int rAt;
int lAt;
 
void setup()
{
  left.attach(11);  
  right.attach(9);
  flag.attach(12);

  zero();
  delay(2000);
 
  //runDiags();
  //mainFunction();
 
}

/*
program functionality; runs once (not in use demo day)
*/
void mainFunction(){  
  goHips(10);
  delay(100);
  goRight(35);
  delay(300);
  goLeft(lAt-10);
  delay(100);
  goHips(10);
  delay(300);
  goRight(10);
  delay(800);
  zero();

  delay(4000);
}
 
void stepLeft(){
  //goFlag(-60);
  goHips(7);
  delay(200);
  goLeft(50);
  delay(500);
  goHips(12);
  delay(50);
  goHips(-7);
  goLeft(10);
  delay(500);
}
 
void stepRight(){
  //goFlag(40);
  goHips(7);
  delay(200);
  goRight(50);
  delay(500);
  goHips(12);
  delay(50);
  goHips(-7);
  goRight(10);
  delay(500);
}
 
 
 /*
 dance and wave flag. for demo day
 */
void loop()
{
  goHips(10);
  delay(100);
  goRight(35);
  delay(300);
  goLeft(lAt-10);
  delay(100);
  goHips(10);
  delay(800);
  zero();
  delay(1000);
 
  goFlag(40);
  delay(500);
  goFlag(-40);
  delay(500);
  goFlag(40);
  delay(500);
 
}

/*
wave flag, bow, kick right, kick left
verify that all servos are working
*/
void runDiags(){
  delay(1000);
  //wave flag
  goFlag(40);
  delay(500);
  goFlag(-40);
  delay(500);
  goFlag(40);
  delay(500);
  goFlag(-40);
  delay(500);
  zeroFlag();
 
  delay(1200);
  //bow
  goHips(20);
  delay(1000);
  goHips(-20);
  delay(1000);
  goHips(20);
  delay(1000);
  goHips(-20);
  delay(1000);
  zeroHips();
 
  delay(1200);
  //kick right
  goRight(55);
  delay(500);
  goRight(-55);
  delay(500);
  goRight(55);
  delay(500);
  goRight(-55);
  delay(500);
  zeroRight();
 
  delay(1200);
  //kick left
  goLeft(55);
  delay(500);
  goLeft(-55);
  delay(500);
  goLeft(55);
  delay(500);
  goLeft(-55);
  delay(500);
  zeroLeft();
}

/*
zero all servos
*/
void zero(){
  //zeroLeft();
  //zeroRight();
  //zeroBody();
  zeroFlag();
  zeroHips();
}

/*
move right leg to a certain angle away from the zero (standing upright)
*/
void goRight(int p) {
  if(-p+ROffset>rAt){
    for(int pos = rAt; pos < -p+ROffset; pos += 1){                                  
      right.write(pos);              
      delay(delayFactor);      
    }
  }else{
    for(int pos = rAt; pos > -p+ROffset; pos -= 1){                                  
      right.write(pos);              
      delay(delayFactor);            
    }
  }
  right.write(-p+ROffset);
  rAt = -p+ROffset;
}

/*
move left leg to a certain angle away from the zero (standing upright)
*/
void goLeft(int p) {
  if(p+LOffset>lAt){
    for(int pos = lAt; pos < p+LOffset; pos += 1){                                  
      left.write(pos);              
      delay(delayFactor);      
    }
  }else{
    for(int pos = lAt; pos > p+LOffset; pos -= 1){                                  
      left.write(pos);              
      delay(delayFactor);            
    }
  }
  left.write(p+LOffset);
  lAt = p+LOffset;
}

/*
void goBody(int p){
  if(p+BOffset>bAt){
    for(int pos = bAt; pos < p+BOffset; pos += 1){                                  
      body.write(pos);              
      delay(delayFactor);      
    }
  }else{
    for(int pos = bAt; pos > p+BOffset; pos -= 1){                                  
      body.write(pos);              
      delay(delayFactor);            
    }
  }
  body.write(p+BOffset);
  bAt = p+BOffset;
}*/


/*
moves both legs to pivot at hips.
@param int p: number of degrees to pivot. negative pivots backwards.
*/
void goHips(int p){
  left.write(lAt+p);
  right.write(rAt-p);
  lAt = lAt+p;
  rAt = rAt-p;

}

void zeroHips(){
  int spec = 0;
  right.write(ROffset-spec);
  left.write(spec+LOffset);
  rAt = ROffset-spec;
  lAt = spec+LOffset;
}

/*
move flag to a certain angle away from the zero (upright)
*/
void goFlag(int p){
  if(p+FOffset>fAt){
    for(int pos = fAt; pos < p+FOffset; pos += 1){                                  
      flag.write(pos);              
      delay(delayFactor);      
    }
  }else{
    for(int pos = fAt; pos > p+FOffset; pos -= 1){                                  
      flag.write(pos);              
      delay(delayFactor);            
    }
  }
  fAt = p+FOffset;
}

void zeroRight(){
  right.write(ROffset);
  rAt = ROffset;
}

void zeroLeft(){
  left.write(LOffset);
  lAt = LOffset;
}


void zeroFlag(){
  flag.write(FOffset);
  fAt = FOffset;
}

/*
a half-second delay
*/
void pause(){
  delay(500);
}