LineFollowUltraSonic

const int trig = 2;
const int echo = 3;

int r1 = 11;
int r2 = 13;

int m1 = 4;
int m2 = 5;
int m3 = 6;
int m4 = 7;


int linha1 = 0;
int linha2 = 0;

void setup()
{
  Serial.begin (9600);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);

  pinMode(r2, INPUT);
  pinMode(r1, INPUT);



  pinMode(m1, OUTPUT);
  pinMode(m2, OUTPUT);
  pinMode(m3, OUTPUT);
  pinMode(m4, OUTPUT);

}
void loop()
{
  long duracao;
  long distancia;
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  duracao = pulseIn(echo, HIGH);
  distancia = duracao / 58;
  Serial.print(distancia);
  Serial.println(" cm");
  delay(5);
  if (distancia<=12)//PUMM!! TEM OBSTÁCULO!
  {
    ler();
    MotorParar();
  }
  else
  {

    ler();

    if(linha1==1)
    {
      MotorEsquerda();
    }

    if(linha2==1)
    {
      MotorDireita();
    }


    if((linha1==0) && (linha2==0))
    {
      MotorParar();
      delay(500);
      MotorDireita();
      delay(2000);
    }
    if((linha1==1) && (linha2==1))
    {
      MotorParar();
      delay(500);
      MotorEsquerda();
      delay(2000);
    }
    delay(2);
  }
}

void ler()
{
  linha1 = digitalRead(r1);
  linha2 = digitalRead(r2);
  Serial.print("linha1: ");
  Serial.println(linha1);
  Serial.print("linha2: ");
  Serial.println(linha2);
  return;
}

void MotorDireita()
{
 digitalWrite(m1, HIGH);
 digitalWrite(m4, LOW);
 digitalWrite(m3, LOW);
 digitalWrite(m2, LOW);
 delay(5);
}

void MotorEsquerda()
{
 digitalWrite(m3, HIGH);
 digitalWrite(m2, LOW);
 digitalWrite(m1, LOW);
 digitalWrite(m4, LOW);
 delay(5);
}

void MotorFrente()
{
 digitalWrite(m2, LOW);
 digitalWrite(m4, LOW);
 analogWrite(m3, 50);
 analogWrite(m1, 50);
}

void MotorParar()
{
 digitalWrite(m3, LOW);
 digitalWrite(m1, LOW);
 digitalWrite(m2, LOW);
 digitalWrite(m4, LOW);
  delay(5);
}