Travaux Pratiques Temps-réel et Système embarqué FIP Lego Mindstorms Tribot : exploration, évitement et odométrie Vous disposez pour effectuer ce TP d Ubuntu et : nbc : cross-compilateur NXC un éditeur de texte gedit (ou gvim,) ressource dans la page de l équipe eavr, section enseignement fip, temps réel : le cours abrégé un lien vers la documentation de nxc qui permet de rechercher les détails d une fonction. Consignes : Appeler un encadrant pour valider votre travail aux points indiqués dans le sujet. Les quelques lignes et paragraphes de commentaires dans vos codes sources sont votre compte rendu (sur vos choix d implémentation, bogues rencontrés, critiques). 1 Prise en Main 1.1 Lego Tribot Utiliser le chargeur pour mettre en charge la batterie de la brique NXT. Utiliser le document d assemblage pour construire le robot Tribot. Aller chercher les pièces nécessaires sur la table centrale. Assembler le robot (sans pince à l avant). Utiliser quelques pièces additionnelles pour fixer le capteur ultrasons à l avant du Tribot. 1.2 Capteurs en NXC 1.2.1 Exercice 1.1 : Créer un fichier Exo1_1.nxc. D après le cours, écrire une tâche main qui : 1. initialise le capteur ultrasons, le capteur de contact; 2. réalise une boucle infinie : pour afficher la distance instantanée mesurée par le capteur ultrasons, pour afficher l heure courante (long CurrentTick() en ms depuis la mise sous tension) de la brique à chaque appui sur le capteur de contact. 3. Pour que l affichage soit correct, il faut effacer l écran avant de réécrire dessus. Ajouter un appel à ClearScreen(). Connecter le NXT au PC avec le câble USB et l allumer. Compiler et downloader le programme en 1 fois avec nbc -d -EF Exo1_1.nxc. Le NXT émet un bip en cas de transfert réussi. L. Cuvillon 1
1.3 Application avec moteurs et capteurs 1.3.1 Exercice 1.2 : Mouvement avec évitement d obstacles Ecrire une tâche main qui : 1. initialise le capteur ultrasons; 2. démarre une nouvelle tâche intitulée move; Ecrire une tâche move qui : 1. fait avancer le robot tant qu aucun obstacle n est détecté (à moins de 14 cm). On utilisera une macro en début de programme : #define DIST_cm 14 pour pouvoir aisément changer cette constante; 2. fait un bip et subir une rotation du robot de RIGOUREUSEMENT 135 degrés vers la gauche si un obstacle est détecté avant de reprendre sa marche en avant. Pour cela, utiliser RotateMotor() et les dimensions suivantes du robot : le demi-entraxe des roues L = 56 mm et le diamètre des roues 2r = 56 mm. 1.3.2 Exercice 1.3 : Mouvement avec évitement d obstacles On veut faire de même que l exercice précedent mais distribuer le code dans 2 tâches : Ecrire une tâche main qui : 1. initialise le capteur ultrasons; 2. démarre deux nouvelles tâches intitulée move et escape; Le code de la tache move fait simplement avancer le robot et est : task move() { while(1) OnFwd(OUT_BC,50); /*ou OUT_AC, OUT_BC suivant montage*/ } Ecrire alors une tâche escape qui se charge uniquement de : 1. tester la présence d un obstacle à moins de 14 cm; 2. en présence d un obstacle, fait subir une rotation du robot de RIGOUREUSEMENT 135 degrés vers la gauche. Tester. Ajouter un objet de synchronisation pour obtenir le comportement souhaité. Point de validation 1 2 Odométrie On souhaite ici calculer la vitesse angulaire des roues à partir de la mesure de position angulaire. 2.1 Exercice 2.1 : Vitesse angulaire des roues ϕ r et ϕ l A partir du programme précédent, remplacer la tâche escape par une tâche odom, qui réalisera l estimation odométrique. A chaque itération de la boucle, la tâche odom doit : 1. Calculer la vitesse angulaire de la roue droite ϕ r et gauche ϕ l à exprimer en rad/s (figure 1). 2. Effacer l écran, et afficher les 2 vitesses à chaque itération. L. Cuvillon 2
ϕ r ϕ r_precedent. ϕ ϕ r_courant r_precedent ϕ ϕ r_courant r_courant == t_courant t_precedent t_precedent t_courant t Figure 1 Dérivation numérique de ϕ r. 3. Avoir une période d échantillonage et d affichage d environ 40 ms(au temps d exécutions des instructions près)via l instruction Wait. On propose l implémentation suivante, qui n a pas de temps mort (moment pendant lequel la rotation des moteurs n est pas comptabilisée). while(1) { /*Calcul période réelle entre 2 boucles*/ Temps_Courant=CurrentTick(); Te=Temps_Courant-Temps_Precedent; /*Calcul des vitesse angulaires */ Phir_Courant=MotorRotationCount(); Phil_Courant= /*Affichage*/ /*Update for the next loop*/ Temps_Precedent=Temps_Courant; Phir_Precedent=Phir_Courant; Wait(40); } Tester avec une puissance de 40% et valider l estimation de vitesse en rad/s. Déterminer alors C r et C l, les coefficients reliant la puissance (entre -100 et 100) envoyée aux moteurs (U r et U l ) à la vitesse angulaire des roues, tels que Point de validation 2 : ( ) ( )( ) ϕr Cr 0 Ur = (1) ϕ l 0 C l U l L. Cuvillon 3
3 Asservissement sur une trajectoire : parcours d une piste On souhaite détérminer la tension à appliquer aux roues pour imposer une vitesse linéaire, v (m.s 1 ) et angulaire, ω (en rad.s 1 ) au robot; soit déterminer une matrice M 1 telle que : ( ) ( ) Ur = M 1 v (2) ω avec U l M 1 rad = 1 rc r 1 rc l L rc r L rc l (3) Application numérique : d après les dimensions du robot Lego (2r = L = 56mm) et avec C r = C l = 0.11rad/s/V. ( ) M 1 324.6753 18.1818 = (4) 324.6753 18.1818 3.1 Exercice 3.1 : Commande en vitesse opérationnelle : v et w 1. Faire l application numérique pour calculer M 1 avec vos coefficients C l et C r. 2. Supprimer la tache odom et remplacer le code dans la tache move par la commande en vitesse opérationnelle sur le lego : Définir une variable pour la vitesse linéaire v de valeur 0.06 m.s 1 par exemple et une variable w = 0 rad.s 1. Implémenter le calcul de U r et U l correspondant à partir de votre matrice M 1. Une commande U {r,l} > 100% ou < 100% est possible si la vitesse demandée est trop grande. Il n est pas possible d appliquer une telle commande hors limite car elle donne alors un comportement erratique du moteur. Mettre en place une saturation des commandes U {r,l} à 100% et -100% avant application aux moteurs pour faire au mieux dans le cas de commandes trop grandes. Envoyer la commande sur les moteurs avec la fonction OnFwd. Note : OnFwd(,-60) = OnRev(,60). 3. Valider votre commande en vitesse du Lego Tribot. Pour cela, tester d abord avec une vitesse linéaire seule, puis avec une vitesse angulaire seule, puis une combinaison des 2. Point de validation 3 3.2 Exercice 3.2 : Parcours de la piste Le parcours est constitué d une piste présentant un dégradé de gris : du blanc vers le noir. La trajectoire à suivre est le gris médian situé en bordure de la piste. Le capteur photosensible est utilisé pour mesurer le niveau de gris et donc la distance d (en % de luminosité) par rapport au gris médian (notre trajectoire). 1. Calibrage du capteur photosensible (a) Positionner le capteur à l avant du Lego, tel que le capteur soit situé à environ 7-8mm au dessus du sol. (b) Utiliser l utilitaire dans le menu View >Reflected Light du NXT pour mesurer la valeur renvoyée par le capteur en % pour le gris médian (voir aussi le noir et le blanc). L. Cuvillon 4
v Capteur lumière réfléchie v v v: vitesse linéaire (v=15cm/s constant) ω: vitesse angulaire loi de commande: ω = (%Réfléchie %Réfléchie_Gris)*gain*v ω +ω Figure 2 Principe du suivi de la piste. 2. Implémentation de la loi de commande (a) Modifier la tâche move pour réaliser l asservissement. (b) Définir v, une consigne de vitesse constante de votre choix en m.s 1. (c) Calculer alors la loi de commande proportionnel w = vdk en fonction de v toujours expriméenm.s 1,d ungaink etded,ladistancealgébriqueparrapportàlatrajectoire exprimée en % de luminosité d après la préparation. On souhaite parcourir la piste dans le sens horaire! (d) Utiliser l expression numérique de M 1 pour calculer les consignes correspondantes sur les moteurs U r et U l. 3. Expérimentation (a) Tester l asservissement avec une consigne de vitesse linéaire raisonnable. Adapter le gain k pour réussir le parcours de la piste. (b) Essayer d augmenter la consigne v pour accélérer le robot (et faire la course). Point de validation 4 L. Cuvillon 5