// TESTE JOYSTICK

#include <ESP32Servo.h>


int Ch1 = 32;
int Ch2 = 33;
int Ch3 = 34;
int Ch4 = 35;
int Ch5 = 36;
int Ch6 = 39;

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

void setup() {
  servo1.attach(25);
  servo2.attach(26);
  servo3.attach(27);
  servo4.attach(14);
  pinMode(Ch1, INPUT);
  pinMode(Ch2, INPUT);
  pinMode(Ch3, INPUT);
  pinMode(Ch4, INPUT);
  pinMode(Ch5, INPUT);
  pinMode(Ch6, INPUT);
  analogReadResolution(12);
  Serial.begin(115200);
}

void loop() {

//Direita & Esquerda
int XR = analogRead(Ch1);
if(analogRead(Ch1) < 2048){
  XR = map(XR, 0, 2048, 1, 100);
}
else{XR = 100;}
int XL = analogRead(Ch1);
if(analogRead(Ch1) >= 2048){
  XL = map(XL, 2048, 4095, 100, 1);
}
else{XL = 100;}

//Frente e Tras
int YB = analogRead(Ch2);
if(analogRead(Ch2) < 2048){
  YB = map(YB, 0, 2048, 1, 100);
}
else{YB = 100;}
int YF = analogRead(Ch2);
if(analogRead(Ch2) >= 2048){
  YF = map(YF, 2048, 4095, 100, 1);
}
else{YF = 100;}

//Rotação
int rotH = analogRead(Ch3);
if(analogRead(Ch3) < 2048){
  rotH = map(rotH, 0, 2048, 1, 100);
}
else{rotH = 100;}
int rotAH = analogRead(Ch3);
if(analogRead(Ch3) >= 2048){
  rotAH = map(rotAH, 2048, 4095, 100, 1);
}
else{rotAH = 100;}

float Mtr1Out = analogRead(Ch4) * (XR / 100) * (YF / 100) * (rotH / 100);
float Mtr2Out = analogRead(Ch4) * (XL / 100) * (YF / 100) * (rotAH / 100);
float Mtr3Out = analogRead(Ch4) * (XL / 100) * (YB / 100) * (rotH / 100);
float Mtr4Out = analogRead(Ch4) * (XR / 100) * (YB / 100) * (rotAH / 100);

int M1 = Mtr1Out;
M1 = map(M1, 0, 4095, 0, 180);
int M2 = Mtr2Out;
M2 = map(M2, 0, 4095, 0, 180);
int M3 = Mtr3Out;
M3 = map(M3, 0, 4095, 0, 180);
int M4 = Mtr4Out;
M4 = map(M4, 0, 4095, 0, 180);

servo1.write(M1);
servo2.write(M2);
servo3.write(M3);
servo4.write(M4);

Serial.print("M1 ");
Serial.print(M1);
Serial.print(" - M2 ");
Serial.print(M2);
Serial.print(" - M3 ");
Serial.print(M3);
Serial.print(" - M4 ");
Serial.println(M4);

Serial.print("Ch1 ");
Serial.print(analogRead(Ch1));
Serial.print(" - Ch2 ");
Serial.print(analogRead(Ch2));
Serial.print(" - Ch3 ");
Serial.print(analogRead(Ch3));
Serial.print(" - Ch4 ");
Serial.println(analogRead(Ch4));

Serial.print("Mtr1 ");
Serial.print(Mtr1Out);
Serial.print(" - Mtr2 ");
Serial.print(Mtr2Out);
Serial.print(" - Mtr3 ");
Serial.print(Mtr3Out);
Serial.print(" - Mtr4 ");
Serial.println(Mtr4Out);

Serial.println(". . .");
delay(200);
}