Bluetooth Controlled Car with Arduino Mega

#include <NMEAGPS.h>
#include <SoftwareWire.h>
#include <Servo.h>
#include <AFMotor.h>
#include <NewPing.h>
#define GPSport_h
#define gpsPort Serial3
#define GPS_PORT_NAME "Serial3"

SoftwareWire MyWire( 20, 21);
#define TRIG_PIN_A 41 
#define ECHO_PIN_A 40 
#define ECHO_PIN_R 39
#define TRIG_PIN_R 38  
#define MAX_DISTANCE 300
#define SENSORE_AVANTI 0
#define SENSORE_INDIETRO 1 
#define RIDUZIONE_POTENZA_LOW 2
#define DIR_INDIETRO 0
#define DIR_AVANTI 1
#define DIR_STOP 2
#define DIR_DESTRA 3
#define DIR_SINISTRA 4

/* Definizione Comandi ricevuti dall'App */
#define ARRESTA           48
#define AVANTI            49
#define INDIETRO          50
#define DESTRA            51
#define SINISTRA          52
#define DESTRA_AVANTI     53
#define SINISTRA_AVANTI   54
#define DESTRA_INDIETRO   55
#define SINISTRA_INDIETRO 56
#define ACCELLERA         57
#define DECELERA          58
#define AUTOMATICO        59
#define STOP              79
#define POTENZA           80
#define SETTING           90
#define OFFSET_GPS        96
#define OFFSET_BUSSOLA    97
#define CALIBRA_GPS       98
#define ATTIVA_GPS        99
#define GPS              100

#define PIN_PRINT_APP     18

NewPing sonar[2] = {
  NewPing(TRIG_PIN_A, ECHO_PIN_A, MAX_DISTANCE),
  NewPing(TRIG_PIN_R, ECHO_PIN_R, MAX_DISTANCE)
  };
AF_DCMotor motore_anteriore_dx(2);
AF_DCMotor motore_anteriore_sx(3);
AF_DCMotor motore_posteriore_sx(4);
AF_DCMotor motore_posteriore_dx(1);
Servo MyServo;
Servo MyServo_Info;
static NMEAGPS  gps; // This parses the GPS characters
static gps_fix  fix;
int direzione; 
int potenza;
int bytericevuto = 0;
String cPotenza;
String cDistanza;
String cVelocita;
int pin_velocita = 19;
unsigned int cm_al_secondo; 
volatile unsigned int pulses;
unsigned long timeold;  // controllo ogni 100mS per print info
unsigned long timeold1; // controllo ogni Secondo per l'avanzamento automatico GPS
unsigned long timeold2; // controllo ogni 500mS per il calcolo della velocit
unsigned int pulsesperturn = 20;
int Dist_Sinistra      = 0;
int Dist_Destra        = 0;
int Dist_Avanti        = 0;
int Dist_Dietro        = 0;
int Dist_Sinistra_Diag = 0;
int Dist_Destra_Diag   = 0;
boolean DestraSinistra = false;
int Gradi              = 0;
int nX;
int Distanza_Frontale  = 30;
int Distanza_Laterale  = 20;
int Distanza_Minima    = 50;
int Potenza_Automatico = 65;
int Potenza_Minima     = 50;
int Accelera_Decelera  = 5;
int offset             = 5;
int Tempo_Rotazione    = 200;
int variabile          = 0;
String dataingps       = "";
long latitudine_car =  0;
long longitudine_car = 0;
long latitudine_tablet = 0;
long longitudine_tablet = 0;
long distanza_tc = 0;
double angolo_tc = 0;
String cLatitudine = "";
String cLongitudine = "";
String cAngolo = "";
String cDistanza_tc = "";
int Angolo;
boolean Attivo_GPS = false;
int offset_bussola = 0;
long offset_lat = 0;
long offset_long = 0;
static double xlow  = 0;
static double ylow  = 0;
static double xhigh = 0;
static double yhigh = 0;

/* ---------- Interupt1----------------- */
void counter()  {
  pulses++;
}

/* ---------- Setup -------------------- */
void setup() {
  MyWire.begin();
  QMC5883L_Configura();
  Arresta();
  int Distanza = 0;
  Serial.begin(115200);     // seriale utilizzata per il debug      
  Serial2.begin(115200);    // seriale utilizzata per la trasmissione all'applicazione android
  gpsPort.begin(115200);      // seriale utilizzata per il colloquio con GPS
  potenza = Potenza_Minima;
  motore_posteriore_dx.setSpeed(potenza);
  motore_posteriore_sx.setSpeed(potenza);
  motore_anteriore_sx. setSpeed(potenza);
  motore_anteriore_dx. setSpeed(potenza);

  motore_posteriore_dx.run(RELEASE);
  motore_posteriore_sx.run(RELEASE);
  motore_anteriore_sx. run(RELEASE);
  motore_anteriore_dx. run(RELEASE);
  
  // interupt utilizzato per calcolare la velocit
  pinMode(pin_velocita, INPUT);
  digitalWrite(pin_velocita, HIGH);
  attachInterrupt(digitalPinToInterrupt(pin_velocita), counter, FALLING);
  pulses = 0;
  
  // interupt utilizzato per inviare le informazioni all'APP sul tablet
  pinMode(PIN_PRINT_APP, INPUT);
  digitalWrite(PIN_PRINT_APP, HIGH);
  attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP), Print_Info, FALLING);

  cm_al_secondo = 0;
  timeold = 0;
  timeold2 = 0;
  
  MyServo_Info.attach(9);
  MyServo_Info.write(90);
  
  MyServo.attach(10);
  MyServoWriteNew(20);
  MyServoWriteNew(160);
  MyServoWriteNew(90);
}

/* ---------- Loop Principale ---------- */
void loop() {
  bytericevuto = 0;
  int appoggio;
  if (Serial2.available() > 0) { bytericevuto = Serial2.read();   }
  else                         { delay(10);                       }
  
  Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
  if (Dist_Avanti < Distanza_Minima && (direzione == DIR_AVANTI)) {
   if (potenza > Potenza_Minima) {
    if (potenza > 200)      { potenza *= 0.50; }
    else if (potenza > 150) { potenza *= 0.65; }
    else                    { potenza *= 0.80; }
    Potenza(potenza);
    }
    if (Dist_Avanti <= 20) {
      Indietro();
      delay(20);
      Arresta(); 
      
    }      
  }
    
  if ( direzione == DIR_INDIETRO ) {
    Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
    if (Dist_Dietro < Distanza_Minima) {
      if (potenza > 200)      { potenza *= 0.50; }
      else if (potenza > 150) { potenza *= 0.65; }
      else                    { potenza *= 0.80; }
      Potenza(potenza);
      }
    if (Dist_Dietro <= 20) {
      Avanti();
      delay(20);
      Arresta();
      Potenza(Potenza_Minima); 
    } 
  }
    
  switch (bytericevuto) {
    case ARRESTA:
      Arresta();
      break;
    case AVANTI:
      Avanti();
      break;
    case INDIETRO:
      Indietro(); 
      break;
    case DESTRA:
      Destra();
      break;
    case SINISTRA:
      Sinistra();
      break;
    case DESTRA_AVANTI:
      Destra_Avanti();
      break;
    case SINISTRA_AVANTI:
      Sinistra_Avanti();
      break;
    case DESTRA_INDIETRO:
      Destra_Indietro();
      break;
    case SINISTRA_INDIETRO:
      Sinistra_Indietro();
      break;
    case ACCELLERA:
      Accelera();
      break;
    case DECELERA:
      Decelera();
      break;
    case AUTOMATICO:
      Automatico();
      break;    
    case STOP:
      Arresta();
      Potenza(0);
      break;
    case POTENZA:
      while (Serial2.available() < 1 ){ delay(10); }
      potenza = Serial2.read();
      Potenza(potenza);
      break;
    case SETTING:
      Impostazioni();
      break;
    case OFFSET_GPS:
      offset_lat  = (latitudine_car-latitudine_tablet);
      offset_long = (longitudine_car-longitudine_tablet);
      break;
    case OFFSET_BUSSOLA:
      offset_bussola = 0;
      offset_bussola = QMC5883L_Angolo();
      break;
    case CALIBRA_GPS:
      QMC5883L_Calibrazione();
      break;
    case ATTIVA_GPS:
      while (Serial2.available() < 1 ){ delay(10); }
      if (Serial2.peek() == 1 || Serial2.peek() == 2) {
         appoggio = Serial2.read();
         if      ( appoggio == 1) { Attivo_GPS = true ;  } 
         else if ( appoggio == 2) { Attivo_GPS = false ; } 
      }     
      break;    
    case GPS:
       Gps();
       break;
    default:
      break;
   }        
}

/* ---------- Avanti ------------------- */
void Avanti(){
  if ( potenza < Potenza_Minima) {potenza = Potenza_Minima;  }
  
  Potenza(potenza);
  motore_posteriore_dx.run(FORWARD);
  motore_posteriore_sx.run(FORWARD);
  motore_anteriore_sx. run(FORWARD);
  motore_anteriore_dx. run(FORWARD);
  direzione = DIR_AVANTI;
}

/* ---------- Indietro ----------------- */
void Indietro() {
  if ( potenza < Potenza_Minima) {potenza = Potenza_Minima;  }
  
  Potenza(potenza);
  motore_posteriore_dx.run(BACKWARD);
  motore_posteriore_sx.run(BACKWARD);
  motore_anteriore_sx. run(BACKWARD);
  motore_anteriore_dx. run(BACKWARD);
  direzione = DIR_INDIETRO;
}

/* ---------- Arresta ------------------ */
void Arresta() {
  motore_posteriore_dx.run(RELEASE);
  motore_posteriore_sx.run(RELEASE);
  motore_anteriore_sx. run(RELEASE);
  motore_anteriore_dx. run(RELEASE);
  Potenza(potenza);
  direzione = DIR_STOP;
}

/* ---------- Potenza ------------------ */
void Potenza(int Speed) {  
  motore_posteriore_dx.setSpeed(Speed);
  motore_posteriore_sx.setSpeed(Speed);
  motore_anteriore_sx. setSpeed(Speed);
  motore_anteriore_dx. setSpeed(Speed); 
  potenza = Speed;
}

/* ---------- Destra ------------------- */
void Destra() {
  motore_posteriore_dx.run(BACKWARD);
  motore_posteriore_sx.run(FORWARD);
  motore_anteriore_sx. run(FORWARD);
  motore_anteriore_dx. run(BACKWARD); 
  direzione = DIR_DESTRA;
}

/* ---------- Sinistra ----------------- */
void Sinistra() {
  motore_posteriore_dx.run(FORWARD );
  motore_posteriore_sx.run(BACKWARD);
  motore_anteriore_sx. run(BACKWARD);
  motore_anteriore_dx. run(FORWARD );
  direzione = DIR_SINISTRA;  
}

/* ---------- Destra Avanti ------------ */
void Destra_Avanti() {
  motore_posteriore_dx.run(FORWARD);
  motore_posteriore_sx.run(FORWARD);
  motore_anteriore_sx. run(FORWARD);
  motore_anteriore_dx. run(FORWARD);
  motore_anteriore_dx. setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
  motore_posteriore_dx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
  motore_anteriore_sx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  motore_posteriore_sx.setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  direzione = DIR_AVANTI;
  }

/* ---------- Sinistra Avanti ---------- */
void Sinistra_Avanti() {
  motore_posteriore_dx.run(FORWARD);
  motore_posteriore_sx.run(FORWARD);
  motore_anteriore_sx. run(FORWARD);
  motore_anteriore_dx. run(FORWARD);
  motore_anteriore_sx. setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
  motore_posteriore_sx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
  motore_anteriore_dx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  motore_posteriore_dx.setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  direzione = DIR_AVANTI;
}

/* ---------- Destra Indietro ---------- */
void Destra_Indietro() {
  motore_posteriore_dx.run(BACKWARD);
  motore_posteriore_sx.run(BACKWARD);
  motore_anteriore_sx. run(BACKWARD);
  motore_anteriore_dx. run(BACKWARD);
  motore_anteriore_dx. setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
  motore_posteriore_dx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
  motore_anteriore_sx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  motore_posteriore_sx.setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  direzione = DIR_INDIETRO;
}

/* ---------- Sinistra Indietro -------- */
void Sinistra_Indietro() {
  motore_posteriore_dx.run(BACKWARD);
  motore_posteriore_sx.run(BACKWARD);
  motore_anteriore_sx. run(BACKWARD);
  motore_anteriore_dx. run(BACKWARD);
  motore_anteriore_sx. setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
  motore_posteriore_sx.setSpeed(potenza / RIDUZIONE_POTENZA_LOW);
  motore_anteriore_dx. setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  motore_posteriore_dx.setSpeed(potenza * RIDUZIONE_POTENZA_LOW);
  direzione = DIR_INDIETRO;
}
 

/* ---------- Accellera ---------------- */
void Accelera() { 
  potenza += Accelera_Decelera;
  if ( potenza > 250) { potenza = 250;  }
  
  Potenza(potenza);
}
  
/* ---------- Decelera ----------------- */
void Decelera() { 
  potenza -= Accelera_Decelera;
  if ( potenza < 0) { potenza = 0;  }
  
  Potenza(potenza);
}

/* ---------- Misura distanza ---------- */
int Misura_Distanza(int a_r) {
  int distanza_cm;
  distanza_cm = sonar[a_r].ping_cm();
  if ( distanza_cm <= 0 ) { distanza_cm = MAX_DISTANCE;  }
  return distanza_cm;
}

/* ---------- Impostazioni ---------- */
void Impostazioni() {
      while (Serial2.available() < 1 ){ delay(10); }
      Distanza_Frontale  = Serial2.read();
      while (Serial2.available() < 1 ){ delay(10); }
      Distanza_Laterale  = Serial2.read();
      while (Serial2.available() < 1 ){ delay(10); }
      Distanza_Minima    = Serial2.read();
      while (Serial2.available() < 1 ){ delay(10); }
      Potenza_Automatico = Serial2.read();
      while (Serial2.available() < 1 ){ delay(10); }
      Potenza_Minima     = Serial2.read();
      while (Serial2.available() < 1 ){ delay(10); }
      Accelera_Decelera  = Serial2.read();
      while (Serial2.available() < 1 ){ delay(10); }
      offset             = (Serial2.read() - 90);
      while (Serial2.available() < 1 ){ delay(10); }
      Tempo_Rotazione    = (Serial2.read()*10);
      MyServoWriteNew(90);
  }


/* ---------- Avanzamento Automatico --- */
int Automatico() {
int returnvalue = 0;

  while (returnvalue != AUTOMATICO ) {
    Gradi = 0;
    if (Serial2.available() > 0){ returnvalue = Serial2.read();}
    Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
    while ( Dist_Avanti > Distanza_Frontale  && returnvalue != AUTOMATICO ){  
      if (Serial2.available() > 0){ returnvalue = Serial2.read();}
      // guarda a destra e sinistra
      if   (DestraSinistra) { Gradi++; }
      // guarda a sinistra
      else                  { Gradi--; }
      
      MyServo.write(90+Gradi+offset);
      if (abs(Gradi) > 30) { DestraSinistra = !DestraSinistra;}
      Avanti();
      Potenza(Potenza_Automatico);
      delay(10);        
      Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
    }
    Arresta();
    if (returnvalue == AUTOMATICO) {break;}
    Distanza_Ostacolo();
    // nel caso viene chiuso su tre lati va indietro
    if  ( Dist_Destra   < Distanza_Laterale && Dist_Destra_Diag   < Distanza_Frontale &&
          Dist_Sinistra < Distanza_Laterale && Dist_Sinistra_Diag < Distanza_Frontale &&
          Dist_Avanti   < Distanza_Frontale && (Dist_Dietro > 25)) { 
      DestraSinistra = false;
      Gradi = 0;
      while ( Dist_Avanti < Distanza_Frontale*2 ){  
        if (DestraSinistra){  Gradi += 2;  }
        else               {  Gradi -= 2;  }
        MyServoWriteNew(90+Gradi);
        if (abs(Gradi) > 70) {
          Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
          if (Dist_Avanti > Distanza_Laterale*2)  {
            Potenza(Potenza_Automatico*3);
            if ( Gradi > 0 ) { Sinistra();        }
            else             { Destra();          }  
            delay(Tempo_Rotazione); 
            break;
          }
          DestraSinistra = !DestraSinistra;
        }
        Potenza(Potenza_Minima);
        Indietro();
        delay(10);        
        Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
        if (Dist_Dietro < 10) {  break;}
      }
      MyServoWriteNew(90);
      }
          
    // decide se girare a sinistra o destra
    else if  (Dist_Sinistra < Dist_Destra   && Dist_Sinistra < Dist_Avanti) {
        Potenza(Potenza_Automatico*3);
        Destra(); 
        delay(Tempo_Rotazione*0.75);
     }
    else if (Dist_Destra < Dist_Sinistra && Dist_Destra   < Dist_Avanti) {
      Potenza(Potenza_Automatico*3);
      Sinistra();
      delay(Tempo_Rotazione*0.75);
      }
    else if (Dist_Destra        < Distanza_Laterale)                     {
      Potenza(Potenza_Automatico*3);
      Sinistra();
      delay(Tempo_Rotazione);
      }
    else if (Dist_Sinistra      < Distanza_Laterale)                     {
      Potenza(Potenza_Automatico*3);
      Destra();
      delay(Tempo_Rotazione);
      } 
    else if (Dist_Sinistra_Diag < Distanza_Frontale)                     {
      Potenza(Potenza_Automatico*3);
      Destra();
      delay(Tempo_Rotazione);
      } 
    else if (Dist_Destra_Diag   < Distanza_Frontale)                     {
      Potenza(Potenza_Automatico*3);
      Sinistra();
      delay(Tempo_Rotazione);
      }
    else if (Dist_Destra>Dist_Sinistra && Dist_Destra>50)                {
      Potenza(Potenza_Automatico*3);
      Destra();
      delay(Tempo_Rotazione); }
    else if (Dist_Sinistra>Dist_Destra && Dist_Sinistra>50)              {
      Potenza(Potenza_Automatico*3);
      Sinistra();
      delay(Tempo_Rotazione); }
      
    else if (Dist_Avanti       <= Distanza_Frontale)                     {  
      DestraSinistra = false;
      Gradi = 0;
      while ( Dist_Avanti < Distanza_Frontale*2 ){  
        if (DestraSinistra){  Gradi += 2;  }
        else               {  Gradi -= 2;  }
        MyServoWriteNew(90+Gradi);
        if (abs(Gradi) > 70) {
          Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
          if (Dist_Avanti > Distanza_Laterale*2)  {
            Potenza(Potenza_Automatico*3);
            if ( Gradi > 0 ) { Sinistra();        }
            else             { Destra();          }  
            delay(Tempo_Rotazione); 
            break;
          }
          DestraSinistra = !DestraSinistra;
        }
        Potenza(Potenza_Minima);
        Indietro();
        delay(10);        
        Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
        if (Dist_Dietro < 10) {  break;}
      }
      MyServoWriteNew(90);
    }
  }
  MyServoWriteNew(90);
  Arresta();
}

/* ---------- Distanza Ostacolo -------- */
void Distanza_Ostacolo()
{ 
  //Misura la distanza degli ostacoli, destra, sinistra, destra diagonale, sinistra diagonale, Avanti e dietro. 
  // Dist_Sinistra, Dist_Destra, Dist_Avanti; Dist_Dietro, Dist_Sinistra_Diag, Dist_Destra_Diag
  detachInterrupt(digitalPinToInterrupt(pin_velocita));
  detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
  Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
  
  MyServoWriteNew(130);
  Dist_Sinistra_Diag = Misura_Distanza(SENSORE_AVANTI);
  
  MyServoWriteNew(170);
  Dist_Sinistra = Misura_Distanza(SENSORE_AVANTI);

  MyServoWriteNew(50);
  Dist_Destra_Diag = Misura_Distanza(SENSORE_AVANTI);
  
  MyServoWriteNew(10);
  Dist_Destra = Misura_Distanza(SENSORE_AVANTI);
  
  MyServoWriteNew(90);
  Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
  attachInterrupt(digitalPinToInterrupt(pin_velocita), counter, FALLING);
  attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP), Print_Info, FALLING);
}

/* ---------- Stampa informazioni in APP -------- */
 void Print_Info() {
    if (millis() - timeold2 >= 500) {
      detachInterrupt(digitalPinToInterrupt(pin_velocita));
      detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
      cm_al_secondo = pulses*2/((millis() - timeold2)/500);
      pulses = 0; 
      timeold2 = millis();
      
      if (direzione != DIR_STOP && cm_al_secondo <= 5) {
        variabile += 1;
        if(variabile >= 10){
          variabile = 0;
          if (bytericevuto == 58) {Distanza_Ostacolo();}
          else {Arresta();}
       }
      }
      else {variabile = 0;}
      attachInterrupt(digitalPinToInterrupt(pin_velocita), counter, FALLING);
      attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP), Print_Info, FALLING);
    }
    if (millis() - timeold >= 100) {
       detachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP));
       detachInterrupt(digitalPinToInterrupt(pin_velocita));
      if (direzione == DIR_INDIETRO) { cDistanza = String(Dist_Dietro);  }     
      else                           { cDistanza = String(Dist_Avanti);  }
      
      while (cDistanza.length() < 3) {  cDistanza = " " + cDistanza;     }
      Serial2.print("D"+cDistanza);
      
      cPotenza = String( map(potenza, 0, 250, 0, 100));
      while (cPotenza.length() < 3 ) {  cPotenza = " " + cPotenza;       }     
      Serial2.print("P" + cPotenza);
      
      cVelocita = String(cm_al_secondo);
      while (cVelocita.length() < 3) {  cVelocita = " " + cVelocita;     }      
      Serial2.print("V" + cVelocita);

      if ( Attivo_GPS ) {
         while (gps.available(gpsPort) ) {
           fix = gps.read(); // save the latest
           if (fix.valid.location) {
             latitudine_car = fix.latitudeL();
             longitudine_car = fix.longitudeL();   
           }      
         }
         String cAngolo_tc = String(angolo_tc,0);
         while (cAngolo_tc.length() < 3 ) {  cAngolo_tc = " " + cAngolo_tc; } 
         Serial2.print("C" + cAngolo_tc);
         
         //Angolo = QMC5883L_Angolo();
         cAngolo = String(Angolo);
         while (cAngolo.length() < 3 ) {  cAngolo = " " + cAngolo; } 
         Serial2.print("A" + cAngolo);
         
         cLatitudine = String(latitudine_car);
         while (cLatitudine.length() < 10) {  cLatitudine = " " + cLatitudine; } 
         Serial2.print("L" + cLatitudine);
      
         cLongitudine = String(longitudine_car);
         while (cLongitudine.length() < 10) {  cLongitudine = " " + cLongitudine; } 
         Serial2.print("G" + cLongitudine);

         cDistanza_tc = String(distanza_tc);
         while (cDistanza_tc.length() < 10 ) {  cDistanza_tc = " " + cDistanza_tc; } 
         Serial2.print("M" + cDistanza_tc);
      }
      timeold = millis();
      attachInterrupt(digitalPinToInterrupt(PIN_PRINT_APP), Print_Info, FALLING);
      attachInterrupt(digitalPinToInterrupt(pin_velocita), counter, FALLING);
    }
 }

 /* ---------- Avanzamento Graduale Servo ------------------ */
void MyServoWriteNew(int Gradi) {  
  int oldposition = MyServo.read();
  if (oldposition > Gradi) {
    for (int i = oldposition; i > Gradi; i -=1) {
      MyServo.write(i+offset);
      delay(3);
    }
  } 
  else if (oldposition < Gradi) {
    for (int i = oldposition; i < Gradi; i +=1) {
      MyServo.write(i+offset);
      delay(3);
    }
  }
  MyServo.write(Gradi+offset);
}


 /* ---------- Configura sensore QMC5883L  ------------------ */ 
 void  QMC5883L_Configura(){
   MyWire.beginTransmission(0x0D);
   MyWire.write(0x0B);
   MyWire.write(0x01);
   MyWire.endTransmission();  
   MyWire.beginTransmission(0x0D);
   MyWire.write(0x09);
   MyWire.write(0b11000000|0b00000000|0b00000100|0b00000001);
   MyWire.endTransmission();
 }

 /* ---------- Calibrazione sensore QMC5883L  ------------------ */ 
void QMC5883L_Calibrazione () {
  int returnvalue = 0;
  xlow = ylow = xhigh = yhigh = 0;
  unsigned long timeold = millis()+20000;
  Potenza(Potenza_Automatico*2);
    
  while ( (millis() < timeold ) && (returnvalue != ARRESTA ))  {    
       QMC5883L_Angolo(); 
       Destra();
       delay(40); 
       if (Serial2.available() > 0){ returnvalue = Serial2.read();} 
    }
    Arresta();
}

 /* ---------- Lettura sensore QMC5883L  ------------------ */ 
int QMC5883L_Angolo () {
  int nX,nY,nZ;
  float fx,fy;
  
  Arresta();
  delay(10);
  MyWire.beginTransmission(0x0D);
  MyWire.write(0x00);
  MyWire.endTransmission();
  MyWire.requestFrom(0x0D, 6);
  nX = MyWire.read() | (MyWire.read()<<8);
  nY = MyWire.read() | (MyWire.read()<<8);
  nZ = MyWire.read() | (MyWire.read()<<8);
  MyWire.endTransmission();
  
  if(nX<xlow)  xlow  += (nX - xlow )*0.2;
  if(nX>xhigh) xhigh += (nX - xhigh)*0.2;
  if(nY<ylow)  ylow  += (nY - ylow )*0.2;
  if(nY>yhigh) yhigh += (nY - yhigh)*0.2; 
 
  nX -= (xhigh+xlow)/2;
  nY -= (yhigh+ylow)/2;
  fx = (float)nX/(xhigh-xlow);
  fy = (float)nY/(yhigh-ylow);
 
  Angolo = (3.81667f + 180.0*atan2(fy,fx)/3.14159265358979323846264338327950288)-offset_bussola;
  if(Angolo<=0) Angolo += 360;
  if(Angolo>=360) Angolo -= 360;
  return Angolo;
}

/* ----------  GPS  ------------------ */ 

void Gps () {
  int returnvalue = 0;
  while (Serial2.available() < 1 ){ delay(10); }
  dataingps   = "" ;
  dataingps   = Serial2.readString();  
  latitudine_tablet  = (dataingps.substring(dataingps.indexOf("LAT")+3, dataingps.indexOf("LONG")).toInt())+offset_lat;
  longitudine_tablet = (dataingps.substring(dataingps.indexOf("LONG")+4, dataingps.length()).toInt())+offset_long;
  NeoGPS::Location_t tablet( latitudine_tablet, longitudine_tablet );
  NeoGPS::Location_t car   ( latitudine_car,    longitudine_car    );
  distanza_tc =  fix.location.DistanceKm      ( car, tablet );    
  angolo_tc   =  fix.location.BearingToDegrees( car, tablet );
  
  while (returnvalue != ARRESTA ) {
    if (Serial2.available() > 0){ returnvalue = Serial2.read();}
    Potenza(Potenza_Automatico*2);
    while (!(((Angolo+10)>=angolo_tc) && ((Angolo-10)<=angolo_tc)) && (returnvalue != ARRESTA) ) { 
      if (Serial2.available() > 0){ returnvalue = Serial2.read();}
      QMC5883L_Angolo();
      if(((Angolo-angolo_tc)<= 180) && ((Angolo-angolo_tc) >= 0)){Sinistra();}
      else {Destra();}
      delay(40);
    }
    if (distanza_tc<=1){
      Arresta();
      returnvalue = ARRESTA;
    }
    else {
      returnvalue=Automatico_GPS();
      if (distanza_tc<=1){
        Arresta();
        returnvalue = ARRESTA;
      }
    }
  }
}

  /* ---------- Avanzamento Automatico --- */
int Automatico_GPS() {
int returnvalue = 0;

    Gradi = 0;
    Potenza(Potenza_Automatico);
    Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
    while ( (Dist_Avanti > Distanza_Frontale)  && (returnvalue != ARRESTA) && (distanza_tc>1) ){  
      if (Serial2.available() > 0){ returnvalue = Serial2.read();}
      NeoGPS::Location_t tablet( latitudine_tablet, longitudine_tablet );
      NeoGPS::Location_t car   ( latitudine_car,    longitudine_car    );
      distanza_tc =  fix.location.DistanceKm      ( car, tablet );    
      angolo_tc   =  fix.location.BearingToDegrees( car, tablet );
      // guarda a destra e sinistra
      if   (DestraSinistra) { Gradi++; }
      // guarda a sinistra
      else                  { Gradi--; }
      
      MyServo.write(90+Gradi+offset);
      if (abs(Gradi) > 30) { DestraSinistra = !DestraSinistra;}
      Avanti();
      Potenza(Potenza_Automatico);
      delay(10);        
      Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
    }
    Arresta();
    if ((returnvalue != ARRESTA) && (distanza_tc>1)) {
      Distanza_Ostacolo();
      
      // nel caso viene chiuso su tre lati va indietro
      if  ( Dist_Destra   < Distanza_Laterale && Dist_Destra_Diag   < Distanza_Frontale &&
            Dist_Sinistra < Distanza_Laterale && Dist_Sinistra_Diag < Distanza_Frontale &&
            Dist_Avanti   < Distanza_Frontale && (Dist_Dietro > 25)) { 
        DestraSinistra = false;
        Gradi = 0;
        while ( Dist_Avanti < Distanza_Frontale*2 ){  
          if (DestraSinistra){  Gradi += 2;  }
          else               {  Gradi -= 2;  }
          MyServoWriteNew(90+Gradi);
          if (abs(Gradi) > 70) {
            Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
            if (Dist_Avanti > Distanza_Laterale*2)  {
              Potenza(Potenza_Automatico*3);
              if ( Gradi > 0 ) { Sinistra();        }
              else             { Destra();          }  
              delay(Tempo_Rotazione); 
              break;
            }
            DestraSinistra = !DestraSinistra;
          }
          Potenza(Potenza_Minima);
          Indietro();
          delay(10);        
          Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
          if (Dist_Dietro < 10) {  break;}
        }
        MyServoWriteNew(90);
      }
            
      // decide se girare a sinistra o destra
      else if  (Dist_Sinistra < Dist_Destra   && Dist_Sinistra < Dist_Avanti) {
          Potenza(Potenza_Automatico*3);
          Destra(); 
          delay(Tempo_Rotazione*0.75);
       }
      else if (Dist_Destra < Dist_Sinistra && Dist_Destra   < Dist_Avanti) {
        Potenza(Potenza_Automatico*3);
        Sinistra();
        delay(Tempo_Rotazione*0.75);
        }
      else if (Dist_Destra        < Distanza_Laterale)                     {
        Potenza(Potenza_Automatico*3);
        Sinistra();
        delay(Tempo_Rotazione);
        }
      else if (Dist_Sinistra      < Distanza_Laterale)                     {
        Potenza(Potenza_Automatico*3);
        Destra();
        delay(Tempo_Rotazione);
        } 
      else if (Dist_Sinistra_Diag < Distanza_Frontale)                     {
        Potenza(Potenza_Automatico*3);
        Destra();
        delay(Tempo_Rotazione);
        } 
      else if (Dist_Destra_Diag   < Distanza_Frontale)                     {
        Potenza(Potenza_Automatico*3);
        Sinistra();
        delay(Tempo_Rotazione);
        }
      else if (Dist_Destra>Dist_Sinistra && Dist_Destra>50)                {
        Potenza(Potenza_Automatico*3);
        Destra();
        delay(Tempo_Rotazione); }
      else if (Dist_Sinistra>Dist_Destra && Dist_Sinistra>50)              {
        Potenza(Potenza_Automatico*3);
        Sinistra();
        delay(Tempo_Rotazione); }
        
      else if (Dist_Avanti       <= Distanza_Frontale)                     {  
        DestraSinistra = false;
        Gradi = 0;
        while ( Dist_Avanti < Distanza_Frontale*2 ){  
          if (DestraSinistra){  Gradi += 2;  }
          else               {  Gradi -= 2;  }
          MyServoWriteNew(90+Gradi);
          if (abs(Gradi) > 70) {
            Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
            if (Dist_Avanti > Distanza_Laterale*2)  {
              Potenza(Potenza_Automatico*3);
              if ( Gradi > 0 ) { Sinistra();        }
              else             { Destra();          }  
              delay(Tempo_Rotazione); 
              break;
            }
            DestraSinistra = !DestraSinistra;
          }
          Potenza(Potenza_Minima);
          Indietro();
          delay(10);        
          Dist_Dietro = Misura_Distanza(SENSORE_INDIETRO);
          if (Dist_Dietro < 10) {  break;}
        }
     }
  motore_posteriore_dx.run(RELEASE);
  motore_posteriore_sx.run(RELEASE);
  motore_anteriore_sx. run(RELEASE);
  motore_anteriore_dx. run(RELEASE);
  Potenza(potenza);
  Distanza_Ostacolo();
  if (direzione == DIR_DESTRA){
    while(Dist_Sinistra < Distanza_Laterale*3){
      Potenza(Potenza_Automatico);
      Avanti();
      MyServoWriteNew(170);
      Dist_Sinistra = Misura_Distanza(SENSORE_AVANTI);
      if( Dist_Sinistra < Distanza_Laterale) {
        Potenza(Potenza_Automatico*3);
        Destra();
        delay(50);
        Arresta();
        }
      MyServoWriteNew(90);
      Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
      if (Dist_Avanti < Distanza_Frontale){
        Potenza(Potenza_Automatico*3);
        Destra(); 
        delay(Tempo_Rotazione*0.75);
        }
    }
    Potenza(Potenza_Automatico*3);
    Sinistra();
    delay(Tempo_Rotazione);
    Potenza(Potenza_Automatico);
    Avanti();
    delay(Tempo_Rotazione*2);
  }
  else if(direzione == DIR_SINISTRA){
    while(Dist_Destra < Distanza_Laterale*3){
      Potenza(Potenza_Automatico);
      Avanti();
      MyServoWriteNew(10);
      Dist_Destra = Misura_Distanza(SENSORE_AVANTI);
      if( Dist_Destra < Distanza_Laterale) {
        Potenza(Potenza_Automatico*3);
        Sinistra();
        delay(50);
        Arresta();
        }
      MyServoWriteNew(90);
      Dist_Avanti = Misura_Distanza(SENSORE_AVANTI);
      if (Dist_Avanti < Distanza_Frontale){
        Potenza(Potenza_Automatico*3);
        Sinistra(); 
        delay(Tempo_Rotazione*0.75);
        }
    } 
    Potenza(Potenza_Automatico*3);
    Destra();
    delay(Tempo_Rotazione);
    Potenza(Potenza_Automatico);
    Avanti();
    delay(Tempo_Rotazione*2);
    }
  
  }
  MyServoWriteNew(90);
  QMC5883L_Angolo();
  return returnvalue ; 
}