Introduction

Après un article sur comment Améliorer vos régulateurs PID, et une première partie sur l’asservissement d’un robot autonome, la présentation de la nouvelle architecture de nos robots, nous allons maintenant vous présenter notre choix final concernant notre asservissement polaire, le code associé, et les explications comment l’utiliser et le mettre en place.

Cela fait plusieurs années que nous avons travaillé en collaboration avec EsialRobotik sur ce sujet, nous avions l’ambition de créer cet article, ainsi que fournir les sources mutualisées, qui sont disponibles sur le github EsialRobotik/asserv_chibios, pour le bonheur des roboticiens qui en auraient besoin.

SOMMAIRE

1ère partie – Asservissement et architecture – Page précédente

1. Quels objectifs

2. Architecture matérielle du robot

3. Architecture logicielle du robot

4. Architecture de l’asservissement (Motion System)

5. La génération de trajectoire du robot

6. La technique du GOTO enchainé

7. La technique de la détection du blocage

8. La notification des données

9. Le Shell de DEBUG

2ème partie – Instancier son code d’asservissement – Cet article

1. Créer sa propre instance

2. Exemples d’instanciation

3. Comment régler son “asserv” ?

4. Documentation des classes

5. Utilisation de Plotjuggler

1. Créer sa propre instance

Comme déjà expliqué précédemment, le code a été pensé pour être instanciable facilement avec du hardware différent. Actuellement EsialRobotik possède 2 instances assez proches pour ses robots et PM-ROBOTIX a sa propre instance. Nous allons ici passer en revue ce qu’il va falloir faire comme choix pour avoir sa propre instance. Se référer à l’architecture logicielle devrait vous aider.

> motorController

Il va falloir choisir et/ou créer une classe qui implémente cette interface qui permet d’appliquer une vitesse et un sens aux moteurs. Voir la documentation de classe MotorController.

> Encoders

Il va falloir choisir et/ou créer une classe qui implémente cette interface qui permet de récupérer un nombre de ticks encodeur depuis le dernier passage dans la boucle d’asservissement.

> PLL

C’est un estimateur de vitesse (voir plus d’info sur la documentation de classes) qui permet d’avoir une bonne estimation de vitesse même avec peu de ticks encodeur. Si votre robot n’est pas très rapide, vous pouvez baisser la fréquence de la boucle d’asservissement et vous passer de la PLL pour simplifier.

> SpeedController

Vous devez ici choisir un contrôleur de vitesse, il en existe 2 :

• Un contrôleur basique basé sur un régulateur PI. La version basique sera amplement suffisante pour un robot assez lent.
• et un contrôleur étendu, basé sur 3 régulateurs PI choisi en fonction de la vitesse actuelle. Voir la documentation de classe pour plus de détails.

> AccelerationDecelerationLimiter

Pour limiter l’accélération et éventuellement la décélération, il va falloir choisir des instances de l’interface [AccelerationDecelerationLimiterInterface].
Plusieurs choix s’offrent à vous, mais dans un 1er temps, contentez-vous d’utiliser le [SimpleAccelerationLimiter] qui ne limite que l’accélération et sera facile à régler. Voir la documentation de classe pour plus de détails sur les instances existantes.

> Vous êtes perdu à ce stade ?

In code Veritas

Pour trouver de l’inspiration, regarder les exemples d’instanciation peut aider, et allez voir les fichiers [main.cpp] existants!

2. Exemples d’instanciation

L’exemple basique :

• Contrôleur en vitesse : SpeedController
• AccelerationDecelerationLimiterInterface d’angle : SimpleAccelerationLimiter
• AccelerationDecelerationLimiterInterface de distance : SimpleAccelerationLimiter

Ici, vous aurez à régler 2 paramètres (Kp;Ki), pour contrôler chaque roue indépendamment en vitesse, à régler une accélération maximale, et aussi 2 paramètres Kp pour le contrôle en distance et angle.

Attention, le SimpleAccelerationLimiter ne gère que l’accélération. La décélération est directement la sortie du régulateur P du contrôleur d’angle ou distance.

L’instance PM-ROBOTIX:

• Contrôleur en vitesse : AdaptativeSpeedController
• AccelerationDecelerationLimiterInterface d’angle : SimpleAccelerationLimiter
• AccelerationDecelerationLimiterInterface de distance : AdvancedAccelerationLimiter
• BlockingDetector : OldSchoolBlockingDetector

L’instance ESIAL 2021 :

• Contrôleur en vitesse : AdaptativeSpeedController
• AccelerationDecelerationLimiterInterface d’angle : SimpleAccelerationLimiter
• AccelerationDecelerationLimiterInterface de distance : AdvancedAccelerationLimiter

Ici, pour chaque contrôleur en vitesse, vous aurez plusieurs sets de PI à régler avec à chaque fois des vitesses d’utilisation. On a un premier set de (Kp;Ki) assez fort de l’arrêt à très basse vitesse (pour contrer les frottements secs et la puissance nécessaire à l’arrachement), puis des sets de (Kp;Ki) qui diminuent lorsque la vitesse augmente. Vous aurez à régler une accélération maximale simple sur l’angle et une méthode de calcul plus complexe pour la distance qui permet de mieux gérer le moment où le robot passe de l’arrêt au mouvement. Il y aura aussi 2 Kp pour le contrôle en distance et angle.

Attention, le SimpleAccelerationLimiter et AdvancedAccelerationLimiter ne gèrent que l’accélération. La décélération est directement la sortie du P du contrôleur d’angle ou distance.

L’instance ESIAL 2022:

• Contrôleur en vitesse : AdaptativeSpeedController
• AccelerationDecelerationLimiterInterface d’angle : SimpleAccelerationLimiter
• AccelerationDecelerationLimiterInterface de distance : AccelerationDecelerationLimiter

Ici, tout pareil que précédemment, sauf que l’accélération et la décélération sont contrôlées par AccelerationDecelerationLimiter, qui nécessite de régler plus de paramètres, voir la documentation de classe AccelerationDecelerationLimiter.

3. Comment régler son “asserv”?

Avant-propos

Pour commencer à régler, il faut avoir un robot avec une instance basique et minimale. A savoir, vos moteurs avec la carte de puissance qui va bien, des roues codeuses et une source d’énergie (de préférence la batterie que vous allez utiliser dans le robot).

Prérequis

• Le tout doit être relié correctement et fonctionnel,
• via l’interface Encoders, on récupère des ticks si on bouge une roue codeuse,
• et une consigne via l’interface MotorController doit faire bouger les moteurs.
• Par ailleurs, vous devez avoir une compréhension minimale du fonctionnement de l’asservissement en polaire d’un robot, voir les articles précédents.

Si tout ce qui est mentionné ici vous est inconnu….. Passez votre chemin 🙂

Les commandes et l’interface de réglage

TODO expliquer les commandes et quel fichier du code/ et l’interface Plotjuggler…

Ensuite, on commence par définir un sens pour le robot. Simplement, définir où est l’avant (et donc à fortiori, où est la droite et la gauche).

Retrouver les différents réglages qui vont suivrent:

• Réglage du sens des encodeurs
• Réglage du sens des moteurs
• Réglage des constances physiques
• Réglage grossier des limiteurs d’accélération
• Réglage des régulateurs en vitesse
• Réglage des régulateurs de cap et de distance
• Réglage fin des limiteurs d’accélération
• Réglage du CommandManager
• Réglage de la détection de blocage

Réglage du sens des encodeurs

Information dans plotjuggler : [raw_encoder/left] & [raw_encodeur_right]

Tout d’abord, régler les inversions dans votre classe Encodeur pour que le codeur droit bouge dans Plotjuggler lorsque vous faites tourner le codeur droit (et respectivement pour le gauche). Ensuite, vérifiez que lorsque vous faites avancer le robot vers l’avant, les ticks des codeurs sont positifs.

Réglage du sens des moteurs

Tout d’abord, surélevez votre robot pour qu’il ne s’enfuie pas 🙂 Ensuite, on va désactiver la sortie de la boucle d’asservissement aux moteurs avec la commande:

• Dans l’interface du plugin, cliquer sur le Bouton “MotorControl > Disable“.

• ou utiliser la commande Shell:

asserv enablemotor 0

Puis, on demande au moteur droit d’avancer avec la commande:

• Dans l’interface du plugin, cliquer sur le Bouton “Consigne directe Vitesse roue droite” et mettre 10 mm/s, puis cliquer sur “send wheel speed command“

• ou utiliser la commande Shell:

asserv motorspeed r 10

Si c’est le mauvais moteur qui tourne, ou si le sens est mauvais, on fait les modifications nécessaires.

Réglage des constances physiques

Il vous faut simplement spécifier :

• ENCODERS_WHEELS_RADIUS_MM : le rayon de vos roues codeuses.
• ENCODERS_WHEELS_DISTANCE_MM : distance entre les 2 roues codeuses.
• ENCODERS_TICKS_BY_TURN : nombre de ticks par tour de vos encodeurs.

Par ailleurs, la valeur MAX_SPEED_MM_PER_SEC permet de donner la vitesse maximale atteignable pour votre robot. Cette vitesse s’applique pour les 2 régulateurs de vitesse, vous pouvez donc donner la vitesse maximum réelle de votre robot (vous pouvez le déterminer en envoyant la commande de vitesse max aux 2 moteurs et regarder le résultat dans Plotjuggler. Mais faites gaffes 🙂 ).

Surtout, la valeur de MAX_SPEED_MM_PER_SEC doit être une vitesse atteignable !

Réglage grossier des limiteurs d’accélération

Il y a une limite d’accélération appliquée à la sortie des régulateurs de cap et de distance, mais pour mettre au point le réglage des régulateurs de vitesse, on essaye de se mettre dans un cas de figure vaguement réaliste, c’est-à-dire avec une consigne de vitesse limitée en vitesse.

Par exemple pour un robot dont la vitesse max est de 1m/s, une limite d’accélération de 0.66 m/s^2 semble réaliste (c’est-à-dire 1,5 sec pour atteindre la Vmax).

Voir la documentation de classe pour choisir la classe “limiter” souhaitée (Simple or Advanced or AccelerationDeceleration) avec les paramètres associés à la classe choisie (DIST_REGULATOR_MAX_ACC, ANGLE_REGULATOR_MAX_ACC, …)

Ne pas hésiter à l’étape suivante, à augmenter/diminuer cette limite !

Réglage des régulateurs en vitesse

Chaque roue est asservie en vitesse via des paramètres PI (et peuvent être réglé à-peu-près indépendamment.

• speed_controller_right_Kp, speed_controller_right_Ki, speed_controller_right_SpeedRange
• speed_controller_left_Kp, speed_controller_left_Ki, speed_controller_left_SpeedRange

Pour cela, on utilise le “clickodrome” dans Plotjuggler pour: régler ces coefficients PI, envoyer des consignes de vitesses et visualiser les résultats sur les courbes.

Notez bien que :

• Testez que vos réglages fonctionnent pour toutes les vitesses possibles, c’est-à-dire: à la vitesse max, à 80%, à 50%, à 20% de la vitesse max et aussi à une vitesse basse raisonnable.
• Envoyez une consigne de vitesse sur une durée assez longue pour être sûr que vos réglages arrivent à tenir la vitesse (1500ms ~ 2000ms, c’est bien).
• Pour régler vos paramètres PI, vous pouvez d’abord régler Kp pour avoir un comportement “grosse masse” puis régler Ki pour minimiser le temps de réponse et l’erreur statique. Cette méthode entraine un Kp grand et un Ki faible. Mais des fois, un Ki grand et un Kp faible marche mieux, il faut tester !

Une fois les 2 paramètres Pi réglés, on demande la même consigne de vitesse aux 2 régulateurs PI pour voir comment ils se comportent. Si les 2 boucles d’asservissement semblent se comporter de manière à peu près similaire (temps de réponse à un échelon de vitesse + tenu de la consigne en régime stabilisé) et que le robot avance vaguement droit, on est bien !

Réglage des régulateurs de cap et de distance

Les régulateurs en cap et distance sont de simples régulateurs proportionnels, ils sont donc facile à régler.

• DIST_REGULATOR_KP
• ANGLE_REGULATOR_KP

Si le robot dérape un peu au démarrage, ce n’est pas grave (le limiteur d’accélération règlera ça), mais il doit atteindre sa position sans oscillation.

Réglage fin des limiteurs d’accélération

Pour éviter de déraper on limite l’accélération à la sortie des 2 régulateurs (avant le mélange des consignes).

Attention:

• la limite s’applique uniquement sur l’accélération, et pas sur la décélération!
• La décélération est gérée directement par le régulateur proportionnel.
• Il est important que les consignes de vitesses limitées en accélération soient atteignables !

La limite d’accélération en sortie du régulateur de cap est simple et est exprimée directement en mm/s^2 (ou par définition : en [delta_max_par_tour_de_boucle / frequence_asserv]).

La limite d’accélération en sortie du régulateur de distance est un peu plus complexe. On peut accélérer assez fort une fois le robot en mouvement, mais beaucoup moins quand il est à l’arrêt ou presque!

Donc nous allons maintenant régler 3 valeurs :

• L’accélération minimale acc_max (DIST_REGULATOR_MAX_ACC) qui va être appliquée quand le robot est à l’arrêt.
• L’accélération maximale acc_min (DIST_REGULATOR_MIN_ACC) qui va être appliquée quand le robot avancera assez vite.
• La vitesse V_seuil (DIST_REGULATOR_HIGH_SPEED_THRESHOLD) à partir de laquelle on considèrera que le robot avance assez vite pour appliquer l’accélération maximale.

Si la vitesse courante est supérieure à V_seuil, alors:

l’accélération sera limitée à acc_max.

Sinon, l’accélération sera limitée à [acc_min + |V_courante| / V_seuil * (acc_max-acc_min)]

Réglage du CommandManager

Le CommandManager est la classe qui prend en entrée, les commandes externes reçues de haut niveau (exemple: va à tel point sur la table, avance de x mm, tourne de y radian, etc…. ) et les transforme en consignes pour les régulateurs de cap et de distance.

Revoir ici la présentation de l’architecture.

Réglage lié aux commandes basiques

Pour les commandes basiques (avance, tourne, fait face à un point), il n’y a que 2 paramètres à régler qui servent à décider si la consigne a été atteinte ou non :

• COMMAND_MANAGER_ARRIVAL_ANGLE_THRESHOLD_RAD : l’erreur sur le régulateur d’angle en dessous duquel on considère être arrivé.
• COMMAND_MANAGER_ARRIVAL_DISTANCE_THRESHOLD_mm : l’erreur sur le régulateur de distance en dessous duquel on considère être arrivé.

Réglage lié au GOTO non enchainé

Pour les commandes de types GOTO, on utilise les paramètres précédents pour savoir si on est arrivé sur la cible. En plus:

• On définit un angle minimale COMMAND_MANAGER_GOTO_ANGLE_THRESHOLD_RAD en dessous duquel, on se contente de tourner sans avancer. Par exemple, si une commande GOTO demande d’aller derrière la position actuelle du robot, on se tourne d’abord pour y faire face, puis on avance.
• On définit la distance en mm à partir de laquelle, on ne calcule plus de consigne d’angle COMMAND_MANAGER_GOTO_RETURN_THRESHOLD_mm . Une fois que le robot est en dessous d’une certaine distance, on ne calcule plus que des consignes d’angles. Sans cette étape, si on passe un mm à droite de la cible, le delta de distance sera de 1mm mais le delta d’angle sera de 90°, et donc le robot va se mettre à tourner autour de la cible.

Réglage lié au GOTO enchainé

Revoir ici l’architecture du GOTO enchaîné.

Le GOTO enchaîné se base sur les réglages précédents. On définit ici 3 éléments:

• Un cercle centré sur le robot de rayon r_vMax = VitesseMax / Kp_distance permettant de générer une erreur de distance qui va entrainer une vitesse max en sortie de régulateur d’angle.
• Un cercle centré sur le robot de rayon r_next inférieur à r_vMax qui permet de décider de passer à la commande suivante si elle est enchaînable.
• Un angle ang_enchaine qui permet de passer à la commande suivante si elle est enchaînable tout en restant à vitesse max.

L’algorithme fonctionne de la même manière que précédemment avec r_enchaine = r_vMax. La principale différence tient dans les conditions d’enchainement de la cible. On utilise la prochaine commande enchaînable si une de ces condition est vérifiée :

• Si la prochaine commande est dans le cercle de rayon r_vMax et que l’erreur de cap (aka deltaTheta) est inférieure à ang_enchaine.
• Si la prochaine commande est dans le cercle de rayon r_next.

Autrement dit, on enchaîne la prochaine commande au loin si c’est presque tout droit, sinon on ralentit pour tourner.

Réglage de la détection de blocage

Voir la documentation de classe BlockingDetector,

Les paramètres de condition du blocage OldSchoolBlockingDetector à régler sont les suivants:

• minimum_considered_speed_percent puissance minimum sur les moteurs, valeur par défaut est de 10%.
• block_angle_speed_threshold : la différence d’angle par rapport au dernier tour de boucle d’asservissement doit être supérieure à cette valeur.
• block_dist_speed_threshold : la différence de distance par rapport au dernier tour de boucle d’asservissement doit être supérieure à cette valeur.
• blocking_detected_duration_threshold : Temps de dépassement en sec indiquant que c’est un blocage.

4. Documentation des classes

Classe AccelerationDecelerationLimiterInterface

> AbstractAccelerationLimiter :

Classe abstraite utilisée par AdvancedAccelerationLimiter et SimpleAccelerationLimiter. C’est le code commun qui permet d’appliquer une limite à l’accélération uniquement.

> SimpleAccelerationLimiter :

La limite d’accélération est ici une constante.

> AdvancedAccelerationLimiter :

Nous avons observé avec nos robots, qu’on peut accélérer plus fort une fois le robot en mouvement qu’à l’arrêt. Jeff a donc bricolé un petit algo pour faire varier l’accélération en fonction de la vitesse actuelle et donc accélérer plus fort une fois en mouvement, tout en ne “burnant” pas au départ, vous savez… ces trous sur la table fait avec ses démarrages en trombe!

Pour cela, on définit une accélération minimale (Acc_min) et une maximale (Acc_max) ainsi qu’une vitesse à partir de laquelle on considère qu’on peut appliquer l’accélération max (high_speed_threshold).

• Si la vitesse actuelle est nulle, on va utiliser Acc_min,
• si la vitesse actuelle est supérieure ou égale à high_speed_threshold, on va utiliser Acc_max,
• et entre 0 et high_speed_threshold, on va faire varier linéairement l’accélération max entre Acc_min et Acc_max.

> AccelerationDecelerationLimiter :

Permet de contrôler finement l’accélération et la décélération en marche avant et en arrière.

Cette classe est basée sur une version modifié de l’algo présentée ici : 

https://www.mdpi.com/1996-1073/12/7/1222/pdf 

https://www.mdpi.com/1996-1073/12/7/1222/pdf 

L’algorithme tire parti du fait que le correcteur de distance est un proportionnel pur, et est donc prévisible. L’algorithme calcule d’abord une vitesse max atteignable avec une accélération/décélération définie pour la consigne actuelle, puis en déduit une courbe qui va être soustraite à la sortie du régulateur P. Cela de manière à générer une belle courbe qui permet de décélérer à la bonne vitesse, depuis le bon endroit, pour atteindre la cible.

Comme toujours dans ce genre de cas, cela ne fonctionne pas de manière magique, il faut régler un coefficient d’amortissement (dampling) qui permet de décélérer plus tôt, car (entre autre) l’inertie du système n’est pas prise en compte dans le calcul.

Classe Encoder

> QuadratureEncoder :

Exploite les décodeurs de quadrature hardware du microcontrôleur de la carte Nucleo. Cela évite donc d’être pollué par des interruptions dans le flot d’exécution du programme principal.

> MagEncoders :

Exploite les codeurs magnétiques fabriqués pas PM-ROBOTIX.

Classe SpeedController

> SpeedController :

Cette classe permet de contrôler en vitesse une des roues du robot. La régulation est basée sur un PI avec une protection antiWindup, dont le principe a été repris de l’ODrive.

> AdaptativeSpeedController :

Comme pour SpeedController, cette classe régule en vitesse une des roues en utilisant un PI. A la différence près qu’il y a plusieurs set de paramètres (Kp;Ki) qui vont être utilisés en fonction de la vitesse actuelle de la roue.

On s’est rendu compte qu’il est difficile de trouver un set de paramètres (Kp;Ki) qui permette à la fois de suivre des consignes de vitesses faibles et fortes (sans un overshoot de taré hein 😀 ).

Actuellement, il y a 3 set de paramètres (Kp;Ki) possibles dans la classe, mais en utilisation, on s’est rendu compte que 2 suffisent en général. Cette classe est un enfant de SpeedController, ce qui permet de la substituer et de ne pas dupliquer le code du PI.

Mais cette implémentation n’est pas vraiment folichonne en terme d’ingénierie logicielle, donc on réfléchit à faire une vraie interface SpeedController dans un prochain temps.

Classe motorController 

> motorController :

Interface qui permet d’ajouter d’autres carte moteur…

> MD22 :

Carte de puissance (https://www.robot-electronics.co.uk/htm/md22tech.htm) qui date de temps immémoriaux!

Elle est grosse, pas super pratique, ne peut pas fournir beaucoup de courant, mais à l’avantage d’être quasi idiot-proof 🙂

Classe Odometry

Estimation de la position en (x,y,thêta) du robot en partant du principe qu’entre 2 calculs, le robot a suivi une trajectoire en forme d’arc de cercle. Voir la partie Odométrie et Positionnement pour plus d’explications.

Classe Regulator

Régulateur proportionnel pur qui permet de réguler la distance ou l’angle.

Classe PLL

Estimateur de vitesse inspiré de celui codé dans l’Odrive. C’est un estimateur basé sur une boucle de régulation PI, j’avoue que c’est un peu magique mais ça a le bon goût de marcher 🙂

Pourquoi utiliser un estimateur de vitesse? Augmenter la fréquence d’asservissement a beaucoup d’avantages, mais lorsque la vitesse est faible, la vitesse minimale estimée devient grande.

Exemple, pour une fréquence d’asservissement de 500 hz, soit Dt=0.002s, des roues de 3cm de rayon, soit un périmètre de 188.5 mm. Pour un encodeur de 4096 ticks par tour, on a 0.04 mm/tick. Donc 1 tick mesuré sur une période d’asservissement donne : (0.041)/0.002=20mm/sec et pour 2 ticks: (0.042)/0.002=40mm/sec

On comprend bien avec cet exemple, qu’on ne peut pas s’asservir sur ce genre de mesure. C’est pourquoi, on ajoute cet estimateur, qui permet de lisser tout ça ! Si vous voulez aller plus loin, vous pouvez lire ce thread : https://discourse.odriverobotics.com/t/rotor-encoder-pll-and-velocity/224

Il faudra par contre régler la bande passante de cet estimateur, et pour cela Jeff n’a rien trouvé de très cadré. Nous l’avons toujours réglé de manière empirique en utilisant Plotjuggler pour superposer les courbes des ticks codeurs brutes et l’estimation de la vitesse.

En résumé:

• Plus la bande passante est faible, plus la vitesse estimée sera lissée, mais plus elle aura du retard!
• A l’inverse plus la bande passante est grande, plus l’estimation de vitesse sera réactive, quitte à devenir instable !

Classe commandManager 

Cette partie permet de gérer le déplacement du robot sur la table, elle prend en entrée des commandes de haut niveau de déplacement sur table et en sort des consignes en distance et angle. Le découpage assez strict pour bien séparer les sujets et factoriser le code :

> CommandList :

Une classe simple qui permet de gérer une liste de Commandes. L’implémentation ne nécessite pas de lock, ce qui est mieux pour notre cas d’utilisation en temps réel.

> CommandManager  :

Classe centrale qui permet d’ajouter de nouvelles commandes dans la liste, de mettre ou enlever un arrêt d’urgence, de remonter un statut pour le haut niveau et de lancer le calcul de nouvelles consignes pour le régulateur de distance et d’angle.

Voici la liste des commandes:

• StraitLine : Comme son nom l’indique, cette commande permet d’avancer tout droit, en faisant varier la consigne du régulateur de distance uniquement.
• Turn : Comme son nom l’indique, cette commande permet de tourner d’un angle spécifié, en faisant varier la consigne du régulateur d’angle uniquement.
• Goto :

Permet d’aller à une position en (x;y) sur la table. L’algorithme fonctionne en 4 étapes :

1. Tant que le robot n’est pas suffisamment aligné vers le point cible, on donne uniquement des consignes d’angle. Par exemple, si on donne une cible derrière nous, cela évite de faire un grand arc de cercle non maîtrisé sur la table.
2. Calculer en temps réel une consigne d’angle et de distance en fonction de notre position actuelle par rapport à la cible
3. Une fois que le robot est en dessous d’une certaine distance, on ne calcule plus que des consignes d’angle. Sans cette étape, si on passe un mm à droite de la cible, le delta de distance sera de 1mm mais le delta d’angle sera de 90°, et donc le robot va se mettre à tourner autour de la cible.
4. Une fois que la distance par rapport au point cible est en dessous d’un seuil, on se considère comme arrivé, et on peut donc passer à la commande suivante.

Il y a donc 3 paramètres à régler :

1. arrivalDistanceThreshold_mm : Distance en mm en dessous de laquelle, on considère qu’on est arrivé sur la cible, pour l’étape 4.
2. gotoReturnThreshold_mm : Distance en mm à partir de laquelle, on ne calcule plus de consigne d’angle, pour l’étape 3.
3. gotoAngleThreshold_rad : Angle en radian au-delà duquel, on ne génère pas de consigne de distance, pour l’étape 1.

• GotoAngle : Permet de faire face à un point en (x;y) sur la table. Il n’y a qu’un seul paramètre à régler, arrivalAngleThreshold_rad, angle en radian en dessous duquel, on considère le but atteint.
• GotoNoStop : Cette commande offre le même service qu’un Goto, mais tente de ne pas s’arrêter sur le point, mais de passer proche en ralentissant si le point suivant est enchaînable.

Classe BlockingDetector

> OldSchoolBlockingDetector

Classe de détection du blocage qui permet de détecter si les roues sont bloquées.

Un blocage doit comporter les « caractéristiques » suivantes :

• Au moins un des moteurs doit tourner à plus de x% de sa puissance (nous avions souvent mis 10%)
• Si le robot est en train de tourner, la différence d’angle par rapport au dernier tour de boucle d’asservissement doit être supérieure à une valeur block_angle_speed_threshold
• Si le robot est en train d’avancer, la différence de distance par rapport au dernier tour de boucle d’asservissement doit être supérieure à une valeur block_dist_speed_threshold

Si toutes ces conditions sont réunies, on incrémente un compteur de la durée de la boucle d’asservissement, et sinon on remet à zéro ce compteur.

Si ce compteur dépasse un certain temps blocking_detected_duration_threshold, le blocage est détecté et peut être notifié.

> SpeedErrorBlockingDetector

Classe qui permet de calculer l’intégrale glissante de l’erreur de la consigne de distance. Et si cette intégrale dépasse un seuil, on considérerait que l’on est bloqué.

Comme indiqué précédemment, cette solution ne fonctionne pas comme souhaitée, et n’est pas concluante.

5. Utilisation de Plotjuggler

Ouvrir la connexion

Ouvrez simplement le plugin asserv_stream, et ouvrez le bon device. De manière générale, soit /dev/ttyACM0 ou /dev/ttyACM1.

La fenêtre classique de PlotJuggler reste ouverte, vous permettant de visualiser les données, et une autre fenêtre s’ouvre, vous permettant d’envoyer des commandes prédéfinies pour vous aider dans votre réglage.

Les données disponibles

Toutes ces données sont disponibles par itération de la boucle d’asservissement.

• raw_encoder/right : Nombre de tick de l’encodeur droit
• raw_encoder/left : Nombre de tick de l’encodeur gauche
• angle_regulator/goal : Consigne d’entrée du régulateur d’angle en rad
• angle_regulator/accumulator : Angle actuel en rad stocké par le régulateur
• angle_regulator/output : Consigne de sortie du régulateur d’angle en rad/sec
• angle_regulator/limited_output : Consigne de sortie du régulateur d’angle en rad/sec après limitation de l’accélération
• distance_regulator/goal : Consigne d’entrée du régulateur de distance en mm
• distance_regulator/accumulator : Distance actuelle en mm stocké par le régulateur
• distance_regulator/output : Consigne de sortie du régulateur de distance en mm/sec
• distance_regulator/limited_output : Consigne de sortie du régulateur de vitesse en mm/sec après limitation de l’accélération
• speed/right/goal : Consigne d’entrée du régulateur en vitesse droite en mm/sec
• speed/left/goal : Consigne d’entrée du régulateur en vitesse gauche en mm/sec
• speed/right/current : Vitesse droite estimée d’après les ticks du codeurs droit.
• speed/left/current : Vitesse gauche estimée d’après les ticks du codeurs gauche.
• speed/right/outputConsign : Consigne de sortie du régulateur de vitesse droite en pourcentage de puissance [-100%;100%]
• speed/left/outputConsign : Consigne de sortie du régulateur de vitesse gauche en pourcentage de puissance [-100%;100%]
• speed/right/output_consign_integrated : Part de l’intégrale dans la valeur de speed/right/outputConsign
• speed/left/output_consign_integrated : Part de l’intégrale dans la valeur de speed/left/outputConsign

Bizarreries

Il y a 2 comportements gênants dans le plugin Plotjuggler, mais qui ne bloquent en rien son utilisation:

• A l’ouverture du port /dev/ttyACMxx, le drivers met du temps à se mettre en marche, et l’ouverture du device peut être refusé pendant quelques secondes. Insistez, ça va finir par marcher.

• Lorsque vous vous déconnectez/reconnectez du stream après avoir reseter la carte Nucleo, les données sont souvent incohérentes pour Plotjuggler. Il faut alors vider le buffer de données de Plotjuggler pour que l’affichage reparte. Pour cela allez dans Data -> “Clear data points”, ou plus rapidement avec le raccourci Ctrl+Shift+c.

>> Cliquer pour lire la page précédente <<