1 Introduction au TP

Automatique et Robotique TP LEGO Segway, page 1

TP LEGO Segway

PID numeriques avec le lieu des racines (Evans)

La duree de ce TP est de 8 heures.Ce TP est construit sur le Lego Segway propose par Yorihisa Yamamoto [1].

Capteur ultrason Brique LEGO: microcontrôleurGyroscope:

Moteur droit Moteur gaucheet codeur position: et codeur position:

✓

r

✓

l

FIGURE 1 – Photo du lego Segway etudie [1]

1 Introduction au TP

1.1 Objectif

Maintenir a l’equilibre le LEGO Segway (figure 1), qui est assimilable a un pendule inverse. Cesysteme a la particularite d’etre naturellement instable en boucle ouverte.

On definit , l’angle entre le corps du Segway et la vertical ; et ✓, l’angle moyen entre les roues etcette meme verticale (figure 2). L’asservissement consiste alors a commander U , la tension communeaux bornes des 2 moteurs afin de :

— maintenir le corps du Segway a l’equilibre (reguler a 0 l’angle ),— asservir ✓ pour une position stationnaire du Segway selon ~x (✓ = 0).

Les correcteurs PID utilises sont regles par placement de poles avec le lieu d’Evans dans le domainecontinu (cours 1A). On utilise ensuite la transformee bilineaire pour obtenir un correcteur numerique.

On utilise une station Linux avec le logiciel Matlab pour la synthese et simulation des correcteurs ;le systeme temps reel nxtOsek et le compilateur gcc pour l’implantation en langage C des correcteurssur la brique Lego.

1.2 Descriptif des entrees et sorties du systeme

Le systeme est de type SIMO (Single Input - Multiple Output).

ENSPS, 2eme Annee, L.Cuvillon 2011-2012


(n=1)réducteur

rotor du moteur

centre de gravitédu corps

stator du moteur, fixé au corps

l

z

M, J

x

R

m, J

w

✓

m

= ✓ �

Roue

J

w

✓

FIGURE 2 – Parametrisation du Lego Segway

L’entree du systeme est la tension d’induit U commune aux 2 moteurs (U = Ur = Ul). Notresegway n’est donc pas prevu pour pouvoir tourner.

Nature Commande Resolution PlageTension d’induit Vitesse du rotor 1% de la tension de la batterie -100% << +100%du moteur U (Volt) (⇡ 8 Volts a pleine charge)

Les sorties disponibles sur le systeme correspondent aux 3 capteurs suivant :Capteur Mesure Resolution PlageCapteur Ultrason d : distance a l’obstacle 1 cm +5 cm< d <+45 cmCodeur incremental ✓m{l,r} : angle entre roue {right/left} 1 deg ±des moteurs et corps du robotGyroscope ˙

: vitesse angulaire 1 deg/s -360�/s < ˙

< +360 �/sdu corps offset du capteur : ⇡ 612 1

Les 2 grandeurs , angle entre le corps du robot et la vertical, et ✓, angle moyen des roues parrapport a la verticales ne sont pas directement accessibles a la mesure via les capteurs disponibles.On peut cependant estimer la valeur de ces 2 grandeurs ( ˆ et ˆ✓) par les procedes suivants que vousverrez implementes sur la brique Lego :

— integration au cours du temps de ˙

et suppression de l’offset par filtrage passe-haut 2.— ˆ

✓ = ✓m +

ˆ

, avec ✓m =

✓mr+✓ml2 : angle moyen entre les roues et le corps du robot.

1. Le capteur fournit une tension analogique convertie en une valeur numerique positive comprise entre 0 et 1024.L’offset correspond a la valeur de sortie en absence de vitesse angulaire ( = 0). Cet offset peut varier d’un capteur al’autre et sera identifie automatiquement au demarrage du systeme.

2. L’integration sans precautions d’une vitesse n’est pas une bonne idee car tout capteur analogique a un biais demesure non nul (meme si infiniment petit) qui a pour consequence une divergence plus ou moins rapide de la mesure



1.3 Systeme a reguler

Du point de vue automatique, le systeme a reguler est celui de la figure 3. Les 2 fonctions detransfert sont obtenues a partir du modele dynamique du systeme avec ses actionneurs.

Lego Segway

U

U

(s) ✓

(s)

✓

FIGURE 3 – Le systeme a reguler

2 Preparation

2.1 Modele du systeme

Les equations d’Euler-Lagrange appliquees au systeme donnent les equations dynamiques sui-vantes pour les coordonnees generalisees et ✓ :

⇥(2m+M)R

2+ 2(Jw + Jm)

⇤¨

✓ + [MRl cos( )� 2Jm]

¨

�MRl sin( )

˙

2= C✓ (1)

⇥Ml

2+ J + 2Jm

⇤¨

+ [MRl cos( )� 2Jm]

¨

✓ �Mgl sin( ) = C (2)

avec

m : masse de la roueM : masse du corps (brique)Jw : moment d’inertie d’une roueJ : moment d’inertie du corpsJm : moment d’inertie du rotor d’un moteurR : rayon de la roue

l : distance centre de roue-centre de gravite ducorpsg : acceleration de la graviteC✓ : Couples appliquees aux rouesC : Couple applique au corps

1. Lineariser le systeme d’equation precedent autour de la position d’equilibre du systeme ( =

0) en fonction des grandeurs , ✓ et leurs derivees : utiliser un developpement limite a l’ordre1 des fonctions trigonometriques autour de 0 et negliger les derniers termes non-lineaires.

2. On neglige le frottements des roues sur le sol (hypothese de mouvement sans glissement).Les 2 moteurs sont situes entre les roues et le corps, on a alors l’egalite suivante (”principed’action-reaction”) :

C✓ = �C (3)

et

C✓ = 2.(Cm � fm(

˙

✓ � ˙

)) (4)= 2.(ktim � fm(

˙

✓ � ˙

)) (5)

avec— Cm : le couple d’un moteur— fm : un coefficient de friction proportionnel a la vitesse de rotation du rotor

(R t

O(x(t) + petit offset)dt = x(t) + petit offset.t.



— kt : la constante de couple du moteur— im : l’intensite du courant dans l’induit du moteur

D’apres le modele du moteur a courant continu (voir cours 1A), exprimer im en fonction de :— kb, la constante de la force contre-electromotrice— U , la tension au borne de l’induit— Rm, la resistance de l’induit (on neglige l’inductance du moteur)

3. Montrer alors que les equations (1) et (2), apres leur linearisation et l’expression des efforts enfonction de la tension d’induit U , peuvent s’ecrire sous la forme :

E11¨

✓ + E12¨

+ 2�(

˙

✓ � ˙

) = 2↵U (6)E12

¨

✓ + E22¨

� 2�(

˙

✓ � ˙

)�Mgl = �2↵U (7)

avec— ↵ =

ktRm

et � =

ktkbRm

+ fm

— E11 = (2m+M)R

2+ 2(Jw + Jm)

— E12 = MRl � 2Jm

— E22 = Ml

2+ J + 2Jm

4. Ecrire les equations (6) et (7) dans le domaine de Laplace. En deduire, par simple combinaisondes equations, la fonction de transfert d’ordre 2, ✓

(s). Calculer par une methode de substi-

tution (conseillee) la fonction de transfert U (s). Simplifier si necessaire les termes identiques

dans la fonction de transfert U (s) pour obtenir une fonction de degre 3.

Note1 : L’expression de U (s) est necessaire pour realiser la suite du TP.

Note2 : Les grandeurs dans ce modele physique sont notamment exprimees en Radians et en Volt.

2.2 Implementation de correcteurs PD et PID en serie

On souhaite ici preparer l’implementation en C des 2 correcteurs necessaires a notre asservisse-ment.

1. A partie de la figure 4, donner l’equation aux differences de ⇤(k), la consigne d’inclinai-

son a l’instant t = k.Te en fonction des coefficients du correcteur PD (Num2i, Den2i) et desechantillons passes des grandeurs suivantes : la consigne pour ✓, ⇤

(k � i) et ✓(k � i).On rappelle que si Z�1{U(z)} = U(k), alors Z{z�1

U(z)} = U(k � 1) dans le cas de conditionsinitiales nulles.

2. Donner finalement l’equation aux differences de la commande U(k) en fonction de Num1i,Den1i et des grandeurs passees ⇤

(k � i), (k � i) et U(k � i).

+−

Correcteur 1 : PIDCorrecteur 2 : PD

+−

Num10

+Num11

z

�1+Num12

z

�2

Den10

+Den11

z

�1+Den12

z

�2

U

Num20

+Num21

z

�1

Den20

+Den21

z

�1

0

✓

⇤

FIGURE 4 – Schema de correction.



3 TP : Synthese des correcteurs et programmation

Partie distribuee en TP.

References

[1] NXTway-GS (Self-Balancing Two-Wheeled Robot) Controller Design by Yorihisa Yamamoto,http ://www.mathworks.com/matlabcentral/fileexchange/19147http ://www.youtube.com/watch ?v=4ulBRQKCwd4


Um

1

✓

1/70

1024

Um

1

Um � Em = RmIm + Lm˙

Im

�m = KmIm Em = Km˙

✓m

�m � �r = fm˙

✓ + Jm¨

✓m

Um Im Em

Rm Lm ✓m˙

✓m¨

✓m �m

Km fm Jm

�r

Um ✓m

G(s) Um ✓m

G(s) G(s) = K/(s(1 + ⌧s))

Gv(s) = K/(1 + ⌧s)

Gvz

Gpz

Gv

Gpz

0 0.1

�0.2 +0.2

0.02

5

1/Ti

0

1/Ti

0 10

5

0

1

�2 2

0

�10 10

1Te

(z�1)z

0 0.04

TP Automates : STEP 7

Documents disponibles lors du TP: documentation STEP 7. Ce document, ainsi que le sujet dont est

inspiré ce TP, est l'œuvre de Patrick TRAU (IPST).

0) Préparation

1-Préparer l'exercice 1).2 de ce TP: Calculer avec un tableau de Karnaugh la fonction combinatoire

simplifiée répondant au problème posé : allumer la sortie si la somme est paire.

2-Préparer l'exercice 2).1 de ce TP: Réaliser sur feuille le grafcet de fonctionnement d'un seul chariot.

C'est un grafcet simple : pas de divergence en ET ou OU, pas d'actions conditionnelles.

3-Analyser brièvement la solution à la question finale de l'exercice 2 du cours : Celle-ci est donnée à

la fin de ce document et vous donnera des idées pour la solution finale de ce TP.

Déroulement du TP : Une fois une question résolue, présenter votre programme (qui fonctionne), passer

à la question suivante.

1) TP: Prise en main et langage de base

1 - Lecture (et compréhension) du document sur STEP7. Se familiariser avec le matériel et logiciel,

préparer le projet en décrivant le matériel dans le logiciel (comme décrit, jusqu'à la valise en haut de la

page 2, mais pas les mnémoniques). Les connexions IP et Profibus doivent être définies (et connectées).

2 – Langage à contacts : Ecrire un programme n'utilisant que des fonctions combinatoires, utilisant

trois entrées tout ou rien, que nous numéroterons 1, 2 et 3, et une sortie qui s'allumera quand la somme

des numéros d'entrée est paire (vous pouvez utiliser un tableau de Karnaugh). Par exemple, si on appuie

sur le 1 et le 3, 3+1 étant pair il faut allumer la sortie. Vous utiliserez obligatoirement le langage à

contacts (CONT). Je propose de considérer que 0 est un nombre pair (quand aucun n'est appuyé).

2) TP: Grafcet en STEP 7

Aujourd'hui, vous allez travailler sur un Grafcet, puis sur un système de plusieurs Grafcets synchronisés.

Nous allons étudier un poste de traitement de surface à deux chariots. Ne disposant que d'une maquette, il

va falloir simuler le fonctionnement sur votre poste (je vous ai préparé un carton détaillant les capteurs et

actionneurs, à introduire à l'endroit prévu sur votre console de simulation). Une fois vérifié, votre

programme pourra être transféré sur l'automate V19 sur lequel est branchée la maquette (ne le faites qu'avec l'accord de l'encadrant!).

ENSPS 20010-2011 L. CUVILLON , P.TRAU 1

1) Vous allez commencer par un Grafcet simple : sur ordre de l'opérateur (DCA), le chariot gauche

va chercher un bac au poste de chargement gauche, le lever, l'amener dans le dernier bain du côté

opposé (B5), attendre 4s puis ramener le bac. Voir descriptif du processus page 5.

• N'oubliez pas d'appeler dans l'OB1 (cyclique) le bloc fonctionnel contenant votre Grafcet.

• Une table des mnémoniques sur le bureau "memo_chariots.asc" peut être importée via

l'éditeur de mnémoniques pour s'éviter la saisie complète des mnémoniques. Cependant, c'est

à vous d'ajouter les mnémoniques correspondants aux actionneurs des chariots et aux boutons

de départ de cycle (DCA-DCB).

Tester sur votre poste.

2) Tester sur la maquette (Automate V19). Pour cela, dans votre projet, copier votre Station Simatic

300/Valise (exemple V10) et la coller à la racine du projet. Renommer la copie ainsi obtenue

(exemple : V10(1) en V19).

Aller dans le matériel de V19 : mettre à jour les adresses réseau profibus et ethernet. Ne pas oublier de réactiver ces 2 réseaux et de mettre à jour la configuration avec le bouton «Enregistrer et Compiler» !

3) A l'aide d'un second Grafcet, faites la même chose pour le chariot droit, qui dépose son bac en B1.

Il faudra également modifier un peu le premier Grafcet, car il faut prévoir le cas où un chariot doit

s'arrêter car l'autre monopolise la voie. Dans ce cas, le second attend que le premier reparte pour

continuer son cycle. Évidement, le second peut déjà commencer à prendre son bac et le lever, en

attendant le mouvement de retour du premier vers sa position d'origine.

Pour cela, on testera l'état des étapes et des transitions du Grafcet correspondant à l'autre chariot.

Note : DCB est utilisé pour le départ indépendamment de A du chariot B.

4) Reprendre la question 2, mais utiliser une action set/reset d'un bit mémoire (M0.1 par exemple)

pour indiquer si le pont est occupé ou non.

Note : représentation d'une transition de la forme « Etape 22 active ET active depuis plus de 4s» :

X22 . t\X22\4s (il y a redondance de ces 2 conditions mais c'est une illustration)


S22 Step 22

totototo.X

>

T#4s

toto.T

Description poste de trempage deux chariots

Deux chariots, A et B, peuvent se déplacer sur un même rail, au dessus de 5 bains de trempage contenant

différents produits. Les pièces à traiter sont déposées dans un bac, qui sera trempé dans les différents

bains par le chariot. Chaque chariot peut avancer ou reculer, depuis une position reculée où le chariot se

place au repos (inaccessible au chariot opposé), puis une position où un bac est chargé et déchargé, puis

les 5 bains. Chaque chariot peut lever ou descendre un bac, et possède un système

d'accrochage/décrochage automatique du bac, actionné en approchant/éloignant le crochet en position

basse. Un tableau de commande comporte un certain nombre de boutons poussoirs, en particulier un

départ cycle pour chaque chariot.

Définition des

actionneurs :

action Chariot A Chariot B

mnémonique sortie mnémonique sortie

avancer chariot AvA A1.4 AvB A0.3

reculer chariot ReA A1.5 ReB A0.2

monter bac MoA A1.2 MoB A0.0

descendre bac DeA A1.3 DeB A0.1

Définition des capteurs (attention, un même capteur peut détecter le chariot A et B, mais cela ne

correspondra pas à la position du crochet sur le même bain, car le crochet n'est pas au centre du chariot) :

capteur Chariot A Chariot B

mnémonique entrée mnémonique entrée

position reculée PRA E0.0 PRB E1,1

chargement/déchargement bac CDA E0.1 CDB E1,0

chariot au dessus du bac 1 B1A E0.2 B1B E0.3





crochet en haut CHA E1.2 CHB E1.4

crochet en bas CBA E1.3 CBB E1.5

départ cycle DCA E1.6 DCB E1.7


CDAB1 B2 B3 B4 B5

CDB

A B

PRA PRB

Position de départ Fin chargement du bac

Solution cours exercice 2 dernière question:

Note: Les étapes d'attente sont nécessaires pour éviter que le chariot et le piston ne dépassent leur butées

ou ne s'abiment dessus.
