12 servo hexapod, arduino

hexapod.ino

#include <Servo.h>
int left1 = 3;
int left2 = 4;
int left3 = 7;
int left4 = 8;
int right1 = 5;
int right2 = 6;
int right3 = 9;
int right4 = 10;

int LED=13;

int left_out = 140;
int left_in = 110;
int left_fwd = 110;
int left_bwd = 70;

int right_out = 70;
int right_in = 110;
int right_fwd = 70;
int right_bwd = 110;

//AC_inner, 110 fwd, 70 bwd
//AC_outer, 110 out, 90 in
//B_inner, 110 fwd, 70 bwd
//B_outer, 140 out, 110 in
//DF_inner, 110fwd, 70 bwd
//DF_outer, 70 out, 110 in
//E_inner, 110 fwd, 70 bwd
//E_outer, 70 out, 90 in


int step_delay = 150;
int incomingByte = 0;

Servo lservo1;
Servo lservo2;
Servo lservo3;
Servo lservo4;
Servo rservo1;
Servo rservo2;
Servo rservo3;
Servo rservo4;

int gait_i = 0;

void f1();
void f2();
void f3();
void f4();
void (*p[4])();

void walk_fwd() {
  f1();
  f4();
  f3();
  f2();
  //rservo4.write(70);
  //rservo2.write(70);
  //digitalWrite(LED, LOW);
  //delay(step_delay);
  //rservo4.write(110);
  //rservo2.write(110);
  //digitalWrite(LED, HIGH);
  //delay(step_delay);
}

//void gait_control( ){
//step next


//step previous

//loop

//stop
//}

void gait_key( char keyPressed ){
  switch( keyPressed ){
  case 'j':
    gait_i++;
    Serial.println("case j");
    Serial.println(gait_i % 4);
    //gait_control( "next" );
    break;
  case 'k':
    gait_i--;
    Serial.println("case k");
    Serial.println(gait_i % 4);
    //gait_control( "previous" );
    break;
    //case 'g':
    //gait_control( "loop" );
    //case 'e':
    //gait_control( "stop" );
  }
  (*p[gait_i % 4])();
}

void setup()
{
  Serial.begin(9600);
  lservo1.attach(left1);//AC_inner, 110 fwd, 70 bwd
  lservo2.attach(left2); //AC_outer, 110 out, 90 in
  lservo3.attach(left3);//B_inner, 110 fwd, 70 bwd
  lservo4.attach(left4);//B_outer, 140 out, 110 in
  rservo1.attach(right1);//DF_inner, 110fwd, 70 bwd
  rservo2.attach(right2);//DF_outer, 70 out, 110 in
  rservo3.attach(right3);//E_inner, 110 fwd, 70 bwd
  rservo4.attach(right4);//E_outer, 70 out, 90 in
  pinMode(LED, OUTPUT);

  p[0] = f1;
  p[1] = f4;
  p[2] = f3;
  p[3] = f2;
}


void loop()
{

  //lservo4.write(left_in);
  //rservo2.write(right_in);
  //digitalWrite(LED, HIGH);
  //delay(step_delay);

  //lservo4.write(left_out);
  //rservo2.write(right_out);

  //digitalWrite(LED, LOW);
  //delay(step_delay);

  if (Serial.available() > 0) {
    incomingByte = Serial.read();
    gait_key(incomingByte);
    Serial.println(incomingByte);
  }
  delay(1);
  //walk_fwd();
}


void sequential_test(){
  lservo1.write(70);
  digitalWrite(LED, LOW);
  delay(1000);
  lservo1.write(110);
  digitalWrite(LED, HIGH);
  delay(1000);

  lservo2.write(90);
  digitalWrite(LED, LOW);
  delay(1000);
  lservo2.write(110);
  digitalWrite(LED, HIGH);
  delay(1000);

  lservo3.write(70);
  digitalWrite(LED, LOW);
  delay(1000);
  lservo3.write(110);
  digitalWrite(LED, HIGH);
  delay(1000);

  lservo4.write(110);
  digitalWrite(LED, LOW);
  delay(1000);
  lservo4.write(140);
  digitalWrite(LED, HIGH);
  delay(1000);

  rservo1.write(70);
  digitalWrite(LED, LOW);
  delay(1000);
  rservo1.write(110);
  digitalWrite(LED, HIGH);
  delay(1000);

  rservo2.write(70);
  digitalWrite(LED, LOW);
  delay(1000);
  rservo2.write(110);
  digitalWrite(LED, HIGH);
  delay(1000);

  rservo3.write(70);
  digitalWrite(LED, LOW);
  delay(1000);
  rservo3.write(110);
  digitalWrite(LED, HIGH);
  delay(1000);

  rservo4.write(70);
  digitalWrite(LED, LOW);
  delay(1000);
  rservo4.write(90);
  digitalWrite(LED, HIGH);
  delay(1000);
}


void f1(){
  lservo1.write(left_fwd);//AC
  rservo3.write(right_fwd);//E

  lservo3.write(left_bwd);//B
  rservo1.write(right_bwd);//DF

  Serial.println("f1");
  delay(step_delay);
}

void f2(){
  lservo2.write(left_out);
  rservo4.write(right_out);

  lservo4.write(left_in);
  rservo2.write(right_in);

  Serial.println("f2");
  delay(step_delay);
}


void f3(){
  lservo1.write(left_bwd);//AC
  rservo3.write(right_bwd);//E

  lservo3.write(left_fwd);//B
  rservo1.write(right_fwd);//DF

  Serial.println("f3");
  delay(step_delay);
}

void f4(){
  lservo2.write(left_in);
  rservo4.write(right_in);

  lservo4.write(left_out);
  rservo2.write(right_out);

  Serial.println("f4");
  delay(step_delay);
}