#include <LiquidCrystal_I2C.h> // Biblioteca do LCD

#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

#define ECHO_PIN_esq A1
#define TRIG_PIN_esq A0
#define ECHO_PIN_dto A3
#define TRIG_PIN_dto A2
#define MOTOR_1 13 // PINO PWM para MOTOR 1 
#define MOTOR_2 12 // PINO PWM para MOTOR 2 
#define MOTOR_3 11 // PINO PWM para MOTOR 3 
#define MOTOR_4 10 // PINO PWM para MOTOR 4 


void setup() {
  
Serial.begin(9600);
 pinMode(MOTOR_1, OUTPUT); 
 pinMode(MOTOR_2, OUTPUT);
 pinMode(MOTOR_3, OUTPUT);
 pinMode(MOTOR_4, OUTPUT);
 pinMode(TRIG_PIN_esq, OUTPUT);
 pinMode(ECHO_PIN_esq, INPUT);
 pinMode(TRIG_PIN_dto, OUTPUT);
 pinMode(ECHO_PIN_dto, INPUT);

 // Iniciar LCD 
lcd.init();
lcd.backlight();


 // Texto Inicial do LCD
  lcd.setCursor(1, 0);
 lcd.print("Robo Transportador");
 lcd.setCursor(2, 1);
 lcd.print("Ricardo Nunes");
  lcd.setCursor(5, 2);
 lcd.print("M10348");
  lcd.setCursor(7, 3);
  lcd.print("UBI");
}

 // Efetuar leitura distancia extremidade esquerda
float readDistanceCM_esq() {
  digitalWrite(TRIG_PIN_esq, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN_esq, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_esq, LOW);
  int duration = pulseIn(ECHO_PIN_esq, HIGH);
  return duration * 0.034 / 2; // Velocidade do som 0.034 cm/ms
}

// Efetuar leitura distancia extremidade direita
float readDistanceCM_dto() {
 digitalWrite(TRIG_PIN_dto, LOW);
 delayMicroseconds(2);
 digitalWrite(TRIG_PIN_dto, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN_dto, LOW);
 int duration = pulseIn(ECHO_PIN_dto, HIGH);
 return duration * 0.034 / 2; // Velocidade do som 0.034 cm/ms
}


void loop() {
 //definir PWM inicial
  int velocidade_esq = 127.5; // Valor inicial PWM ( 0 A 255) - V=0,45 m/s 
  analogWrite(MOTOR_1, velocidade_esq );
  analogWrite(MOTOR_2, velocidade_esq );

  int velocidade_dto = 127.5; // // Valor inicial PWM ( 0 A 255) - V=0,45 m/s 
  analogWrite(MOTOR_3, velocidade_dto );
  analogWrite(MOTOR_4, velocidade_dto );

// Definir a distancia que aparece no LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Dist Esq: ");
  lcd.print(readDistanceCM_esq());
  lcd.print(" CM");
  lcd.setCursor(0, 2);
  lcd.print("Dist Dto: ");
  lcd.print(readDistanceCM_dto());
  lcd.print(" CM");

 // Condição para velocidade constante a direito.
  if(readDistanceCM_esq() <= 55 and readDistanceCM_esq() >=45 and readDistanceCM_dto() <= 55 and readDistanceCM_dto() >=45 ) {
  analogWrite(MOTOR_1, velocidade_esq);
  analogWrite(MOTOR_2, velocidade_esq);
  analogWrite(MOTOR_3, velocidade_dto);
  analogWrite(MOTOR_4, velocidade_dto);

  lcd.setCursor(0, 1);
  lcd.print("PWM Esq: ");
  lcd.print(velocidade_esq);
  lcd.setCursor(0, 3);
  lcd.print("PWM Dto: ");
  lcd.print(velocidade_dto);
  }

  // Condição para aumentar velocidade quando distancia estiver entre 55<X<75 a direito. *AUMENTO DE 63,75 - PWM*
  if(readDistanceCM_esq() >= 55 and readDistanceCM_dto() >=55 and readDistanceCM_esq() <= 75 and readDistanceCM_dto() <=75 ) {
  analogWrite(MOTOR_1, velocidade_esq + 63.75);
  analogWrite(MOTOR_2, velocidade_esq + 63.75);
  analogWrite(MOTOR_3, velocidade_dto + 63.75);
  analogWrite(MOTOR_4, velocidade_dto + 63.75);
  lcd.setCursor(0, 1);
  lcd.print("PWM Esq: ");
  lcd.print(velocidade_esq + 63.75);
  lcd.setCursor(0, 3);
  lcd.print("PWM Dto: ");
  lcd.print(velocidade_dto + 63.75);
  }

  // Condição para aumentar velocidade quando distancia estiver superior a 75 a direito. *AUMENTO DE 127.5 - PWM*
   if(readDistanceCM_esq() >= 75 and readDistanceCM_dto() >= 75) {
  analogWrite(MOTOR_1, velocidade_esq + 127.5);
  analogWrite(MOTOR_2, velocidade_esq + 127.5);
  analogWrite(MOTOR_3, velocidade_dto + 127.5);
  analogWrite(MOTOR_4, velocidade_dto + 127.5);
  lcd.setCursor(0, 1);
  lcd.print("PWM Esq: ");
  lcd.print(velocidade_esq + 127.5);
  lcd.setCursor(0, 3);
  lcd.print("PWM Dto: ");
  lcd.print(velocidade_dto + 127.5);
  }

  // Condição para diminuir velocidade quando distancia estiver entre 25 < X < 45 a direito. *DIMINUIÇÃO DE 63,75 - PWM*
   if(readDistanceCM_esq() >= 25 and readDistanceCM_dto() <= 45) {
  analogWrite(MOTOR_1, velocidade_esq - 63.75);
  analogWrite(MOTOR_2, velocidade_esq - 63.75);
  analogWrite(MOTOR_3, velocidade_dto - 63.75);
  analogWrite(MOTOR_4, velocidade_dto - 63.75);
  lcd.setCursor(0, 1);
  lcd.print("PWM Esq: ");
  lcd.print(velocidade_esq - 63.75);
  lcd.setCursor(0, 3);
  lcd.print("PWM Dto: ");
  lcd.print(velocidade_dto - 63.75);
  }
  
  // Condição para diminuir a velocidade quando distancia estiver inferior a 25 a direito. *DIMINUIÇÃO DE 127.5 - PWM*
   if(readDistanceCM_esq() <= 25 and readDistanceCM_dto() <= 25) {
  analogWrite(MOTOR_1, velocidade_esq - 127.5);
  analogWrite(MOTOR_2, velocidade_esq - 127.5);
  analogWrite(MOTOR_3, velocidade_dto - 127.5);
  analogWrite(MOTOR_4, velocidade_dto - 127.5);
  lcd.setCursor(0, 1);
  lcd.print("PWM Esq: ");
  lcd.print(velocidade_esq - 127.5);
  lcd.setCursor(0, 3);
  lcd.print("PWM Dto: ");
  lcd.print(velocidade_dto - 127.5);
  }

// Condição para virar 10º para a direita
   if(readDistanceCM_esq() >= 75 and readDistanceCM_esq() <=134 and readDistanceCM_dto() >=45 and readDistanceCM_dto() <=55 )  {
  analogWrite(MOTOR_1, velocidade_esq + 42.5);
  analogWrite(MOTOR_2, velocidade_esq + 42.5);
  analogWrite(MOTOR_3, velocidade_dto);
  analogWrite(MOTOR_4, velocidade_dto);
  lcd.setCursor(0, 1);
  lcd.print("PWM Esq: ");
  lcd.print(velocidade_esq + 42.5);
  lcd.setCursor(0, 3);
  lcd.print("PWM Dto: ");
  lcd.print(velocidade_dto);
  }

  // Condição para virar 30º para a direita
   if(readDistanceCM_esq() >= 135 and readDistanceCM_esq() <=196 and readDistanceCM_dto() >=45 and readDistanceCM_dto() <=55 )  {
  analogWrite(MOTOR_1, velocidade_esq + 42.5+42.5);
  analogWrite(MOTOR_2, velocidade_esq + 42.5+42.5);
  analogWrite(MOTOR_3, velocidade_dto);
  analogWrite(MOTOR_4, velocidade_dto);
  lcd.setCursor(0, 1);
  lcd.print("PWM Esq: ");
  lcd.print(velocidade_esq + 42.5+42.5);
  lcd.setCursor(0, 3);
  lcd.print("PWM Dto: ");
  lcd.print(velocidade_dto);
  }

// Condição para virar 50º para a direita
   if(readDistanceCM_esq() >= 197 and readDistanceCM_dto() >=45 and readDistanceCM_dto() <=55 )  {
  analogWrite(MOTOR_1, velocidade_esq + 42.5+42.5+42.5);
  analogWrite(MOTOR_2, velocidade_esq + 42.5+42.5+42.5);
  analogWrite(MOTOR_3, velocidade_dto);
  analogWrite(MOTOR_4, velocidade_dto);
  lcd.setCursor(0, 1);
  lcd.print("PWM Esq: ");
  lcd.print(velocidade_esq + 42.5+42.5+42.5);
  lcd.setCursor(0, 3);
  lcd.print("PWM Dto: ");
  lcd.print(velocidade_dto);
  }


// Condição para virar 10º para a esquerda
   if(readDistanceCM_dto() >= 75 and readDistanceCM_dto() <=134 and readDistanceCM_esq() >=45 and readDistanceCM_esq() <=55 )  {
  analogWrite(MOTOR_1, velocidade_esq );
  analogWrite(MOTOR_2, velocidade_esq );
  analogWrite(MOTOR_3, velocidade_dto + 42.5);
  analogWrite(MOTOR_4, velocidade_dto + 42.5);
  lcd.setCursor(0, 1);
  lcd.print("PWM Esq: ");
  lcd.print(velocidade_esq);
  lcd.setCursor(0, 3);
  lcd.print("PWM Dto: ");
  lcd.print(velocidade_dto + 42.5);
  }

  // Condição para virar 30º para a esquerda
   if(readDistanceCM_dto() >= 135 and readDistanceCM_dto() <=196 and readDistanceCM_esq() >=45 and readDistanceCM_esq() <=55 )  {
  analogWrite(MOTOR_1, velocidade_esq);
  analogWrite(MOTOR_2, velocidade_esq);
  analogWrite(MOTOR_3, velocidade_dto + 42.5+42.5);
  analogWrite(MOTOR_4, velocidade_dto + 42.5+42.5);
  lcd.setCursor(0, 1);
  lcd.print("PWM Esq: ");
  lcd.print(velocidade_esq);
  lcd.setCursor(0, 3);
  lcd.print("PWM Dto: ");
  lcd.print(velocidade_dto + 42.5+42.5);
  }

// Condição para virar 50º para a esquerda
   if(readDistanceCM_dto() >= 197 and readDistanceCM_esq() >=45 and readDistanceCM_esq() <=55 )  {
  analogWrite(MOTOR_1, velocidade_esq);
  analogWrite(MOTOR_2, velocidade_esq);
  analogWrite(MOTOR_3, velocidade_dto + 42.5+42.5+42.5);
  analogWrite(MOTOR_4, velocidade_dto + 42.5+42.5+42.5);
  lcd.setCursor(0, 1);
  lcd.print("PWM Esq: ");
  lcd.print(velocidade_esq);
  lcd.setCursor(0, 3);
  lcd.print("PWM Dto: ");
  lcd.print(velocidade_dto + 42.5+42.5+42.5);
  }



  delay(1000);
}