PS2 Handle don't work when use Blynk

Hello everyone, i’m using ps2 handle to control my robot arm mounted on a car with 2 dc motor. I’m using blynk app to switch mode, my project has 3 mode and when i choose control by RF mode then it does not work. This is my code, please help me !!!

#include <DHT.h>
#include <SPI.h>
#include <Blynk.h>
#include <ESP8266_Lib.h>
#include <BlynkSimpleShieldEsp8266.h>
#include <SimpleTimer.h>
#include <Servo.h>
#include <PS2X_lib.h> 
#include <Adafruit_Sensor.h>
#include <MechaQMC5883.h>
#include <Adafruit_INA219.h>

int dem = 0;

//IP blynk
char auth[] = "";
//Name & pass WIFI
char ssid[] = "";
char pass[] = "";
#define BLYNK_EXPERIMENTAL
// Hardware Serial on Mega
#define EspSerial Serial3
// Your ESP8266 baud rate:
#define ESP8266_BAUD 115200
ESP8266 wifi(&EspSerial);

WidgetLCD lcd(V7);
WidgetLED ledred(V14);
WidgetLED ledgreen(V15);
WidgetLED ledblue(V16);

Adafruit_INA219 ina219;
MechaQMC5883 qmc;
SimpleTimer timer;
//Cam bien nhiet
#define DHTPIN 52
#define DHTTYPE DHT11
DHT dht(DHTPIN, DHTTYPE);
// Bien de dieu khien huong
float initial_heading;
float target_heading;
float diff_heading;
bool turn_init;
bool target_reached;
int g = 90;//Goc quay
  // Left & right
#define LEFT 0
#define RIGHT 1
// Servo
Servo s1;  
Servo s2;
Servo s3;
Servo s4;
Servo s5;
Servo srada;
//phan biet mau
#define S0 24
#define S1 25
#define S2 26
#define S3 27
#define sensorOut 28
//rada
#define trig 9 
#define echo 10 
#define TIME_OUT 5000
//Motor
#define inA1 4 //Định nghĩa chân in1 của động cơ A
#define inA2 5 //Định nghĩa chân in2 của động cơ A
#define inB1 6 //Định nghĩa chân in1 của động cơ B
#define inB2 7 //Định nghĩa chân in2 của động cơ B
const byte enA=11;
const byte enB=12;

#define PIN_DO 3
volatile unsigned int pulses = 0;
float rpm =1.00;
unsigned long timeOld;
#define HOLES_DISC 11
int g1=110,g2=100,g3=90,g4=30, g5 =110;// Gia tri ban dau cua cac servo

int frequency = 0;
int color=0;
// Toc do xe di
float v;
int t;
float st; // Quang duong di thang
float s = 0.2; //Quang duong quay phai
  float sleft=0.25;
//thứ tự màu
int mau1 = 1;
int mau2 = 2;
int mau3 = 3;
//Button
int button = 49;
int buttonState = 0;  
//RF
#define PS2_DAT        A2  //Dữ liệu từ gamepad về vdk
#define PS2_CMD        A3  //Dữ liệu từ vdk về gamepad
#define PS2_SEL        A4  // chip section
#define PS2_CLK        A5  //Xung
#define pwm11 3 
#define pwm22 2
#define pressures   false
#define rumble      false
PS2X ps2x;// tạo PS2 điều khiển lớp
int pwm1=255,pwm2=255;
int error = 0;
byte type = 0;
byte vibrate = 0;
//TOC DO
void counter()
{
  pulses++;
}

void sendSensor()
{
  float h = dht.readHumidity();
  // or dht.readTemperature(true) for Fahrenheit
  float t = dht.readTemperature(true); 
  if (isnan(h) || isnan(t)) {
    Serial.println("Failed to read from DHT sensor!");
    return;
  }
  // You can send any value at any time.
  // Please don't send more that 10 values per second.
  Blynk.virtualWrite(V5, h);
  Blynk.virtualWrite(V6, t);
}


void autorun(){

  
  Serial.println("Vi tri thu nhat");
  robotMover(inA1,inA2,inB1,inB2,1);
  delay(1000);
  Blynk.run();
  timer.run();
 
  //v=speedmotor();
  v = 15;
  t = int(10000*s+10000*(s/v)-9*v);
  robotMover(inA1,inA2,inB1,inB2,2);
  delay(1000);
 Blynk.run();
  timer.run();
  thang(inA1, inA2, inB1, inB2, 0.7);// s = 0.7m
  robotMover(inA1,inA2,inB1,inB2,0);  
  Serial.println("Dừng");
  delay(1000);
  Blynk.run();
  timer.run();
  testgapvat(inA1, inA2, inB1, inB2, 0);
  robotMover(inA1,inA2,inB1,inB2,0);  
  Serial.println("Dừng");
  delay(1000);
  Blynk.run();
  timer.run();
                                              //Vi tri 2//
  Serial.println("Vi tri thu hai");
  pulses =0;
  robotMover(inA1,inA2,inB1,inB2,1);
  delay(1000);
 Blynk.run();
  timer.run();
  //v=speedmotor();
  v=15;
  t = int(10000*s+10000*(s/v)-9*v);
  robotMover(inA1,inA2,inB1,inB2,2);
  delay(1000);
  Blynk.run();
  timer.run();
  thang(inA1, inA2, inB1, inB2, 0.5);// 0.5m
  turn(g, RIGHT,t);//quay phai 90
  thang(inA1, inA2, inB1, inB2, 0.3);//0.3m
  turn(g, LEFT,t);//quay trai 90
  thang(inA1, inA2, inB1, inB2, 0.2);//0.2m
   robotMover(inA1,inA2,inB1,inB2,0);  
  Serial.println("Dừng");
  delay(1000);
  Blynk.run();
  timer.run();
  testgapvat(inA1, inA2, inB1, inB2, 1);
  robotMover(inA1,inA2,inB1,inB2,0);  
  Serial.println("Dừng");
  delay(1000);
  Blynk.run();
  timer.run();
                                            //Vi tri 3//
  Serial.println("Vi tri thu ba");
  pulses =0;
  robotMover(inA1,inA2,inB1,inB2,1);
  delay(1000);
  Blynk.run();
  timer.run();
  //v=speedmotor();
  v=15;
  t = int(10000*s+10000*(s/v)-9*v);
  robotMover(inA1,inA2,inB1,inB2,2);
  delay(1000);
  Blynk.run();
  timer.run();
  thang(inA1, inA2, inB1, inB2, 0.5);// 0.5m
  turn(g, RIGHT,t);//quay phai 90
  thang(inA1, inA2, inB1, inB2, 0.6);//0.6m
  turn(g, LEFT,t);//quay trai 90
  thang(inA1, inA2, inB1, inB2, 0.2);//0.2m
   robotMover(inA1,inA2,inB1,inB2,0);  
  Serial.println("Dừng");
  delay(1000);
  Blynk.run();
  timer.run();
  testgapvat(inA1, inA2, inB1, inB2, 2);
  robotMover(inA1,inA2,inB1,inB2,0); 
  delay(100000);
  Blynk.run();
  timer.run();
}
//Toc do dong co
float speedmotor(){
  float vt=1.00;
 
    detachInterrupt(digitalPinToInterrupt(PIN_DO));
    rpm = rpm * (pulses) / (HOLES_DISC);
    Serial.println(rpm);
    vt = vt  * rpm;
    Serial.print("pulses=");
    Serial.println(pulses);
    Serial.print("v=");
    Serial.println(vt);
    timeOld = millis();
    attachInterrupt(digitalPinToInterrupt(PIN_DO), counter, FALLING);  
    rpm = 1.00;
   
  pulses = 0;
    return vt;
}

void readDistance(){
  long duration, distance;
  digitalWrite(trig, LOW);  
  //delayMicroseconds(2); 
  digitalWrite(trig, HIGH);
  //delayMicroseconds(10); 
  digitalWrite(trig, LOW);
  duration = pulseIn(echo, HIGH);
  distance = (duration/2) / 29.1;
  Blynk.virtualWrite(V17,distance);
  }
// Khoang vcach rada
float getDistance()
{
  long duration, distanceCm;
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  duration = pulseIn(echo, HIGH, TIME_OUT);
  // convert to distance
  distanceCm = duration / 29.1 / 2;
  return distanceCm;
}
int objectDistance_cm (byte angle)
{
  srada.write(angle);
  delay(500);
  
  unsigned long duration;//biến đo thời gian
  int distance;//biến lưu khoảng cách
  /* phát xung từ chân trig */
  digitalWrite(trig,0);//tắt chân trig
  //delayMicroseconds(2);
  digitalWrite(trig,1);// phát xung từ chân trig
  //delayMicroseconds(5);// xung có độ dài 5 microSeconds
  digitalWrite(trig,0);//tắt chân trig
  /* tính toán thời gian */
  duration = pulseIn(echo,HIGH);//đo độ rộng xung HIGH ở chân echo. 
  distance = int(duration/2/29.412);//tính khoảng cách đến vật.
  /* in kết quả ra Serial monitor */
 Serial.println(distance);
  Serial.println("cm");
  delay(200);
  
  return distance;
}

void ReadColor()
{
  
  long duration, distance;
  digitalWrite(trig, LOW);  
  delayMicroseconds(2); 
  digitalWrite(trig, HIGH);
  delayMicroseconds(10); 
  digitalWrite(trig, LOW);
  duration = pulseIn(echo, HIGH);
  distance = (duration/2) / 29.1;
  Serial.println(distance);
  Blynk.virtualWrite(V17,distance);
  lcd.print(0,1,distance);
  digitalWrite(S2,LOW);
  digitalWrite(S3,LOW);
  int redFrequency = pulseIn(sensorOut, LOW);
  int R = redFrequency;
  digitalWrite(S2,HIGH);
  digitalWrite(S3,HIGH);
  int greenFrequency = pulseIn(sensorOut, LOW);
  int G = greenFrequency;
  digitalWrite(S2,LOW);
  digitalWrite(S3,HIGH);
  int blueFrequency = pulseIn(sensorOut, LOW);
  int B = blueFrequency;
    if(R<G & R<B ){
    lcd.clear();
    lcd.print(0,0,"Red");
    lcd.print(0,1,distance);
    ledred.on();
    ledgreen.off();
    ledblue.off();
  }
  else if(G<R & G<B){
    lcd.clear();
    lcd.print(0,0,"Green");
    lcd.print(0,1,distance);
    ledred.off();
    ledgreen.on();
    ledblue.off();
  }
  else if( B<R& B<G){
    lcd.clear();
    lcd.print(0,0,"Blue");
    lcd.print(0,1,distance);
    ledred.off();
    ledgreen.off();
    ledblue.on();
  }
  else
  {
  lcd.clear();
  lcd.print(0,0,"Unknown");
  lcd.print(0,1,distance);
  ledred.off();
    ledgreen.off();
    ledblue.off();
  }
  return ; 
}
// Color
int readColor() {
 
  // Setting red filtered photodiodes to be read
  digitalWrite(S2, LOW);
  digitalWrite(S3, LOW);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  int R = frequency;
  // Printing the value on the serial monitor
  Serial.print("R= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.print("  ");
  //delay(50);
  // Setting Green filtered photodiodes to be read
  digitalWrite(S2, HIGH);
  digitalWrite(S3, HIGH);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  int G = frequency;
  // Printing the value on the serial monitor
  Serial.print("G= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.print("  ");
  //delay(50);
  // Setting Blue filtered photodiodes to be read
  digitalWrite(S2, LOW);
  digitalWrite(S3, HIGH);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  int B = frequency;
  // Printing the value on the serial monitor
  Serial.print("B= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.print("  ");
  //delay(500);
  if(R<G & R<B){
    color = 1; // Red
  }
  if(G<R & G<B){
    color = 2; // Green
  }
  if(B<R & B<G){
    color = 3; // Blue
  }
  Serial.print("co= ");
  Serial.println(color);
  return color;  
}
//Huong di cua xe
void thang(byte inR1, byte inR2, byte inL1, byte inL2, float st)
{
   Blynk.run();
  timer.run();
  int  t_thang = int(10000*st+10000*(st/v)-9*v);  
  robotMover(inR1,inR2,inL1,inL2,1);  
  Serial.print("Tiến: ");
  Serial.print(t_thang); Serial.println(" ms");
  delay(t_thang); 
 Blynk.run();
  timer.run();
}
void lui(byte inR1, byte inR2, byte inL1, byte inL2)
{
  Blynk.run();
  timer.run();
  float sl = 0.2;
  int t_lui = int(10000*sl+10000*(sl/v)-9*v);   
  robotMover(inR1,inR2,inL1,inL2,2);  
  Serial.print("Lùi: ");
  Serial.print(t_lui); Serial.println(" ms");
  delay(t_lui);
  Blynk.run();
  timer.run();
}
void robotMover (byte inR1, byte inR2, byte inL1, byte inL2, byte action)
{
  switch (action)
  {
    case 0:// không di chuyển
      motorControlNoSpeed(inR1, inR2, 0);
      motorControlNoSpeed(inL1, inL2, 0);
      break;
    case 1://đi thẳng
      motorControlNoSpeed(inR1, inR2, 1);
      motorControlNoSpeed(inL1, inL2, 1);
      break;
    case 2:// lùi lại
      motorControlNoSpeed(inR1, inR2, 2);
      motorControlNoSpeed(inL1, inL2, 2);
      break;
    case 3:// quay trái
      motorControlNoSpeed(inR1, inR2, 1);
      motorControlNoSpeed(inL1, inL2, 2);
      break;
    case 4:// quay phải
      motorControlNoSpeed(inR1, inR2, 2);
      motorControlNoSpeed(inL1, inL2, 1);
      analogWrite(enA,130);
      analogWrite(enB,150);
      break;
    case 5:// rẽ trái
      motorControlNoSpeed(inR1, inR2, 1);
      motorControlNoSpeed(inL1, inL2, 0);
      break;
    case 6:// rẽ phải
      motorControlNoSpeed(inR1, inR2, 0);
      motorControlNoSpeed(inL1, inL2, 1);
      break;
    case 7:// rẽ lùi trái
      motorControlNoSpeed(inR1, inR2, 2);
      motorControlNoSpeed(inL1, inL2, 0);
      break;
    case 8:// rẽ lùi phải
      motorControlNoSpeed(inR1, inR2, 0);
      motorControlNoSpeed(inL1, inL2, 2);
      break;
    default:
      action = 0;     
  }
}

void motorControlNoSpeed (byte in1,byte in2, byte direct)
{

  switch (direct) 
  {
    case 0:// Dừng không quay
      digitalWrite(in1,LOW);
      digitalWrite(in2,LOW);
      break;
    case 1:// Quay chiều thứ 1
      digitalWrite(in1,HIGH);
      digitalWrite(in2,LOW);
      break;    
    case 2:// Quay chiều thứ 2
      digitalWrite(in1,LOW);
      digitalWrite(in2,HIGH);
      break;
    //default: 
  }
}
void testthavat(){
  
  robotMover(inA1,inA2,inB1,inB2,0);
  //Servo 1
  s1.write(g1);
  //Servo 2
  for(g2=100;g2<=155;g2+=1){
    s2.write(g2);  delay(15);
  }
  //Servo 3
  for(g3=90;g3>=45;g3-=1){
    s3.write(g3);
    delay(15);
  
  }
  //Servo 4
  s4.write(g4);
  //Servo 5
  for(g5=160;g5>=110;g5-=1){
    s5.write(g5);
    delay(15);
  }
  delay(1000);
  Blynk.run();
  timer.run();
 
  //Sau khi thả robot arm tro lai vi tri cu
  for(g2=155;g2>=100;g2-=1){
    s2.write(g2);
    delay(15);
  }
  for(g3=45;g3<=90;g3+=1){
    s3.write(g3);
    delay(15);
  }
}
void gapvat(){
  for(g2=100;g2<=155;g2+=1){
    s2.write(g2);
    delay(15);
  }
  for(g3=90;g3>=45;g3-=1){
    s3.write(g3);
    delay(15);
  }
  s4.write(g4);
  for(g5=110;g5<=160;g5+=1){
    s5.write(g5);
    delay(15);
  }
  delay(1000);
 Blynk.run();
  timer.run();
  for(g2=155;g2>=100;g2-=1){
    s2.write(g2);
    delay(15);
  }
  for(g3=45;g3<=90;g3+=1){
    s3.write(g3);
    delay(15);
  }
}

void testgapvat(byte inR1, byte inR2, byte inL1, byte inL2, int dem)
{

  Blynk.run();
  timer.run();
  long distance1[17];//So goc quay cua srada
  int dem2 =0;
  int dem1=0;
  int d=10;
  int goc;
  srada.write(15);
  robotMover(inR1,inR2,inL1,inL2,0);// Dừng
  for(int i = 0; i<17; i+=1){
    delay(400);
    srada.write(10+i*10);
    distance1[i] = getDistance();
    Serial.print("Gia tri: ");
    Serial.println(distance1[i]);
    if(distance1[i]>=4 & distance1[i]<=10){
       d = d+i*10;  
       dem2++;   
       Serial.println(d);
        //phan biet mau 
       color = readColor();
       delay(10);  
      
    }
   }
   dem1=d/dem2;
   goc = dem1+(120-dem1)/2;
   Serial.println("Goc");
   Serial.println(dem1);
    if(dem1<=90)
      {
        //robot arm gap vat
        s1.write(goc+90-dem1);
        gapvat();
      }
     else if(dem1>90)
      {
        //robot arm gap vat
        s1.write(goc-dem1+90);
        gapvat();
      }
      dem2=0;
      d=10;
      dem1=0;
      lui(inR1, inR2, inL1, inL2);
      turn(g, RIGHT,t);//quay phai 90
      if(color == 1){ // Red
          if(dem ==0){
            thang(inR1, inR2, inL1, inL2, 0.6);//0.6m
            turn(g, RIGHT,t);//quay phai 90
          }
          if(dem == 1){
            thang(inR1, inR2, inL1, inL2, 0.3);
            turn(g, RIGHT,t);//quay phai 90
          }
          if(dem == 2){
            turn(g, RIGHT,t);//quay phai 90
          }
            thang(inR1, inR2, inL1, inL2, 0.5);
            testthavat();
            turn(g, RIGHT,t);//quay phai 90
            thang(inR1, inR2, inL1, inL2, 0.7);
            turn(g, RIGHT,t);//quay phai 90
            robotMover(inR1,inR2,inL1,inL2,0);// Dừng
            delay(1000);
            Blynk.run();
  timer.run();
        }
        if(color == 2){
          if(dem ==0){
            thang(inR1, inR2, inL1, inL2, 0.6);
            turn(g, RIGHT,t);//quay phai 90
          }
          if(dem == 1){
            thang(inR1, inR2, inL1, inL2, 0.3);
            turn(g, RIGHT,t);//quay phai 90
          }
          if(dem == 2){
            turn(g, RIGHT,t);//quay phai 90
          }
            thang(inR1, inR2, inL1, inL2, 0.5);
            turn(g, RIGHT,t);//quay phai 90
            thang(inR1, inR2, inL1, inL2, 0.1);
            turn(g, LEFT,t);//quay phai 90
            testthavat();
            turn(g, RIGHT,t);//quay phai 90
            thang(inR1, inR2, inL1, inL2, 0.6);
            turn(g, RIGHT,t);//quay phai 90
            robotMover(inR1,inR2,inL1,inL2,0);// Dừng
            delay(1000);
            Blynk.run();
  timer.run();
        }
        if(color == 3){ // Blue
          if(dem ==0){ //vi tri 1
            thang(inR1, inR2, inL1, inL2, 0.6);
            turn(g, RIGHT,t);//quay phai 90        
          }
          if(dem == 1){ //vi tri 2
            thang(inR1, inR2, inL1, inL2, 0.2);
            turn(g, RIGHT,t);//quay phai 90
          }
          if(dem == 2){
            turn(g, RIGHT,t);//quay trai 90
          }
            thang(inR1, inR2, inL1, inL2, 0.5);
            turn(g, RIGHT,t);//quay phai 90
            thang(inR1, inR2, inL1, inL2, 0.2);
            turn(g, LEFT,t);//quay phai 90
            testthavat();
            turn(g, RIGHT,t);//quay phai 90
            thang(inR1, inR2, inL1, inL2, 0.5);
            turn(g, RIGHT,t);//quay phai 90
            robotMover(inR1,inR2,inL1,inL2,0);// Dừng
            delay(1000);
            Blynk.run();
  timer.run();
        }
}




// Xoay một góc cho truoc
void turn(float angle, int turn_direction,int tg) {
  
  // Get initial heading
  robotMover(inA1,inA2,inB1,inB2,0);// Dừng
  delay(500);
  Blynk.run();
  timer.run();
    if (!turn_init) {
    Serial.println("Starting turn ...");
    initial_heading = get_heading();
    delay(50);
    
    if (turn_direction) {//Hướng phải - RIGHT
      target_heading = initial_heading + angle;
      if(target_heading >360){ 
        target_heading = target_heading-360;}
      else if(target_heading<0) {
        target_heading = target_heading+360;}
      if (target_heading>=191 && target_heading<=280){
        target_heading+=target_heading-angle*(target_heading/100-0.05);
      }
      if (target_heading>=281 && target_heading<=310){
        target_heading+=target_heading-angle*(target_heading/100-0.2);
      }
      if (target_heading>=311 && target_heading<=340){
        target_heading+=target_heading-angle*(target_heading/100+0.1);
      }
      if (target_heading>=341 && target_heading<=360){
        target_heading+=target_heading-angle*(target_heading/100+0.2);
      }
        if (target_heading>=0 && target_heading<=180){
        target_heading-=target_heading-angle*(target_heading/100-0.1);
      }
      if(target_heading >360){ 
        target_heading = target_heading-360;}
      else if(target_heading<0) {
        target_heading = target_heading+360;
      }


    }
    else {//Hướng trái - LEFT
      target_heading = initial_heading - angle;
      if(target_heading >360){ 
        target_heading = target_heading-360;}
      else if(target_heading<0) {
        target_heading = target_heading+360;}
///////////////        //////////////
      if (target_heading>=191 && target_heading<=280){
        target_heading-=target_heading-angle*(target_heading/100-0.05);
      }
      if (target_heading>=281 && target_heading<=310){
        target_heading-=target_heading-angle*(target_heading/100-0.2);
      }
      if (target_heading>=311 && target_heading<=340){
        target_heading+=target_heading-angle*(target_heading/100+0.1);
      }
      if (target_heading>=341 && target_heading<=360){
        target_heading+=target_heading-angle*(target_heading/100+0.15);
      }
        if (target_heading>=91 && target_heading<=180){
        target_heading-=target_heading-angle*(target_heading/100-0.1);
      }
      if (target_heading>=45 && target_heading<=90){
        target_heading+=angle*(0.25-target_heading/1000);
      }
      if (target_heading>=0 && target_heading<=44){
        target_heading+=angle*(0.45-target_heading/1000);
      }
      //////////////////////
      if(target_heading >360){ 
        target_heading = target_heading-360;}
      else if(target_heading<0) {
        target_heading = target_heading+360;
      }
    }
    
    Serial.print("Initial heading: ");
    Serial.println(initial_heading);
    Serial.print("Target heading: ");
    Serial.println(target_heading);
    
    turn_init = true;
    target_reached = false;
    diff_heading = 0;
  }
  
  float heading = get_heading();
  if (turn_direction) {
    diff_heading = target_heading - heading;
  }
  else {
    diff_heading = heading - target_heading;
  }
 
  while ( !target_reached) {
    
    // Move & stop
    if (turn_direction) {
      robotMover(inA1,inA2,inB1,inB2,3); 
    }
    else {
      robotMover(inA1,inA2,inB1,inB2,4); 
    }
    delay(tg);
    Blynk.run();
  timer.run();
    robotMover(inA1,inA2,inB1,inB2,0); 
    delay(100);
    Blynk.run();
  timer.run();
    
    // Measure heading again
    float heading = get_heading();
    
    if (turn_direction) {
      diff_heading = target_heading - heading;
    }
    else {
      diff_heading = heading - target_heading;
    }
    
    Serial.print("  Difference heading (degrees): "); 
    Serial.println(diff_heading);
    if(diff_heading <=70 && diff_heading>10){
      turn(diff_heading,turn_direction,100);
    }
    if(diff_heading <-2 && diff_heading>=-100){
      turn(diff_heading,!turn_direction,100);
    }
    if (diff_heading <=5 && diff_heading>=-5 && !target_reached) {
      target_reached = true;
      turn_init = false;
      diff_heading = 0;
    }
  }
  
  // Stop
  Serial.println("Stopping turn ...");
}
// Lay goc radian tu la ban
int get_heading() {
  
  int x,y,z;
  qmc.read(&x,&y,&z);
  Serial.print("  x: ");
  Serial.print(x);
  Serial.print(" y: ");
  Serial.print(y);
  Serial.print(" z: ");
  Serial.print(z);
  Serial.println();
  float heading;
  float declinationAngle = (4.0 + (26.0 / 60.0)) / (180 / PI);
  heading += declinationAngle;
  if(x>=0) heading += atan2(y,x)+PI/45;
  else if(x<0) {
    heading -= atan2(y,-x) ;
    Serial.println(heading);
    if(heading<=PI/2){
      heading += PI-PI/45;
    }
    else heading -= PI-PI/45;
  }

  

  // Correct for heading < 0deg and heading > 360deg
  if (heading < 0){
    heading += 2 * PI;
  }

  if (heading > 2 * PI){
    heading -= 2 * PI;
  }
  // Convert to degrees
  float headingDegrees = heading * 180/PI; 

  // Output
  Serial.print(" Degress = ");
  Serial.print(headingDegrees);
  return headingDegrees;
}
void setup(){
  Serial.begin(115200);

  dht.begin();
  timer.setInterval(1000L, sendSensor);
  timer.setInterval(1000L, ReadColor);
  //timer.setInterval(1L, controlRF);
  timer.setInterval(1000L,readDistance);

  // Set ESP8266 baud rate
  EspSerial.begin(ESP8266_BAUD);
  delay(10);
//Test connect Blynk
  Serial.println("Connecting to Blynk Server");
  Blynk.begin(auth, wifi, ssid, pass);
  Serial.println("Connection etablished!");
//Motor
  pinMode(inA1, OUTPUT);//Set chân in1 của dc A là output
  pinMode(inA2, OUTPUT);//Set chân in2 của dc A là output
  pinMode(inB1, OUTPUT);//Set chân in1 của dc B là output
  pinMode(inB2, OUTPUT);//Set chân in2 của dc B là output
//toc do motor
  pinMode(enA,OUTPUT);
  pinMode(enB,OUTPUT);
  analogWrite(enA,90);
  analogWrite(enB,90);
//rada
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  srada.attach(35);
  srada.write(90);//rada o 90o
//phan biet mau
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT);
  digitalWrite(S0, HIGH);
  digitalWrite(S1, LOW);
//robot arm  
 s1.attach(29);  
 s2.attach(33);
 s3.attach(32);  
 s4.attach(36);
 s5.attach(34);
 s4.write(g4);
//Tinh van toc xe
  pinMode(PIN_DO, INPUT);
  timeOld = 0;
  attachInterrupt(digitalPinToInterrupt(PIN_DO), counter, FALLING);
//Cam bien la ban
  Wire.begin();
  uint32_t currentFrequency;
  ina219.begin();
  qmc.init();
  // Khởi tạo biến hướng
 
  // Khởi tạo biến hướng
  turn_init = false;
  target_reached = false;

  //Button
  pinMode(button, INPUT);
//RF
    //delay(300);
    //Blynk.run();
    
    error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
  
  if(error == 0){
    Serial.print("Đã tìm thấy bộ điều khiển ");
    Serial.print("pressures = ");
  if (pressures)
    Serial.println("true ");
  else
    Serial.println("false");
  Serial.print("rumble = ");
  if (rumble)
    Serial.println("true)");
  else
    Serial.println("false");
    Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
    Serial.println("giữ L1 hoặc R1 sẽ in ra các giá trị analog");
    Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
  }  
  else if(error == 1)
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
   
  else if(error == 2)
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

  else if(error == 3)
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
  type = ps2x.readType(); 
  switch(type) {
    case 0:
      Serial.print("Tay điều khiển không phù hợp ");
      break;
    case 1:
      Serial.print("DualShock Controller found ");
      break;
    case 2:
      Serial.print("GuitarHero Controller found ");
      break;
  case 3:
      Serial.print("Wireless Sony DualShock Controller found ");
      break;
   }
  analogWrite(pwm11,255);
  analogWrite(pwm22,255);
}

void motorSpeed(int prmA, byte prmA1, byte prmA2, int prmB, byte prmB1, byte prmB2)
{
  analogWrite(enA,prmA);
  analogWrite(enB,prmB);
  
  digitalWrite(inA1,prmA1);
  digitalWrite(inA2,prmA2);
  digitalWrite(inB1,prmB1);
  digitalWrite(inB2,prmB2);
}

BLYNK_WRITE(V0) {
  int x = param[0].asInt();
  int y = param[1].asInt();
// x =  -2 -1 0 1 2 
// Y =  -2 -1 0 1 2 
 if ((x==0) && (y==0)) 
 {
   motorSpeed(0,LOW,LOW,0,LOW,LOW); 
 }
 else if ((x==0) && (y>0))
 {
  if (y==1){ motorSpeed(900,HIGH,LOW,900,HIGH,LOW); }
  else { motorSpeed(1023,HIGH,LOW,1023,HIGH,LOW); }
 }
 else if ((y==0) && (x>0))
  motorSpeed(900,HIGH,LOW,900,LOW,HIGH); 
 else if ((y>0) && (x>0))
  motorSpeed(1023,HIGH,LOW,900,LOW,LOW); 
 else if ((y==0) && (x<0))
  motorSpeed(900,LOW,HIGH,900,HIGH,LOW); 
 else if ((y>0) && (x<0))
   motorSpeed(900,LOW,LOW,1023,HIGH,LOW);   
 if ((y<0) && (x<0))
    motorSpeed(900,LOW,LOW,1023,LOW,HIGH); 
 else if ((y<0) && (x>0))
    motorSpeed(1023,LOW,HIGH,900,LOW,LOW); 
 else if ((y<0) && (x==0))
   motorSpeed(900,LOW,HIGH,900,LOW,HIGH); 
}

BLYNK_WRITE(V1)
{
   s1.write(param.asInt());
}

BLYNK_WRITE(V2)
{
  s4.write(param.asInt());
}
BLYNK_WRITE(V8)
{
   s3.write(param.asInt());
}

BLYNK_WRITE(V9)
{
  s2.write(param.asInt());
}
BLYNK_WRITE(V10)
{
   s5.write(param.asInt());
}

BLYNK_WRITE(V11){
//gapvat();
int i=0;
  Blynk.run();
  timer.run();
if(i<1)
{

  for(g2=100;g2<=155;g2+=1){
    s2.write(g2);
    delay(15);
    
  }
  for(g3=90;g3>=45;g3-=1){
    s3.write(g3);
    delay(15);
  }
  s4.write(g4);
  for(g5=110;g5<=160;g5+=1){
    s5.write(g5);
    delay(15);
  }
  delay(1000);
  Blynk.run();
  timer.run();
  for(g2=155;g2>=100;g2-=1){
    s2.write(g2);
    delay(15);
  }
  for(g3=45;g3<=90;g3+=1){
    s3.write(g3);
    delay(15);
  }
  i++;
  Serial.print("i=");
  Serial.println(i);
}
  delay(1000);
  Blynk.run();
  timer.run();
}

BLYNK_WRITE(V12){
  testthavat();
}


BLYNK_APP_DISCONNECTED() {

Serial.println("Trying connect !");
Blynk.connect();
}



 BLYNK_WRITE(V3)
{
  //demm=param.asInt();
  
switch(param.asInt()){
  case 1:

  //demm =1;
  //Serial.print("DEM = ");
  //Serial.println(demm);
  
  Blynk.disconnect();
    Serial.println("Auto");
    Blynk.notify("Đã bật chế độ Auto");
    
    autorun();
    Blynk.run();
  timer.run();
    Serial.println("Auto Finish");
    break;
  case 2:
  //demm=2; 
  //Serial.print("DEM = "); 
  //Serial.println(demm);
  robotMover(inA1,inA2,inB1,inB2,0);
  
  
  
    Serial.println("Manual");
    Blynk.notify("Đã bật chế độ Manual");
    controlapp();
    
    break;
  case 3:
  //demm =3;
  //Serial.print("DEM = ");
  //Serial.println(demm);

  //Blynk.disconnect();
  
  //type = 1;
    Serial.println("RF");
    Blynk.notify("Đã bật chế độ RF");
   // timer.setInterval(1/100000000L, controlRF);
   Blynk.disconnect();
   controlRF();
    
    
    break;

  }
} 

void loop() 
{ 

   Blynk.run();
    timer.run();

    //timer.setInterval(1L, controlRF);
    controlRF();
    if(!Blynk.connected()){
      Blynk.connect();
    Blynk.run();
    timer.run();
    //controlRF();
  }
/*
  if(demm != 1){
    robotMover(inA1,inA2,inB1,inB2,0);
    controlapp();
  }
  */
    //autorun();
}

void controlapp(){
  loop();
}

void controlRF(){

//Blynk.run();
//timer.run();
  
  if(error == 1)
    return; 
  if(type == 1)
    ps2x.read_gamepad(false, vibrate); 
  if(ps2x.Button(PSB_START))      
    Serial.println("Start is being held");
  if(ps2x.Button(PSB_SELECT))
    Serial.println("Select is being held");  
    if(type == 1){
  if(ps2x.ButtonPressed(PSB_PAD_UP)) {   
    robotMover(inA1,inA2,inB1,inB2,1);
    Serial.println("Di thang ");
  }
  if(ps2x.ButtonReleased(PSB_PAD_UP)){
     robotMover(inA1,inA2,inB1,inB2,0);
     Serial.println("NGUNG");
   }
   //////////////////////////////
  if(ps2x.ButtonPressed(PSB_PAD_RIGHT)){
    robotMover(inA1,inA2,inB1,inB2,3);
    Serial.println("phai ");
  }
  if(ps2x.ButtonReleased(PSB_PAD_RIGHT)){
    robotMover(inA1,inA2,inB1,inB2,0);
    Serial.println("NGUNG");
  }
    ///////////////////////////////////////////
  if(ps2x.ButtonPressed(PSB_PAD_LEFT)){
      robotMover(inA1,inA2,inB1,inB2,4);
      Serial.println("trai ");
    }
  if(ps2x.ButtonReleased(PSB_PAD_LEFT)){
     robotMover(inA1,inA2,inB1,inB2,0);
     Serial.println("NGUNG");}
    ///////////////////////////////////////////////
   if(ps2x.ButtonPressed(PSB_PAD_DOWN)){
      if(ps2x.Analog(PSAB_PAD_DOWN)==0){
        robotMover(inA1,inA2,inB1,inB2,2);
        Serial.println("nghich");}    
    }   
    if(ps2x.ButtonReleased(PSB_PAD_DOWN)){
      robotMover(inA1,inA2,inB1,inB2,0);
      Serial.println("NGUNG");}
    /////////////////////////////////////////////////
    /* if(ps2x.ButtonPressed(PSB_TRIANGLE)){
      if(ps2x.Analog(PSAB_TRIANGLE)==0){Serial.println("GAP");g4=40;}    
    }   
    if(ps2x.ButtonReleased(PSB_TRIANGLE)){Serial.println("NHA");g4=120;}*/
    /////////////////////////////////////////////////
    
    vibrate = ps2x.Analog(PSAB_CROSS); // nút X  
    if (ps2x.NewButtonState()) {        
      if(ps2x.Button(PSB_L1)){
        Serial.println("L1 pressed");
        pwm1=pwm1+10;
        if(pwm1>=255){pwm1=255;}
        Serial.println(pwm1);
      }  
      if(ps2x.Button(PSB_L2)){
        Serial.println("L2 pressed");
        pwm1=pwm1-10;
        if(pwm1<=0){pwm1=0;}
        Serial.println(pwm1);
      }  
      if(ps2x.Button(PSB_R1))
        {
        Serial.println("R1 pressed");
        pwm2=pwm2+10;
        if(pwm2>=255){pwm2=255;}
        Serial.println(pwm2);
      }  
      if(ps2x.Button(PSB_R2))
        {
        Serial.println("R2 pressed");
        pwm2=pwm2-10;
        if(pwm2<=0){pwm2=0;}
        Serial.println(pwm2);
      }  
    }
 
    if(ps2x.Button(PSB_CIRCLE)) // Nút tròn
    {              
      if(ps2x.Analog(PSS_LX)>200){
        g4+=10;    //delay(15);

        if(g4>180)g4=180;
        }
      if(ps2x.Analog(PSS_LX)<50){
        g4-=10;
            //delay(15);
        if(g4<0)g4=0;
        }}
 //////////////////////////////////////////////////////        
     if(ps2x.Button(PSB_CROSS)) {    
      if(ps2x.Analog(PSS_LX)>200){
        g1+=10;
            //delay(15);
        if(g1>180)g1=180;
        }
      if(ps2x.Analog(PSS_LX)<50){
        g1-=10;
            //delay(15);
        if(g1<0)g1=0;
        }        
      if(ps2x.Button(PSB_L1)){
        g2+=10;
            //delay(15);
        if(g2>180)g2=180;
        }
      if(ps2x.Button(PSB_L2)){
        g2-=10;
            //delay(15);
        if(g2<0)g2=0;
        } 
      }        
   /////////////////////////////////////////       
       if(ps2x.Button(PSB_SQUARE)) { 
           
      if(ps2x.Button(PSB_L1)){
        g3+=10;
        //delay(15);
        if(g3>180)g3=180;
        }
      if(ps2x.Button(PSB_L2)){
        g3-=10;
        //delay(15);
        if(g3<0)g3=0;
        } 
       }
    if(ps2x.Button(PSB_TRIANGLE)) { 
      if(ps2x.Analog(PSS_LX)>200){
        g5+=10;
            //delay(15);
        if(g5>180)g5=180;
        }
      if(ps2x.Analog(PSS_LX)<50){
        g5-=10;
            //delay(15);
        if(g5<0)g5=0;
        } }
        //delay(50);
   // Blynk.run();
    //timer.run();
  analogWrite(pwm11,90);
  analogWrite(pwm22,90);
  
   s1.write(g1);
   s2.write(g2);
   s3.write(g3);
   s4.write(g4);
   s5.write(g5);
  }
  else return;
}