Robot "pedinatore" tramite GPS

Buon pomeriggio!
Dopo esser riuscito a far funzionare il modulo ESP anche grazie ai post di questo forum (grazie ancora!), ho deciso di voler costruire un robottino comandato tramite un app per Android da me realizzata via WiFi e, vedendo che funzionava, ho voluto proseguire cercando un modo per farmi seguire dal robot, senza che fossi io a comandarlo. Per fare ciò ho pensato di utilizzare due dispositivi Android, uno posto sul robot ed uno nella mia mano, comunicanti via WiFi per mezzo del quale si scambiano informazioni relative alla posizione GPS (latitudine e longitudine). Ho trovato su internet una formula che consente di determinare di quanto si deve spostare il robot e con buona approssimazione il calcolo risulta corretto, ciò che non mi riesce è il determinare la direzione verso cui il robot si deve girare... Ho provato a fare così:

bool determinaRotazione()
{
  calcolaAngoloCellulare();
  dirArd = 90 - dirArd; //Perché la direzione in arrivo è presa considerando il Nord come 0 e prosegue in senso positivo per il verso orario, quindi va portata in "normali"
  float difDir = dirPhone - dirArd;
  Serial.println("Direzione cellulare ");
  Serial.println(dirPhone);
  Serial.println("Direzione arduino: ");
  Serial.println(dirArd);
  Serial.println("Differenza rotazione: ");
  Serial.println(difDir);

  if (difDir >= DIFF_ANGOLO_MAX)
  {
    if (difDir > 180) { //Se la differenza è maggiore di 180 gradi, gli conviene girare nell'altro senso per arrivare prima al punto d'interesse
      giraDestra();
    } else {
      giraSinistra();
    }

    return true;
  }
  else if (difDir < -DIFF_ANGOLO_MAX)
  {
    if (difDir > 180) { //Se la differenza è maggiore di 180 gradi, gli conviene girare nell'altro senso per arrivare prima al punto d'interesse
      giraSinistra();
    } else {
      giraDestra();
    }
    return true;
  }
  return false;
}

void calcolaAngoloCellulare() {
  float distanzaDaPercorrere = distanceInMetersBetweenEarthCoordinates(latArd, lonArd, latPhone, lonPhone);
  float y = latPhone - latArd;
  Serial.println("La y e ");
  Serial.println(y);
  float x = lonPhone - lonArd;
  Serial.println("La x e ");
  Serial.println(x);
  dirPhone = 2 * atan( y / ( sqrt(x * x + y * y) + x)); //restituisce il valore in radianti
  dirPhone = dirPhone * 180 / M_PI; //trasformiamo il valore in gradi
  Serial.println("La dirPhone e ");
  Serial.println(dirPhone);
}

Ma non ha funzionato, quindi cercando ho trovato un progetto che è simile di Hacker house, "Autonomous-Follow-Me-Cooler" che fa così:

 float y = sin(b.lon-a.lon) * cos(b.lat);

  float x = cos(a.lat)*sin(b.lat) - sin(a.lat)*cos(b.lat)*cos(b.lon-a.lon);

  return atan2(y, x) * RADTODEG;

Ma ugualmente non ha funzionato ed inoltre non essendoci note non si capisce bene la motivazione di quei calcoli...! Allego lo sketch completo.
Qualcuno ha mai provato a fare una cosa del genere?
Mi chiedevo inoltre se potesse dipendere dal fatto che debba arrotondare il valore del GPS dato che in uno dei dispositivi compaiono molte cifre decimali..!
Grazie in anticipo!

pi_Argomenti.ino (7.61 KB)

Mi è stato consigliato di vedere questa pagina
https://www.movable-type.co.uk/scripts/latlong.html

In cui sono riportate molte formule per il calcolo della distanze e degli angoli... Quella che fa al caso mio mi pare essere questa, "Rhumb-line distance between two points"

/**
 * Returns the bearing from ‘this’ point to destination point along a rhumb line.
 *
 * @param   {LatLon} point - Latitude/longitude of destination point.
 * @returns {number} Bearing in degrees from north.
 *
 * @example
 *     var p1 = new LatLon(51.127, 1.338);
 *     var p2 = new LatLon(50.964, 1.853);
 *     var d = p1.rhumbBearingTo(p2); // 116.7 m
 */
LatLon.prototype.rhumbBearingTo = function(point) {
    if (!(point instanceof LatLon)) throw new TypeError('point is not LatLon object');

    var φ1 = this.lat.toRadians(), φ2 = point.lat.toRadians();
    var Δλ = (point.lon-this.lon).toRadians();
    // if dLon over 180° take shorter rhumb line across the anti-meridian:
    if (Δλ >  Math.PI) Δλ -= 2*Math.PI;
    if (Δλ < -Math.PI) Δλ += 2*Math.PI;

    var Δψ = Math.log(Math.tan(φ2/2+Math.PI/4)/Math.tan(φ1/2+Math.PI/4));

    var θ = Math.atan2(Δλ, Δψ);

    return (θ.toDegrees()+360) % 360;
};

Il problema è che andando a fare i conti con la calcolatrice non ottengo gli stessi risultati del calcolatore presente sulla pagina... Le operazioni da me eseguite (con Wolfram) sono state le seguenti:

lon2 - lon1 = 1.16775 , che il sito chiama Δλ ;

log(tan(0.7392/2+PI/4)/tan(0.8791/2+PI/4)) = -0.203252 , il Δψ del sito; 

arctan(Δλ, Δψ) = -0.172328  ( θ ) ;

in gradi : -9.874 ;

(-9.874 +360) % 360 = 0.03 (dove la % è l'operazione di resto della divisione)

Qualcuno ha qualche idea sul perché non ottenga gli stessi risultati??