//Sensores
#define S1 A0
#define S2 A1
#define S3 A2
#define trig 12
#define echo 11
//Motor
#define H1 5
#define H2 6
#define H3 9
#define H4 10

//void
void trigPuls ();

//matriz
int VS1 = 0;
int VS2 = 0;
int VS3 = 0;
float pulse;
float dist_cm;



void setup() {
  //
  pinMode(S1, INPUT);
  pinMode(S2, INPUT);
  pinMode(S3, INPUT);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  //
  pinMode(H1, OUTPUT);
  pinMode(H2, OUTPUT);
  pinMode(H3, OUTPUT);
  pinMode(H4, OUTPUT);
  //
  Serial.begin(9600);
  digitalWrite(trig, LOW);
}
void trigPuls()
{
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
}

void loop()
{
  trigPuls();
  pulse = pulseIn(echo, HIGH);
  dist_cm = pulse/58.82;

  Serial.println(dist_cm);
  delay(500);

  VS1 = analogRead(S1);
  VS2 = analogRead(S2);
  VS3 = analogRead(S3);
  //000
    if((VS1 > 500) && (VS2 > 500) && (VS3 > 500)){
    analogWrite(H1, 255);
    analogWrite(H4, 255);
    analogWrite(H2, 0);
    analogWrite(H3, 0);

    delay(500);

  }
  //Reto 010
  if((VS1 > 500) && (VS2 < 500) && (VS3 > 500)){
    analogWrite(H1, 255);
    analogWrite(H4, 255);
    analogWrite(H2, 0);
    analogWrite(H3, 0);
    delay(500);
  }
  //Direita 011
    if((VS1 > 500) && (VS2 < 500) && (VS3 < 500)){
    analogWrite(H1, 255);
    analogWrite(H4, 63);
    analogWrite(H2, 0);
    analogWrite(H3, 0);
    delay(2000);
  }
  //Esquerda 110
  if((VS1 < 500) && (VS2 < 500) && (VS3 > 500)){
    analogWrite(H1, 0);
    analogWrite(H4, 255);
    analogWrite(H2, 0);
    analogWrite(H3, 0);
    Serial.println("m");
    delay(2000);
  }
  //Ajustar p/ esquerda 100
  if((VS1 < 500) && (VS2 > 500) && (VS3 > 500)){
    analogWrite(H1, 63);
    analogWrite(H4, 255);
    analogWrite(H2, 0);
    analogWrite(H3, 0);
    delay(500);
  }
  //Ajustar p/ direita 001
    if((VS1 > 500) && (VS2 > 500) && (VS3 < 500)){
    analogWrite(H1, 255);
    analogWrite(H4, 63);
    analogWrite(H2, 0);
    analogWrite(H3, 0);
    delay(500);
  }
}