#include <Servo.h>

// Declare servo objects
Servo servo1; // Right leg - top
Servo servo2; // Left leg - top
Servo servo3; // Left leg - bottom
Servo servo4; // Right leg - bottom

// Servo neutral positions
int servo1Neutral = 90; // Right leg - top neutral position
int servo2Neutral = 90; // Left leg - top neutral position
int servo3Neutral = 90; // Left leg - bottom neutral position
int servo4Neutral = 90; // Right leg - bottom neutral position

// Movement parameters
int stepAngle = 20; // Angle to move for a step
int delayTime = 300; // Delay between movements (ms)

void setup() {
  // Attach servos to pins
  servo1.attach(3); // Pin for right leg top
  servo2.attach(5); // Pin for left leg top
  servo3.attach(6); // Pin for left leg bottom
  servo4.attach(9); // Pin for right leg bottom

  // Set servos to neutral positions
  resetToNeutral();
}

void loop() {
  takeStep(); // Make Otto take a step
  delay(500); // Short pause between steps
}

// Function to reset all servos to their neutral positions
void resetToNeutral() {
  servo1.write(servo1Neutral);
  servo2.write(servo2Neutral);
  servo3.write(servo3Neutral);
  servo4.write(servo4Neutral);
  delay(500); // Allow servos to move to position
}

// Function for a walking step
void takeStep() {
  // Step 1: Lift left leg
  servo3.write(servo3Neutral + stepAngle); // Move left bottom servo up
  delay(delayTime);

  // Step 2: Swing left leg forward
  servo2.write(servo2Neutral - stepAngle); // Move left top servo forward
  delay(delayTime);

  // Step 3: Place left leg down
  servo3.write(servo3Neutral); // Return left bottom servo to neutral
  delay(delayTime);

  // Step 4: Lift right leg
  servo4.write(servo4Neutral + stepAngle); // Move right bottom servo up
  delay(delayTime);

  // Step 5: Swing right leg forward
  servo1.write(servo1Neutral - stepAngle); // Move right top servo forward
  delay(delayTime);

  // Step 6: Place right leg down
  servo4.write(servo4Neutral); // Return right bottom servo to neutral
  delay(delayTime);

  // Reset to neutral after each step
  resetToNeutral();
}