Robot

// ************************************************************************************
// * Robo comanda via bluetooth que evita obstculos - Robotica Industrial - 2015     *
// * Arduino >> Bluetooth                                                             *
// * D11   >>>  Rx                                                                    *
// * D12   >>>  Tx                                                                    *
// * Servo RS2                                                                        *
// * D2                                                                               *
// * Sonar                                                                            *
// * D3 - trigger                                                                     *
// * D2 - echo                                                                        *
// ************************************************************************************

#include <SoftwareSerial.h>           // importa a biblioteca "serial software"
#include <Servo.h>                    // importa a biblioteca "servo"
Servo servo;
const int echo = 2;
const int trigger = 3;
int mode = 0;
long duracao;
long distancia = 0;
long distancia_direita = 0;
long distancia_esquerda = 0;
long distancia_frente = 0;

SoftwareSerial SerialSoft(12, 11);    // Define as potas RX, TX para o bluetooth
int ledPin = 13;                      // led on D13
String readString;

// ************************************************************************************
// * Setup                                                                            *
// ************************************************************************************

void setup() {
  // Sensor HC-SR05
  pinMode(trigger, OUTPUT);
  pinMode(echo, INPUT);
  
  // motor direito
  pinMode(5, OUTPUT);                 // Habilita 
  pinMode(6, OUTPUT);                 // Frente
  pinMode(7, OUTPUT);                 // Tras
  //motor esquerdo
  pinMode(8, OUTPUT);                 // Habilita
  pinMode(9, OUTPUT);                 // Frente
  pinMode(10, OUTPUT);                // Tras
  
  pinMode(ledPin,OUTPUT);             // Configura a porta d13 como saida, Led
  
  servo.attach(4);                    // Define a porta para o servo, D2
  servo.write(90);                    // Centrar o servo
  
  Serial.begin(9600);                 // Inicializa comunicao serie
  SerialSoft.begin(9600);             // Inicializa a porta srie por software
}

// ************************************************************************************
// * Loop                                                                             *
// ************************************************************************************

void loop() {
  
  while (SerialSoft.available()) {
    delay(3);  
    char c = SerialSoft.read();
    readString += c; 
  }
  if (readString.length() >0) {
    SerialSoft.println(readString);
    if (readString == "m") { mode = 1; parar();}
    if (readString == "n") { mode = 2; }
    if (readString == "l") { digitalWrite(ledPin, HIGH); }
    if (readString == "r") { digitalWrite(ledPin, LOW);  }
    if (readString == "f" && mode == 1) { frente(); }
    if (readString == "t" && mode == 1) { tras(); }
    if (readString == "e" && mode == 1) { esquerda(); }
    if (readString == "d" && mode == 1) { direita(); }
    if (readString == "p") { parar(); }
    readString="";
  }

  distancia_frente = medir_distancia();
  Serial.println(distancia_frente);
  
  if (mode == 0){
    digitalWrite(ledPin, HIGH);
    delay(100);
    digitalWrite(ledPin, LOW);
  }
  
  if (mode == 1){
    if (distancia_frente < 10){
      parar();
      delay(250);
      tras();
      delay(1000);
      parar();
    }
  }
  
  if(mode == 2){
   //Serial.println("Modo auto");
   frente();
    if (distancia_frente < 10){
      parar();
      delay(250);
      tras();
      delay(1000);
      parar();
      servo.write(0);                 // Vira o servo para 0 graus (esquerda)
      delay(500);
    
      distancia_direita = medir_distancia();
    
      servo.write(180);               // virar o servo para 180 graus (Direita)
      delay(500);
      
      distancia_esquerda = medir_distancia();  
    
      servo.write(90);                // Centrar o servo
      delay(500); 
  
      if (distancia_direita > distancia_esquerda)
        {
        direita();
        //Serial.println("Vira a direita");
        delay(1500);
        }
    
      if (distancia_direita < distancia_esquerda)
        {
        esquerda();
        //Serial.println("Vira a esquerda");
        delay(1500);
        } 
      
      if (distancia_direita <= 14 &&  distancia_esquerda <=14 &&  distancia_frente <=14 )
        {
        tras();
        //Serial.println("andar para tras");
        delay(1500);
        }
    }
  }
}

// ************************************************************************************
// * Funo medir distncia                                                           *
// ************************************************************************************

long medir_distancia()

{
  digitalWrite(trigger, LOW);
  delayMicroseconds(2);
  digitalWrite(trigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigger, LOW);
  
  duracao = pulseIn(echo, HIGH);
  distancia = duracao/58;                   // converte para cm
  
  delay(100);
  return distancia;
  
  }
  
void tras() {
  // motor direito
  digitalWrite(5, HIGH);
  digitalWrite(6, LOW);
  digitalWrite(7, HIGH);
  //motor esquerdo
  digitalWrite(8, HIGH);
  digitalWrite(9, LOW);
  digitalWrite(10, HIGH);
}

void frente() {
  // motor direito
  digitalWrite(5, HIGH);
  digitalWrite(6, HIGH);
  digitalWrite(7, LOW);
  //motor esquerdo
  digitalWrite(8, HIGH);
  digitalWrite(9, HIGH);
  digitalWrite(10, LOW);
}

void direita() {
  // motor direito
  digitalWrite(5, HIGH);
  digitalWrite(6, LOW);
  digitalWrite(7, HIGH);
  //motor esquerdo
  digitalWrite(8, HIGH);
  digitalWrite(9, HIGH);
  digitalWrite(10, LOW);
}

void esquerda() {
  // motor direito
  digitalWrite(5, HIGH);
  digitalWrite(6, HIGH);
  digitalWrite(7, LOW);
  //motor esquerdo
  digitalWrite(8, HIGH);
  digitalWrite(9, LOW);
  digitalWrite(10, HIGH);
}

void parar() {
  // motor direito
  digitalWrite(5, LOW);
  digitalWrite(6, LOW);
  digitalWrite(7, LOW);
  //motor esquerdo
  digitalWrite(8, LOW);
  digitalWrite(9, LOW);
  digitalWrite(10, LOW);
}