#include <ESP32Servo.h>  // Include the ESP32Servo library

// Define servo pins
#define servo1Pin 26
#define servo2Pin 27

// Define joystick pins
#define joystickVertPin 34  // Vertical axis (VP pin)
#define joystickHorizPin 32 // Horizontal axis (VN pin)

ESP32Servo servo1;
ESP32Servo servo2;

void setup() {
  // Initialize serial communication
  Serial.begin(115200);
  
  // Attach servos to PWM pins
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);

  // Initialize joystick pins
  pinMode(joystickVertPin, INPUT);
  pinMode(joystickHorizPin, INPUT);
}

void loop() {
  // Read the joystick analog values (0 to 4095)
  int joystickVert = analogRead(joystickVertPin);
  int joystickHoriz = analogRead(joystickHorizPin);
  
  // Map joystick vertical axis (0-4095) to servo angle (0-180 degrees)
  int angleVert = map(joystickVert, 0, 4095, 0, 180);
  
  // Map joystick horizontal axis (0-4095) to servo angle (0-180 degrees)
  int angleHoriz = map(joystickHoriz, 0, 4095, 0, 180);

  // Set servo positions based on joystick input
  servo1.write(angleVert);  // Control the first servo with vertical axis
  servo2.write(angleHoriz); // Control the second servo with horizontal axis

  // Print joystick values for debugging
  Serial.print("Vertical: ");
  Serial.print(joystickVert);
  Serial.print("  Horizontal: ");
  Serial.println(joystickHoriz);
  
  // Delay to stabilize readings
  delay(15);
}