MIRO est une plate-forme de robot intérieure ouverte. Partie 5 - Composant logiciel: ARDUINO (AVR), on grimpe "sous le capot"

Cette fois, nous allons approfondir un peu la mise en œuvre de certaines méthodes de bibliothèque clés pour ARDUINO (AVR), qui sont responsables du déplacement du robot MIRO. Cette partie sera intéressante pour tous ceux qui se demandent comment contrôler la vitesse linéaire et angulaire du robot sur l'ARDUINO, équipé de moteurs avec les encodeurs les plus simples.

Table des matières: Partie 1 , Partie 2 , Partie 3 , Partie 4 , Partie 5 .

Les méthodes responsables de la conduite avec odométrie sont toujours pénibles pour expliquer comment, quoi et pourquoi. La première chose que vous devez savoir sur le contrôle du mouvement du robot est le fait simple et évident que les moteurs collecteurs du robot ne tournent jamais à la même vitesse sans réglage supplémentaire. Embrayage différent, différentes caractéristiques de sortie des canaux de commande, moteurs électriques légèrement différents et lubrification dans la boîte de vitesses.

Le deuxième fait que vous devez comprendre et connaître est la présence d'inertie dans le moteur, même avec un rapport de transmission suffisamment important. C'est-à-dire lors de la suppression de la tension aux bornes du moteur, la roue, même non chargée, fait un mouvement de quelques degrés de plus. L'amplitude de cette rotation supplémentaire dépend de la force de charge sur la roue, de la vitesse de rotation avant de relâcher la tension et des mêmes facteurs invisibles comme le type et la quantité de lubrifiant dans la boîte de vitesses.

Ces faits déterminent la mise en œuvre d'un ensemble de méthodes liées au déplacement d'un châssis équipé de capteurs odomètre (dans le cas de MIRO, codeurs numériques pour chaque roue).

Comme nous l'avons découvert dans la quatrième partie, la classe Châssis existe dans le modèle logiciel, qui met en œuvre le contrôle de rotation des moteurs de châssis individuels. Je veux souligner - non pas le contrôle du mouvement du châssis, du chariot, mais le contrôle des moteurs du chariot. Le contrôle direct du chariot est implémenté dans les classes Robot et Miro .

Commençons par le haut. Ci-dessous, une méthode de la classe Miro qui implémente le mouvement d'un robot sur une certaine distance ( dist , mètres) avec des vitesses linéaires ( lin_speed , m / s) et angulaires ( ang_speed , deg / s) données. Nous ne prêtons pas encore attention au paramètre en_break .

int Miro::moveDist(float lin_speed, float ang_speed, float dist, bool en_break) { float _wheelSetAngSpeed[WHEEL_COUNT]; _wheelSetAngSpeed[LEFT] = MIRO_PI2ANG * (lin_speed - (ROBOT_DIAMETER * ang_speed / (2 * MIRO_PI2ANG))) / WHEEL_RADIUS; _wheelSetAngSpeed[RIGHT] = MIRO_PI2ANG * (lin_speed + (ROBOT_DIAMETER * ang_speed / (2 * MIRO_PI2ANG))) / WHEEL_RADIUS; float _wheelSetAng[WHEEL_COUNT]; _wheelSetAng[RIGHT] = _wheelSetAngSpeed[RIGHT] * dist / lin_speed; _wheelSetAng[LEFT] = _wheelSetAngSpeed[LEFT] * dist / lin_speed; return this->chassis.wheelRotateAng(_wheelSetAngSpeed, _wheelSetAng, en_break); } 

Dans cette méthode, les vitesses angulaires REQUISES pour les moteurs gauche et droit sont d'abord calculées. Selon des formules assez évidentes, ce qui n'est pas un problème à déduire. Il suffit de garder à l'esprit que la vitesse linéaire dans la méthode est spécifiée en mètres par seconde, et la vitesse angulaire en degrés par seconde (pas en radians). Par conséquent, nous pré-calculons la constante MIRO_PI2ANG = 57,29 = 180 / pi. ROBOT_DIAMETER - distance entre les roues gauche et droite du robot (en mètres), WHEEL_RADIUS - rayon de roue (également en mètres). Toutes les constantes numériques pour de tels cas sont contenues dans le fichier defs.h et les paramètres personnalisés du robot et du châssis se trouvent dans le fichier config.h.

Après cela, l'angle est calculé par lequel chaque roue doit être tournée de sorte que le robot parcourt la distance dist (également en mètres).

Ainsi, à ce stade, nous obtenons à quelle vitesse et à quel angle vous devez faire tourner chaque roue du châssis du robot. Et puis la méthode wheelRotateAng () de l'objet châssis est appelée.

La méthode wheelRotateAng (float * speed, float * ang, bool en_break) est utilisée pour faire tourner les roues du robot avec les vitesses angulaires spécifiées par le tableau speed [] (en m / s) selon les angles spécifiés par le tableau ang [] (en degrés). Le dernier paramètre en_break (déjà rencontré par nous plus tôt) définit l'exigence d'un arrêt dur des roues après avoir effectué un virage en leur appliquant une tension inverse à court terme. Cela est nécessaire pour supprimer l'inertie du robot, l'empêchant de se déplacer au-delà de la distance requise déjà après avoir supprimé la tension de commande des moteurs. Pour une satisfaction complète, bien sûr, il existe la méthode wheelRotateAngRad () , similaire à wheelRotateAng (), à la différence près qu'elle prend les valeurs des angles de rotation et des vitesses angulaires en radians et radians par seconde comme paramètres.

L'algorithme de la méthode wheelRotateAng () est le suivant.

1. Tout d'abord, la correspondance des valeurs de vitesse [] et ang [] avec certaines conditions aux limites est vérifiée. Évidemment, le châssis présente des limitations physiques à la fois sur la vitesse angulaire maximale de rotation des roues, et sur la vitesse minimale (vitesse minimale d'éloignement). De plus, les angles en ang [] ne peuvent pas être inférieurs à l'angle de rotation fixe minimum, déterminé par la précision des codeurs.

2. Ensuite, le sens de rotation de chaque roue est calculé. De toute évidence à travers le signe du produit ang [i] * vitesse [i] ;

3. La «distance de rotation» Dw [i] pour chaque roue est calculée - le nombre d'échantillons d'encodeur qui doivent être effectués pour tourner de l' ang donné [i] .
Cette valeur est déterminée par la formule:

Dw [i] = ang [i] * WHEEL_SEGMENTS / 360 ,
où WHEEL_SEGMENTS est le nombre de segments de la roue codeuse (révolution complète).

4. La valeur de tension sur le pilote de moteur est enregistrée.


5. Selon le tableau d'étalonnage de chaque roue, la valeur initiale du signal PWM est déterminée, ce qui garantit la rotation de la roue à la vitesse requise [i] . De quel type de tableau d'étalonnage et d'où provient-il - nous analyserons plus avant.

6. La rotation des moteurs démarre en fonction des valeurs calculées de vitesse et de sens de rotation. Dans le texte de l'implémentation de la classe, la méthode privée _wheel_rotate_sync () en est responsable.

Nous allons encore plus loin. La méthode _wheel_rotate_sync () fonctionne selon l'algorithme suivant:

1. Dans une boucle infinie, une vérification est effectuée pour obtenir le compteur des réponses du codeur de la distance de rotation Dw [i] pour chaque roue. Si l'un des compteurs Dw [i] est atteint, toutes les roues s'arrêtent et sortent du cycle puis quittent la fonction (étape 5). Cela se fait pour les raisons suivantes. En raison de la discrétion de mesurer l'angle de rotation, c'est une situation très courante lorsque la distance calculée Dw [i] d' une roue est obtenue en arrondissant une valeur non entière à un petit côté et Dw [j] de la deuxième roue à une plus grande. Cela conduit au fait qu'après l'arrêt d'une des roues, la deuxième roue continue de tourner. Pour un châssis avec un entraînement différentiel (et pour beaucoup d'autres), cela conduit à un «virage» imprévu du robot à la fin de la tâche. Par conséquent, dans le cas de l'organisation du mouvement spatial de l'ensemble du châssis, il est nécessaire d'arrêter tous les moteurs à la fois.

2. Si Dw [i] n'est pas atteint, alors dans la boucle le fait de la prochaine opération de l'encodeur est vérifié (la variable _syncloop [w] , mise à jour à partir de l'interruption de l'encodeur et réinitialisée dans cette boucle infinie). Lorsque la prochaine intersection se produit, le programme calcule le module de la vitesse angulaire actuelle de chaque roue (deg / s), selon la formule évidente:

W [i] = (360 * tau [i]) / WHEEL_SEGMENTS ,
où:
tau [i] - la valeur moyenne du temps entre les deux dernières réponses des encodeurs. La «profondeur» du filtre de moyenne est déterminée par MEAN_DEPTH et vaut 8 par défaut.

3. Sur la base des vitesses de roue calculées, les erreurs absolues sont calculées comme les différences entre les vitesses angulaires définies et réelles.

4. Sur la base des erreurs calculées, l'action de contrôle (valeur du signal PWM) est corrigée pour chaque moteur.

5. Après avoir atteint Dw [i] , dans le cas d'un en_break actif, une tension inverse à court terme est appliquée aux moteurs. La durée de cet effet est déterminée à partir du tableau d'étalonnage (voir ci-dessous) et varie généralement de 15 à 40 ms.

6. Il y a une libération complète des contraintes des moteurs et quittez _wheel_rotate_sync () .

J'ai déjà mentionné deux fois un certain tableau d'étalonnage. Ainsi, dans la bibliothèque, il y a une table spéciale de valeurs stockées dans l'EEPROM de la mémoire du robot et contenant des enregistrements de trois valeurs liées:

1. Tension aux bornes du moteur. Il est calculé en convertissant la valeur du signal PWM en tension réelle. Pour cela, à l'étape 4 de la méthode wheelRotateAng () , la tension réelle sur le pilote du moteur est enregistrée.

2. La vitesse angulaire de rotation de la roue (sans charge) correspondant à une tension donnée.

3. La durée du signal d'arrêt dur correspondant à cette vitesse angulaire.
Par défaut, la taille de la table d'étalonnage est de 10 enregistrements (déterminée par la constante WHEEL_TABLE_SIZE dans le fichier config.h ) - 10 triplets de valeurs "tension - vitesse angulaire - durée du signal d'arrêt".

Pour déterminer les valeurs des entrées 2 et 3 dans ce tableau, une méthode spéciale est utilisée - wheelCalibrate (roue d'octets) .

Examinons-y un peu. Cette méthode met en œuvre une séquence d'actions pour déterminer les valeurs manquantes dans le tableau d'étalonnage moteur / roue, ainsi que pour connaître la vitesse angulaire minimale de démarrage et la vitesse angulaire maximale de la roue.

Pour effectuer l'étalonnage, le robot est monté sur un support; toute la rotation des roues pendant l'étalonnage est effectuée sans charge.

1. Vous devez d'abord déterminer la vitesse de démarrage minimale. Cela se fait très simplement. Dans un cycle, la commande PWM est envoyée au moteur, à partir de 0, avec un incrément de 1. À chaque étape, le programme attend un certain temps, déterminé par la constante WHEEL_TIME_MAX ( délai normal () ). Une fois le temps d'attente écoulé, il vérifie si le démarrage est terminé (en modifiant la valeur du compteur du codeur). Si le retrait est terminé, la vitesse angulaire de rotation de la roue est calculée. Pour plus de certitude, la valeur de 10 est ajoutée à la valeur du PWM correspondant à cette vitesse de démarrage, ce qui donne la première paire de valeurs «tension sur le moteur» - «vitesse angulaire».

2. Une fois la vitesse de démarrage trouvée, le pas PWM est calculé pour remplir uniformément la table d'étalonnage.

3. Dans le cycle, pour chaque nouvelle valeur PWM, la roue tourne de 2 tours complets et la vitesse angulaire est mesurée selon un algorithme similaire à la méthode _wheel_rotate_sync () . Dans le même cycle, également par approximation successive, la valeur optimale de la durée du signal d'arrêt dur est mesurée. Dans un premier temps, une certaine valeur évidemment grande est prise. Et puis il est testé en mode "turn-stop". Comme valeur optimale, la valeur maximale de la durée du signal d'arrêt est sélectionnée, à laquelle la «distance de virage» définie n'est pas dépassée. En d'autres termes, une telle valeur de la durée du signal, lors de l'alimentation du moteur d'une part, l'inertie est supprimée, et d'autre part, il n'y a pas de mouvement inverse à court terme (qui est fixé par le même codeur).

4. Une fois l'étalonnage terminé, la tension de commande du moteur étalonné cesse d'être appliquée et le tableau d'étalonnage de cette roue est enregistré dans l'EEPROM.

J'ai omis toutes sortes de trivialités de mise en œuvre et j'ai essayé de dire l'essentiel. Vous pouvez remarquer que les méthodes wheelRotateAng () et wheelRotateAngRad () sont des fonctions de blocage. C'est le prix pour la précision du mouvement et une intégration assez simple dans les esquisses de l'utilisateur. Il serait possible de créer un petit gestionnaire de tâches avec un calendrier fixe, mais cela obligerait l'utilisateur à intégrer ses fonctionnalités strictement dans le quota de temps alloué.

Et pour une application non bloquante, l'API a une fonction wheelRotate (float * speed) . Il, comme on peut le voir dans la liste des paramètres, effectue simplement la rotation des roues avec les vitesses définies. Et la vitesse de rotation est ajustée dans la méthode Sync () du châssis du robot, qui est appelée dans la méthode Sync () de l'objet de classe Miro du même nom. Et selon les exigences de la structure de l'esquisse de l'utilisateur, cette méthode devrait être appelée à chaque itération de la boucle loop () principale de l' esquisse ARDUINO.

À l'étape 4, dans la description de la méthode _wheel_rotate_sync () , j'ai mentionné la "correction de contrôle" du moteur. Comment avez-vous deviné)? Il s'agit du contrôleur PID). Eh bien, plus précisément contrôleur PD. Comme vous le savez (en fait - pas toujours), la meilleure façon de déterminer les coefficients du régulateur est la sélection). Il existe une définition dans le fichier de configuration config.h:

 #define DEBUG_WHEEL_PID 

Si vous la décommentez, lorsque vous appelez la méthode moveDist () de la classe Miro, le graphique inversé suivant de l'erreur relative dans le contrôle de la vitesse angulaire de l'une des roues du robot (à gauche) s'affiche dans la console du robot.



Ça ne ressemble à rien)? Down est le temps (chaque barre est une étape du cycle de contrôle), et la valeur d'erreur est enregistrée à droite (avec le signe conservé). Voici deux paires de graphiques sur la même échelle avec des coefficients différents du contrôleur PD. Les "bosses" ne sont que les "vagues" de dépassement. Les nombres sur les barres horizontales sont une erreur relative (avec conservation du signe). Visualisation simple du régulateur, aidant à ajuster manuellement les coefficients. Au fil du temps, j'espère faire une configuration automatique, mais pour l'instant.

Voici un tel adok :-)

Enfin, regardons un exemple. Directement à partir de la bibliothèque API_Miro_moveDist:

 #include <Miro.h> using namespace miro; byte PWM_pins[2] = { 5, 6 }; byte DIR_pins[2] = { 4, 7 }; byte ENCODER_pins[2] = { 2, 3 }; Miro robot(PWM_pins, DIR_pins, ENCODER_pins); int laps = 0; void setup() { Serial.begin(115200); } void loop() { for (unsigned char i = 0; i < 4; i++) { robot.moveDist(robot.getOptLinSpeed(), 0, 1, true); delay(500); robot.rotateAng(0.5*robot.getOptAngSpeed(), -90, true); delay(500); } Serial.print("Laps: "); Serial.println(laps); laps++; } 

D'après le texte du programme, tout doit être clair. Comment ça marche - dans la vidéo.


Carreaux 600 x 600 mm et intervalles de carreaux 5 mm. En théorie, le robot devrait faire le tour d'un carré d'un côté de 1 mètre. Bien sûr, la trajectoire «s'envole». Mais pour être honnête, il convient de dire que dans la version du robot que j'ai laissée pour les tests, il existe des moteurs assez rotatifs qui sont difficiles à obtenir pour rouler lentement. Mais à grande vitesse et en glissement, il y a un endroit où être, et l'inertie n'est pas facile à gérer. Les moteurs avec un rapport d'engrenage plus élevé (comme même dans nos robots MIRO, n'étaient tout simplement pas à portée de main pendant le test) devraient se comporter un peu mieux.

S'il y a des moments incompréhensibles - je suis heureux de clarifier, discuter et améliorer. Les commentaires sont généralement intéressants.