#define ledPin 13
#define motorDerecha1 4
#define motorDerecha2 5
#define motorIzquierda1 2
#define motorIzquierda2 3
#define echoPin 6
#define triggerPin 7

char estado = 'q';

void setup() {
  pinMode(ledPin, OUTPUT);
  pinMode(motorDerecha1, OUTPUT);
  pinMode(motorDerecha2, OUTPUT);
  pinMode(motorIzquierda1, OUTPUT);
  pinMode(motorIzquierda2, OUTPUT);
  pinMode(triggerPin, OUTPUT);
  pinMode(echoPin, INPUT);

  digitalWrite(ledPin, LOW);
  digitalWrite(motorDerecha1, LOW);
  digitalWrite(motorDerecha2, LOW);
  digitalWrite(motorIzquierda1, LOW);
  digitalWrite(motorIzquierda2, LOW);
  
  Serial.begin(9600);
}

void loop() {
 
  long tiempo;
  long distancia;
  
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

  tiempo = pulseIn(echoPin, HIGH);
  distancia = (tiempo/29)/3;
  Serial.println(distancia);
  Serial.println(estado);


  if(distancia <= 15) {
    digitalWrite(ledPin, HIGH);
  } else {
    digitalWrite(ledPin, LOW);
  }
  
  //adelante y evadir
  if(estado == 'w' && distancia <= 15) {
    digitalWrite(motorDerecha1, LOW);
    digitalWrite(motorDerecha2, HIGH);
    digitalWrite(motorIzquierda1, HIGH);
    digitalWrite(motorIzquierda2, LOW);
  }


  //adelante
  if(estado == 'w' && distancia > 15) {
    digitalWrite(motorDerecha1, HIGH);
    digitalWrite(motorDerecha2, LOW);
    digitalWrite(motorIzquierda1, HIGH);
    digitalWrite(motorIzquierda2, LOW);
   
  }

  //atras
  if(estado == 's') {
    digitalWrite(motorDerecha1, LOW);
    digitalWrite(motorDerecha2, HIGH);
    digitalWrite(motorIzquierda1, LOW);
    digitalWrite(motorIzquierda2, HIGH);
  }

  //stop
  if(estado == 'q') {
    digitalWrite(motorDerecha1, LOW);
    digitalWrite(motorDerecha2, LOW);
    digitalWrite(motorIzquierda1, LOW);
    digitalWrite(motorIzquierda2, LOW);
  }

  //derecha
  if(estado == 'd' && distancia <= 15) {
    digitalWrite(motorDerecha1, LOW);
    digitalWrite(motorDerecha2, HIGH);
    digitalWrite(motorIzquierda1, HIGH);
    digitalWrite(motorIzquierda2, LOW);
  }


  if(estado == 'd' && distancia > 15) {
    digitalWrite(motorDerecha1, LOW);
    digitalWrite(motorDerecha2, LOW);
    digitalWrite(motorIzquierda1, HIGH);
    digitalWrite(motorIzquierda2, LOW);
  }

  //izquierda
  if(estado == 'a' && distancia <= 15) {
    digitalWrite(motorDerecha1, HIGH);
    digitalWrite(motorDerecha2, LOW);
    digitalWrite(motorIzquierda1, LOW);
    digitalWrite(motorIzquierda2, HIGH);
  }

  if(estado == 'a' && distancia > 15) {
    digitalWrite(motorDerecha1, HIGH);
    digitalWrite(motorDerecha2, LOW);
    digitalWrite(motorIzquierda1, LOW);
    digitalWrite(motorIzquierda2, LOW);
  }
 
}


void serialEvent() {
    if(Serial.available() > 0 ){
      int  x = Serial.read();
      if(x>=97) {
        estado = (char) x;
      }
  }
}