#include <ESP32Servo.h>

Servo servo1;  // First Servo
Servo servo2;  // Second Servo

const int servo1Pin = 26;  // GPIO26 for first servo
const int servo2Pin = 27;  // GPIO27 for second servo

const int joystickVRY = 36; // Joystick Vertical (Y-axis) → VP (GPIO36)
const int joystickVRX = 39; // Joystick Horizontal (X-axis) → VN (GPIO39)

void setup() {
    Serial.begin(115200);

    // Attach servos to ESP32 PWM pins
    servo1.attach(servo1Pin, 500, 2400);  // Min & Max pulse width for ESP32
    servo2.attach(servo2Pin, 500, 2400);
}

void loop() {
    int vertValue = analogRead(joystickVRY); // Read Y-axis
    int horzValue = analogRead(joystickVRX); // Read X-axis

    // Convert joystick readings (0-4095) to servo angles (0-180)
    int angle1 = map(vertValue, 0, 4095, 0, 180);
    int angle2 = map(horzValue, 0, 4095, 0, 180);

    // Move servos
    servo1.write(angle1);
    servo2.write(angle2);

    Serial.print("Servo 1 Angle: ");
    Serial.print(angle1);
    Serial.print(" | Servo 2 Angle: ");
    Serial.println(angle2);

    delay(50);  // Small delay for smooth operation
}