#include <Servo.h>

const int joyXPin = A0;
const int joyYPin = A1;

Servo leftServos[5];
Servo rightServos[5];

int neutralX = 512; // Adjust this value based on your joystick's neutral position
int neutralY = 512;

void setup() {
  for (int i = 0; i < 5; i++) {
    leftServos[i].attach(2 + i);
    rightServos[i].attach(7 + i);
  }
}

void loop() {
  int joyX = analogRead(joyXPin);
  int joyY = analogRead(joyYPin);

  if (isJoystickInNeutral(joyX, joyY)) {
    // Joystick is in the neutral position
    returnToNeutral();
  } else if (joyX < 500) {
    // Joystick is moved left
    turnServosCounterClockwise();
  } else if (joyX > 800) {
    // Joystick is moved right
    turnServosClockwise();
  }

  if (joyY < 500) {
    // Joystick is moved up
    turnServosClockwise();
  } else if (joyY > 800) {
    // Joystick is moved down
    turnServosCounterClockwise();
  }
}

void returnToNeutral() {
  for (int i = 0; i < 5; i++) {
    leftServos[i].write(90); // Assuming 90 is the original/neutral position
    rightServos[i].write(90);
  }
  delay(50); // Delay for smoother movement
}

void turnServosClockwise() {
  for (int i = 0; i < 5; i++) {
    leftServos[i].write(leftServos[i].read() + 10);
    rightServos[i].write(rightServos[i].read() + 10);
  }
  delay(50); // Delay for smoother movement
}

void turnServosCounterClockwise() {
  for (int i = 0; i < 5; i++) {
    leftServos[i].write(leftServos[i].read() - 10);
    rightServos[i].write(rightServos[i].read() - 10);
  }
  delay(50); // Delay for smoother movement
}

bool isJoystickInNeutral(int x, int y) {
  return (abs(x - neutralX) < 50) && (abs(y - neutralY) < 50);
}