#include <Servo.h>

// Servo objects
Servo myServo1;
Servo myServo2;

int servo1 = 3;
int servo2 = 5;
int joyX = A0;  // Joystick X
int joyY = A1;  // Joystick Y
int selButton = 2; // SEL button (connected to digital pin 2)

int angle1 = 90;  // Start at middle position
int angle2 = 90;
const int deadZone = 50; // Joystick dead zone
const int stepSize = 1;  // Movement step size

void setup() {
  myServo1.attach(servo1);
  myServo2.attach(servo2);
  myServo1.write(angle1);
  myServo2.write(angle2);

  pinMode(selButton, INPUT_PULLUP); // Configure SEL button pin
}

void loop() {
  int valX = analogRead(joyX);
  int valY = analogRead(joyY);
  int buttonState = digitalRead(selButton);  // Read the SEL button state

  // If the button is pressed, reset servo positions
  if (buttonState == LOW) {
    angle1 = 90;
    angle2 = 90;
    myServo1.write(angle1);
    myServo2.write(angle2);
    delay(500);  // Prevent bouncing (delay while button is pressed)
  } else {
    // Joystick movement logic
    if (valX < 512 - deadZone) {
      angle1 = constrain(angle1 - stepSize, 10, 170);
    } else if (valX > 512 + deadZone) {
      angle1 = constrain(angle1 + stepSize, 10, 170);
    }

    if (valY < 512 - deadZone) {
      angle2 = constrain(angle2 - stepSize, 10, 170);
    } else if (valY > 512 + deadZone) {
      angle2 = constrain(angle2 + stepSize, 10, 170);
    }

    myServo1.write(angle1);
    myServo2.write(angle2);
  }

  delay(20);  // Delay for smooth control
}