Aller au contenu


Photo

Le RP Lidar A2


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

#1 Path

Path

    Made By Humans

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

Posté 12 novembre 2017 - 12:54

RP LIDAR A2 

 

J'ai ces quelques points que je voudrais éclaircir à propos de ce joli jouet :)

 

1. J'ai lu la spec du protocole Fichier joint  rpk-02-communication-protocol.pdf   1,49 Mo   500 téléchargement(s). Si je comprends bien, il peut envoyer 2 types de paquets. Un premier ("SCAN") avec des paquets de coordonnées polaires : des couples (angle, distance). Un 2eme type ("EXPRESS_SCAN") avec un angle de départ et une liste de 32 sous-paquet de 5 octets contenant 2 mesures de distance avec chacune un delta d'angle.

NB. Le 3e type sert au debug. Si je comprend bien, ce truc envoie toujours des coordonnées polaires. 

 

C'est le coup de AngleDiff dans la formule que je comprends pas. Il faut l'angle de référence du paquet suivant ? Donc il faut une puissance de calcul suffisante pour ne jamais rater un paquet. Et on ne connait les angle du paquet Pn seulement après avoir reçu la paquet Pn+1 ??? Je raté un truc ?

 

2. Pour recevoir et analyser ces paquets, il faut quelle puissance de calcul. On sait que cela fonctionne avec le PIC32 de Pascal. Un RPI3 peut-il absorber cette charge ? Quitte à le faire un C. A votre avis ? On le voit dans la video mais j'aimerai l'avis de quelqu'un qui l'a utilisé. 

 

3. qu'est-ce qui est prévu comme fixations ?

 

4. Il est livré avec l'interface USB ? 

 

5. Avec ou sans, le vitesse de transmission de la liaison série c'est toujours 115200bps ?

 

6. Le jack pour l'alimenter est fourni ? :D

 

D'avance merci de votre aide.

 

PS. J'adore le commentaire client sur la page de robotshop ^^ 



#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é 12 novembre 2017 - 01:54

1 Oui tu as bien compris : 

Si on représent les donnés des packets  comme ça =>    AngleA0  dA0 dA1 dA2  .... dAn        AngleB0  dB0 dB1 ....

 

 

pour récupérer les données au format polaire sous la forme  AngleA1, dA1  il faut extrapoler l'angle correspondant à la distance donnée avec un simple calcul:

AngleA1 = AngleA0 + 1 * (AngleB0-AngleA0) / n 

AngleAj = AngleA0 + j * (AngleB0-AngleA0) / n 

 

 

Donc oui il faut absolument dans cette configuration récupérer un packet complet et l'angle du packet suivant pour extraire les données complètes. 

Du coup le traitement doit se faire de manière asynchrone ...  obliger d'attendre d'avoir l'angle A et l'angle B pour pouvoir faire la conversion cartésienne =)

 

2)  Le mode 4K est pas possible sur une arduino uno ou mega. D'ailleurs sur le pic32 pascal n'utilise pas encore le mode 4K sur le pic 32. On en a pas besoin pour le moment mais on verra pour le faire bientôt quand même ;)

Le Pi3 peut le gérer sans problèmes =) ( on a utilisé pire sur une Pi3 à octopus ;)

 

3) Hum ... Une pièce imprimée en 3D ? Pour le moment rien est prévu ... Ils sont même pas encore en stock =) 


4) Oui l'interface usb fait partie du produit ;)

 

5) Oui la vitesse de transmission c'est toujours 115200 bps =) 

 

6) Une alimentation est fournie avec le packaging. Pour le moment la prise n'est pas EU mais je vais faire en sorte de fournir la prise EU.  
 


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 Serveurperso

Serveurperso

    Membre passionné

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

Posté 12 novembre 2017 - 02:00

lol il a répondu avant moi


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)


#4 Path

Path

    Made By Humans

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

Posté 12 novembre 2017 - 02:02

Pascal, tu utilise le driver cpp fourni avec ?

Tu l'as fixé comment le tiens ?



#5 Serveurperso

Serveurperso

    Membre passionné

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

Posté 12 novembre 2017 - 02:04

Pour la connectique c'est juste 5 fils au pas 2.54, GND/VCC, TX/RX, et entrée PWM pour la vitesse du moteur, attention cette commande ne fonctionne qu'a une seule fréquence données dans le datachiotte (25 ou 30KHz je crois)

Après la carte USB donnée avec le produit comporte un générateur PWM

T'as aussi un driver / API tout fait pour l'exploiter dans différents langages et c'est plug'n'play sous ROS

 

Moi sur PIC32 en bare-metal je suis en 2K pour des raisons de performances, car en 4K il faut récupérer les cabines qui comportent un certain nombre de valeurs et donc faire un gros burst de calculs d'un seul coup ce qui dérange mon ordonnanceur pour l'instant.


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 Serveurperso

Serveurperso

    Membre passionné

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

Posté 12 novembre 2017 - 02:05

non j'ai développé mon propre driver beaucoup plus rapide que le leurs qui d'ailleur est mal fichu si bien que j'ai ouvert un github chez eux


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)


#7 Serveurperso

Serveurperso

    Membre passionné

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

Posté 12 novembre 2017 - 02:12

Voici mon driver actuel de lecture RPLIDAR 2K "hyper optimisée et facile à comprendre" que j'utilise sur le robot :

void readLidar() {
 uint8_t current;
 static uint8_t n = 0;
 static uint8_t quality;
 static bool start;
 static uint16_t angleq6;
 static uint16_t distanceq2;
 static uint16_t i = 0;
 float angle;
 float distance;
 float lidarx;
 float lidary;

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

  switch(n) {

   case 0:
    if((current & 1) != (current >> 1 & 1)) {
     quality = current >> 2;
     start = current & 1;
     n = 1;
    }
    break;

   case 1:
    if(current & 1) {
     angleq6 = current >> 1;
     n = 2;
    } else
     n = 0;
    break;

   case 2:
    angleq6 |= current << 7;
    n = 3;
    break;

   case 3:
    distanceq2 = current;
    n = 4;
    break;

   case 4:
    distanceq2 |= current << 8;

    if(start) {
     if(i && i < NBMESURESMAX) {
      lidarReady = true;
      for(uint8_t j = i; j < NBMESURESMAX; j++) {
       lidarsx[j] = MESURENULLE;
       lidarsy[j] = MESURENULLE;
       lidarsCartex[j] = MESURENULLE;
       lidarsCartey[j] = MESURENULLE;
      }
     }
     i = 0;
    } else if(i == NBMESURESMAX) {
     lidarReady = false;
     n = 0;
     break;
    }

    if(distanceq2) {                                                  // Si la lecture est valide
     angle = float(angleq6) / 64.0 * PI / 180.0;
     distance = float(distanceq2) / 4.0;
     lidarx = POSITIONLIDARX + distance * sin(angle);
     lidary = POSITIONLIDARY + distance * -cos(angle);

     if(lidarx > POSITIONSROBOTXMAX || lidarx < POSITIONSROBOTXMIN ||
        lidary > POSITIONSROBOTYMAX || lidary < POSITIONSROBOTYMIN) { // Si la lecture est hors du robot
      lidarsx[i] = int(lidarx);
      lidarsy[i] = int(lidary);
      lidarsCartex[i] = int(lidarx * cos(theta) + lidary * -sin(theta) + odometriex * 1000.0);
      lidarsCartey[i] = int(lidarx * sin(theta) + lidary * cos(theta) - odometriey * 1000.0);
      i++;
     }
    }

    n = 0;
  }
 }
}

et en 4K la fonction n'est pas terminée (mais ça fonctionne) il reste à corriger un petit bug d'angle courant / passé qui fait un défaut bien visible, mais voila à quoi le code ressemble, c'est de suite plus compliqué qu'en 2K et ça ajoute une latence de 1 cabine de long ou lieu d'un seul point de long en 2K  :

void lidar(bool enable) {
 if(enable) {
  digitalWrite(LIDARPWMPIN, true);
  TXLIDAR.write(0xA5);
  TXLIDAR.write(0x82);
  TXLIDAR.write(0x05);
  TXLIDAR.write((uint8_t)0x00);
  TXLIDAR.write((uint8_t)0x00);
  TXLIDAR.write((uint8_t)0x00);
  TXLIDAR.write((uint8_t)0x00);
  TXLIDAR.write((uint8_t)0x00);
  TXLIDAR.write(0x22);
 } else {
  digitalWrite(LIDARPWMPIN, false);
  TXLIDAR.write(0xA5);
  TXLIDAR.write(0x25);
 }
}

#define RPLIDARNBSAMPLES 32

bool readLidar() {
 uint8_t current;
 static uint8_t n = 0;
 static uint8_t checksum;
 static uint8_t sum = 0;
 static uint16_t oldStartAngleq6;
 static uint16_t startAngleq6 = 0;
 static bool start;
 static uint16_t diffAngleq6;
 static uint8_t m = 0;
 static uint8_t s = 0;
 static int8_t derivAnglesq5[RPLIDARNBSAMPLES];
 static uint16_t distances[RPLIDARNBSAMPLES];
 float distance;
 float angle;
 static uint16_t j = 0;
 float lidarx;
 float lidary;

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

  switch(n) {

   case 0:
    if(current >> 4 == 0xA) {
     checksum = current & 0xF;
     n = 1;
    }
    break;

   case 1:
    if(current >> 4 == 0x5) {
     checksum |= current << 4;
     n = 2;
    } else
     n = 0;
    break;

   case 2:
    sum ^= current;
    oldStartAngleq6 = startAngleq6;
    startAngleq6 = current;
    n++;
    break;

   case 3:
    sum ^= current;
    startAngleq6 |= (current & 0x7F) << 8;
    start = current >> 7;
    if(start)
     TXDEBUG.println("start");








    if(oldStartAngleq6 > startAngleq6)
     diffAngleq6 = (360 << 6) + startAngleq6 - oldStartAngleq6;
    else
     diffAngleq6 = startAngleq6 - oldStartAngleq6;

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


      //lidarReady = true; // Debug
      angle = (float(oldStartAngleq6) + float(diffAngleq6) * float(i) / float(RPLIDARNBSAMPLES) - float(derivAnglesq5[i] << 1)) / 64.0 * PI / 180.0;


      lidarx = POSITIONLIDARX + distance * sin(angle);
      lidary = POSITIONLIDARY + distance * -cos(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 * cos(theta) + lidary * -sin(theta) + odometriex * 1000.0);
       lidarsCartey[j] = int(lidarx * sin(theta) + lidary * cos(theta) - odometriey * 1000.0);
       j++;
       if(j == NBMESURESMAX)
        j = 0;
      }
     }
    }






    n++;
    break;

   default:
    sum ^= current;
    n++;
    switch(m) {
     case 0:
      derivAnglesq5[s] = (current & 3) << 6;
      distances[s] = current >> 2;
      m++;
      break;
     case 1:
      distances[s] |= current << 6;
      m++;
      break;
     case 2:
      derivAnglesq5[s + 1] = (current & 3) << 6;
      distances[s + 1] = current >> 2;
      m++;
      break;
     case 3:
      distances[s + 1] |= current << 6;
      m++;
      break;
     case 4:
      derivAnglesq5[s] |= (current & 0x0F) << 2;
      derivAnglesq5[s + 1] |= (current & 0xF0) >> 2;
      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;

  }
 }

}

le code du driver du fournisseur (une belle usine à gaz plongez vous dedans et essayez de comprendre) : https://github.com/r...idar_driver.cpp

le ticket d'info que j'ai mis chez eux : https://github.com/r...r_ros/issues/48


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)


#8 Serveurperso

Serveurperso

    Membre passionné

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

Posté 12 novembre 2017 - 02:19

question oubliée : pour fixer le lidar j'ai juste 3D imprimé un disque du diamètre du lidar avec les 4 trous, je met des toutes petites entretoises nylon et roulez !

 

ce disque sert d'adapateur RPLIDAR -> ACTOBOTICS...

 

 

 

 

 

 

 

 

 

Fichier joint  SupportLidar.stl   140,71 Ko   227 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)


#9 Path

Path

    Made By Humans

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

Posté 12 novembre 2017 - 02:30

Merci pour ces réponses !!!

Beau boulot sur le code :) On a lu la même doc ^^



#10 Path

Path

    Made By Humans

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

Posté 12 novembre 2017 - 03:39

Et à propos de cette carte USB ?

Elle a un générateur PWM à la bonne fréquence (c'est 25kHz). Elle converti le 3,3 en 5V (pour le format USB). Et ?

Le protocole est le même avec et sans cette carte ? Elle ne le simplifie pas ?

 

Je trouve pas d'exemple d'utilisation avec le GPIO du raspberry sur le net. Ou alors le câblage est tellement évidant que les gens ne prennent pas la peine de le décrire. Les tensions sont compatibles. Le RPI sait gérer le PWM en duty et en fréquence. Et il a un port UART sur son GPIO.



#11 Serveurperso

Serveurperso

    Membre passionné

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

Posté 12 novembre 2017 - 04:28

Ouais c'est juste un basique convertisseur USB -> Serial genre FTDI/Prolific/CP210x et compagnie...

Les I/O sont en 3.3 5V tolerant (à vérifier) et l'alimentation du lidar est 5V

Ils encapsulent juste une petite commande pour faire varier le rapport cyclique du générateur de PWM mais ça change rien au protocole.


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 Path

Path

    Made By Humans

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

Posté 12 novembre 2017 - 05:30

Pas certain que le port série soit compatible 5V. En tout cas compatible avec la RPI.

 

Sans titre.png

 

Mike, tu as déjà branché le rplidar directement sur le TX RX du port GPIO du RPI 3 ? (Pas le RPI 2)



#13 Serveurperso

Serveurperso

    Membre passionné

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

Posté 12 novembre 2017 - 06:44

Oui seul le MOTOCTL est 5V tolérant. En même temps qui vas utiliser ce lidar sur un micro 5V faudrait être dingue:)


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)


#14 Serveurperso

Serveurperso

    Membre passionné

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

Posté 12 novembre 2017 - 08:30

jojo l'as déjà branché sur le TX d'un Arduino mega 16MHz 5 VOLTS !

M'enfin c'est pas tip top !

 

toutes façons les GPIO de la PI sont pas en 5V quand même pas


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 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é 12 novembre 2017 - 08:33

Tu branches le rplidar A2 sur l'USB de la raspberry pi avec l'adaptateur fournis ou tu le branches sur les GPIO UART de la pi ( les GPIO de la pi sont en 3.3V  )


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  

 

 

 


#16 Path

Path

    Made By Humans

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

Posté 12 novembre 2017 - 08:37

Je demandais ça, sur le RPI 3 puisque l'UART du GPIO à changé depuis le 2 avec l'apparition du bluetooth.

Je prendrai l'option USB ;)



#17 Serveurperso

Serveurperso

    Membre passionné

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

Posté 12 novembre 2017 - 08:39

Ben avec l'adaptateur c'est neuneu mais plus gros,

Avec les GPIO de la PI t'as un brun de conf mais c'est plus classe:D

Puis le PWM tu t'en fiche met le à fond si c'est un A1, si c'est un A2 à fond t'auras besoin de l'équilibrer !


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 Path

Path

    Made By Humans

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

Posté 12 novembre 2017 - 09:33

Bien sûr que c'est plus classe :)

Y a cet article qui résume bien le problème : http://www.framboise314.fr/le-port-serie-du-raspberry-pi-3-pas-simple/

Je ferai comme lui. Je n'ai pas besoin du bluetooth.



#19 Serveurperso

Serveurperso

    Membre passionné

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

Posté 12 novembre 2017 - 09:38

Ah merdouille ! En même temps actuellement j'ai un modem branché à un FTDI sur ma PI3


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)


#20 Path

Path

    Made By Humans

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

Posté 05 décembre 2017 - 09:57

C'est un très beau produit !!

 

Un premier test est fait rapidement avec les outils du SDK pré-compilés pour windows !! Ca fonctionne direct. J'ai hâte de compiler le driver sur la pi !! Et de capter moi-même les coordonnées polaires.

 

Je vous tiens au courant sur le sujet du Hector.

 

Je kiffe !!






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

0 members, 0 guests, 0 anonymous users