Aller au contenu


Photo
* * * * * 1 note(s)

SLAMTEC RPLIDAR A2: développement en C d'un driver 4K ultra rapide

SLAMTEC RPLIDAR A2 4K Embarqué Integer FPU FIxed point

  • Veuillez vous connecter pour répondre
57 réponses à ce sujet

#1 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 417 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 17 avril 2018 - 08:15

Bonjour les roboticiens !

 

Pour ceux qui ne savent pas, je bosse avec Mike118 sur un framework complet de robot web autonome à faible latence. Du hardware en passant par le système d'exploitation jusqu’à l'interface web. Mettant en avant la plus faible réactivité possible et des déplacements rapides et précis...

 

Mike m'aide souvent à créer des algos avec des partie mathématique que je n'ai pas en tête et je l'en remercie.

Ce logiciel existe sur 3 robots au monde actuellement, les 2 miens et le sien, c'est tout. J'ouvre ce thread comme un "BLOG" de suivi sur l'avancement de ce problème sur lequel je rame depuis maintenant 2 jours, avec en prime ce driver de parcing 4K ulta-optimisé et fonctionnel à la fin...

 

Suite à l'implémentation d'arithmétique entière et d'une table de sinus 16-bits/16-bits dans la mémoire flash de notre PIC32 qui ne dispose pas d'une FPU, la puissance de traitement disponible à été démultipliée et du coup le mode 4K de ce LIDAR devient intéressant pour améliorer les perfs de nos algos de localisation:D

J'ai donc re-testé mon algo de décodage RPLIDAR 4K et il toujours bugué, le vilain bug n'est toujours pas mort malgré tout ce temps lol

Par contre niveau ressources dispo sur le PIC32 c'est la fête, il faut donc qu'on trouve ce problème absolument !

 

Le thread de Path qui à déjà fait le décodage 4K mais en JavaScript :

http://www.robot-maker.com/forum/topic/11759-le-rp-lidar-a2/

Le code concerné que je connais presque par coeur mais qui ne m'a pas aidé a résoudre mon bug :

https://github.com/P...express-scan.js

 

Les pages du datasheet RPLIDAR A2 qui nous intéressent vont de 19 à 23. Pour rappel :

 

La trame :

Fichier joint  Cabin.png   45,38 Ko   27 téléchargement(s) Fichier joint  Format.png   61,15 Ko   28 téléchargement(s)

 

Description des donnés :

Fichier joint  Format1.png   135,71 Ko   27 téléchargement(s) Fichier joint  Format2.png   69,43 Ko   26 téléchargement(s)

 

Le calcul de l'angle :

Fichier joint  Maths.png   38,74 Ko   27 téléchargement(s)

 

Le BUG :

Les points verts sont les impacts lasers avec un historique de quelques milliers de points = plusieurs frames lidar complètes.

Les lignes blanches sont celles qui sont affichées en ce moment = un fragment d'une trame lidar, soit 30 points (limitation bande passante radio)

 

0) L'algo 2K fonctionne parfaitement, mais pas en 4K

1) L'angle correct est horizontal, le robot est perpendiculaire au mur de devant, il est aligné à la pièce.

2) Une partie des mesures, si il en existe qui sont à une petite distance du robot sont correctes, on visualise les mystérieux décrochages de chaque côtés quand la distance augmente.

3) Le reste du nuage de point est en retard de 9 degrés !

 

Je recule légèrement le robot du mur de charge :

Fichier joint  1.png   40,02 Ko   27 téléchargement(s) Fichier joint  2.png   80,88 Ko   26 téléchargement(s)

 

Et encore plus loin :

Fichier joint  3.png   43,31 Ko   31 téléchargement(s)

 

Et sur cette capture, avec le robot au centre de la pièce, on vois bien les 9 degrés d'erreur niveau affichage du Theta

La pièce est inclinée or que le robot est parfaitement aligné :

Fichier joint  Erreur9deg.png   768,58 Ko   28 téléchargement(s)


  • Melmet aime ceci

Make your own raspberry robot, control it over the internet in FPV mode with super low latency and securely share access on it to your friends with Vigibot.com! You are welcome to test our robots now!
https://www.vigibot.com(100% free, no ads)


#2 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 9 934 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 17 avril 2018 - 08:45

Quelques notes pour aider à la réflexion : 

 

Une cabine est composée de 16 sous-cabines  : "cabin[0] à cabin[15] " contenant chacune 2 mesures de distances soit 32 mesures par cabines. 

En mode 4K le but c'est d'avoir 4000 mesures par secondes soit environ 125 cabines par secondes... 
Soit 200 mesures par tours...  soit environ 1,8 ° entre chaque mesures ...

( Le lidar tourne à 20 Hz ,  => 125/20  = 6,25 donc en gros on a entre 6 et 7 cabines par tours  ...  ) 

 

Une cabine complète représente environ  1,8° * 32 = 57, 6 ° ...

 

 

En regardant les photo de pascal ( serveurperso ) je me dit que le problème n'est pas en lien avec le fait de sauter une cabine ou une sous cabine ... 

On voit clairement que la zone qui diffère des autres n'est pas " angulairement constante " ce qui serait le cas je pense si c'était un problème de lecture de cabine ... 
Par contre il semblerait que le problème soit lié à la distance de l'obstacle ...  En gros j'ai l'impression qu'on observe le décalage pour tous les points de lidar dont la distance est inférieur à une certaine limite ...  Ce qui permet d'expliquer l'écart angulaire variable en fonction de la distance du robot par rapport au mur ...

 

 

En poussant le raisonnement j'ai envie de supposer que dès que la distance mesurée est top grande le calcul de la virgule se fait mal ... Et 9° est ajouté à l'angle au lieu de 0,9° ou 10° au lieu de 0,1° ...  Ou un truc du genre ce qui expliquerait le décalage décalage de 9 à 10° ... 


  • Serveurperso et Melmet aiment ceci

Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 
Si vous souhaitez un robot pilotable par internet n'hésitez pas à visiter www.vigibot.com et à lire le sous forum dédié à vigibot!

 

Les réalisations de Mike118  

 

 

 


#3 Path

Path

    Made By Humans

  • Modérateur
  • PipPipPipPipPip
  • 2 504 messages
  • Gender:Male
  • Location:Paris

Posté 17 avril 2018 - 08:49

Content que tu n'aies pas vu de bug dans le javascript ^^ Si t'as des remarques, n'hésites pas. Je le partage pour ça :)

 

Ton code, c'est toujours celui-là ? http://www.robot-maker.com/forum/topic/10903-hector/?p=90957

 

De mémoire, j'avais un pb du même genre (décalage plus de petites distorsion) avant que je gère bien le bit de signe. (http://www.robot-maker.com/forum/topic/10903-hector/?p=91236) Mais chez toi, il ne semble pas y avoir de distorsions comme j'avais.



#4 Ulysse

Ulysse

    Membre passionné

  • Membres
  • PipPipPip
  • 486 messages
  • Gender:Not Telling

Posté 17 avril 2018 - 08:53

Dites-moi les enfants,

j'ai beau programmer (c'est mon métier) depuis XX années, connaitre les sujets techniques que vous abordez (les PIC, la radio, le numérique, l'analogique) - et quand je dis connaître c'est pratiquer -  et je suis plutôt à l'aise pour parler technique (sauf Linux, c'est comme ça, chacun son truc)

Et bien voyez-vous j'ai un souci : je comprend pas tout. Que dalle en fait. Ça doit venir de moi ... C'est grave docteur ?



#5 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 417 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 17 avril 2018 - 08:59

En fait j'ai pas fini de rédiger ni le premier post ni celui avec tout le code ultra commenté ligne à ligne (façon debug canard jaune) !!!

ça arrive:)

 

 

 

Content que tu n'aies pas vu de bug dans le javascript ^^ Si t'as des remarques, n'hésites pas. Je le partage pour ça :)

 

Ton code, c'est toujours celui-là ? http://www.robot-maker.com/forum/topic/10903-hector/?p=90957

 

De mémoire, j'avais un pb du même genre (décalage plus de petites distorsion) avant que je gère bien le bit de signe. (http://www.robot-maker.com/forum/topic/10903-hector/?p=91236) Mais chez toi, il ne semble pas y avoir de distorsions comme j'avais.

 

En fait j'ai que la remarque du code en double pour ton javascript mais il est hyper clair plus que le mien qui se veux optimisé a mort

En fait c'est exactement ce que j'ai fait a l'époque mais en moins condensé.


Make your own raspberry robot, control it over the internet in FPV mode with super low latency and securely share access on it to your friends with Vigibot.com! You are welcome to test our robots now!
https://www.vigibot.com(100% free, no ads)


#6 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 9 934 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 17 avril 2018 - 09:05

Dites-moi les enfants,

j'ai beau programmer (c'est mon métier) depuis XX années, connaitre les sujets techniques que vous abordez (les PIC, la radio, le numérique, l'analogique) - et quand je dis connaître c'est pratiquer -  et je suis plutôt à l'aise pour parler technique (sauf Linux, c'est comme ça, chacun son truc)

Et bien voyez-vous j'ai un souci : je comprend pas tout. Que dalle en fait. Ça doit venir de moi ... C'est grave docteur ?

 

En fait le thème en simplifié  : utiliser le RPLIDAR A2 Slametech en mode 4K  = 4000 mesures par secondes au lieu de l'utiliser en mode 2K comme on l'utilisait jusqu'à présent et qui lui fonctionne très bien sur notre projet. 

Donc on étudie la doc technique du capteur. On a fait un code en fonction de cette doc technique mais on observe un petit bug et du coup on cherche où se cache l'erreur. 

Bon en bonus le but c'est de faire un truc optimisé et élégant. 



 


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 
Si vous souhaitez un robot pilotable par internet n'hésitez pas à visiter www.vigibot.com et à lire le sous forum dédié à vigibot!

 

Les réalisations de Mike118  

 

 

 


#7 Path

Path

    Made By Humans

  • Modérateur
  • PipPipPipPipPip
  • 2 504 messages
  • Gender:Male
  • Location:Paris

Posté 17 avril 2018 - 09:06

Pour répondre à ta question sur l'autre sujet, oui, les angles sont bons. J'ai laissé les 3 axes du repère sur la restitution pour vérifier tout ça. Les distances sont bonnes et les formes que les points reproduisent sont très fidèles.



#8 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 417 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 17 avril 2018 - 09:23

bool readLidar() {
 uint8_t current;                  // Octet courant récupéré dans le FIFO, on while la dessus tant qu'il y en a.
 static uint8_t n = 0;             // Etat courant de la state machine
 static uint8_t checksum;          // Checksum présent dans la trame lidar
 static uint8_t sum = 0;           // Variable de calcul du checksum
 static uint16_t oldStartAngleQ6;  // Ancien angle de référence
 static uint16_t startAngleQ6 = 0; // Comporte l'angle de référence complet
 //static bool start;              // Pas utile
 static uint16_t diffAngleQ6;      // Calcul 
 static uint8_t m = 0;             // Etat courant de la sous-state machine pour le décodage des cabines
 static uint8_t s = 0;             // Compteur des mesures de cabines, 32 mesures entre les angles de références

 static int8_t deltaAnglesQ5[RPLIDARNBSAMPLES]; // Comporte les 32 compensations angulaires
 static uint16_t distances[RPLIDARNBSAMPLES];   // Comporte les 32 mesures de distances

 // Résultats, a passer en arithmétique entière quand ce sera OK
 float distance;
 float angle;
 float lidarx;
 float lidary;

 static uint16_t j = 0; // Position d’écriture des tableaux comportant les résultats.
 lidarReady = true;     // Globale qui devra indiquer si le lidar fonctionne ou pas (pas encore fait en 4K)



 while(RXLIDAR.available()) {
  current = RXLIDAR.read();

  switch(n) {

   case 0:                     // Figure 4-11 offset +0
    if(current >> 4 == 0xA) {
     checksum = current & 0xF;
     n = 1;
    }
    break;

   case 1:                     // Figure 4-11 offset +1
    if(current >> 4 == 0x5) {
     checksum |= current << 4;
     n = 2;
    } else
     n = 0;
    break;

   case 2:                     // Figure 4-11 offset +2
    sum ^= current;
    oldStartAngleQ6 = startAngleQ6;
    startAngleQ6 = current;
    n++;
    break;

   case 3:                     // Figure 4-11 offset +3
    sum ^= current;
    startAngleQ6 |= (current & 0x7F) << 8;
    /*start = current >> 7;
    if(start)
     TXDEBUG.println("start");*/







    // Voir début de la page 23, c'est juste pour obtenir l'angle relatif entre 2 bloc de 32 mesures !
    // Le test sert juste à soustraire le wrap quand le lidar passe par Theta = zéro.
    // Je bosse directement en Q6 (partie déjà optimisée)

    if(oldStartAngleQ6 > startAngleQ6)
     diffAngleQ6 = (360 << 6) + startAngleQ6 - oldStartAngleQ6;
    else
     diffAngleQ6 = startAngleQ6 - oldStartAngleQ6;





    // On boucle sur les 32 mesures #define RPLIDARNBSAMPLES 32 à renommer (lol)
    // Le but est d'ajouter l'angle de compensation à chaque mesures
    // par rapport à l'angle complet donné qu'une fois toute les 32 mesures


    for(uint8_t i = 0; i < RPLIDARNBSAMPLES; i++) {
     distance = distances[i];
     if(distance) {                                                       // Si la lecture est valide

      // Calcul de l'angle en float
      angle = (
                float(oldStartAngleQ6) +                                   // Je prend l'ancienne valeur de l'angle absolu initial car on bosse avec une nouvelle entête.
                float(diffAngleQ6) * float(i) / float(RPLIDARNBSAMPLES) -  // Calcul de l'angle théorique par division bêtes...
                (float(deltaAnglesQ5[i]) * 2)                              // Petite correction angulaire "pour prendre en compte les variations de vitesse du moteur" le "* 2" c'est juste pour passer de Q5 en Q6
               ) / 64.0 * PI / 180.0;





/*                              Ici c'est la même chose en plus bourrin (comme le datasheet)
      angle = (
                (float(oldStartAngleQ6) / 64.0) +
                (float(diffAngleQ6) / 64.0) * float(i) / float(RPLIDARNBSAMPLES)//-
                //(float(deltaAnglesQ5[i]) * 2.0 / 64.0)
               ) * PI / 180.0;
*/




      // Cette partie est propre à nos robots, on stock les données brutes et les données cartésiennes.
      lidarx = POSITIONLIDARX + distance * sinFloat(angle);
      lidary = POSITIONLIDARY + distance * cosFloat(angle);
      if(lidarx > POSITIONSROBOTXMAX || lidarx < POSITIONSROBOTXMIN ||
         lidary > POSITIONSROBOTYMAX || lidary < POSITIONSROBOTYMIN) { // Si la lecture est hors du robot
       lidarsx[j] = int(lidarx);
       lidarsy[j] = int(lidary);
       lidarsCartex[j] = int(lidarx * cosFloat(theta) + lidary * sinFloat(theta) + odometriex * 1000.0);
       lidarsCartey[j] = int(-lidarx * sinFloat(theta) + lidary * cosFloat(theta) + odometriey * 1000.0);
       j++;
       if(j == NBMESURESMAX)
        j = 0;
      }
     }
    }

    n++;       // On passe en mode cabine (case default:)
    break;


   // Mode cabine !
   default:
    sum ^= current;
    switch(m) {
     case 0:                                         // Figure 4-11 offset +0 d'une cabine
      deltaAnglesQ5[s] = (current & 0b11) << 6;
      distances[s] = current >> 2;
      m++;
      break;
     case 1:                                         // Figure 4-11 offset +1 d'une cabine
      distances[s] |= current << 6;
      m++;
      break;
     case 2:                                         // Figure 4-11 offset +2 d'une cabine
      deltaAnglesQ5[s + 1] = (current & 0b11) << 6;
      distances[s + 1] = current >> 2;
      m++;
      break;
     case 3:                                         // Figure 4-11 offset +3 d'une cabine
      distances[s + 1] |= current << 6;
      m++;
      break;
     case 4:                                         // Figure 4-11 offset +4 d'une cabine
      deltaAnglesQ5[s] |= (current & 0b1111) << 2;
      deltaAnglesQ5[s + 1] |= (current & 0b11110000) >> 2;
      m = 0;
      s += 2;
      if(s == RPLIDARNBSAMPLES) {
       /*if(sum != checksum) {
        TXDEBUG.print(sum);
        TXDEBUG.print(" ");
        TXDEBUG.println(checksum);
       }*/
       n = 0; // On sort du mode cabine...
       s = 0;
       sum = 0;
      }
      break;
    }
    break;

  }
 }

}


Fichier(s) joint(s)


  • Mike118 aime ceci

Make your own raspberry robot, control it over the internet in FPV mode with super low latency and securely share access on it to your friends with Vigibot.com! You are welcome to test our robots now!
https://www.vigibot.com(100% free, no ads)


#9 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 417 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 17 avril 2018 - 09:32


En regardant les photo de pascal ( serveurperso ) je me dit que le problème n'est pas en lien avec le fait de sauter une cabine ou une sous cabine ... 

On voit clairement que la zone qui diffère des autres n'est pas " angulairement constante " ce qui serait le cas je pense si c'était un problème de lecture de cabine ... 
Par contre il semblerait que le problème soit lié à la distance de l'obstacle ...  En gros j'ai l'impression qu'on observe le décalage pour tous les points de lidar dont la distance est inférieur à une certaine limite ...  Ce qui permet d'expliquer l'écart angulaire variable en fonction de la distance du robot par rapport au mur ...

 

 

En poussant le raisonnement j'ai envie de supposer que dès que la distance mesurée est top grande le calcul de la virgule se fait mal ... Et 9° est ajouté à l'angle au lieu de 0,9° ou 10° au lieu de 0,1° ...  Ou un truc du genre ce qui expliquerait le décalage décalage de 9 à 10° ... 

 

J'ai même testé de garder le tableau deltaAngles en Q3 pour faire le changement de signe à la mano comme Path... Exactement le même bug


   default:
    sum ^= current;
    switch(m) {
     case 0:
      deltaAnglesQ3[s] = (current & 0b11) << 4;
      distances[s] = current >> 2;
      m++;
      break;
     case 1:
      distances[s] |= current << 6;
      m++;
      break;
     case 2:
      deltaAnglesQ3[s + 1] = (current & 0b11) << 4;
      distances[s + 1] = current >> 2;
      m++;
      break;
     case 3:
      distances[s + 1] |= current << 6;
      m++;
      break;
     case 4:





      // Test pour faire comme Path, c'est à dire faire le changement de signe à la mano plutôt qu'en décalant les bits directement au bons endroits...

      deltaAnglesQ3[s] |= current & 0b1111;
      if(deltaAnglesQ3[s] & 0b100000) {
       deltaAnglesQ3[s] &= 0b11111;
       deltaAnglesQ3[s] = ~deltaAnglesQ3[s] + 1;
      }

      deltaAnglesQ3[s + 1] |= (current & 0b11110000) >> 4;
      if(deltaAnglesQ3[s + 1] & 0b100000) {
       deltaAnglesQ3[s + 1] &= 0b11111;
       deltaAnglesQ3[s + 1] = ~deltaAnglesQ3[s + 1] + 1;
      }





      m = 0;
      s += 2;
      if(s == RPLIDARNBSAMPLES) {
       /*if(sum != checksum) {
        TXDEBUG.print(sum);
        TXDEBUG.print(" ");
        TXDEBUG.println(checksum);
       }*/
       n = 0;
       s = 0;
       sum = 0;
      }
      break;
    }
    break;


Make your own raspberry robot, control it over the internet in FPV mode with super low latency and securely share access on it to your friends with Vigibot.com! You are welcome to test our robots now!
https://www.vigibot.com(100% free, no ads)


#10 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 417 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 17 avril 2018 - 09:40

Dites-moi les enfants,

j'ai beau programmer (c'est mon métier) depuis XX années, connaitre les sujets techniques que vous abordez (les PIC, la radio, le numérique, l'analogique) - et quand je dis connaître c'est pratiquer -  et je suis plutôt à l'aise pour parler technique (sauf Linux, c'est comme ça, chacun son truc)

Et bien voyez-vous j'ai un souci : je comprend pas tout. Que dalle en fait. Ça doit venir de moi ... C'est grave docteur ?

 

Non c'est pas toi, j'ai essayé de résumer le problème au maximum mais ça suffit pas car nous on baigne dedans et du coup c'est plus facile qu'il n'y parait.

 

Le but c'est de lire un à un les bytes en provenance d'un UART (PIN RX du PIC32) qui est reliée au TX du RPLIDAR.

Le RPLIDAR est un produit qui comporte un télémètre laser rotatif, qui balance la distance et l'angle, la vitesse du moteur est non asservie, il tourne librement et donc l'angle est une donnée critique pour repositionner le point.

 

Aussi et c'est la le problème : pour pouvoir envoyer 4000 mesures par secondes en 115200 baud, RPLIDAR doit compresser les données redondantes.

 

Il s'agit d'un bug dans mon algorithme de décompression des données que j'essai de corriger avec l'aide de Mike.


Make your own raspberry robot, control it over the internet in FPV mode with super low latency and securely share access on it to your friends with Vigibot.com! You are welcome to test our robots now!
https://www.vigibot.com(100% free, no ads)


#11 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 417 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 17 avril 2018 - 09:43

Pour répondre à ta question sur l'autre sujet, oui, les angles sont bons. J'ai laissé les 3 axes du repère sur la restitution pour vérifier tout ça. Les distances sont bonnes et les formes que les points reproduisent sont très fidèles.

 

ça confirme que j'ai une boulette de rotation de bit dans ce code... Mais OU ??? C'est ça qui est fou, c'est très simple à comprendre ce code mais comme je baigne dedans je ne vois pas la connerie...


Make your own raspberry robot, control it over the internet in FPV mode with super low latency and securely share access on it to your friends with Vigibot.com! You are welcome to test our robots now!
https://www.vigibot.com(100% free, no ads)


#12 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 417 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 17 avril 2018 - 09:47

Ah mais si ça se trouve j'ai inversé les bits MSB et LSB du deltaAngles !!!!!

 

Code de Path

		const cabin = {};

		const byte0 = buffer[0];
		const distance1LSB = (byte0 & 0b11111100) >>> 2;
		const deltaAngle1Q3MSB = byte0 & 0b00000011;

		const distance1MSB = buffer[1];
		cabin.distance1 = (distance1MSB << 6) | distance1LSB;

		const byte2 = buffer[2];
		const distance2LSB = (byte2 & 0b11111100) >>> 2;
		const deltaAngle2Q3MSB = byte2 & 0b00000011;

		const distance2MSB = buffer[3];
		cabin.distance2 = (distance2MSB << 6) | distance2LSB;

		const byte4 = buffer[4];
		const deltaAngle2Q3LSB = (byte4 & 0xF0) >>> 4;
		const deltaAngle1Q3LSB = byte4 & 0x0F;

		let deltaAngle1Q3 = (deltaAngle1Q3MSB << 4) | deltaAngle1Q3LSB;	
		let deltaAngle2Q3 = (deltaAngle2Q3MSB << 4) | deltaAngle2Q3LSB;

		if(deltaAngle1Q3 & 0b00100000) {
			deltaAngle1Q3 = deltaAngle1Q3 & 0b00011111;
			deltaAngle1Q3 = ~deltaAngle1Q3 + 1;
		}
		cabin.deltaAngle1 = deltaAngle1Q3 / 8;


		if(deltaAngle2Q3 & 0b00100000) {
			deltaAngle2Q3 = deltaAngle2Q3 & 0b00011111;
			deltaAngle2Q3 = ~deltaAngle2Q3 + 1;
		}
		cabin.deltaAngle2 = deltaAngle2Q3 / 8;


Make your own raspberry robot, control it over the internet in FPV mode with super low latency and securely share access on it to your friends with Vigibot.com! You are welcome to test our robots now!
https://www.vigibot.com(100% free, no ads)


#13 Path

Path

    Made By Humans

  • Modérateur
  • PipPipPipPipPip
  • 2 504 messages
  • Gender:Male
  • Location:Paris

Posté 17 avril 2018 - 09:53

Si tu fais 

      // Calcul de l'angle en float
      angle = (
                float(oldStartAngleQ6) +                                   // Je prend l'ancienne valeur de l'angle absolu initial car on bosse avec une nouvelle entête.
                float(diffAngleQ6) / 32.0 * float(i) / float(RPLIDARNBSAMPLES) -  // Calcul de l'angle théorique par division bêtes...
                (float(deltaAnglesQ5[i]))                              // Petite correction angulaire "pour prendre en compte les variations de vitesse du moteur" le "* 2" c'est juste pour passer de Q5 en Q6
               ) * PI / 180.0;


au lieu de

      // Calcul de l'angle en float
      angle = (
                float(oldStartAngleQ6) +                                   // Je prend l'ancienne valeur de l'angle absolu initial car on bosse avec une nouvelle entête.
                float(diffAngleQ6) * float(i) / float(RPLIDARNBSAMPLES) -  // Calcul de l'angle théorique par division bêtes...
                (float(deltaAnglesQ5[i]) * 2)                              // Petite correction angulaire "pour prendre en compte les variations de vitesse du moteur" le "* 2" c'est juste pour passer de Q5 en Q6
               ) / 64.0 * PI / 180.0;

 

ça améliore les chose ?

Je peux me tromper mais ça me semble pas conforme à la doc.



#14 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 417 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 17 avril 2018 - 09:55

Je vais tester de suite.

Mais dit moi page 19 du datachiotte la Cabin : Mike me dit qu'il ne vois qu'un seul bit de deltaTheta à droite des bits de distance !!! il a pas raison ???


Make your own raspberry robot, control it over the internet in FPV mode with super low latency and securely share access on it to your friends with Vigibot.com! You are welcome to test our robots now!
https://www.vigibot.com(100% free, no ads)


#15 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 417 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 17 avril 2018 - 10:01



Si tu fais 

      // Calcul de l'angle en float
      angle = (
                float(oldStartAngleQ6) +                                   // Je prend l'ancienne valeur de l'angle absolu initial car on bosse avec une nouvelle entête.
                float(diffAngleQ6) / 32.0 * float(i) / float(RPLIDARNBSAMPLES) -  // Calcul de l'angle théorique par division bêtes...
                (float(deltaAnglesQ5[i]))                              // Petite correction angulaire "pour prendre en compte les variations de vitesse du moteur" le "* 2" c'est juste pour passer de Q5 en Q6
               ) * PI / 180.0;


au lieu de

      // Calcul de l'angle en float
      angle = (
                float(oldStartAngleQ6) +                                   // Je prend l'ancienne valeur de l'angle absolu initial car on bosse avec une nouvelle entête.
                float(diffAngleQ6) * float(i) / float(RPLIDARNBSAMPLES) -  // Calcul de l'angle théorique par division bêtes...
                (float(deltaAnglesQ5[i]) * 2)                              // Petite correction angulaire "pour prendre en compte les variations de vitesse du moteur" le "* 2" c'est juste pour passer de Q5 en Q6
               ) / 64.0 * PI / 180.0;

 

ça améliore les chose ?

Je peux me tromper mais ça me semble pas conforme à la doc.

 

Alors déjà à première vue tu casses la normalisation en Q6 t'as du oublié une division

En suite si j'essai c'est la guerre des étoiles:D

Fichier joint  starwars.png   51,1 Ko   0 téléchargement(s)


Make your own raspberry robot, control it over the internet in FPV mode with super low latency and securely share access on it to your friends with Vigibot.com! You are welcome to test our robots now!
https://www.vigibot.com(100% free, no ads)


#16 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 417 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 17 avril 2018 - 10:02

Ah merde je ne prend pas en compte le fait d’être en Q6 dans mon calcul de la portion d'angle théorique !!!

Edit : oui mais ça fonctionne trop bien pour que ce soit aussi bête

 

Mais Path t'es sur que t'as DEUX bits ici :

		const byte0 = buffer[0];
		const distance1LSB = (byte0 & 0b11111100) >>> 2;
		const deltaAngle1Q3MSB = byte0 & 0b00000011;           // 2 bits ici ????

		const distance1MSB = buffer[1];
		cabin.distance1 = (distance1MSB << 6) | distance1LSB;

		const byte2 = buffer[2];
		const distance2LSB = (byte2 & 0b11111100) >>> 2;
                const deltaAngle2Q3MSB = byte2 & 0b00000011;           // 2 bits ici ????

Fichier joint  Format.png   61,79 Ko   0 téléchargement(s)

  default:
    sum ^= current;
    switch(m) {
     case 0:
      deltaAnglesQ5[s] = (current & 0b11) << 6;      // Moi j'ai mis 2 bits comme toi !
      distances[s] = current >> 2;
      m++;
      break;
     case 1:
      distances[s] |= current << 6;
      m++;
      break;
     case 2:
      deltaAnglesQ5[s + 1] = (current & 0b11) << 6;  // 2 bits que je pousse en face du bit de signe de l'int8_t
      distances[s + 1] = current >> 2;
      m++;
      break;
     case 3:
      distances[s + 1] |= current << 6;
      m++;
      break;
     case 4:
      deltaAnglesQ5[s] |= (current & 0b1111) << 2;          // Puis je complète avec les 4 autres bits et je laisse deux bits faible pour faire un Q5 afin de remplir l'octet
      deltaAnglesQ5[s + 1] |= (current & 0b11110000) >> 2;  // Idem


Make your own raspberry robot, control it over the internet in FPV mode with super low latency and securely share access on it to your friends with Vigibot.com! You are welcome to test our robots now!
https://www.vigibot.com(100% free, no ads)


#17 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 417 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 17 avril 2018 - 10:15

Path j'ai edit massivement le dernier post regarde et confirme moi ta compréhension du datachiotte car la on a un truc bien bizarre ya que UN SEUL BIT et c'est justement le signe, nous on en prend deux

Si ça se trouve t'as le même bug que moi, en tout cas c'est pas clair cette partie !!!

 

Si jamais c'est ça le bug et que tu l'as aussi c'est MIKE118 qui win car c'est lui qui m'a mis la puce a l'oreille en parlant de cette anomalie du datasheet


Make your own raspberry robot, control it over the internet in FPV mode with super low latency and securely share access on it to your friends with Vigibot.com! You are welcome to test our robots now!
https://www.vigibot.com(100% free, no ads)


#18 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 9 934 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 17 avril 2018 - 10:19

Bon comme je le disait à pascal, 

 

Moi la partie qui me perturbe le plus dans la doc c'est la description des sous cabines ... 
post-1264-0-18002700-1523999227.png

Le byte 0 par exemple est censé contenir : 

  • Les bits de poids faible d'une distance

ainsi

  • qu'un bout de deltatheta ....

Sauf qu'en lisant la description je vois un mis-match au niveau de cette répartition de bits dans le byte ... 

D'un côté on peut lire distance1[0,6]  => ce qui suppose 7 bits  
De l'autre côté on peut lire deltatheta où on peut supposer [0, 1] ...  soit 2 bits ... 

Hors on a que 8 bits et pas 9 ...  

Donc la question est ...

c'est le bout de distance qui est sur 6 bits ? => Ce que vous faites déjà ? 
Ou c'est et le bout de deltatheta qui est sur 1 bits ?  

 

Autre chose encore ?
 


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 
Si vous souhaitez un robot pilotable par internet n'hésitez pas à visiter www.vigibot.com et à lire le sous forum dédié à vigibot!

 

Les réalisations de Mike118  

 

 

 


#19 Path

Path

    Made By Humans

  • Modérateur
  • PipPipPipPipPip
  • 2 504 messages
  • Gender:Male
  • Location:Paris

Posté 17 avril 2018 - 10:24

Ben c'est ce que dit la doc, je l'invente pas :)

 

Bien sûr que c'est le fameux bit de signe qui m'a aussi emmerdé.

 

Pour le gérer, j'ai ajouté ça :

 

  if(deltaAngle1Q3 & 0b00100000) {
   deltaAngle1Q3 = deltaAngle1Q3 & 0b00011111;
   deltaAngle1Q3 = ~deltaAngle1Q3 + 1;
  }

Je complémente à 1 si le bit est à 1 ;)

Je me suis arraché les cheveux pour le faire quand même :) Lâche rien !!



#20 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 417 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 17 avril 2018 - 10:26

Mais oui mais toi tu prends aussi 2 bits de cette partie la or que si ça se trouve il ne faut prendre QUE le signe.

Du coup ce serait du Q6 donc encore plus élégant !!!


Make your own raspberry robot, control it over the internet in FPV mode with super low latency and securely share access on it to your friends with Vigibot.com! You are welcome to test our robots now!
https://www.vigibot.com(100% free, no ads)





0 utilisateur(s) li(sen)t ce sujet

0 members, 0 guests, 0 anonymous users