Servo Walking example

byte Servo1_Target = 90;
byte Servo2_Target = 90;
byte Servo1_Current = 0;
byte Servo2_Current = 180;

void setup()
{
  // Init pins here

  // Init servos here

}

void loop()
{
  // --------------- HANDLE SERVO WALKING -------------------
  if (Servo1_Current < Servo1_Target)
  {
    Servo1_Current++;
    servo_1.write(Servo1_Current);
  }
  if (Servo1_Current > Servo1_Target)
  {
    Servo1_Current--;
    servo_1.write(Servo1_Current);
  }
  if (Servo2_Current < Servo2_Target)
  {
    Servo2_Current++;
    servo_2.write(Servo2_Current);
  }
  if (Servo2_Current > Servo2_Target)
  {
    Servo2_Current--;
    servo_2.write(Servo2_Current);
  }

  delay(1); // Make sure the code takes more than 1ms to execute (otherwise the while below might fire of more than 1 time per 25ms)

  while (millis()%25 !=0); // This will ensure proper timing of 25 ms for the loop independent of code length
}