Cap a gauche ou à droite?

Nous avons enfin nos coordonnées et notre cap!
Il reste encore quelques petits calculs a faire:
Si notre mobile suit un capGPS à 30° et qu'il doit suivre un capDir à 230°, doit t'il tourner a droite ou a gauche pour se retrouver le plus rapidement possible dans l'axe de son nouveau cap?

C'est bien l'un des simple problème qui m'a pourtant donné le plus de fil a retordre!

J'ai fini par le résoudre en décidant de prendre CapDir comme référence et de calculer CapDir + 180 comme opposé + la correction nécessaire si CapDir + 180 dépasse les 360°... et en conditionnant que si "capGPS" empiète sur le secteur de 180° a droite de CapDir, le mobile doit virer a gauche...Et vice-versa...

Voici encore un dessin pour comprendre le principe et le bloc de programme Processing a mettre en œuvre pour les tests :

int capGPS = 81;
int capDir = 80;
int OpposeCapDir =0; // la valeur en degré de l'opposé de la diagonale de CapDir
int ComplementOpposeCapDir = 0; // si le calcul de la diagonale dépasse 360°
void setup() {
  size(200, 200);
  noLoop();
  println ( "capGPS = " +capGPS);
  println ("capDir ="+capDir);
}

void draw (){
  CarteDir();
}


void CarteDir(){
  if ( capGPS != capDir ){ // si égal, ne fait rien
  OpposeCapDir = capDir +180;
  //------------------------------------------------------
  if (OpposeCapDir <= 360){
    if ((capGPS > capDir) && (capGPS < OpposeCapDir)){
      println ("virage a gauche");
  } else {
      println ("virage a droite");
  }
  }
  
  if (OpposeCapDir > 360) {// débordement des 360°
  ComplementOpposeCapDir = OpposeCapDir - 360;
  if ((capGPS > capDir) || (capGPS < ComplementOpposeCapDir)){
    println ("virage a gauche");
  } else {
      println ("virage a droite");
    }
  }
  }
}
    
    

Note: Virage à droite = Allumage moteur gauche.
          Virage à gauche =  Allumage moteur droit.

Dans l'exemple ci-dessus, si "CapGPS" se trouve dans la partie en jaune, le mobile doit tourner sur la gauche pour se caler sur "CapDir". En effet, "CapGPS" est plus grand que "CapDir", mais plus petit que"OpposeCapDir" .
(Le mobile devrait tourner sur la droite si  "CapGPS"était plus grand que "OpposeCapDir", mais plus petit que "CapDir").
 Le calcul n'est pas compliqué...

 Dans l'exemple ci-dessus, les choses se compliquent, car CapDir + 180 donne 430°, ce qui n'existe pas:
"ComplementOpposeCapDir" représente donc la partie qui "repart de zéro" et vient finaliser le demi-cercle quand "CapDir + 180°" dépasse les 360°...
Il faut finalement comparer si "CapGPS"est dans le secteur de "CapDir" à 360 ou de 0 à "ComplementOpposeCapDir"...
Si "CapGPS" se trouve dans ces secteurs, notre mobile doit virer a gauche pour se rapprocher de la direction "CapDir".
C'est assez difficile a interpréter, mais facile a comprendre si l'on se fait (comme moi) une maquette de compas en papier et carton...

Ouf!

Propulsion et Pilotage...

A présent, notre module sait quelle direction il doit prendre pour se déplacer et a quelle altitude il doit voler...
Il faut maintenant lui en donner les moyens.

J'ai opté pour deux moteurs électriques fixes de propulsion principaux + deux moteurs d'appoint orientables qui serviront a élever le mobile ou le faire descendre, mais aussi a lui faire changer de direction en accélérant un des moteurs. De cette manière, on se passe de systèmes de gouvernes, ce qui allège l'ensemble...

Il nous faut donc:
- 1 relais commandé pour les moteurs propulsion principaux.
- 1 relais commandé pour le moteur de direction droit.
- 1 relais commandé pour le moteur de direction gauche.
- 1 servos commandé pour l'inclinaison verticale des moteurs de direction.

 Outre l'occupation de 2 pins ( RX, TX ) pour l'acquisition des données du module GPS,
Ce système nécessite 4 pins de l’Arduino pour commander le module,
ce qui revient a 6 pins pour commander l'ensemble du mobile.
(On pourra, a l'occasion, réserver 1 ou 2 pins pour d'autres fonctions (appareil photo, flash lumineux...)

Moteurs:
Il s'agit de moteurs de "jouets", bien connus des bricoleurs appelé aussi le "130".
Ils n'ont pas un rendement terrible, mais ils ne sont pas cher et on trouve facilement des hélices en plastique tri-pales qui s'adaptent rapidement sur leur axe...
Poids : 17 grammes.
Leur alimentation se fait sur 3 Volts et ils consomment 0,38 Ampères... (1,23 Waths)
Calcul de l'autonomie:
1,23 Whats /3 Volts = 0,41 Ampères.

2 piles baton AAA rechargeables de 2000 mAh en série=
3 Volts et 4000 mAh

4000 mAh / 0,41 mAh = 9, 75 heures de fonctionnement au maximum...

Poids:
-1 moteur  =                                17g
-2 piles AA 2100 mAh =             52g
                                                 -------
                                                   69g  X 4  = 276 g

Programme Arduino pour calcul de Cap

Suite au programme de test convainquant sous Processing, le voici à la sauce Arduino:
Comme indiqué, il faut entrer les valeurs dans les 8 premières variables...

int LatDepart = 473771;
char DirectionLatDepart = 'N';
int LongDepart = 37082;
char DirectionLongDepart = 'E';

int LatArrive = 479027;
char DirectionLatArrive = 'N';
int LongArrive = 19057;
char DirectionLongArrive = 'E';

char Tendance1 = 'A'; // pour Nord ou Sud 
char Tendance2 = 'A'; // pour Est ou West
float DimensionRectangleLatitudeDegres = 0.3;
float DimensionRectangleLongitudeDegres = 0.3;
float DimensionRectangleLatitudeMetres = 0.3;
float DimensionRectangleLongitudeMetres = 0.3;
float DimensionRectanglePourcent = 025.03;
int DegresBase = 0;
int Cap =0;
void setup() {
  Serial.begin(9600);
  Serial.println ("Connection OK!..");
  Serial.print ("Calculateur de Cap");
  Serial.println ("Entrer valeurs en INT dans les 8 premières variables");
}

void loop() {
DetectionDirectionLatitude (); 
 DetectionDirectionLongitude ();
 DefinitionRectanglePourcent ();
 DefinitionRectangleDegres ();
 DefinitionCap ();
}
void DetectionDirectionLatitude (){
if ((LatDepart > LatArrive) && (DirectionLatDepart == 'N') && (DirectionLatArrive == 'N')){
  Tendance1 = 'S';
  DimensionRectangleLatitudeDegres = LatDepart - LatArrive;
}
if ((LatDepart > LatArrive) && (DirectionLatDepart == 'S') && (DirectionLatArrive == 'S')){
  Tendance1 = 'N';
   DimensionRectangleLatitudeDegres = LatDepart - LatArrive;
}
if ((LatDepart < LatArrive) && (DirectionLatDepart == 'N') && (DirectionLatArrive == 'N')){
  Tendance1 = 'N';
   DimensionRectangleLatitudeDegres = LatArrive - LatDepart;
}
if ((LatDepart < LatArrive) && (DirectionLatDepart == 'S') && (DirectionLatArrive == 'S')){
  Tendance1 = 'S';
  DimensionRectangleLatitudeDegres = LatArrive - LatDepart;
}
if  (DirectionLatDepart != DirectionLatArrive){
  Tendance1 = DirectionLatArrive;
   DimensionRectangleLatitudeDegres = LatDepart + LatArrive;
}
 DimensionRectangleLatitudeMetres = DimensionRectangleLatitudeDegres * 111730;
} 
void DetectionDirectionLongitude (){
if ((LongDepart > LongArrive) && (DirectionLongDepart == 'E') && (DirectionLongArrive == 'E')){
  Tendance2 = 'W';
  DimensionRectangleLongitudeDegres = LongDepart - LongArrive;
}
if ((LongDepart > LongArrive) && (DirectionLongDepart == 'W') && (DirectionLongArrive == 'W')){
  Tendance2 = 'E';
  DimensionRectangleLongitudeDegres = LongDepart - LongArrive;
}
if ((LongDepart < LongArrive) && (DirectionLongDepart == 'E') && (DirectionLongArrive == 'E')){
  Tendance2 = 'E';
  DimensionRectangleLongitudeDegres = LongArrive - LongDepart;
}
if ((LongDepart < LongArrive) && (DirectionLongDepart == 'W') && (DirectionLongArrive == 'W')){
  Tendance2 = 'W';
   DimensionRectangleLongitudeDegres = LongArrive - LongDepart;
}
if  (DirectionLongDepart != DirectionLongArrive){
  Tendance2 = DirectionLongArrive;
  DimensionRectangleLongitudeDegres = LongDepart + LongArrive;
}
DimensionRectangleLongitudeMetres = DimensionRectangleLongitudeDegres * 75840;
  Serial.print ("Tendance  =    ");
  Serial.print (Tendance1);
  Serial.print (",");
  Serial.println (Tendance2);
  
   Serial.print ("  Dimension Rectangle Latitude en Degres =   ");
   Serial.println (DimensionRectangleLatitudeDegres);
   Serial.print ("                               en metres =   ");
   Serial.println (DimensionRectangleLatitudeMetres);
   Serial.print ("  Dimension Rectangle Longitude en Degres =  ");
   Serial.println (DimensionRectangleLongitudeDegres);
   Serial.print ("                               en metres =   ");
   Serial.println (DimensionRectangleLongitudeMetres);
} 
//-----------------------------------------------------------------------------------------------------
void DefinitionRectanglePourcent (){
  if (DimensionRectangleLatitudeMetres > DimensionRectangleLongitudeMetres ){
    DimensionRectanglePourcent = (100 * DimensionRectangleLongitudeMetres) / DimensionRectangleLatitudeMetres;
}
 if (DimensionRectangleLatitudeMetres < DimensionRectangleLongitudeMetres ){
    DimensionRectanglePourcent = (100 * DimensionRectangleLatitudeMetres) / DimensionRectangleLongitudeMetres;
 }
   Serial.print("                               en pourcent =   ");
   Serial.print (DimensionRectanglePourcent);
   Serial.println ("  %");
}
void DefinitionRectangleDegres (){
  if (DimensionRectanglePourcent <= 5.241){
    DegresBase = 0;
  }
  if (DimensionRectanglePourcent > 5.241){
    DegresBase = 3;
  }
   if (DimensionRectanglePourcent > 9){
    DegresBase = 6;
  }
   if (DimensionRectanglePourcent > 12){
    DegresBase = 9;
  }
   if (DimensionRectanglePourcent > 20){
    DegresBase = 12;
  }
   if (DimensionRectanglePourcent > 24){
    DegresBase = 15;
  }
   if (DimensionRectanglePourcent > 30){
    DegresBase = 18;
  }
   if (DimensionRectanglePourcent > 35){
    DegresBase = 21;
  }
   if (DimensionRectanglePourcent > 42){
    DegresBase = 24;
  }
   if (DimensionRectanglePourcent > 48){
    DegresBase = 27;
  }
   if (DimensionRectanglePourcent > 55){
    DegresBase = 30;
  }
   if (DimensionRectanglePourcent > 62){
    DegresBase = 33;
  }
   if (DimensionRectanglePourcent >70){
    DegresBase = 36;
  }
   if (DimensionRectanglePourcent > 78){
    DegresBase = 39;
  }
   if (DimensionRectanglePourcent > 88){
    DegresBase = 42;
  }
   if (DimensionRectanglePourcent > 93){
    DegresBase = 45;
  }
 Serial.print ("Degres de base  =    ");
 Serial.print (DegresBase);
 Serial.println ("   degres");   
}
void DefinitionCap () { 
  if  ((Tendance1== 'N') && (Tendance2 == 'E') && (DimensionRectangleLongitudeDegres < DimensionRectangleLatitudeDegres)){
  Cap = DegresBase;
}
if  ((Tendance1== 'N') && (Tendance2 == 'E') && (DimensionRectangleLongitudeDegres > DimensionRectangleLatitudeDegres)){
  Cap = 90 - DegresBase;
}
 if  ((Tendance1== 'S') && (Tendance2 == 'E') && (DimensionRectangleLongitudeDegres < DimensionRectangleLatitudeDegres)){
  Cap = 180 - DegresBase;
}
if  ((Tendance1== 'S') && (Tendance2 == 'E') && (DimensionRectangleLongitudeDegres > DimensionRectangleLatitudeDegres)){
  Cap = 90 + DegresBase;
}
 if  ((Tendance1== 'S') && (Tendance2 == 'W') && (DimensionRectangleLongitudeDegres < DimensionRectangleLatitudeDegres)){
  Cap = 180 + DegresBase;
}
if  ((Tendance1== 'S') && (Tendance2 == 'W') && (DimensionRectangleLongitudeDegres > DimensionRectangleLatitudeDegres)){
  Cap = 270 - DegresBase;
}
 if  ((Tendance1== 'N') && (Tendance2 == 'W') && (DimensionRectangleLongitudeDegres < DimensionRectangleLatitudeDegres)){
  Cap = 360 - DegresBase;
}
if  ((Tendance1== 'N') && (Tendance2 == 'W') && (DimensionRectangleLongitudeDegres > DimensionRectangleLatitudeDegres)){
  Cap = 270 + DegresBase;
}
 Serial.print ("CAP a suivre =  ");
 Serial.println (Cap);     
}

Faites vous-même vos essais...
Dans le programme final, les paramètres de départ sont remplacés par les paramètres de route envoyés par le GPS

Test convert

Note:

Dans ce programme de test de conversion d'une chaine en {int}, "ChaineLongitude" doit toujours avoir la même longueur,

(ce qui est bien le cas de la Longitude renvoyée par le module GPS).

char ChaineLongitude[11] = "123.123456";
char CaractereChaineLongitude = 'A';
int TestCaractereLongitude = 0;
int ChiffreLongitude = 0;
int Compteur1 = 0;
int Compteur2 = 0;


void setup() {
Serial.begin(9600); 

}

void loop() {
  DecodageCharenInt();
  AffichageValeur();
}

void DecodageCharenInt(){
 Compteur1 = 10; //pour multiplication (dixaines, cent...)
Compteur2 = 10; //pour capture caractere dans chaine 

 ChiffreLongitude = ChaineLongitude[Compteur2]; // capture premier caractere sur la droite (unités)

while (Compteur1 < 100000000){
 TestCaractereLongitude = int (ChaineLongitude[Compteur2]);
 if  (( TestCaractereLongitude >= 48) && ( TestCaractereLongitude <= 57)){
   ChiffreLongitude = ChaineLongitude[Compteur2]*Compteur1;
   Compteur1 = Compteur1 *10;
 }
 Compteur2--;
 ChiffreLongitude = ChaineLongitude[Compteur2];   
} 
}
void AffichageValeur(){
  Serial.println (ChaineLongitude);
  Serial.println (ChiffreLongitude);
  Serial.println ("------------");
}

Mise en oeuvre du GY 26

La vitesse de transmission série du module GY 26 est de 9600bps.

Il s'agit bien d'un véritable compas (comparable a une bousole) et non pas un gyro-compas 3 axes (qu'il est d'ailleur difficile a configurer et est plutôt réservé pour la stabilisation des drones-hélico...)
Ce module là est bien facile a utiliser et est relativement bien précis, suivant mes tests.
Quand on envoie la "commande" (0x31) sur le port série, le module GY 26 répond par une trame de 8 octets au format ASCII dont voici le détail:

Octet0:0x0D              "Enter" en ASCII
Octet1:0x0A              "new line" en ASCII
Octet2:0x30~0x33     Chiffre des centaine de l'angle (0 à 3)
Octet3:0x30~0x39     Chiffre des dizaine de l'angle (0 à 9)
Octet4:0x30~0x39     Chiffre des unités de l'angle (0 à 9)
Octet5:0x2E               "point décimal" en ASCII
Octet6: 0x30~0x39     Chiffre de la décimale de l'angle (0 à 9)
Octet7: 0x00~0xFF     (somme de vérification)

Octet7= partie basse des octets (Octet0+ Octet1+ Octet2+……Octet6)

Exemple: <0x0D-0x0A-0x33-0x35-0x39-0x2E-0x36-0x1C> = 359.6°


Dans notre projet, seuls les octets 2,3 & 4 nous intéressent.
Nous allons créer un tableau "char" pour réceptionner nos 8 octets, car ils sont au format ASCII, puis nous capturerons les octets 2,3 & 4 pour les convertir au format "int".
 La valeur rendue va de 0° a 359°.


Voici un programme Arduino (Mega) de test:
_____________________________________________________________

/*
Gy-26 - Compass.
Base créee par Igor Araujo
Ce programme récupére les données d'une boussole électronique
et l'affiche sur le port serie vers l'ordinateur
a tester!!!
 Serial 2: 17 (RX) and 16 (TX)
*/

 // RX is digital pin 17 (connect to TX of other device)
 // TX is digital pin 16 (connect to RX of other device)

char ChaineBoussole[8];
int ResultatDegresBoussole = 0;
int compteur = 0;
byte Drapeau = 0;

void setup() {
  // Mise en route du port série et serie2
  Serial2.begin(9600);
  Serial.begin(9600);
  // test connection serie:
  Serial.println("Goodnight moon!");
}


void loop() {
  Lecture();
}

void Lecture() {
  // vidange tampon serie
  compteur = 0;
  while (Serial2.available()) {
      ChaineBoussole[compteur] = Serial2.read();
  }
  Drapeau = 0; // drapeau va bloquer le programme le temps que les datas soient transmis par le module
  // demande de réception datas
  Serial2.write(0x31);
  while (Drapeau == 0) {
    if (Serial2.available()) {
      ChaineBoussole[compteur] = Serial2.read();
      compteur = (++ compteur);
      if (compteur == 5) {
        ResultatDegresBoussole = (ChaineBoussole[2] - 48) * 100 + (ChaineBoussole[3] - 48) * 10 + (ChaineBoussole[4] - 48);
        Drapeau = 1;
      }
    }
  }
  Serial.println(ResultatDegresBoussole);
  delay(500);
}