 /* 

  Adaptación de control de silla de ruedas.
   
  Movimientos de motores a través de joystick y a través del móvil usando bluetooh.  
    
    
    
  Creado en 2020.
  Por David Flores Román.
  Modificado el 29 de Septiembre de 2020.
  
  */
#include <Robojax_L298N_DC_motor.h> //libreria para control de motores

//librerias para bluetooth
#define CUSTOM_SETTINGS
#define INCLUDE_GAMEPAD_MODULE
#include <DabbleESP32.h>

//Definicion de variables para los motores
/////////////////////////////////////////////////////////////////////////////////////////////

// motor 1 
#define CHA 0 //define el canal 0
#define ENA 19 //pin que controla la velocidad del motor1
#define IN1 18 //pines que controlan la dirección del motor 1
#define IN2 5 //

//motor 2 
#define IN3 17//pines que controlan la dirección del motor 2
#define IN4 16//
#define ENB 4 //pin que controla la velocidad del motor2
#define CHB 1 //define el canal 1

//control de direcciones, deben tener este nombre ya que así viene en la librería
const int CCW = 2; 
const int CW  = 1; 

//define los motores 1 y 2
#define motor1 1 
#define motor2 2 


Robojax_L298N_DC_motor robot(IN1, IN2, ENA, CHA, IN3, IN4, ENB, CHB, true);//inicializa los dos motores a la vez, listos para usar las funciones de la libreria.

//variables para mostrar indicador de batería a través de LEDs
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int r=0;//variable que almacena el valor leído por el pin 34
float R = 0; //variable que almacena el valor de voltaje de la batería.
int au=0; //variable auxiliar para almacenar el valor de r


//defición de pines para LEDs
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const int ledPIN1 = 23;//led verde
const int ledPIN2 = 22;//led amarillo
const int ledPIN3 = 21; //led rojo
const int ledPIN4 = 0; //led verde
const int ledPIN5 = 2; //led verde

//boton para encender los LEDs

const int F_boton=13; //pin que suministra una corriente necesaria para ser leida por otros pines para saber si se ha pulsado un botón u otro
const int boton = 15; // pin de lectura para saber si se ha pulsado el boton para encender los leds.

bool aux=0;//variable auxiliar para el botón.
 

//Variables para joystick
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int valorX = 0; // variable de lectura de eje X
int valorY = 0; // variable de lectura de eje Y
int pinJX = 35; // eje X conectado al pin analogico 35
int pinJY = 32; // eje Y conectado al pin analogico 32


int const inf=2000; //limite inferior del joystick
int const sup=2100; //limite superior del joystick


int X = 0; //se almacena el cambio de valores del joystick de 0 a 4095 en -2000 hasta 2100
int Y = 0; //se almacena el cambio de valores del joystick de 0 a 4095 en -2000 hasta 2100
float auxi=0;//variable auxiliar

int angulo; //variable utilizada en la función angulos
int distan; //variable utilizada en la función distancia
int vel; //variable utilizada en la función velocidad
int v;  //variable que almacena el valor devuelto por la función velocidad 

int ac; //variable para cambiar la potencia de los motores

//variables para botones de velocidad
//////////////////////////////////////////////////////////////////////////////////

const int PINjoy1 = 33; //almacena boton para velocidad 1 en el pin 33
const int PINjoy2 =25; //almacena boton para velocidad 1 en el pin 25
bool b=0; //variable que cambia de un boton a otro
int joy1=0; //boton velocidad 1
int joy2 =0; //boton velocidad 2


void setup() {

 Serial.begin(115200);
   
Dabble.begin("MyEsp32");       //nombre de dispositivo bluetooth
  robot.begin();               //inicia los motores
  
  

  /////////////////////////////////////////////////////////////////////////////
  pinMode(ledPIN1 , OUTPUT);  //definir pin como salida
  pinMode(ledPIN2 , OUTPUT);
  pinMode(ledPIN3 , OUTPUT);
  pinMode(ledPIN4 , OUTPUT);
  pinMode(ledPIN5 , OUTPUT);
  pinMode(F_boton , OUTPUT);
  
  pinMode(boton,INPUT);//definir pin como entrada
  pinMode(PINjoy1,INPUT);
  pinMode(PINjoy2,INPUT);
 
}


void loop() {
  
Dabble.processInput(); 




digitalWrite(F_boton , HIGH); //pone e HIGH el valor del pin 13 para saber si se ha pulsado algún botón


//llamadas joystick
/////////////////////////////////////////////////////////////////////////////////////////////////
valorX = analogRead ( pinJX); // lectura eje X
valorY = analogRead ( pinJY); // lectura eje Y


//codigo para cambiar los valores de X e Y de -2000 a 2100
if(valorX >= sup){
   X=valorX-sup;
}

else if(valorX<=inf){
  X=valorX-inf;
}
else 
X=0;

if(valorY >= sup){
   Y=valorY-sup;
}

else if(valorY<=inf){
  Y=valorY-inf;
}
else Y=0;

///////////////////////////////////////////////////////////////////////////////////////////////////

//Serial.print("ejeX;  "); Prueba de testeo por pantalla del ordenador
//Serial.print(valorX);
//Serial.print("  ejeY;  ");
//Serial.print(valorY);
//
//
//Serial.print ("angulo: ");
//Serial.print(angulos (X,Y));

v=velocidad(distancia(X,Y)); //llamada a la función velocidad que a su vez llama a la función distancia para saber los distintos puntos donde va cambiando la velocidad

//Serial.print ("   velocidad: ");
//Serial.println(v);

//lectura de botones de velocidad
joy1=digitalRead(PINjoy1);
joy2=digitalRead(PINjoy2);

//codigo para cambiar de un boton a otro almacenando el valor en b
if(joy2==HIGH){
b=1;
}



if(joy1==HIGH){
b=0;
}


//codigo para poner distintas velocidades seún el boton pulsado
if(b==0){
if(v==1){
  ac=50;
}
if(v==2){
  ac=60;
}
if(v==3){
ac=70;  
}
} 

if(b==1){
if(v==1){
  ac=80;
}
if(v==2){
  ac=90;
}
if(v==3){
ac=100;  
}
} 



//codigo para los movimientos de los motores con el joystick

//up
if(angulos (X,Y)>=60 && angulos (X,Y)<=120 && v>0 ){
robot.rotate(motor1, ac, CCW);
robot.rotate(motor2, ac, CCW);
}

//back
if(angulos (X,Y)>=240 && angulos (X,Y)<=300 && v>0){
robot.rotate(motor1, ac, CW);
robot.rotate(motor2, ac, CW);
}

//left
if(angulos (X,Y)>=145 && angulos (X,Y)<=215 && v>0 ){
  robot.rotate(motor1, ac, CW);
robot.rotate(motor2, ac, CCW);

}

//right
if(((angulos (X,Y)>0 && angulos (X,Y)<=35) || (angulos (X,Y)>=325 && angulos (X,Y)<360)) && v>0) {

robot.rotate(motor1, ac, CCW);
robot.rotate(motor2, ac, CW);
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////7


aux=digitalRead(boton);//lectura del pin 15

//divisor de tensión para saber voltaje de la batería

r=analogRead(34);
au=r;
R=(au*0.003224); // como el valor maximo que obtiene ESP32 es 4095 (3,3v) se hace una conversión para pasar el resultado a voltios y se multiplica por 4 que es valor 
                // que se obtiene del divisor de tensión con resistencias de 3k y 1k  

      
//Movimientos motores bluetooth
////////////////////////////////////////////////////////////////////////////////////////////////////
   
//Motor A hacia adelante y Motor B hacia adelante
//////////////////////////////////////////////////////////////////////////////////////////////////////
   if (GamePad.isUpPressed()==1 && GamePad.isLeftPressed()==0 && GamePad.isRightPressed()==0 && GamePad.isDownPressed()==0 && v==0) {

robot.rotate(motor1, 50, CCW);
delay(10);
robot.rotate(motor2, 50, CCW);
   }  






//Motor A hacia atras y Motor B hacia atras
/////////////////////////////////////////////////////////////////////////////////////////////////////

   if (GamePad.isDownPressed()==1  && GamePad.isUpPressed()==0 && GamePad.isLeftPressed()==0 && GamePad.isRightPressed()==0 && v==0){

robot.rotate(motor1, 50, CW);
delay(10);
robot.rotate(motor2, 50, CW);
  }  






//Los dos motores apagados 
/////////////////////////////////////////////////////////////////////////////////////////////////
if (GamePad.isDownPressed()==0 && GamePad.isUpPressed()==0 && GamePad.isLeftPressed()==0 && GamePad.isRightPressed()==0 && v==0 ) {

  robot.brake(1);
  delay(10);
  robot.brake(2); 

}

//Girar a la izquierda
/////////////////////////////////////////////////////////////////////////////////////////////////////
if (GamePad.isLeftPressed ()==1 && GamePad.isDownPressed()==0 && GamePad.isUpPressed()==0  && GamePad.isRightPressed()==0  && v==0){
robot.rotate(motor1, 50, CW); 
 delay(10);
robot.rotate(motor2, 50, CCW);
}




//Girar a la derecha
//////////////////////////////////////////////////////////////////////////////////////////////////////

if (GamePad.isLeftPressed ()==0 && GamePad.isDownPressed()==0 && GamePad.isUpPressed()==0  && GamePad.isRightPressed()==1 && v==0){

  robot.rotate(motor1, 50, CCW);
 delay(10);
robot.rotate(motor2, 50, CW);

}


//LEDs
//////////////////////////////////////////////////////////////////////////////////////////////////////

leds(aux, R); //llamada a la función leds 

}

//Funciones
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////

//Funcion leds
///////////////////////////////////////////////////////////////////
void leds(int aux, int r){ //utiliza un auxiliar para el boton y la variable R para apagar los leds en función del voltage de la bateria

if(aux==1){
         digitalWrite(ledPIN1 , LOW); 
         digitalWrite(ledPIN2 , LOW); 
         digitalWrite(ledPIN3 , LOW);  
         digitalWrite(ledPIN4 , LOW);  
         digitalWrite(ledPIN5 , LOW);
        

if(r>=10 ){
  
     digitalWrite(ledPIN1 , HIGH);  
     digitalWrite(ledPIN2 , HIGH); 
     digitalWrite(ledPIN3 , HIGH);  
     digitalWrite(ledPIN4 , HIGH);  
     digitalWrite(ledPIN5 , HIGH);  
      
    delay(2000);

  }
  
  if(r>=8 && r<10 ){
     digitalWrite(ledPIN1 , HIGH); 
     digitalWrite(ledPIN2 , HIGH);  
     digitalWrite(ledPIN3 , HIGH);  
     digitalWrite(ledPIN4 , HIGH);  
     digitalWrite(ledPIN5 , LOW);  
     
            delay(2000);
 
  }

  if(r<=6 && r<8 ){

      digitalWrite(ledPIN1 , HIGH); 
      digitalWrite(ledPIN2 , HIGH); 
      digitalWrite(ledPIN3 , HIGH);
      digitalWrite(ledPIN4 , LOW);
      digitalWrite(ledPIN5 , LOW);     
    delay(2000);
  }

 if(r<=3 && r<6 ){

      digitalWrite(ledPIN1 , LOW); 
      digitalWrite(ledPIN2 , HIGH); 
      digitalWrite(ledPIN3 , HIGH);
      digitalWrite(ledPIN4 , LOW);
      digitalWrite(ledPIN5 , LOW);     
    delay(2000);
  }

 if(r<3){

      digitalWrite(ledPIN1 , LOW); 
      digitalWrite(ledPIN2 , LOW); 
      digitalWrite(ledPIN3 , HIGH);
      digitalWrite(ledPIN4 , LOW);
      digitalWrite(ledPIN5 , LOW);     
    delay(2000);
  }
  
 
  }
else
if(aux==0){
       digitalWrite(ledPIN1 , LOW); 
       digitalWrite(ledPIN2 , LOW); 
       digitalWrite(ledPIN3 , LOW);
       digitalWrite(ledPIN4 , LOW);
       digitalWrite(ledPIN5 , LOW);   
}

  
}

//Funcion angulos
int angulos (int a,int b){ //devuelve el angulo de 0 a 359 en el que se encuentra el joystick calculando su tangente.

auxi=atan2(a,b);
angulo=(auxi*180)/PI;

if(angulo<0){
angulo=angulo+360;
}
  return angulo;
}
//Funcion distancia
int distancia(int c,int d){ // devuelve la distancia desde el centro del joystick usando pitagoras.

distan=sqrt(pow(c,2)+pow(d,2));

 return distan; 
}

//Funcion velocidad
int velocidad(int ve){ // devuelve un valor para asignar la velocidad en base a la función distancia.

  if(ve > 300 && ve < 900){
vel=1;  
}

else if (ve >= 900 && ve < 1995){
vel=2;  
}

else if(ve >= 1995){
vel=3;  
}

else
vel=0;

return vel;
}
