//Librerias
#include <Servo.h>
#include <LiquidCrystal_I2C.h>

//Declarar los pines
Servo servomoto;
LiquidCrystal_I2C lcd(0x27,20, 4);

#define pinRGBBlue 4
#define pinRGBRed 6
#define pinRGBGreen 5
#define pinTrig 9
#define pinEcho 10
#define pinServo 11
#define pinSDA A4
#define pinSCL A5

void setup() {
  Serial.begin(9600);
  //Definir conexiones
  pinMode(pinTrig, OUTPUT);
  pinMode(pinEcho, INPUT);
  pinMode(pinSDA, OUTPUT);
  pinMode(pinSCL, OUTPUT);
  pinMode(pinRGBBlue, OUTPUT);
  pinMode(pinRGBGreen, OUTPUT);
  pinMode(pinRGBRed, OUTPUT);
  servomoto.attach(pinServo);
  //Pantalla LCD
  lcd.init();
  lcd.backlight();


}

void loop() {
  /*Realizar que registre por proximidad cercana entre 1 y 99 e cambie el led de color Rojo y motor rotativo de 180 a 0,
   registro medio de 100 a 199 con color Amarillo y el motor intermitente de 0 a 180 grados,
   registro lejano de 200 a 299 de color Magenta con el motor cambios intermitentes cada 30 grados,
   muy lejano apagado
  */

  int angle = 90;
  long time;
  long distancia;

  digitalWrite(pinTrig, LOW);
  delayMicroseconds(5);
  digitalWrite(pinTrig, HIGH);
  delayMicroseconds(10);

  time = pulseIn(pinEcho, HIGH);
  distancia = time/59;

  //Condicionales para marcar un rango
  if(distancia >=1 && distancia <=99){
    //Pantalla
    lcd.clear();
    lcd.display();
    lcd.setCursor(0,0);
    lcd.print("Distancia :");
    lcd.setCursor(12,0);
    lcd.print(distancia); 
    //Led a rojo
    digitalWrite(pinRGBRed, LOW);
    digitalWrite(pinRGBGreen, HIGH);
    digitalWrite(pinRGBBlue, HIGH);
    //delay(1000);
    for(angle = 180; angle >=0; angle -=1){
        servomoto.write(angle);
        delay(25);
    }
  }else if(distancia >=100 && distancia <= 199){
          //Pantalla
          lcd.clear();
          lcd.display();
          lcd.setCursor(0,0);
          lcd.print("Distancia :");
          lcd.setCursor(12,0);   
          lcd.print(distancia); 
          //Led a amarillo
          digitalWrite(pinRGBRed, LOW);
          digitalWrite(pinRGBGreen, LOW);
          digitalWrite(pinRGBBlue, HIGH);
          //delay(1000);
          /*for(angle = 0; angle <=180; angle +=180){
               servomoto.write(angle);
               delay(1000);
          }*/
          servomoto.write(0);
          delay(1000);
          servomoto.write(180);
          delay(1000);

          
        }else if(distancia >= 200 && distancia <=299){
                lcd.clear();
                lcd.display();
                lcd.setCursor(0,0);
                lcd.print("Distancia :");
                lcd.setCursor(12,0);
                lcd.print(distancia); 
                //led a Magenta
                digitalWrite(pinRGBRed, LOW);
                digitalWrite(pinRGBGreen, HIGH);
                digitalWrite(pinRGBBlue, LOW);
                //delay(1000);
                for(angle = 0; angle <=180; angle +=30){
                    servomoto.write(angle);
                    delay(1000);
                }
              }else {
                lcd.noDisplay();
                //Led Apagado
                digitalWrite(pinRGBRed, HIGH);
                digitalWrite(pinRGBGreen, HIGH);
                digitalWrite(pinRGBBlue, HIGH);
                servomoto.write(90);
              }
}