URA L-BOT

#include <Servo.h>

#define TAM 50
#define TRIG 3
#define ECHO 4
#define BOTAO A5
#define BIP 5
#define SERVO1 13
#define SERVO4 2
#define DMOTOR 500
#define LINHA1 6
#define LINHA2 A3

//Inicialização das variaveis de controle.
  //Motor:
  Servo motor1,motor2;
  //Geral:
  char option , comando;
  int flag = 0;
  bool bipgeral=false;
  //Variaveis do modo remoto:
  int seg=0,but=0;
  bool fast;
  char aux;
  bool flag4;
  //Variaveis do modo ultrassom
  int te=0,td=0,t;

//Função de Espera (aprimorada para se receber comando de parada durante a espera).
void delay2(int tempo){
  for(int i=0;i<tempo;i++){
    if(Serial.available() && flag4==0){
      if (option == 'c'){
        comando = Serial.read();
        if(comando == 'p' || comando == 's'){
          break;
        }
      }else{
        if(Serial.read() == 's'){
          flag++;
          break;
        }
      }
    }
    delay(1);
  }
}

//Função de cálculo da sistância.
int dist(){
  long distance,duration;
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  duration = pulseIn(ECHO, HIGH);
  distance = duration/29/2;
  return distance ;
}

//Função de Configuração do Arduino.
void setup(){
  Serial.begin(9600);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(BOTAO,INPUT);
  pinMode(BIP,OUTPUT);
  pinMode(LINHA1,INPUT);
  pinMode(LINHA2,INPUT);
  digitalWrite(BOTAO,HIGH);
  for(int i=0;i<TAM;i++){
    v[i]='p'; n[i]='p';
  }
  while(!Serial.available());
}

//Função de Repetição do Arduino.
void loop(){

  //Inicialização da variaveis de controle.
  option = 'p'; comando='p'; flag = 0; flag4 = 0;
  if(bipgeral==false){
    digitalWrite(BIP,HIGH);
    delay(200);
    digitalWrite(BIP,LOW);
    bipgeral=true;
  }
  //Leitura do comando (Pagina Escolha do Modo) vindo do bluetooth.
  if(Serial.available()){
    option = Serial.read();
  }

  //                                                                                               Modo Remoto
  if(option=='c'){
    digitalWrite(BIP,HIGH);
    delay2(200);
    digitalWrite(BIP,LOW);
    //Inicializando variaveis
    fast=false;

    //Laço de repetição do Modo Remoto:
    while(comando!='s'){
      //Recebe Comando do bluetooth (Verificando se o comando for o mesmo do anterior) (Pagina do Modo Remoto)
      if(Serial.available()){
        comando = Serial.read();
        if(aux!=comando && fast){
          motor1.detach();
          motor2.detach();
          delay2(DMOTOR);
          if(comando=='s'){
            break;
          }
        }
        aux= comando;
        flag4=0;
      }

      //Para frente sem segurança
      if(comando=='y'){
        fast=true;
        motor1.attach(SERVO1);
        motor2.attach(SERVO4);
        motor1.write(180);
        motor2.write(0);
      }

      //Para frente com segurança:
      seg=11;
      if(comando=='f' && seg>10){
        fast=true;
        seg=dist();
        delay2(50);
        if(comando=='s') break;
        seg=seg+dist();
        seg = seg/2;
        if(seg>0 && seg<=10 ){
          motor1.detach();
          motor2.detach();
          fast=false;
          flag4 = 1;
        }else{
          motor1.attach(SERVO1);
          motor2.attach(SERVO4);
          motor1.write(180);
          motor2.write(0);
          flag4 = 0;
          fast=true;
        }
      }

      //Para tras com segurança:
      but=0;
      if(comando=='t' && but==0 ){
        fast=true;
        but=digitalRead(BOTAO);
        if(but==0){
          fast = false;
          motor1.detach();
          motor2.detach();
          flag4 = 1;
        }
        else{
          motor1.attach(SERVO1);
          motor2.attach(SERVO4);
          motor1.write(0);
          motor2.write(180);
          flag4 = 0;
          fast=true;
        }
      }

      //Para tras sem segurança:
      if(comando=='b'){
        fast=true;
        motor1.attach(SERVO1);
        motor2.attach(SERVO4);
        motor1.write(0);
        motor2.write(180);
      }

      // Para esquerda:
      if(comando=='e'){
        fast=true;
        motor1.attach(SERVO1);
        motor2.attach(SERVO4);
        motor1.write(120);
        motor2.write(120);
      }

      //Para direita:
      if(comando=='d'){
        fast=true;
        motor1.attach(SERVO1);
        motor2.attach(SERVO4);
        motor1.write(50);
        motor2.write(50);
      }

      //Pára:
      if(comando=='p'){
        fast=false;
        motor1.detach();
        motor2.detach();
      }
    }
    bipgeral=false;
  }

  flag4 = 0;
  motor1.detach();
  motor2.detach();

  //                                                                                            Modo Ultrassom:
  if(option=='u'){
    digitalWrite(BIP,HIGH);
    delay2(200);
    digitalWrite(BIP,LOW);
    flag=0;

    //Laço de repetição do Modo Ultrassom
    while(comando!='s'){
      //Ler comando vindo por bluetooth ( neste modo, so receberemos comando se for para sair):
      if(Serial.available()){
        comando = Serial.read();
      }
      if(comando == 's'){
        break;
      }

      //Liga para frente:
      t=30;
      motor1.attach(SERVO1);
      motor2.attach(SERVO4);
      motor1.write(180);
      motor2.write(0);
      delay2(DMOTOR);
      //OBS: sempre que houver este if, estamos checando se o usuário pediu para parar o processo:
      if(flag!=0){
        comando='s';
        break;
      }

      //Checa se está a uma distancia menor que 23cm de algum objeto (caso verdadeiro: ele irá calcular a distancias para os dois lados para depois decidir para que lado irá.)
      //                                                             (caso falso: continua indo para frente.)
      t=dist();
      if(t>0 && t<20){
        //Gira para esquerda (aproximadamente 90 graus)
        motor1.write(40);
        motor2.write(40);
        delay2(600);
        if(flag!=0){
          comando='s';
          break;
        }
        //Delay Motor
        motor1.detach();
        motor2.detach();
        delay2(DMOTOR);
        if(flag!=0){
          comando='s';
          break;
        }
        //Calcula a distancia para a esquerda:
        do{
          te=dist();
          delay2(2);
          if(flag!=0){
            comando='s';
            break;
          }
        }while(te<0);
        if(flag!=0){
          comando='s';
          break;
        }

        //Gira para direita (aproximadamente 180 graus):
        motor1.attach(SERVO1);
        motor2.attach(SERVO4);
        motor1.write(140);
        motor2.write(140);
        delay2(1000);
        if(flag!=0){
          comando='s';
          break;
        }
        //Delay Motor
        motor1.detach();
        motor2.detach();
        delay2(DMOTOR);
        if(flag!=0){
          comando='s';
          break;
        }

        //Calcula a distancia para a direita:
        do{
          td=dist();
          delay2(3);
          if(flag!=0){
            comando='s';
            break;
          }
        }while(td<0);
        if(flag!=0){
          comando=='s';
          break;
        }


        //Checa se a distancia para esquerda é maior que a distancia para direita (Caso verdadeiro: ele deve girar 180 graus para a esquerda para depois seguir em frente, já que ele esta virado para a esquerda do ponto 'inicial'.
        //                                                                        (Caso falso: ele simplismente segue em frente , já que ele já está virado para a direita do ponto 'inicial'.
        if(te>td){
          //Caso verdadeiro:
          //Gira para a esquerda (aproximadamente 180 graus)
          motor1.attach(SERVO1);
          motor2.attach(SERVO4);
          motor1.write(40);
          motor2.write(40);
          delay2(1000);
          if(flag!=0){
            comando='s';
            break;
          }
          //Delay Motor
          motor1.detach();
          motor2.detach();
          delay2(DMOTOR);
          if(flag!=0){
            comando='s';
            break;
          }
        }
        //OBS: O caso contrário será executado no começo do laço de repetição do Modo Ultrassom, já que no inicio ele sempre 'liga' pra frente.
      }
    }
    bipgeral=false;
  }

  motor1.detach();
  motor2.detach();

  //                                                                                  Modo de Calibração
  if(option=='r'){
    digitalWrite(BIP,HIGH);
    delay2(200);
    digitalWrite(BIP,LOW);

    motor1.attach(SERVO1);
    motor2.attach(SERVO4);
    motor1.write(90);
    motor2.write(90);
    while(comando!='s'){
      if(Serial.available()){
        comando = Serial.read();
      }
    }
    bipgeral=false;
  }

  motor1.detach();
  motor2.detach();

}