#include<Servo.h>
#include <SoftwareSerial.h>
#include <BlynkSimpleSerialBLE.h>

int vel=0;
int velblynk=0;
int velfsr=0;

int subvelfsr1=0;
int subvelfsr2=0;
int subvelfsr3=0;

int mode;

int velcontador;

int red;
int blue;
int green;

int pinfsr1=A0;
int pinfsr2=A1;

int valorfsr1;
int valorfsr2;

char auth[] = "2a4da3a232764a6a87368d9735aede6e";

SoftwareSerial SerialBLE(2, 3); // RX, TX
BlynkTimer timer;

BLYNK_WRITE(V1)
{
  velblynk=param.asInt();
}

BLYNK_WRITE(V3)
{
  mode=param.asInt();
}

BLYNK_WRITE(V4)
{
  red=param.asInt();
}

BLYNK_WRITE(V5)
{
  green=param.asInt();
}

BLYNK_WRITE(V6)
{
  blue=param.asInt();
}

void contadorvelocidad(){
  Blynk.virtualWrite(V2, velcontador);
}

Servo ESC;
Servo ESC2;

void readfsr(){
  valorfsr1=analogRead(pinfsr1);
  valorfsr2=analogRead(pinfsr2);
  subvelfsr1=map(valorfsr1,0,1023,1000,2000);
  subvelfsr2=map(valorfsr2,0,1023,1000,2000);
  subvelfsr3=subvelfsr1-subvelfsr2;
  velfsr=subvelfsr3;
  
  if (valorfsr1==0 and valorfsr2==0){
    velfsr=1000;
  }

  if (valorfsr1==1023 and valorfsr2==1023){
    velfsr=1000;
  }

    if (valorfsr1==0 and valorfsr2!=0){
    velfsr=1000;
  }
}
 
void setup()
{
  SerialBLE.begin(9600);
  //Serial.begin(9600);
  Blynk.begin(SerialBLE, auth);
  //Serial.println("Waiting for connections...");
  //Asignar un pin a los ESCs
  ESC.attach(5);
  ESC.writeMicroseconds(1000);
  ESC2.attach(6);
  ESC2.writeMicroseconds(1000);
  delay(5000);
  timer.setInterval(1000L, contadorvelocidad);
    pinMode(5,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(11,OUTPUT);
  pinMode(10,OUTPUT);
  pinMode(9,OUTPUT);
  pinMode(A0,INPUT);
  pinMode(A1,INPUT);
  pinMode(A2,INPUT);
}
 
 
void loop()
{
    Blynk.run();
    timer.run();
    
    void readfsr();
    
    if (mode=2){
      vel=velblynk;
    }
    if(mode=1){
      vel=velfsr;
    }
    
    ESC.writeMicroseconds(vel); //Generar un pulso con el numero recibido
    ESC2.writeMicroseconds(vel);
    

    analogWrite(11,red);
    analogWrite(10,green);
    analogWrite(9,blue);
    delay(100);
    
    velcontador=map(vel,1000,2000,0,100);
  }