/Programa prueba modulo bluetooth GP-GC021 o HC-06
//Probado con programas para android Blueterm y Amarino 2.0
//Probado con bluetooth portatil y software Octoplus Terminal

#include <AFMotor.h>
#include <Servo.h>
#include <Ultrasonic.h>

Ultrasonic ultrasonic(12,13); // (Trig PIN,Echo PIN)


AF_DCMotor motorLeft(1);
AF_DCMotor motorRight(2);
AF_DCMotor pito(3);
int potVelocidad = A0;            //potenciometro que varìa la velocida del motor
int velocidad;                 //variable velocidad del motor (0 a 255)
int random1, random2,random3;
boolean automatico=true;
int led1=52;
int led2=53;
//Servo servo1;
//int val=90;    // variable to read the value from the analog pin


void setup()
{
   //Configuracion de la velocidad del modulo 9600 por defecto, se puede cambiar
   //mediante comandos AT
   //Serial.begin(9600);
   pinMode(potVelocidad, INPUT);
   pinMode(led1, OUTPUT);
   pinMode(led2, OUTPUT);
    // turn on servo
 // servo1.attach(9);

 Serial.begin(9600);
 Serial.begin(9600);
}

void loop()
{

 int x=analogRead(potVelocidad);
 velocidad=map(x,0, 1023, 0, 255);

  motorLeft.setSpeed(velocidad);
  motorRight.setSpeed(velocidad);
  pito.setSpeed(250);
  //servo1.write(val);

  if(automatico==true)
 {
   digitalWrite(led1, HIGH);
      digitalWrite(led2, LOW);
   autoControl();  //delay(500);
 }
 else if(automatico==false)
  {
    digitalWrite(led2, HIGH);
        digitalWrite(led1, LOW);
    controlRemoto();
  }



}


 //***************************** AUTO CONTROL  *********************************
 void autoControl()
  {
       delay(200);
       random1= 2; //random(2000);
       random2= random(5000, 30000);
       random3= random(5000, 10000);
       Serial.println("si");
       switch(random1)
                {
                  case 1:
                  circulos(5000);
                  break;

                  case 2:
                  obstaculos(random3);
                  break;

                  case 3:
                  zigZag();
                  break;

                  default:
                  esquivar2();
                  break;
                }
                controlRemoto();
  }

//***************************  CONTROL REMOTO ***************************************
void controlRemoto()
  {
   //Mientras el puerto serie del modulo bluetooth esta disponible

   //Serial.println(ultrasonic.Ranging(CM));

   while (Serial.available())
   {

       //delay(100);
      //Guardamos en la variable dato el valor leido por el modulo bluetooth
       char dato= Serial.read();

       //Comprobamos el dato
         if(dato=='1')
           {
             motorLeft.run(FORWARD);
             motorRight.run(BACKWARD); automatico=false;
           }

         if(dato=='2') atras();
         if(dato=='3') izq();
         if(dato=='4') der();
         if(dato=='5') parar(300);
         if(dato=='6') pitar();
         if(dato=='9') {automatico=true; break;}
         if(dato=='0') parar(30);


       //delay(100);

   }
  }

// ********************** DESPLAZAMIENTOS DE JASGBOT **********************
void zigZag()
{
  //convierte numero de ciclos a segundos
  int aux=5;
  while(aux>0)
  {
    //controlRemoto();
  //adelante();  delay(1000);  parar(300);
  for(int x=0; x<4; x++)
   {
    izq();delay(100); parar(100);
    esquivar();
   }
  for(int x=0; x<4; x++)
  {
    esquivar();
    der();delay(100); parar(100);
  }
  aux--;
  }
}

void circulos(int time)
{

  //convierte numero de ciclos a segundos
  int aux=(time*(0.25));
  while(aux>0)
  {
      der();
   esquivar();
      aux--;
  }

   //der();
   //esquivar();
   //delay(6000);

  parar(300);
}

void obstaculos(int time)
{
 // controlRemoto();
  //convierte numero de ciclos a segundos
  int aux=(time*(0.25));
  while(aux>0)
  {
      adelante();
      aux--;
  }
}


//************************* FUNCIONES BASICAS  **********************
void parar(int time)
{
   motorLeft.run(RELEASE);
   motorRight.run(RELEASE);
   delay(time);
}

void adelante()
{
   motorLeft.run(FORWARD);
   motorRight.run(BACKWARD);
   esquivar();
}

void izq()
{
   motorLeft.run(RELEASE);
   motorRight.run(BACKWARD);
}

void der()
{
  motorLeft.run(FORWARD);
  motorRight.run(RELEASE);
}

void atras()
{
  motorLeft.run(BACKWARD);
  motorRight.run(FORWARD);
}

void pitar()
{
  pito.run(BACKWARD);
  delay(60);
  pito.run(RELEASE);
}

void esquivar()
{
  //
  if(ultrasonic.Ranging(CM)<10)
          {
            atras();
            delay(1000);
             izq();
             delay(500);
             parar(100);
          }
}

void esquivar2()
{
  if(ultrasonic.Ranging(CM)<30)
          {
            atras();
            delay(500);
             izq();
             delay(500);
             parar(10);
          }
}