/*
Codigo base para robot RC, utilizando la placa Tapatio Motor Board
By: Miguel Angel Delgado López - miguel.delgado.lop@gmail.com
*/

//Velocidad con la que quieres que se mueva el robot
//debe ser un valor máximo de 255 y dependerá de lo motores que uses
int power=150;

//Pines necesario para controlar los diver Tapatio Driver
#define M01 6
#define M02 5
#define M03 10
#define M04 9

//Pines para los sensores P= Principal I= Izquierdo D= Derecho
#define sensP A0
#define sensI A3
#define sensD A7


#define led1  13

int     estrategia = 0;
boolean start       = true;
boolean inicio      = true;
int     umbral      = 900;

int tiempoReversa=.5; //en segundos




//Configuracion de los pines como salida e iniciar el Bluetooth
void setup() {
  Serial.begin(9600);
  pinMode(M01,OUTPUT);
  pinMode(M02,OUTPUT);
  pinMode(M03,OUTPUT);
  pinMode(M04,OUTPUT);
  //-----------SENSORES----------------
  pinMode(sensP,INPUT);
  pinMode(sensI,INPUT);
  pinMode(sensD,INPUT);

}

//Método principal que al recibir una letra hace que se mueva el robot RC
void loop() {

  if(!start){
    velocidad(0, 0);
    int x= leerSensores();
    Serial.println(x);
  }else{
    if(inicio){
      for(int x=0;x<30;x++){
        led(0);
        delay(50);
        led(1);
        delay(50);
      }
      inicio=false;
    }else{
      int sensores=leerSensores();
      if(sensores==0){
          velocidad(power,power);
          Serial.println("adelante");
      }else{
        if(sensores==1){
          switch(estrategia){
            case 0:
              //Frente por izquierda
              velocidad(power,-power);
              Serial.println("Frente Izquieda");    
              break;
            case 1:
              //Frente por la derecha
              velocidad(-power,power);
              Serial.println("Frente Derecha");
              break;
            case 2:
              //Atras por izquierda
              velocidad(-power,-power*.3);
              Serial.println("atras izquierda");
              break;  
            case 3:
              //atras por derecha
              velocidad(-power*.3,-power);
              Serial.println("atras derecha");
              break;
            }
          }
        if(sensores==2|| sensores==3){
          //case 1://se activo sensor izquierdo
          Serial.println("izquerdo");
          velocidad(-power,-power);
          delay(tiempoReversa*1000);
        }

        if(sensores==4|| sensores==5){
          //case 2://Se activo sensor derecho
          Serial.println("derecho");
          velocidad(-power,-power);
          delay(tiempoReversa*1000);
        }

        if(sensores==6||sensores==7){
          // case 3://se activaron los 2 sensores
          //reversa
          Serial.println("ambos");
          velocidad(-power,-power);
          delay(tiempoReversa*1000);
        }
      }         
    }
  }
}


  int leerSensores(){
  int v=0;
  v+=digitalRead(sensP);
  v+=analogRead(sensI)<umbral?2:0;
  v+=analogRead(sensD)<umbral?4:0;
  return v;
}

void led(bool l1){
  digitalWrite(led1,l1);
}


//Método que permite controlar la velocidad y sentido de giro a los motores
//El valor minimo es -255 y el máximo 255, Negativo hacia atrás y positivo hacia adelante
void velocidad(int izq, int der){
  if(izq>255){
    izq=255;
  }else{
    if(izq<-255){
      izq=-255;
    }
  }
  if(der>255){
    der=255;
  }else{
    if(der<-255){
      der=-255;
    }
  }
  if(izq>0){
    analogWrite(M01,izq);
    analogWrite(M02, 0);
  }else{
    analogWrite(M01, 0);
    analogWrite(M02, abs(izq));
  }
  if(der>0){
    analogWrite(M03,der);
    analogWrite(M04, 0);
  }else{
    analogWrite(M03, 0);
    analogWrite(M04, abs(der));
  }
}