#include <Servo.h>

// Define the 4 shoulder servos
Servo FL, BR, FR, BL;

void setup() {
  FL.attach(3);
  BR.attach(5);
  FR.attach(6);
  BL.attach(9);
  
  // Initialize to neutral standing position (90 degrees)
  FL.write(90);
  BR.write(90);
  FR.write(90);
  BL.write(90);
  delay(1000);
}

void loop() {
  smoothTrot();
}

void smoothTrot() {
  // Phase 1: Diagonal Pair A moves forward, Pair B moves back
  for (int angle = 0; angle <= 40; angle += 2) {
    FL.write(90 + angle); // Front-Left forward
    BR.write(90 + angle); // Back-Right forward
    FR.write(90 - angle); // Front-Right backward
    BL.write(90 - angle); // Back-Left backward
    delay(15); // Controls the speed of the step
  }

  // Phase 2: Reverse the motion
  for (int angle = 40; angle >= 0; angle -= 2) {
    FL.write(90 + angle);
    BR.write(90 + angle);
    FR.write(90 - angle);
    BL.write(90 - angle);
    delay(15);
  }
  
  // (You would repeat the opposite for the other legs to complete the gait)
}