#include <Servo.h>
// O robô possui dois motores DC independentes.
// cada motor tem dois fios
//BOTAO
#define BOTAO1 7
#define BOTAO2 8
//MOTOR DIREITA - pinos para controlar o motor
#define DIR_VEL  10 // pino 10 Arduino - controla a corrente para o motor da direita
#define DIR_PIN1  5 //pino 5 do Arduino
#define DIR_PIN2  6 //pino 6 do arduino

//MOTOR ESQUERDA - pinos para controlar o motor
#define ESQ_VEL   9 // pino 9 do Arduino - controla a corrente para o motor da esquerda
#define ESQ_PIN1  3 //pino 3 do Arduino
#define ESQ_PIN2  4 //pino 4 do arduino

#define ECHO  12 // Pino 12 do Arduino - eco (rx) - onda refletida pelo objeto é capturada.
#define TRIG  13 // Pino 13 Arduino - Trig (tx) - transmitido como ultrassom pelo ar.

#define NORMAL_VEL 255 // Valor constante para alimentar os motores. Valores de 0 à 255.
#define VEL_LADOS 255
#define PARAR 0
Servo mServo;

// A função Setup apenas é executada uma vez e
// normalmente utilizada para executar a inicialização
// de variáveis ou pinos por exemplo.
void setup()
{
  //Serial.begin(9600); // Permite a inicialização da comunicação Série - USB.
  setupMove(); // Chamada de função que seta os pinos dos motores com saída (OUTPUT). E prepara os motores colocando os pinos em LOW - baixo.
  setupUltrasonic(); //Seta os pinos do Sensor SR04 e prepara o pino trigger em LOW - baixo
  mServo.attach(11);
  mServo.write(90); //90 graus (para frente)
}

void loop()
{
  unsigned int esquerda; // leitura sonar na direção esquerda (de)
  unsigned int direita; // leitura sonar na direção direita (dd)
  unsigned int frente; // leitua do ssonar na direção frontal (d)
  mServo.write(90); // coloca a posição do servo no angulo 90º (para frente)
  delay(400);

   movePara();
   delay (150);

   //Botões
  if(BOTAO1, LOW)
  {
    moveTras(); // função para mover para trás
    delay(60);
    moveDireita(); // função para mover para direita
    delay(45); // 100 milissegundos
    movePara();
   delay (50);
  }
    if(BOTAO2, LOW)
  {
    moveTras(); // função para mover para trás
    delay(60);
    moveEsquerda(); // função para mover para esquerda
    delay(45); // 100 milissegundos
    movePara();
    delay (50);
  }

   //leituras
    mServo.write(0);// direita
    delay (400);
    direita = leDistancia(); // a variável d recebe o retorno da função leDistancia

    mServo.write(90);
    delay (300);
    frente = leDistancia();

    mServo.write(180);// direita
    delay (300);
    esquerda = leDistancia();

    mServo.write(90); // coloca a posição do servo no angulo 90º (para frente)
  delay(400);

  if (frente > 20)
  {
    moveFrente();// Função para mover motores para frente
   delay(400);
   moveTras();
   delay(80);
   movePara();
   delay (50);
  }

  //direita maior
  else if (frente<direita && esquerda<direita) {
        moveDireita();
        delay(85);
  }
  //esquerda maior
  else if (frente<esquerda && direita<esquerda){
        moveEsquerda();
        delay(85);
  }
  else if (frente<20 && direita<20 && esquerda <20)
{
  moveEsquerda();
      delay(180);
}

   movePara();
  delay (150);
   mServo.write(90); // coloca a posição do servo no angulo 90º (para frente)
   delay(500);



}

void moveFrente() // Implementação da função para acionar os motores para frente.
{
  analogWrite(ESQ_VEL, NORMAL_VEL); //Possibilita a utilização dos pinos PWM. analogWrite(ESQ_VEL, 255); Valores de 0 à 255.
  digitalWrite(ESQ_PIN1, HIGH); // digitalWrite passa como parâmetro o pino e as constantes HIGH/LOW
  digitalWrite(ESQ_PIN2, LOW);

  analogWrite(DIR_VEL, NORMAL_VEL);
  digitalWrite(DIR_PIN1, HIGH);
  digitalWrite(DIR_PIN2, LOW);
}

void moveTras() // Implementação da função para acionar os motores para trás.
{
  analogWrite(ESQ_VEL, NORMAL_VEL); //Possibilita a utilização dos pinos PWM. analogWrite(ESQ_VEL, 255); Valores de 0 à 255.
  digitalWrite(ESQ_PIN1, LOW); // digitalWrite passa como parâmetro o pino e as constantes HIGH/LOW
  digitalWrite(ESQ_PIN2, HIGH);

  analogWrite(DIR_VEL, NORMAL_VEL);
  digitalWrite(DIR_PIN1, LOW);
  digitalWrite(DIR_PIN2, HIGH);
}

void moveEsquerda() // Implementação da função para acionar os motores para esquerda.
{
  analogWrite(ESQ_VEL, VEL_LADOS); //Possibilita a utilização dos pinos PWM. analogWrite(ESQ_VEL, 255); Valores de 0 à 255.
  digitalWrite(ESQ_PIN1, HIGH); // digitalWrite passa como parâmetro o pino e as constantes HIGH/LOW
  digitalWrite(ESQ_PIN2, LOW);

  analogWrite(DIR_VEL, VEL_LADOS);
  digitalWrite(DIR_PIN1, LOW);
  digitalWrite(DIR_PIN2, HIGH);
}

void moveDireita() // Implementação da função para acionar os motores para direita.
{
  analogWrite(ESQ_VEL, VEL_LADOS); //Possibilita a utilização dos pinos PWM. analogWrite(ESQ_VEL, 255); Valores de 0 à 255.
  digitalWrite(ESQ_PIN1, LOW); // digitalWrite passa como parâmetro o pino e as constantes HIGH/LOW
  digitalWrite(ESQ_PIN2, HIGH);

  analogWrite(DIR_VEL, VEL_LADOS);
  digitalWrite(DIR_PIN1, HIGH);
  digitalWrite(DIR_PIN2, LOW);
}

void movePara() // Implementação da função para parar os motores.
{
  analogWrite(ESQ_VEL, PARAR); //Possibilita a utilização dos pinos PWM. analogWrite(ESQ_VEL, 0); Valores de 0 à 255.
  digitalWrite(ESQ_PIN1, LOW); // digitalWrite passa como parâmetro o pino e as constantes HIGH/LOW
  digitalWrite(ESQ_PIN2, LOW);

  analogWrite(DIR_VEL, PARAR);
  digitalWrite(DIR_PIN1, LOW);
  digitalWrite(DIR_PIN2, LOW);
}

void setupMove() // seta os pinos dos motores como saída
{
  pinMode(ESQ_VEL, OUTPUT);
  pinMode(ESQ_PIN1, OUTPUT);
  pinMode(ESQ_PIN2, OUTPUT);

  pinMode(DIR_VEL, OUTPUT);
  pinMode(DIR_PIN1, OUTPUT);
  pinMode(DIR_PIN2, OUTPUT);

  pinMode(BOTAO1, INPUT);
  pinMode(BOTAO2, INPUT);

  analogWrite(ESQ_VEL, PARAR); //Possibilita a utilização dos pinos PWM. analogWrite(ESQ_VEL, 0); Valores de 0 à 255.
  digitalWrite(ESQ_PIN1, LOW); // digitalWrite passa como parâmetro o pino e as constantes HIGH/LOW
  digitalWrite(ESQ_PIN2, LOW);

  analogWrite(DIR_VEL, PARAR);
  digitalWrite(DIR_PIN1, LOW);
  digitalWrite(DIR_PIN2, LOW);
}


unsigned int leDistancia() // Implementação da função do sensor SR04. Retorna a distância em cm.
{

  int duracao; // Variável para receber o cumprimento do pulso firme  em microssegundos e dividir por 58.
  digitalWrite(TRIG, LOW);//Deixa trigo embaixo
  delayMicroseconds(2);// por 2 uS - microssegundos
  digitalWrite(TRIG, HIGH); //Deixa trigo em estado alto, Um sinal de um pulso curto de 10uS é disparado
  delayMicroseconds(10); //O sensor envia uma ‘explosão’ de 8 ciclos de ultra-som em 40khz
  digitalWrite(TRIG, LOW); // diminui a sua linha de eco para LOW - baixo.

  duracao = pulseIn(ECHO, HIGH); // aumenta a sua linha de eco para alto.
                                // Lê um pulso (HIGH ou LOW) em um pinto. Retorna o comprimento do pulso em microssegundos.
                               //pulseIn retorna (tempo de alto nível * velocidade do som (340M/S)/2).
  return duracao/58; //retorna a divisão por 58 da duração para converter em cm.

}

void setupUltrasonic() // seta os pinos do sensor SR04 e prepara o trigger como LOW
{
  pinMode(TRIG, OUTPUT); // seta pin 13 como saída - transmissor - trigger
  pinMode(ECHO, INPUT); // seta pin 12 como entrada - receptor - echo

  digitalWrite(TRIG, LOW);
}