#include <ESP32Servo.h>
#include <AnalogWrite.h>
#include <ESP32Tone.h>
#include <ESP32PWM.h>

#include <ps4.h>
#include <ps4Controller.h>
#include <ps4_int.h>

#define LeftMotor 25
#define RightMotor 26

Servo Motor Esquerdo;
Servo Motor Direito;

//01:01:01:01:01:01
void setup() {
  PS4.begin("01:01:01:01:01:01");
  Serial.begin(9600);

  while (!PS4.isConnected()) {
    Serial.print("esperando conexão");
    delay(250);
  }
  PS4.setLed(100,0,0);
  PS4.sendToController();

  MotorEsquerdo.attach(LeftMotorPin);
  MotorDireito.attach(RightMotorPin);
  MotorEsquerdo.write(90);
  MotorDireito.write(90);

  Serial.print("Conexão Estabelecida");
}
   
void loop() {
  if(PS4.isConnected()) {
    int LStickX = LStickX();
    int LStickY = LStickY();

    int LeftMotorOutput = constrain(map(LStickY,-127,127,0,180) + map(LStickX,-127,127,0,180);
    int RightMotorOutput = constrain(map(LStickY,-127,127,0,180) - map(LStickX,-127,127,0,180);

    if(LeftMotorOutput > 95 || LeftMotorOutput < 85){
      MotorEsquerdo.write(LeftMotorOutput);
    } else{
      MotorEsquerdo.write(90);

    if(RightMotorOutput > 95 || RightMotorOutput < 85){
      MotorDireito.write(RighttMotorOutput);
    } else{
      MotorDireito.write(90);
    }
  }else{
     MotorEsquerdo.write(90);
     MotorDireito.write(90);
  }
  }
}
Serial.print(LeftMotorOutput);
Serial.print(" ");
Serial.print(RightMotorOutput);