#include <ESP32Servo.h>
#include <LiquidCrystal_I2C.h>

#define pinPot1 13
#define pinPot2 12
#define pinPot3 14
#define pinPot4 35 //SD CARD
#define pinPot5 26
#define pinPot6 25

#define pinServo1 15
#define pinServo2 2
#define pinServo3 4
#define pinServo4 27
#define pinServo5 32
#define pinServo6 33

LiquidCrystal_I2C lcd(0x27, 20, 4);

int pinServo[6] = { 
  pinServo1,
  pinServo2,
  pinServo3,
  pinServo4,
  pinServo5,
  pinServo6  
};

int pinPot[6] = {
  pinPot1,
  pinPot2,
  pinPot3,
  pinPot4,
  pinPot5,
  pinPot6
};

Servo myServo[6];

int valPot[6] = {0, 0, 0, 0, 0, 0};
int valDeg[6] = {0, 0, 0, 0, 0, 0};

byte derajat[] = {
  B00110,
  B01001,
  B01001,
  B00110,
  B00000,
  B00000,
  B00000,
  B00000
};

int temp[6] = {0,0,0,0,0,0};

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

  for (int i=0; i<6; i++) {
    myServo[i].attach(pinServo[i]);
  };

  lcd.init();
  lcd.backlight();
  lcd.createChar(0, derajat);
  lcd.setCursor(0, 0);      
  lcd.print("  Servo Controller  ");  
}

void loop() {   
  for (int i=0; i<=5; i++) {
    valPot[i] = analogRead(pinPot[i]);
    Serial.println("Potensio: " + String(valPot[i]));
    valDeg[i] = map(valPot[i], 0, 4095, 0, 180);
    myServo[i].write(valDeg[i]);
    
    if (i == 0 && (temp[i] != valDeg[i])) {
      lcd.setCursor(0, 1);      
      lcd.print("A: " + String(valDeg[i]) + " ");
      lcd.setCursor(6, 1);
      lcd.write(byte(0));
    } else if (i==1 && (temp[i] != valDeg[i])) {
      lcd.setCursor(0, 2);      
      lcd.print("B: " + String(valDeg[i]) + " ");
      lcd.setCursor(6, 2);
      lcd.write(byte(0));
    } else if (i==2 && (temp[i] != valDeg[i])) {
      lcd.setCursor(0, 3);      
      lcd.print("C: " + String(valDeg[i]) + " ");
      lcd.setCursor(6, 3);
      lcd.write(byte(0));
    } else if (i==3 && (temp[i] != valDeg[i])) {
      lcd.setCursor(10, 1);      
      lcd.print("D: " + String(valDeg[i]) + " ");
      lcd.setCursor(16, 1);
      lcd.write(byte(0));
    } else if (i==4 && (temp[i] != valDeg[i])) {
      lcd.setCursor(10, 2);      
      lcd.print("E: " + String(valDeg[i]) + " ");
      lcd.setCursor(16, 2);
      lcd.write(byte(0));
    } else if (i==5 && (temp[i] != valDeg[i])) {
      lcd.setCursor(10, 3);      
      lcd.print("F: " + String(valDeg[i]) + " ");
      lcd.setCursor(16, 3);
      lcd.write(byte(0));
    }    
    temp[i] = valDeg[i];
  }
  
  delay(3); // this speeds up the simulation
}