/*********************************
Autor: David Lucas
Programa: Projeto carro robot 1.0
Data: 20/10/2010
Arduino: DUEMILANOVE
Inc. All Rights Reserved

IN1,IN2--Motor A(roda direita)
IN3,IN4--Motor B(roda esquerda)

L298N    Arduino    Code
 IN1-------11-------pinRF
 IN2-------10-------pinRB
 IN3-------9--------pinLF
 IN4-------6--------pinLB

IN1 IN2
1   0    Motor A no sentido anti-horário
0   1    Motor A sentido horário
1   1    stop

IN3 IN4
0   1    Motor B no sentido horário
1   0    Motor B no sentido anti-horário
1   1    stop

pinRF  pinRB  pinLF  pinLB
IN1    IN2    IN3    IN4   Car
1      0      0      1     advance
0      1      1      0     back
1      1      1      1     stop
1      0      1      1     Única roda vire à esquerda
1      1      0      1     Única roda para a direita
1      0      1      0     Roda dupla vire à esquerda
0      1      0      1     Roda dupla para a direita





    L = left
    R = right
    F = advance
    B = back
*/
#include <Servo.h>
int pinLB=6;     // volta esquerda
int pinLF=9;     // frente de esquerda

int pinRB=10;    // Volta direita
int pinRF=11;    // frente de esquerda

int inputPin = A0;  // ultrasom echo
int outputPin =A1;  // ultrasom trig

int Fspeedd = 0;      // distância da frente
int Rspeedd = 0;      // distância certa
int Lspeedd = 0;      // distância à esquerda
int directionn = 0;   // Determinar a direção de voltas de carro
Servo myservo;        //  myservo
int delay_time = 250; // Servo motor de direção estável

int Fgo = 8;         // avança
int Rgo = 6;         // vire à direita
int Lgo = 4;         // vire à esquerda
int Bgo = 2;         // volta

void setup()
{
  pinMode(pinLB,OUTPUT); // pin 6 (PWM)
  pinMode(pinLF,OUTPUT); // pin 9 (PWM)
  pinMode(pinRB,OUTPUT); // pin 10 (PWM)
  pinMode(pinRF,OUTPUT); // pin 11 (PWM)

  pinMode(inputPin, INPUT);    // Defini entrada pino de ultra-som
  pinMode(outputPin, OUTPUT);  // Defini o pino de saída de ultra-som

  myservo.attach(5);    // Defini o servo motor saída pin5 (PWM)
}
void advance(int a)     // avanço
    {                   //No ponto médio das duas rodas como referência
     digitalWrite(pinRB,LOW);  //avanço da roda direita
     digitalWrite(pinRF,HIGH);
     digitalWrite(pinLB,HIGH);  //avanço da roda esquerda
     digitalWrite(pinLF,LOW);
     delay(a);
    }
void right(int b)        //Vire à direita (única roda)
    {
     digitalWrite(pinRB,HIGH);   //Para Direita
     digitalWrite(pinRF,HIGH);
     digitalWrite(pinLB,HIGH);  //avanço esquerdo
     digitalWrite(pinLF,LOW);
     delay(b);
    }
void left(int c)         //Vire à esquerda (a única roda)
    {
     digitalWrite(pinRB,LOW); //avanço da roda direita
     digitalWrite(pinRF,HIGH);
     digitalWrite(pinLB,HIGH);   //para esquerda
     digitalWrite(pinLF,HIGH);
     delay(c);
    }
void turnR(int d)        //Vire à direita (rodado duplo)
    {
     digitalWrite(pinRB,HIGH);  //roda direita volta
     digitalWrite(pinRF,LOW);
     digitalWrite(pinLB,HIGH); //avanço da roda esquerda
     digitalWrite(pinLF,LOW);
     delay(d);
    }
void turnL(int e)        //Vire à esquerda (rodado duplo)
    {
     digitalWrite(pinRB,LOW);   //avanço da roda direita
     digitalWrite(pinRF,HIGH);
     digitalWrite(pinLB,LOW);   //roda esquerda volta
     digitalWrite(pinLF,HIGH);
     delay(e);
    }
void stopp(int f)         //Pare
    {
     digitalWrite(pinRB,HIGH);
     digitalWrite(pinRF,HIGH);
     digitalWrite(pinLB,HIGH);
     digitalWrite(pinLF,HIGH);
     delay(f);
    }
void back(int g)          //Volta
    {
     digitalWrite(pinRB,HIGH);  //roda direita volta
     digitalWrite(pinRF,LOW);
     digitalWrite(pinLB,LOW);  //roda esquerda volta
     digitalWrite(pinLF,HIGH);
     delay(g);
    }
void detection()        //Três ângulos de medição(2.90.178)
    {
     myservo.write(90); //medida de distância na frente
     delay(delay_time);      // Esperando o servo, motor estável
     ask_pin_F();            // Ler a distância da frente

     if(Fspeedd < 20)         // Se a distância for inferior a 20cm na frente
     {
         stopp(1);               // limpar a saída, parada do motor
         myservo.write(178);      //Meça a distância à esquerda
         delay(delay_time);
         ask_pin_L();

         myservo.write(2);     //distância medida certa
         delay(delay_time);
         ask_pin_R();

         if(Lspeedd > Rspeedd)   //comparar a distância de direita e esquerda
         {
            directionn = Lgo;      //Vire à esquerda
         }
      else {
         if(Lspeedd <= Rspeedd)   //se a distância for menor ou igual à distância no lado direito
         {
            directionn = Rgo;      //Vire à direita
         }
      }
      }
      else
      {
        directionn = Fgo;
       }
       myservo.write(90);
       delay(delay_time);
    }
void ask_pin_F()   // Medir a distância em frente
    {
      digitalWrite(outputPin, LOW);   //ultrasom 2us baixo nível
      delayMicroseconds(2);
      digitalWrite(outputPin, HIGH);  // ultrasom de alta transmissão 10us, há pelo menos 10us
      delayMicroseconds(11);
      digitalWrite(outputPin, LOW);    // ultrasom baixo nível
      float Fdistance = pulseIn(inputPin, HIGH);  //medir o tempo
      Fdistance= Fdistance/5.8/10;       // Tempo de distância （cm）
      Fspeedd = Fdistance;              //
    }
 void ask_pin_L()   //
    {
      delay(delay_time);
      digitalWrite(outputPin, LOW);   //
      delayMicroseconds(2);
      digitalWrite(outputPin, HIGH);  //
      delayMicroseconds(11);
      digitalWrite(outputPin, LOW);    //
      float Ldistance = pulseIn(inputPin, HIGH);  //
      Ldistance= Ldistance/5.8/10;       //
      Lspeedd = Ldistance;              //
    }
void ask_pin_R()   //
    {
      delay(delay_time);
      digitalWrite(outputPin, LOW);   //
      delayMicroseconds(2);
      digitalWrite(outputPin, HIGH);  //
      delayMicroseconds(11);
      digitalWrite(outputPin, LOW);    //
      float Rdistance = pulseIn(inputPin, HIGH);  //
      Rdistance= Rdistance/5.8/10;       //
      Rspeedd = Rdistance;              //
    }

void loop()
 {
  detection();        //Medir o ângulo e determinar o rumo a seguir
  if(directionn == 2)
  {
    back(600);
  }
  if(directionn == 6)
  {
    turnR(350);
    stopp(1);
  }
  if(directionn == 4)
  {
    turnL(350);
    stopp(1);
  }
  if(directionn == 8)
  {
    advance(10);
    ask_pin_F();
    if(Fspeedd < 20) stopp(1);
  }
 }