#include <Arduino.h>
#include <ESP32Servo.h>

Servo servoX;
Servo servoY;

// Definição dos Pinos
#define PIN_SERVO_X 22
#define PIN_SERVO_Y 4
#define PIN_JOY_X 14
#define PIN_JOY_Y 12

int valX = 90;
int valY = 90;

int movs[] = {-2, 0, 2}; 

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

  pinMode(PIN_JOY_X, INPUT);
  pinMode(PIN_JOY_Y, INPUT);

  servoX.setPeriodHertz(50);
  servoX.attach(PIN_SERVO_X, 500, 2400);
  
  servoY.setPeriodHertz(50);
  servoY.attach(PIN_SERVO_Y, 500, 2400);

  servoX.write(valX);
  servoY.write(valY);
}

void loop() {

  int xReading = analogRead(PIN_JOY_X);
  int yReading = analogRead(PIN_JOY_Y);

  int joyXIndex = 1;
  int joyYIndex = 1;

  if (xReading < 1500) joyXIndex = 0;
  else if (xReading > 2600) joyXIndex = 2;

  if (yReading < 1500) joyYIndex = 0;
  else if (yReading > 2600) joyYIndex = 2;

  valX = valX + movs[joyXIndex];
  valY = valY + movs[joyYIndex];

  valX = constrain(valX, 0, 180);
  valY = constrain(valY, 0, 180);

  servoX.write(valX);
  servoY.write(valY);

  delay(20); 
}