de z-theta-manipulator - pure - aanmelden · een positioneer-robot, die een translatie z en een...
TRANSCRIPT
De z-theta-manipulator
Citation for published version (APA):Mussaeus, M. (1996). De z-theta-manipulator: voorbereidingen en een regeling. (DCT rapporten; Vol.1996.145). Eindhoven: Technische Universiteit Eindhoven.
Document status and date:Gepubliceerd: 01/01/1996
Document Version:Uitgevers PDF, ook bekend als Version of Record
Please check the document version of this publication:
• A submitted manuscript is the version of the article upon submission and before peer-review. There can beimportant differences between the submitted version and the official published version of record. Peopleinterested in the research are advised to contact the author for the final version of the publication, or visit theDOI to the publisher's website.• The final author version and the galley proof are versions of the publication after peer review.• The final published version features the final layout of the paper including the volume, issue and pagenumbers.Link to publication
General rightsCopyright and moral rights for the publications made accessible in the public portal are retained by the authors and/or other copyright ownersand it is a condition of accessing publications that users recognise and abide by the legal requirements associated with these rights.
• Users may download and print one copy of any publication from the public portal for the purpose of private study or research. • You may not further distribute the material or use it for any profit-making activity or commercial gain • You may freely distribute the URL identifying the publication in the public portal.
If the publication is distributed under the terms of Article 25fa of the Dutch Copyright Act, indicated by the “Taverne” license above, pleasefollow below link for the End User Agreement:
www.tue.nl/taverne
Take down policyIf you believe that this document breaches copyright please contact us at:
providing details and we will investigate your claim.
Download date: 25. Jan. 2020
De z-&manipulator; voorbereidingen
en een regeling
Marc Mussaeus WFW-rapport 96.145
Professor: prof.dr.ir. J.J. Kok Begeleiders: ir. J.P.A. Banens
ing. N.R. Olthuis -
tu3 Technische Universiteit Eindhoven
Faculteit: werktuigbouwkunde Vakgroep: fundamentele Werktuigkunde, WFW
Sectie: systeem- en regeltechniek
stagerapport, oktober 1996
Samenvatting U
De vakgroep WFW van de faculteit werktuigbouwkunde aan de Technische Universiteit Eindhoven beschikt sinds kort over een z-&manipulator, afkomstig van Philips CFT. De z-&manipulator is een positioneer-robot, die een translatie z en een rotatie 8 om de translatie-as kan uitvoeren door twee door servomotoren voorgeschreven ingaande rotaties.
Deze stage is de eerste stage aan deze z-&manipulator. Veel van de t e verrichten werkzaamheden waren dan ook van praktische aard. Git rapport beschrijft daarom enkei een deei van de stage. De bewegingsvergelijkingen, de regeling en een beschrijving van de geschreven software zullen aan de orde komen.
Tijdens deze stage werd de robot aangesloten op een PC. Er werd software geschreven om de posities van de robot te kunnen uitlezen en om motorstromen te kunnen uitsturen. Vervolgens werden de bewegingsvergelijkingen van het systeem bepaald. Uiteindelijk kon met een PD-regelaar en Kalman-filter een voorgeschreven trajectorie redelijk worden gevolgd. Voor volgende stages aan de z-&manipulator kunnen de nu beschikbare hardware- en software- beveiligingsfuncties van groot belang zijn. Ook is een geautomatiseerde calibratiefunctie geschre- ven. Het programma dat de z-&manipulator naar een op te geven positie brengt, komt bij toe- komstige experimenten zeker van pas. Tevens zijn demonstratieprogramma’s voor uiteenlopende bewegingen van de z-&manipulator geschreven.
1
Voorwoord Tijdens de laatste fase van de studie tot werktuigkundig ingenieur wordt bij de vakgroep WFW van elke student verwacht dat hij/zij, in het kader van een stage, de theoretische vakken in een on- derzoeksfeer toepast. Als Meet- en Regelstudent was ik op zoek naar een stageopdracht waarin het gehele traject van machine c.q. prototype tot geregeld systeem kon worden doorlopen. Met zeer veel voldoening heb ik gewerkt aan de z-$-manipulator in het laboratorium van de vakgroep WFW.
Bij deze wil ik mijn begeleiders en ook de heer K.T.M. Koekkoek bedanken voor hun bijdragen.
Marc Mussaeus,
Eindhoven, oktober 1996.
... 111
Lijst
scalar enco encl Imotor J J* Jo Ji
L m
T TO Ti
s
U
U0
U1
Ustuur V
WB wa ~
2
a 8 W
vector e P
4 Q*
s g d
matrix - A - B - K
symbolen
betekenis encoderwaarde voor verdraaiing spiemoer encoderwaarde voor verdraaiing schroefmoer motorstroom traagheidsmoment van de spindel in vereenvoudigd systeem totaal traagheidsmoment traagheidsmoment van de spiemoer in vereenvoudigd systeem traagheidsmoment van de schroefmoer in vereenvoudigd systeem Lagrangiaan massa van de spindel in vereenvoudigd systeem spoed van de spindelgroef kinetische energie koppel op spiemoer koppel op schroefmoer gewenste regelinspanning regelinspanning, koppel op spiemoer regelinspanning, koppel op schroefmoer uitgangen van de HCTL’s potentiële energie totale geschatte wrijving in schroefmoer totale geschatte wrijving in spiemoer verticale positie van de spindel verdraaiing van de schroefmoer verdraaiing van de spiemoer hoeksnelheid
betekenis fouten ten opzichte van gewenste trajectories polen kolom met niet van een potentiaal af te leiden belastingen kolom met vrijheidsgraden werkelijke toestand gewenste toestand
betekenis systeemmatrix ingangsmatrix filtermatrix regelmatrix
V
Inhoudsopgave
1 Inleiding 1
2 Modelvorming 3 2.1 Systeembeschrijving . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 2.2 Model van het systeem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
3 Regelwet 7
3.2 Toestandsreconstructie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 3.3 Implementatie van de regeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
3.3.1 Omrekening van de koppels . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 3.3.2 Coördinaten-transformatie . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 3.3.3 Wrijvingsidentificatie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
4 Beschrijving van de geschreven software 11 4.1 Algemene functies voor de z-û-manipulator . . . . . . . . . . . . . . . . . . . . . . 11
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 De regeling 7
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1.1 Exp2m.C 11 4.1.2 Zm.C 11 4.1.3 Move 11
4.2.1 Harmo 12 4.2.2 Const 12 4.2.3 Trajec 12
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Voorbeeldprogramma’s voor experimenten . . . . . . . . . . . . . . . . . . . . . . . 12
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5 Conclusies en aanbevelingen 5.1 Conclusies . . . . . . . . . . . . . . . . . . . . . 5.2 Aanbevelingen . . . . . . . . . . . . . . . . . .
13 . . . . . . . . 13 . . . . . . . . 13
Bibliografie 15
A Aansluit gegevens 17 A . l Deencoders . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 A.2 De proximity-switches . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 A.3 De motoren . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
B Numerieke waarden van het model 19
C Computed-torque 21 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
C.l Wrijving 21 C.2 Zwaartekracht 21 C.3 Traagheid 21 C.4 Conclusie 22
vii
D Programma’s 23 D.l Exp2m.C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 D.2 Zm.C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 D.3 Move . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
D.3.1 M0ve.M . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 D.3.2 M0ve.C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
D.4 Harmo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32 D.4.1 Harm0.M . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32 D.4.2 Harmo . C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
D.5 Const . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 D.5.i C0nst.M . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 D.5.2 Const . C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
D.6 Trajec . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 D.6.1 Trajec.M . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 D.6.2 Trajec.C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
... v111
Hoofdstuk 1
Inleiding
Onderwerp van deze stage is de z-6-manipulator1, opgesteld in het laboratorium van de vakgroep WFW. De z-8-manipulator is een positioneer-robot, die een translatie z en een rotatie 8 om de translatie-as kan uitvoeren door twee door servomotoren voorgeschreven ingaande rotaties.
Dit is de eerste stage aan de z-&manipulator. De opdracht was dan ook om een zo robuust mo- gelijke basis te leggen voor volgende stagiair(e)s. Allereerst waren er die werkzaamheden, die erop gericht waren de manipulator te doen werken. Dit behelste onder andere het aansluiten van enco- ders en proximity-switches. Verder moesten de twee motoren middels eindversterkers van de juiste motorstromen voorzien kunnen worden. Twee eindstoppen en een beschermkap werden ter bevei- liging gemonteerd. Uiteindelijk kon met de geschreven software een trajectorie redelijk gevolgd worden. Vervolgens kon voor volgende stages voorbereidend werk worden verricht. Dit hield onder andere modelvorming in, evenals een eerste wrijvingsidentificatie en het programmeren van de benodigde beveiligingsfuncties. Ook is een geautomatiseerde calibratiefunctie geschreven. Verder is er nu software die de spindel in een op te geven positie kan brengen. Enkele programma’s die de robot, bij wijze van demonstratie, uiteenlopende bewegingen laten uitvoeren, zijn nu ook beschikbaar.
lDe naamgeving van de z-&manipulator is gedaan in navolging van het TUE dictaat Constructieprincipes 1 [5].
Hoofdstuk 2
2.1 Systeembeschrijving De z-&manipulator staat weergegeven in figuur 2.1. In figuur 2.2 worden de belangrijkste onderde- len benoemd. De eindstoppen en de beschermkap zijn niet in de figuren opgenomen. De groeven in de spindel staan in de figuren als zwarte lijnen weergegeven. De gereedschapshouder, niet zichtbaar in figuur 2.2, is gemonteerd onder aan de spindel en de stop-flens is boven aan de spindel bevestigd.
Figuur 2.1: De z-8-manipulator
4 Hoofdstuk 2: Modelvorming
De spindel en de daaraan bevestigde gereedschapshouder kan door middel van de rotatie van twee moeren een voorgeschreven rotatie û en/of translatie z worden opgelegd. De zogenaamde schroef- moer grijpt in een rechtse schroefdraad op de spindel. De spiemoer grijpt in zes langsgroeven op de spindel. Dit is duidelijk te zien in de schematische weergave van het systeem, figuur 2.3. De moe- ren worden door twee motoren via twee tandriemen met een overbrenging van l op 2 afzonderlijk aangedreven. De motor die de schroefmoer aandrijft wordt motor0 genoemd, de andere motorl. Voor de overbrengingen zijn pulley’s met een kleine diameter gemonteerd aan de motorassen en pulley’s met een twee maal zo grote diameter om de moeren.
1 : Spiemoer met pulley 2: Schroefmoer met pulley 3: Spindel met schroefdraad
en langsgroeven 4: Tandriemen 5: Pulley op motoras 6: Proximity-switch voor
bovenste uiterste stand 7: Proximity-switch voor
referentie in omtreks- richting
8: Proximity-switch voor onderste uiterste stand
Figuur 2.2: De belangrijkste onderdelen van de z-&manipulator
Stilhouden van de spiemoer en verdraaien van de schroefmoer laat de spindel en dus ook de gereed- schapshouder transleren zonder rotatie. Met gelijke snelheid beide moeren roteren laat de spindel roteren zonder translatie. Combinatie van rotatie en translatie is mogelijk door verschillende rota- ties van beide moeren. De motoren zijn voorzien van encoders die 8000 pulsen per omwenteling geven. Verder zijn drie proximity-switches gemonteerd. Twee geven de uiterste standen van de spindel tian: de onderste registreert de nabijheid van de gereedschapshouder hetgeen wordt gebruikt om een referentie voor de translatie van de spindel te vinden. De bovenste registreert een door mij gemonteerde flens bovenaan de spindel, de stop-flens. De derde proximity-switch kan een klein stalen plaatje op de spiemoer detecteren en levert zo een referentie voor de rotatie van de spindel.
2.2 Model van het systeem 5
De encoders en proximity-switches zijn aangesloten op twee HCTL's in een interface-kast. In de interface-kast bevinden zich ook twee eindversterkers voor de motorstromen. De eindversterkers worden bestuurd door de HCTL's. Met een digitale IO-kaart is de interface-kast verbonden met een PC. Van hieruit kunnen dus alle metingen ingelezen en de motorstromen uitgestuurd worden. Voor de exacte aansluitgegevens zie Bijlage A.
2.2 Model van het systeem De schematische weergave van de z-0-manipulator staat in figuur 2.3. Alle traagheidsmomenten worden geconcentreerd in de beide moeren (Je en Ji) en de spindel ( J ) . Alle van belang zijnde massa's worden geconcentreerd in de massa van de spindei (m). in Bijlage B staat dit concentreren beschreven evenals (schattingen van) de numerieke waarden. De motorkoppels, omgerekend naar de moeren, worden aangeduid met TO en TI.
schroefmoer, J,
spindel, J en m
spiemoer, J,
p Fl
i" po _T.
Figuur 2.3: De schematische weergave van de z-$-manipulator
De verdraaiing van de spiemoer wordt aangeduid met 0, de verdraaiing van de schroefmoer met a en de verticale verplaatsing van de spindel met z . Het verband tussen a, 0 en z wordt gegeven door:
S (2.1) z = (0 - a)-
27r met s = O.O32[m], de spoed van de schroefdraad
Met behulp van de vergelijkingen van Lagrange zullen de bewegingsvergelijkingen in termen van z en 0 worden afgeleid. Wrijvingstermen zullen vooralsnog niet worden meegenomen. De vergelij- kingen van Lagrange luiden:
d dt 4 I
-(L .) - L,q = Q* I
Voor de kolom met vrijheidsgraden q I is gekozen:
4' [:] Voor de kinetische energie T van het onderhavige systeem geldt:
(2.3)
T = --mi2 1 + - 1 (J + Jo)e2 + - 1 Ji b2 (2.4) 2 2 2
6 Hoofdstuk 2: Modelvorming
De potentiële energie V is gelijk aan O en daarom geldt L = T . Na enig rekenwerk volgt:
Om de kolom met niet van een potentiaal af te leiden belastingen Q* te bepalen wordt gebruik gemaakt van het principe van virtuele arbeid (paragraaf 2.4 van [2]j. Hiervoor wordt de arbeid bepaald die ieder van de krachten verricht bij een virtuele standverandering van elk der vrijheids- graden. De virtuele arbeid wordt gegeven door:
Q * ~ s ~ = TOM + ~ ~ ó c x - mgdz (2.6)
Uit formule 2.1 volgt:
27r 6a = 66 - -6z S
Formule 2.6 kan dan ook worden geschreven als:
Aldus wordt voor de kolom met niet van een potentiaal af te leiden belastingen gevonden:
Invullen van de uitdrukkingen 2.5 en 2.9 in de vergelijkingen van Lagrange (2.2) levert de bewe- gingsvergelij kingen:
(2.10) 27r ( J + Jo + J i ) j - Ji-Z = To +Ti S
S (2.11)
De bewegingsvergelijkingen kunnen nu in toestandsvorm worden geschreven. Met 1, = J + JO + Ji , I2 = m + J i g en 13 = JI? worden formules 2.10 en 2.11 hiervoor genoteerd als:
-$TI - mg
Oftewel:
(2.12)
(2.13)
Voor de toestandsvector wordt gekozen: z = [û z û i lT. Het systeem ziet er in toestandsvorm als volgt uit:
O 0 1 0 O 0 O
O 0 0 0 d e
(2.14)
Met a = I2 , b = I2 - %I3 , c = -I3 , d = I3 , e = I3 - ?I1 en f = -II . De numerieke waarden van vergelijking 2.14 staan in Bijlage B.
Hoofdstuk 3
Regelwet
Om de spindel in een gewenste positie te brengen, te calibreren en voor de verschillende demonstratie- bewegingen is een geregeld systeem vereist. Het gesloten-lus-systeem zal worden verkregen door een PD-regelaar met behulp van poolplaatsing en toestandsreconstructie met een Kalman-filter.
3.1 De regeling De fouten ten opzichte van de gewenste trajectories worden als volgt gedefinieerd:
e = % d - Z (3.1)
= [û z 8 i ] T de werkelijke toestand en g d de gewenste toestand. Het is de doelstelling
g = (3.2)
Hierin is de fouten naar O te brengen. Onderstaande regelwet (PD) zal hiervoor worden gebruikt:
Voor een optimale regelwet is er vooralsnog te weinig systeemkennis en daarom heb ik voor pool- plaatsing gekozen. De gewenste polen worden bepaald uit het karakteristiek polynoom:
(s2 + 2pwos + W$)(S2 + 2pw1s + u;) (3.3)
Voor ,f3 wordt gekozen p = 0.8, dit in verband met het gewenste doorschot. Voor WO en w1 wordt WO = 30[rad/s] en w1 = 3l[rad/s] gekozen. Dit levert polen die niet te ver in het linker- halfvlak liggen, onder andere in verband met de gekozen sample-frequentie van 500[Hz]. Uit het karakteristiek polynoom werden de polen bepaald:
-24.0 + 18.0i -24.0 - 18.0i
-24.8 - 18.6i (3.4)
Met behulp van de MATLAB-routine place (aanroep: L=place(A ,B,P)) werd de regelmatrix L bepaald:
4.42 17.88 L = [ - 2.19 - 420.79 0.11 - 22.44 (3.5)
3.2 Toest andsreconst ruct ie Voor het berekenen van de benodigde regelinspanningen g moet de gehele toestand (of schattingen daarvan) bekend zijn. Omdat de tijdsafgeleiden niet worden gemeten moeten deze gereconstrueerd worden. Hiervoor wordt een Kalman-filter gebruikt. De filtermatrix K wordt ook met behulp van poolplaatsing bepaald. Gekozen is voor onderstaand verband om de filterpolen te vinden. Door deze keuze liggen ook de filterpolen niet te ver in het linker halfvlak.
Wfilter 4 Uregelaar (3.6)
8 Hoofdstuk 3: Regelwet
De aldus gevonden filterpolen zijn:
'&=&+I3g+y e i! ' L
-96.0 + 72.0i -96.0 - 72.0i
-99.2 - 74.43
& - ' C
(3.7)
- x
Wederom gebruik maken van de MATLAB-routine place (aanroep: K=place (A' , C ' , P> ' ) leverde de filtermatrix:
i=(A-KC) ft+Eg+Ky -,.. 4
De gesloten lus staat schematisch weergegeven in figuur 3.1.
3.3 Implementatie van de regeling Onderstaand zullen de verdere ingrediënten die nog benodigd zijn voor de regeling afzonderlijk worden uitgewerkt.
3.3.1 Omrekening van de koppels
De regelwet bepaalt de koppels die de moeren moeten uitoefenen op de spindel. De eigenlijke uitgangen zijn echter gehele getallen tussen -127 en +127 [-l. Deze signalen zullen ustuur worden genoemd. De eindversterkers zetten deze signalen om in motorstromen en de motoren oefenen koppels uit op de pulley's. Na de overbrengingen zijn dit dus de koppels die de moeren op de spindel uitoefenen. Dit zijn de door de regelwet bepaalde ingangen en deze zullen dan ook met u worden aangeduid. Verderop zal de omrekening van de stuursignalen naar de koppels op de moeren plaatsvinden . Een van de motorassen werd geblokkeerd en in serie met de motor werd een ampèremeter aangeslo- ten. Door nu verschillende stuursignalen tussen -127 en +127 uit te sturen (m.b.v. een aangepaste versie van Const. EXE zie hoofdstuk 4) en de bijbehorende motorstromen te meten, kon het verband tussen de stuursignalen en de motorstromen bepaald worden:
3.3 Implementatie van de regeling 9
De motoren hebben een koppel-constante van O.lG[Nm/A] (zie [8]) en de overbrengingsverhouding van de motoren naar de moeren is 0.5[-1. Aldus volgt:
1 0.5 U = 0.16 . - .0.020. Ustuur = 0.0064. Ustuur (3.10)
Bovenstaande constante werd opgenomen in de file Expzm.def, zodat de door de regelwet bere- kende koppels kunnen worden omgerekend en de juiste uitgangen kunnen worden bepaald.
3.3.2 Coördinat en-t ransformat ie Met, hehdp van twee encoders worderi de hoekverdraaiingen van beide motoren gemeten. De en- coders geven 8000 pulsen per omwenteling. De overbrengingsverhouding van de motoren naar de moeren is gelijk aan 0.5[-1. De encoder-waarden kunnen met behulp van onderstaande uitdrukkin- gen worden omgerekend naar de verdraaiingen van de moeren:
1 27r 2 8000 1 27r 2 8000
8 = --enco [rad]
a = --enei [rad]
(3.11)
(3.12)
Bovenstaande constanten werden opgenomen in de file Expzm . def , zodat de hoekverdraaiingen in radialen beschikbaar komen. Meestal zijn we echter geïnteresseerd in de verdraaiing van de spindel 8 en de verticale verplaatsing z. De encoder-waarden worden door de calibratiefunctie op nul gezet als de spindel in de bovenste uiterste stand staat. Bij de coördinaten-transformatie kan ervoor worden gezorgd dat het nulpunt van de verticale postitie overeenkomt met het midden van het werkgebied van de spindel. Het totale werkgebied van de spindel bedraagt 0.22[m]. De verticale positie wordt dan niet met formule 2.1 berekend, maar met:
s z = (a - 6')- + 0.11 [m] (3.13) 27r
3.3.3 Wrijvingsidentificatie Om een Comp.iLted-torque-regeling mogelijk te maken, zijn wrijvingstermen bepaald. Alle wrijvings- termen in het systeem kunnen worden geconcentreerd in een viertal termen. Twee wrijvingstermen, die bij de moeren aangrijpen, kunnen de wrijving in de motoren, in beide tandriemen en in de la- geringen van de moeren in rekening brengen. Formeel zijn er nog twee wrijvingstermen, namelijk tussen de spiegleuven en de spiemoer en tussen de spindelgroef en de schroefmoer. Onderstaand zullen schattingen worden gedaan voor de eerste twee wrijvingstermen. De andere twee termen worden vooralsnog verwaarloosd.
Redelijke schattingen van de wrijving kunnen worden verkregen door een van de motoren gedu- rende een bepaalde tijd een koppel te laten uitoefenen en de verplaatsingen te meten. In verband met de zwaartekracht werd de z-Smanipulator horizontaal gelegd. Vervolgens werd gedurende 0.7 seconden de schroefmoer met het door de motoren maximaal te leveren koppel van 0.8[Nm] belast. De spiemoer blijft stilstaan en de gemeten hoeksnelheid van de schroefmoer is te zien in grafiek 3.2.
Ervan uitgaande dat alleen sprake is van Coulombse wrijving en dat alle wrijvingstermen tezamen kunnen worden geschat als één extra koppel op de schroefmoer, kunnen twee bewegingsvergelijkin- gen worden opgeschreven voor de beide delen van de beweging.
I: J*G1+ W, = 0.8 (3.14)
11: J*&I + w, = o (3.15)
Hierin is J* het totale traagheidsmoment. Eliminatie van deze term levert:
(3.16)
10 Hoofdstuk 3: Regelwet
l o c
50
0
d d d t [rads]
O 0 . 5 1
Figuur 3.2: Meting van de hoeksnelheid van de schroefmoer
Schattingen van de waarden van &I en 611 zijn uit de resultaten van de hierboven beschreven meting te bepalen en leveren na invullen in vergelijking 3.16 een schatting van W,. Na drie keer meten en middelen van de gevonden wrijvingswaarden werd W, = 0.62[Nm] gevonden. Uit het niet-rechte verloop van de snelheid tegen de tijd in figuur 3.16 blijkt overigens ook nog dat er naast Coulombse ook sprake is van viskeuze wrijving.
Om de wrijvingsterm te bepalen die aangrijpt aan de spiemoer, werd gedurende 0.7 seconden de spiemoer belast met een koppel van 0.8[Nm]. De schroefmoer blijft daarbij stilstaan en de spiemoer vertoont eenzelfde snelheidsprofiel als te zien is in grafiek 3.2. De gezochte wrijvingsterm kan op eenzelfde wijze als hierboven worden afgeleid:
0.8 we = ___ I - + - li (3.17)
Wederom drie keer meten en middelen van de berekende wrijvingstermen levert We = 0.47[Nm]. Beide wrijvingstermen kunnen nu dienen voor een Computed-torque-regeling.
Hoofdstuk 4
Beschrijving van de geschreven software
Onderstaand volgt een beschrijving van de verschillende functies van de geschreven software en globale uitleg daarvan. Alle software is voorzien van uitgebreid commentaar, zodat de precieze beschrijving van de implementatie hier achterwege kan blijven. De geschreven software is voorzien van M-files, zodat de functies in MATLAB kunnen worden gestart. De MATLAB-routines zorgen ook nog voor een grafische weergave van de posities, de snelheden en de ingangen tegen de tijd.
4.1 Algemene functies voor de x-&manipulator
4.1.1 Exp-zm.C
E x p z m . C bevat lower level expfunties voor de z-&manipulator. Deze functies vervullen alle experiment-interface taken. Om te beginnen moet ieder experiment worden afgebroken als de spindel (bijna) in een van de aanslagen loopt. Verder moet worden gecalibreerd, om absolute coördinaten van de spindel te kennen. Dit bete- kent dat de spindel de proximity-switches opzoekt om zijn absolute positie te vinden. Dit moet gebeuren als de PC wordt opgestart of als de spindel in de aanslagen is gelopen. De in het vorige hoofdstuk bepaalde regelwet en toestandsreconstructie worden hiervoor gebruikt. Van het calibre- ren kan meer informatie (zoals coördinaten en encoder-waarden) worden opgevraagd door vóór het compileren de variabele DEBUG gelijk aan 1 te maken. De encoder-waarden moeten worden vertaald naar voor de gebruiker interessante coördinaten. De coördinaten die nu worden gegeven zijn de hoeken 0 en a van de moeren. Desgewenst kan de gebruiker dit weer vertalen naar 8 en z met behulp van formule 3.13. De file E x p z m . C is voorzien van toereikend commentaar zodat een gedetailleerde bespreking hier achterwege kan blijven.
4.1.2 Zm.C
Deze eenvoudige functie kan vanuit de Dos-prompt de encoders uitlezen (Zm e) of initialiseren (zm i). Voor de werking zie de file Zm.C.
4.1.3 Move
Met behulp van de functie Move kan de spindel naar een gewenste positie in een op te geven tijd gestuurd worden. Hierbij wordt de trajectorie bepaald met een scheve sinus. Desgewenst wordt een grafische weergave van de beweging in MATLAB gemaakt. In de MATLAB-fik Move. M staat hoe de verschillende parameters opgegeven kunnen worden.
12 Hoofdstuk 4: Beschrijving vm de geschreven software
4.2 Voorbeeldprogramma's voor experimenten
4.2.1 Harmo
Middels de in het vorige hoofdstuk bepaalde regelwet en toestandreconstructie kan een voorgeschre- ven trajectorie worden gevolgd. De file Harmo schrijft zowel z als 0 harmonische trajectories voor. De amplitudes en de frequentie van deze trajectories zijn vooraf in te stellen. In grafiek 4.1 is een meting te zien waarbij de gestippelde lijn het gewenste trajectorie voorstelt en de doorgetrokken lijn het gemeten trajectorie. Ook zijn de koppels op de moeren weergegeven.
theta[rad] I
-0.21 I O u? [Nm] 3
Figuur 4.1: Grafiek van experiment (gestippelde lijn: gewenst; doorgetrokken: gemeten)
(Indien gewenst, kan nog gebruik gemaakt worden van Computed-torque-termen voor de wrijvings- kracht, de zwaartekracht en traagheidstermen. Hoe de Computed-torque-termen tot stand zijn gekomen staat in Bijlage C.) In de MATLAB-file Harm0.M staat de precieze aanroep van de functie beschreven.
4.2.2 Const Met behulp van Const is het experiment van figuur 3.2 uitgevoerd. Vooraf op te geven constante koppels worden op de moeren gezet gedurende de opgegeven tijd. Daarna worden de ingangen gelijk aan nul gemaakt en worden de verplaatsingen nog gemeten gedurende een opgegeven tijd. De manier waarop de parameters opgegeven dienen te worden, staat beschreven in Const. M.
4.2.3 Trajec Deze functie maakt het mogelijk de z-&manipulator een vooraf berekend trajectorie te doen volgen. In de MATLAB-file Trajec.M staat de precieze aanroep beschreven. De file Trajec. C is voorzien van uitgebreid commentaar, zodat de implementatie duidelijk is te volgen.
Hoofdstuk 5
Conclusies en aanbevelingen
5.1 Conclusies o Aan de hoofddoelstelling van deze stage (het doen werken van de z-0-manipulator) is voldaan.
e De z-6-manipulator bleek met een PD-regelaar en Kalman-filter een voorgeschreven trajec- torie redelijk te volgen.
o Er zijn bewegingsvergelijkingen van het systeem opgesteld.
o Schattingen van de parameters van de z-&manipulator zijn beschikbaar. Ook zijn schattingen van twee wrijvingstermen gedaan.
o In het systeem werd, naast Coulombse wrijving, ook viskeuze wrijving gevonden.
5.2 Aanbevelingen o Identificatie van de parameters van de z-0-manipulator kan betere gegevens opleveren. Zo
zijn nog geen schattingen beschikbaar van de wrijvingen tussen de spindel en de moeren. Misschien zijn ook de viskeuze wrijvingstermen van groter belang dan in het voorgaande is verondersteld.
0 Metingen met een Computed-torque-regeling waren veelbelovend. Grotere nauwkeurigheden zijn op deze wijze zeker haalbaar. Met name goede Computed-torque-termen voor de traagheid zijn nog niet gevonden.
0 Ook andere (geavanceerde) regelwetten kunnen worden toegepast ...
Bibliografie
[i] Banens, J. (april 1994): Documentation on TCE modules, WFW Report 94.050, Eindhoven.
[2] Campen, D.H. van en A. de Kraker (1990/1991): Het dynamisch gedrag wan constructies, TUE dictaat 4552, Eindhoven.
[3] Kok, J.J. (1990): Werktuigkundige Regeltechniek 11, TUE dictaat 4594, Eindhoven.
[4] Stroustrup, B. (1995): The C++ Programming Language, AT&T Laboratories, New Jersey, ISBN 0-201-53992-6.
[5] vakgroep WOC/WPA, TUE (februari 1996): Constructieprincipes I, TUE dictaat 4007, Eindhoven.
Geraadpleegde Catalogi: [6] De z-&manipulator: Philips CFT (1995): Z/Phi-test model, Design and Measurements, Phi-
lips CFT-rapport (Alleen paragrafen 3.3, 3.4 en 3.5 en Appendices C en D stonden ter be- schikking).
[7] Encoders: HEIDENHAIN (1993): ROD 425M.zUUU7 Montageanleitung, Montagevoorschrift
[8] Motoren: GEC ALSTHOM: RS, DC Servomotors, Catalogus van de producent.
[9] Proximity-switches: Honeywell (1996): Proximity sensors 922 Series, Catalogus van de pro-
van de producent, Germany.
ducent.
[lo] Spindel: T.H.K., LM System: THK Ball Screw-Spline type BNS, Catalogus van de produ- cent, Tokyo, Japan.
Bijlage A
kleuren van de draden groen-bruin wit en wit-bruin bruin
gnj s
Aansluit gegevens
nummers van nummers van kleuren van de pinnen de pinnen de draden 1 1 rood 2 2 oranje 3 3 bruin 4 4 gnj s
Onderstaand zullen de exacte aansluitgegevens opgesomd worden van de encoders, de proximity- switches en de motoren aan de interface-kast. De interface-kast zelf is met behulp van twee 40- polige bandkabelconnectoren verbonden met een digitale IO-kaart in de PC. Omdat vooralsnog de interface-kast van de kanteltafel gebruikt wordt, moet de beveiligingsfunctie van de kanteltafel worden uitgeschakeld door de twee hiervoor bestemde pluggen te verbinden met een meetsnoer.
A.l De encoders Aan iedere encoder is een signaalkabel aangesloten met een 15-polige male D-connector. Deze is verbonden met een 15-polige female D-connector waaraan een 10-polige fiatcabel is aangesloten. Het andere uiteinde van de fiatcabel is middels een 10-polige female bandconnector verbonden met de interface-kast. Voor de kleuren van de aansluitdraden en de nummers van de aansluitpinnen zie de tabel in figuur A.1.
Figuur A.l: Aansluitgegevens van encoders
In de interface-kast zijn voor de encoders geen wijzigingen uitgevoerd.
A.2 De proximity-switches Aan iedere proximity-switch is een 3-aderige signaalkabel aangesloten. Deze drie kabels zijn ver- bonden met een 9-polige female D-connector. In de interface-kast is een 9-polige male D-connector gemonteerd, die (binnen in de kast) verbonden is met een 10-aderige fiatcable. Deze fiatcable is gesoldeerd op de printplaat die met de twee HCTL’s is verbonden. Zie de tabel in figuur A.2.
18 Hoofdstuk A: Aansluitgegevens
nummers van de pinnen
1 5 7 8 9
Proximity- 9-polige female 9-polige male 10-aderiae switches -+ D-connector-
kleuren van nummers van de draden de Pimen
bruin 37 blauw 1 P m s 24 grijs 23 wit 22
kleuren van de draden
alle bruine alleblauwe zwartproxl zwartProx2 zwartProx3
nummers van de pinnen
1 5 7 8 9
Figuur 8.2: kansiuitgegevens van proximity-switches
kleuren van de draden
A.3 De motoren
nummers van nummers van de pinnen de uinnen
De motoren zijn middels 2-aderige draden verbonden met een 6-polige male Din-plug. In de interface-kast is een 6-polige female Dan-plug gemonteerd. Voor de kleuren van de aansluitdra- den en de nummers van de aansluitpinnen zie de tabel in figuur A.3.
zwart Motor1
Figuur A.3: Aansluitgegevens van motoren
In de interface-kast zijn voor de motoren geen wijzigingen uitgevoerd.
Bijlage B
Numerieke waarden van het model
In het model zijn alle traagheidsmomenten geconcentreerd in de beide moeren (Jo en J I ) en de spindel ( J ) . Alle van belang zijnde massa's zijn geconcentreerd in de massa van de spindel (m). De numerieke waarden, of schattingen hiervan, worden verderop gegeven. Vervolgens zullen deze numerieke waarden in de bewegingsvergelijking in toestandsvorm (formule 2.14) worden ingevuld. Allereerst worden de massa's en traagheidsmomenten van de bewegende delen van de z-6-manipulator gegeven. Uit het CFT-rapport [6] blijkt:
mspindel' 1.8 ikgl
0 Jpulleyl= 3.93-5 [kgm2] (Met pulley1 worden de pulley's aan de motorassen aangeduid.)
0 Jpulley2= 5.53-4 [kgm2] (Met pulley2 worden de pulley's om de moeren aangeduid.)
Js&roefmoer= 6-743-4 [kgm2]
0 Jspiemoer= 5.53-5 [kgm2]
0 s=0.032[m] (s is de spoed van de spindelgroef.)
i= 0.5 [-] (i is de overbrengingsverhouding van de motorassen naar de moeren.)
Uit de gegevens van de motorfabrikant [8] blijkt:
Jmotor= 2.253-4 [kgm2]
De overige gegevens moeten zelf geschat worden. Het traagheidsmoment van de spindel wordt gevonden door deze als een cylinder te beschouwen. De afmetingen van de spindel zijn te vinden in de gegevens verstrekt door de leverancier [lo]. De buitendiameter van de spindel Dspindel is O.O32[m] en de binnendiameter dspindel O.O23[m].
1 O Jspindel = gmspindel (Dipindel i- dgpindel) = 3*5E-4 [kgm2] (B-1)
De afmetingen van de gereedschapshouder heb ik gemeten: hhouder=0.05[m] , Dhouder'0.085[m] en dhouder=0.032[m]. De dichtheid van staal pstaal is 7.8E3[kg/m3], zodat:
(B.2) 1 2 O mhouder = qPstaalnhhouder (Dhouder - 'gouder) = 'e9 fkg]
1 O Jhouder = gmhouder (Dgouder -t dho,der) = fkgm21 (B-3)
De afmetingen van de flens zijn bekend omdat ik de flens heb laten maken: hflens=0.013[m] , Dflens =0.06 [m] en clflens =O .O32 [m].
b (B.4) 1 mflens = ZPstaalnhñens (Diens - diens) = 0.2 [kg]
20 Hoofdstuk B: Numerieke waarden van het model
Bovenstaande numerieke gegevens zullen nu gebruikt worden om alle relevante massa’s te concen- treren in de spindel. Hierdoor ontstaat de aangepaste massa van de spindel m. Alle relevante traag- heidsmomenten zullen worden geconcentreerd in een aangepast traagheidsmoment van de spindel J , een aangepast traagheidsmoment van de spiemoer JO en een aangepast traagheidsmoment van de schroefmoer J i . Het gehele systeem wordt aldus vereenvoudigd tot het systeem weergegeven in figuur 2.3.
mflens %pindel + mhouder = 3-9 [kg] (B.6)
= Jflens + JspindeI Jhouder = 0.0024 [kgm2] (B.7)
1 JO = Jspiemoer + ~pulley2 + 7 (Jpulleyl+ Jmotor) = 0.0022 [kgm21 P . 8 )
(B.9) 1
= Jschroefmoer Jpulley2 7 (Jpulleyl + Jmotor) = OLKE3 [kgm2]
De numerieke waarden kunnen nu in de bewegingsvergelijkingen in toestandsvorm (formule 2.14) worden ingevuld. Dit leidt tot:
r O O 1 O i r O O 1 r O i O 0 0 1 I o O O
= [û 0 0 0 J + 213.0442 9.0508 J [ 2-J + 1-39.74841 O 0 0 0 1 1.0389 -2.0947 -0.6106
(R.10)
Bijlage C
Vanwege het feit dat het implementeren van een Computed-torque-regeling geen deel uitmaakte van mijn stageopdracht, is de beschrijving van deze toch uitgevoerde implementatie beperkt gebleven tot onderstaande uiteenzetting.
C.l Wrijving Bij het koppel dat door de moeren dient te worden uitgeoefend, kunnen extra termen worden opgeteld om voor de wrijving ter plaatse van de moeren te compenseren:
T°C.T. = TO + sign (ad) W, (C-1)
(De d staat voor desired oftewel gewenst.)
C.2 Zwaartekracht In navolging van de manier van aanpak bij het afleiden van het model worden hier de Computed- torque-termen voor de compensatie van de zwaartekracht bepaald. De term -mgóz in formule 2.6 stelt de arbeid verricht door de zwaartekracht voor. Met behulp van formule 2.1 kan dit ook worden geschreven als:
S S S mgóz = mg(68 - 6a)- = mg-SB - mg-óa
2n 2n 2n Zodat:
De gevonden Computed-torque-termen voor de zwaartekracht kunnen overigens ook worden gevon- den door de toestandsbeschrijving van het systeem uit te werken.
C.3 Traagheid De laatstgenoemde aanpak om de Computed-torque-termen te bepalen, is gebruikt om de termen voor de traagheid te verkrijgen. Bij weglaten van de zwaartekracht blijkt uit de toestandsbeschrij- ving van het systeem (formule B.lO):
1.0389 -2.0947 1 ["I Ti 213.0442 9.0508 [;I = [
22 Hoofdstuk C: Computed-torque
Oftewel:
0.0023 -0.4675 I r1 Z 0.0046 0.0199 [a] = [
Zodat: S
TocoT. = To + sign (bd) W, + mg- + 0.00468d + 0.0199Zd 2T
S ~ 1 ~ ~ ~ . = T~ + sign (sd) WO - mg- + 0.0023ëd - 0.46752d
27r
C .4 Conclusie Uit metingen bleek dat het gedrag van de z-&manipulator substantieel verbeterde door het invoeren van Computed-torque-termen voor de wrijving en de zwaartekracht. De termen voor de traagheid zorgden echter voor een slechter gedrag. Laatstgenoemde maakt uiteraard deel uit van de punten van aanbeveling.
Bijlage D
Programma's
D.l Exp2m.C
Defines lower level exp--functions for Z-Module experiment and creates complete exp--interface for application programs.
See exp-zm.def for measurements and commands. */
#ifndef DEBUG #define DEBUG O #endif
#include <dos.h> #include <exp-zm.def> #include <math.h>
#include "exp. c" #include "clk. c" #include "enc-zm. c"
/* Standard portion of exp-
#define TTL-REG OxIb6 /* /* (de) activated. /* /* /*
Through TTL-REG the power to the motors must be
It is in the same PPI as the HCTL control registers in enc-2rn.c. Upon enc-init, the direction of the register is set appropriate. Do not touch it anymore..
int exp-init-status = O;
long enc-add E21 = c o, o I , enc-last [Zl = I o, o 3 ;
/* Proximity information #define proximity0 ((inport(TTL_REG)>>4)&0~07)
/* Help functions for calibration: exp-kf-predict exp-kf-correct
exp-go
double exp_c-x[41, exp-c-xd[41, eXp-C-y[21, exp-c-u[21;
*/
*/ */ */ */ */
*/
*/
24 Hoofdstuk D: Programma's
void exp-kf-predict(void) { /* predictor */ exp-c-x [O] += exp-dt*exp-c-x[21;
exp-c-x [i] += exp-dt*exp-c-x C31; exp-c-x [2] += exp-dt* (exp-c-B31*exp-c-u [O] +exp-c-B32*exp-c-u [11) ; exp-c-x [SI += exp-dt* (exp-c-B4l*exp-c-u [O] +exp-c-B42*exp-c-u cl]) ;
1
void exp-kf-correct (void) { /* corrector */ exp-c-x [O] += exp-dt*exp-c-KOll* (exp-c-y[O] -exp-c-x[O] ) ;
exp-c-x Cl1 += exp-dt*exp-c-K022* (exp-c-y [i] -exp-c-x [11) ; exp-c-x C2l += exp-dt*exp-c-K03l* (exp-c-y[O] -exp-c-x [O! ; exp-c-x E31 += exp-dt*exp-c-K042* (exp-c-y E11 -exp-c-x E11 ;
3
void exp-go(void) { exp-kf -predict (1 ; exp-get 1 0 ; exp-c-y CO1 =exp-z CO] ; exp-c-y[ll= (exp-z [O] -exp-z C11) *O .032/ (2*M-PI) ; exp-kf-correct 0 ; exp-c-uE01 =exp-c-LOll* (exp-c-xd[O] -exp-c-x [O] )+
exp_c_LOl2* (exp-c-xd Cl] -exp-c-x [l] + exp-c-L013* (exp-c-xd [a] -exp-c-x [2] )+ exp_c-L014* (exp-c-xd [SI -exp-c-x [SI 1 ;
exp-c-L022* (exp-c-xd [i] -exp-c-x [i] )+ exp-c-L023* (exp-c-xd E21 -exp-c-x [2] + exp-c-L024* (exp-c-xd[31 -exp-c-x[3] ) ;
exp-c-u cl1 =exp_c_LO21* (exp-c-xd CO1 -exp-c-x [O] +
exp-u CO1 =exp-c-u [O] ; exp-u [i1 =exp-c-u Cl]; exp-put 1 0 ; 1
void exp-calibreer (void) { /* exp-calibreer calibreert door de spindel eerst naar boven te laten
bewegen tot de betreffende proximity-switch. Langzaam weer zakken tot buiten het bereik van de proximity-switch levert een nauwkeurige
calibratie in z richting. Vervolgens draait de spindel totdat de andere proximity-switch het stalen plaatje op de spiemoer detecteert. Ook nu wordt langzaam in tegenovergestelde richting bewogen tot buiten het bereik van de proximity-switch. Ook in theta richting is de calibratie nu voltooid. De HCTL's worden nu geinitialiseerd. Als laatste wordt de spindel nog in de middenpositie gebracht.
int i; double posz,post,t,te;
printf ( ' I calibration.. . , please wait\n") ;
*/
/* eerst z calibreren: (naar boven bewegen)
exp-c-xd[O]=O; exp-c-xd[2]=0; i=O; exp-zero 0 ; while ( ! ( (proximityO==l) I I (proximityO==3) ) {i++; if (i<500) { exp-c-xd[ll=( (double) i) /500*i/500*0.04; 1 else {exp-c-xd Cl] += O. 081'500; 1 if (i<500) { exp-c-xd [SI =O. 08* (double) i/500 ; 1 else Cexp-c-xd [SI =O. 08; > exp-go 0 ; 1
*/
D.l Expzm.C 25
exp-zero 0 ;
exp-c-x [2] =O; exp-c-x [3] =O; /* kalman vertellen dat er geen snelheden meer zijn.
/* (weer langzaam naar benden bewegen) posz=exp-c-x Cl]; i=O ;
€i++; if (i<IOO) {exp-c-xd[l] =posz- ( (double) i) /500*i/500*0.01; 3 else (exp~c~xd[1]-=0.4*0.01/500;~ if (i<IOO) {exp-c-xd [3] =-2*0.001* (double) i/500; 1 else €exp~c~xd~31=-0.4*0.01;~ exp-go ( 1 ; 3 exp-zero (1 ;
while ( (proximityO==l) I I (proximityO==3) )
/* dan theta calibreren: (draaien) exp-c-x E21 =O; exp-c-x C31 =O; /* kalman vertellen dat
er geen snelheden meer zijn.
i=O ; exp-c-xd [i] =exp-c-x [i] ; exp-c-xd [3] =O ; post=exp-c-x [O] ; while ( ! ( (proximity()==2) I I (proximityO==3) ) €i++ ; if (i(500) í exp-c-xd[O] =post + ( (double) i) /500*i/500*2; 3 else Cexp-c-xd [O] += (double) 4/500; 3 if (i<500) € exp-c-xd [2] =4* (double)i/500;3 else Cexp-c-xd [al =4; 3 exp-go 0 ; 3 exp-zero (1 ; exp-c-x [2l =O; exp-c-x [SI =O; /* kalman vertellen dat
er geen snelheden meer zijn.
/* (langzaam andersom draaien) post=exp-c-x [O] ; i=O ;
€i++; if (i<IOO) Cexp-c-xd[O] =post - ((double) i)/500*i/500*0.5;3 else Cexp-c-xd [O] -=. 4*0.5/500 ; 3 if (i<IOO) Cexp-c-xd[21=-2*0.5* (double) i/500; 3 else Cexp-c-xd 121 =- .4*0.5; 3 exp-go 0 ; 3 exp-zero () ;
while ( (proximityO==2) I I (proximity()==3)
/* Nieuwe meetsituatie ... (HCTL’s initialiseren ) */ enc-hctl-reset0 ; enc-addC01 = enc-add[I] = O;
/* aan exp-c-x vertellen exp-c-x [O] =O ; exp-c-x [i] =O ;
exp-c-x E21 =O; exp-c-x C3l =O; /* ook snelheden nul.
/* dan naar het midden gaan : i=O ; exp-c-xd [O] =exp-c-x [O] ; exp-c-xd [21 =O ;
*/
*/
*/
*/
*/
*/
*/
*/
*/
te = 0.7; t = o; while ( t<te )
26 Hoofdstuk D: Programma's
c exp-c-xd[lI = -(. ll*t/te-(. 11/(2*M-PI))*sin(2*M-PI*t/te)) ; exp-c-xd[3] = -(.ll/te-.ll/te*cos(2*M~PI*t/te)); exp-go ( 1 ; t += exp-dt; 3
/* En even stil zetten exp-c-xd[l] = -.11; exp-c-xdC31 = O; te = 0.3; t = o; vhile ( t<te 1 C exp-go0 ; +, += oxp-st; 3 exp-zero O ; printf ( I 1 ready \nll) ; 3
int exp-cali-check(void) C int b,c;
/* Find out if HCTLJs need to be reset */ c = o;
/* Register Ox3c of the HCTL is the Comand(ed) /* Velocity, only used in Integral Velocity mode. */ /* We never start that mode, so any value in regeister */ /* Ox3c doesnJt harm and it will stay there ... */ /* Its default value (upon power-up, reset etc.) is O */ /* We use it to see if the encoder mearurements are */ /* valid, i.e., created by a calibration pass. */ if ( enc_get_byte(O,Ox3c)==O ) c = i;
*/
#if DEBUG>O printf ("c=%4d\n", c) ; #endif
/* Deze funktie levert na aanzetten van de PC toch c=O. */ if ( c==O 1 c /* Looks OK .. */ b = proximity0 ; if ( b != O && b != 2) c = 1; /* illegal now */ 3 return c ;
3 void exp-f inii (int error-code) C
outportb (TTL-REG, O) ; /* De-activate power */
switch ( error-code c case 1 : printf ("clk failure (timeout) . . . \n'l) ; break; 1 1
int exp-get1 (void) C int prox;
#if DEBUG>O static int first = i; #endif
*/
if ( (exp-wait-c=clk-wait ())<O return 1; enc-get O ;
D. 1 Expzm . C 27
#if DEBUG>O if ( first 1 C printf ("enc-buf = %Id %ld\n",enc-buf [O] ,ent-buf cl]) ; first = O; 1 #endif if ( enc-buf [O] -ent-last [O] > SOOOOOOL ) enc-add [O] -= Ox1OOOOOOL; else if ( enc-buf [O] -ent-last [O] <-8OOOOOOL 1 enc-add [O] += Ox1OOOOOOL; if ( enc-buf [l]-enc-lact cl]> SOOOOOOL ) enc-add[lI -= Ox1OOOOOOL; else if ( enc-buf [i] -ent-last Cl] <-SOOOOOOL ) enc-addl11 += Ox1OOOOOOL;
exp-z [O] = exp-alpha-res* (-enc-buf [O] +ent-add [O] /* het minteken voor enc-buf[O] zorgt ervoor dat u0 de juiste
exp-z [i] = exp-theta-res* (enc-buf [i] +ent-add [i] + exp-theta-bias;
+ exp-alpha-bias ;
richting heeft */
enc-last [O] = enc-buf [O] ; enc-last Cl] = enc-buf E11 ;
/* Read proximity information (in geval van contact met proximity-
prox = proximity0; if ( exp_nz>2 exp-zC21 = prox; if ( prox != O && prox != 2) return 2; return O; 3
switch experiment afbreken) */
int exp-initl (void) C double dt = exp-dt; int n;
if ( exp_nz<2 ) C printf("0nly nz=2 or 3 allowed\n"); return 1; 3
if ( exp-init-status==O ) exp-init-status = exp-cali-check();
if ( exp-init-status && exp-dt>0.002 ) exp-dt = 0.002; /* Use own clock */
if ( clk-init(exp-dt)<=O ) printf ("clk-init failure\n") ; return 1; 3 if ( enc-init (exp-init-status) 1 C printf ("Encoder troubles\n") ; return 1 ; 3 enc-da-put (O ,O) ; /* zero current to the motors */
/* PPI-C is configured: Lower output/Upper input, */ /* see enc-zm.c. Its address is TTL-REG */ /* In use is: */ /* bit O (output) : power activation of electronics*/ /* bit 4 .. 6 (input) : proximity signals of ZM */
outportb(TTL-REG,l); /* Activate power */
/* Wait 0.3 seconds for electronics to become */ /* operationai. */ n = ( lat) (O. 3/exp_dt+O. 5) ; while ( n>O ) C clk-wait(); n--; 3 enc-da-put (O, 0 ) ; /* zero current to the motors */
if ( exp-init-status í
28 Hoofdstuk D: Promamma's
exp-calibreer 0 ;
enc-put-byte(O,Ox3~,1); /* Flag init done .. */ exp-init-status = O;
if ( exp-dt!=dt ) < exp-dt = dt; if ( clk-init(exp_dt)<=O 1 < printf ("clk-init f ailure\n") ; return 1 ; 3 3 1
enc-da-put (O, O) ; /* zero current to the motors */
enc-addC01 = O; /* get -ent-buf [O] +enc-add[O] between -8000 and +7999 by having enc-add[Ol = 8000*m with some m. */
enc-add [O] = 16000L* (enc-buf [O] /16000L) ; if ( enc-add [O] -ent-buf [O] >=8000 ) enc-add [O] -= 16000; if ( enc-add CO] -ex-buf [O] (-8000 ) enc-add[O] += 16000;
enc-add[ll = enc-addC01; /* identical to enc-add[Ol */ return O; 1
int exp-put1 (void) C int u0,ul;
exp-u [O] = (exp-u [O] -exp-uO-bias) /exp-uO-res ; exp-uC11 = (exp-u[ll -exp-ul-bias) /exp-ul-res; exp-clip(uO,exp-u[01,-127,127) ; exp-clip(ul,exp-u[i1,-127,127) ; enc-da-put (u0 ,ui) ; exp-u [O] = exp-uO-res*exp-u CO1 +exp-uO-bias ; exp-u [i] = exp-ul-rec*exp-u [I1 +exp-ul-bias ; return O; 3 void exp-stop(void) C /* Speeds to zero ... exp-zero 0 ; 3 void exp-zero (void) < exp-uCûl = exp-uC11 = O ; exp-put 1 ( 1 ; 3 /* end of Exp-zm.C
D.2 Zm.C
Services for z-theta-manipulator
*/
*/
*/
0 . 3 Move 29
#include <tce.h>
#include <ctype.h> #include <math.h> #include <stdio.h>
double u cl1 ,wait Cal , z E21 ; extern long enc-buf [I] ; extern int exp-init-status;
void epilog(void) { 3
void main(int argc,char **argv) c
if ( argc==l ) { printf ( "Usage: \n" I' zm e(ncoders) - reads encoders\n"
zm i(nit> - initiaiizes\n" I r zm r(eset) - resets encoders\n"
re turn ; 1;
3
switch ( toupper(argv[I]C01) { case >I1: case 'RI : exp-init-status = 1; /+ Must calibrate */ case >E': exp-init (z,2 ,u,O. OOI, wait) ; printf ("%10.41f X10.41f %101d %lOld\n", z [O] , z Cl], enc-buf CO1 , enc-buf Cl]) ; exp-f ini 0 ; break; 3 3
/* end of Zm.C */
D.3 Move
D.3.1 M0ve.M
function move(theta-d, z-d, tm, R, sf) move(theta-d, z-d, tm) plaatst de spindel in de opgegeven positie.
theta-d : De gewenste hoek theta. Cradl z-d: De gewenste hoogte z . íml tm: De duur van de verplaatsing. Es]
De functie aanroep move(theta-d, z-d, tm, R) met R ongelijk aan nul slaat tevens gegevens van de afgelegde baan op in t-out.mat en maakt hiervan twee grafieken. Met de functie aanroep moveítheta-d, z-d, tm, R, sf) kan ook nog de sampletijd worden ingesteld. De default waarde is 500 [Hz]. Na de beweging wordt de bereikte positie wordt op het scherm afgedrukt.
opm.l Het trajectorie wordt berekend middels een scheve sinus. opm.2 De laatste 0.3 seconden van tm worden gebruikt om de spindel
stil te zetten.
30 Hoofdstuk D: Programma’s
% % M0ve.M maakt gebruik van Move.EXE.
if (tmc.11, disp(’Rede1ijke waarden voor tm zijn: te.1’)
else
end if (RI ,
end
D.3.2
if (nargin==3), R=O; sf=500 ; end if (nargin==4), sf=500 ; end save input-Mo theta-d z-d tm sf R; !move
load t-out t= [O: l/sf : tml ’ ; clg subplot (221) plot (t (i :length(xd) 1, [xd(:, 1) y(:, 1) x(: ,i)]), . . . subplot (222) plot(t(l:length(xd)),Cxd(:,2) y(:,2) x(:,2)1),-..
subplot (223) plot(t(l:length(xd)-l), Cxd(l:length(xd)-1,3) sf*diff (y(: ,i)) . . . subplot (224) plot(e(i:iength(xo)-I), [xci(l:iength(xd)-1,4) sf*diff (y(: ,211 . . . pause
subplot (211) plot (t (i : length(xd) ) ,u ( : ,1) 1, title ( ’u1 [Nml ’ ,grid; subplot (212) plot(t(l:length(xd)),u(: ,2)),title(’u2 [Nml ’),grid;
title(’thetaCrad1’) ,grid;
title(’z[m] (R:x-d G:y B:xhat)’) ,grid;
x(i : length(xd)-l,3)] ) ,title ( ’theta-D [rad/sl ’ ,grid;
x (1 : length(xd) -1,411 ) ,title ( ’z-D [m/sl ’ ,grid;
clg
Move. C
Zet de spindel in de gevraagde positie. Hierbij wordt een Kalman-filter en een PD-regelaar gebruikt. Ook wordt gebruik worden gemaakt van Computed torque termen voor de wrijvingen en de zwaartekracht. De laatste 0.3 sekonden van tm worden gebruikt om de spindel stil te zetten. Daarna wordt de nieuwe positie (theta en z ) op het scherm afgedrukt. De gewenste baan wordt berekend op basis van een scheve- sinus-funktie.
Verwacht de Matlab file input-Mo.MAT met gewenste duur van de beweging: samplefrekwentie gewenste eindpositie voor z : gewenste eindpositie voor theta: variabele die aangeeft of tijdreeksen bijgehouden moeten worden (O=nee):
en Katlab file syst-Mo.MAT met: systeem matrices: regel matrix: filter matrix:
tm sf z-d thet a-d
R
A , B, C LO KO
i
D.3 Move 31
Als R ongelijk is aan O wordt de Matlab file T-OUT.MAT aangemaakt met :
tijdreeks met theta en z gemeten en
tijdreeks met gewenst trajectorie: xd tijdreeks met toestandschattingen: X
wait counts: wait
getransformeerd: Y
tijdreeks met ingangen: U
*/
#define NZ 3
#include <tce.h> #include <stdio.h> #include <math.h> #include <mat.h>
#include <exp-zm.def>
double sf, z-d, theta-d, t, tm, pos-theta, p0S-z;
mat u(2,l) ,z(3,1),x(4,1) ,y(2,1); mat xd(4,1), alpha-d-D(i,l) , A , B, c, LO, KO ; double wait Cal ; int R;
void epilog(void) í mi-open("t-out", 1) ; ts-save-all(í) ; d-put-vec(wait,3,"wait") ; 3
void run(void) exp-init(z,u,l/sf ,wait) ; printf ( I t moving. . . \nlf) ; printf (" exp-zero 0 ; exp-get 0 ; x .p CO] =pos-theta=z .p CO] ;
while ( t<tm ) c
/%2.3f \r" , tm) ;
x .p [l]=pos-z=(z .p [O] -z .p Cl] )*O. 032/ (2*M_PI)+O. 11 ;
if (t<tm-.3) /* mbv scheve sinus gewenste trajectorie bepalen. */
xd.p~0]=pos~theta+(~theta~d-pos~theta~/~tm-.3)*t-
xd.pCll=pos-z+((z-d-pos-z)/(tm-.3)*t-
xd.p[21=(theta-d-pos-theta)/(tm-.3)-
xd.pC31=(z-d-pos-z)/(tm-.3)-
1 else exp-get 0 ;
(theta-d-pos-theta)/(2*M-PI)*sin(2*M-PI*t/(tm-.3)));
(z-d-pos-z)/(2*M_PI)*sin(2*M_PI*t/(tm- .3))) ;
(theta~d-pos~theta)/~tm-.3~*cos(2*M~PI*t/~tm-.3));
(z-d-pos-z) /(tm- .3) *cos (2*M_PI*t/ (tm-. 3) ) ;
xd.p [O] =theta-d; xd.p Cl1 =z-d; xd.p C21=xd.p C31 =O; 3
y.pc03=z.pc01; y .p [iI=(z .p [O] -Z .p [I]) *O .032/ (2*M_PI) +O. 11 ; x += l/sf *KO* (y-C*x) ; /* corrector */ u=LO* (xd-x) ; /* regeiwet */ x += i/sf*(A*x+B*u) ; /* predictor */ /* M0ve.C maakt gebruik van Computed-torque-termen voor
32 Hoofdstuk D: Programma's
de wrijving en de zwaartekracht: */ alpha-d-D. p [O] =xd .p [2] -2*M_PI/O. 032*xd. p [SI; u.p[O] += W-theta*sign(xd) .p [21+3.9*9.81*0.032/ (2*M-PI) ; u. p [i] += W-alpha*sign(alpha-d-D) .p [O] -3.9*9.81*0.032/ (2*M_PI) ; exp-put ( 1 ; ts-put-ailO ; printf ("%2.3f\ro1,t) ; t+=l/sf; 3 exp-f ini 0 ; printf (" ready (theta~X3.3frad ; z=%1.4fm) \n" , y . p m , y . p w ; ; 3
void main(void) {
ml-open("input-Mo", O) ; /* openen input-f ile tm=mï-get-scahr ("tm", 5) ; z_d=ml-get-scalar("z-d" ,O> ; thet a-d=ml-get-scalar ("theta-d" ,O) ; R=(int) ml-get-scalar("R",O) ; sf =ml-get-scalar("sf" ,500) ;
*/
ml-open("syct-Mo",O) ;/* openen file met systeemmatrices */ A-l-get-mat ( "A" ) ; Bal-get-mat ("B") ; C=mï-get-mat ("C") ; LO=mï-get-mat ("LO") ; KO=ml-get-mat ("KO") ;
if (R) { /* eventueel tijdreeksen bijhouden: ts-init(u, (int) (sf*tm) ,"u") ; ts-init(x, (int) (sf*tm) ,"XI') ; ts-init (y, (int) (sf *tm), "y") ; ts-init(xd, (int) (sf*tm) ,"xd"); 3 run0 ; printf (l'\r\n'') ; epilog0 ; 3
/* end of M0ve.C */
*/
D.4 Harmo
D.4.1 Harmo .M % Harm0.m laat de spindel een harmonische beweging uitvoeren in zowel z % als theta richting. % Aangeraden wordt om in Matlab de file input-Ha.MAT te laden, eventueel % enkele parameters te wijzigen en dan harm0.M te starten. % In input-Ha.MAT staan de volgende grootheden: % matrix (1x3) die CT-termen aanhit zet % % de tweede voor de zwaartekracht en de % derde voor de traagheden: We % (vb: We=[1 i O] zet CT-termen voor % wrijving en zwaartekracht aan)
de eerste term is voor de wrijving,
% duur van experiment: tm % sample-ti jd: sf
0.4 H m o 33
x amplitude van theta-beweging: A-theta % periode van theta-beweging: P-theta x amplitude van z-beweging: A-2 x periode van z-beweging: P-2 x % opm.1 Aan het eind van de beweging wordt het een en ander grafisch x weergegeven. x opm.2 De spindel moet voor het aanroepen van deze funktie in een positie % x maakt. % J. 1 1 a 1 u u . z ~ mamLi gebruik v a Hmo.EXE.
worden gebracht die de gevraagde beweging in z richting mogelijk
*I u--- u --..LL
save input-Ha We tm sf A-theta P-theta A-z P-z ! harmo load t-out t=CO: l/sf : tml ’ ; c%
. subplot (221) plot(t(l:length(xd)) ,Cxd(: ,1) y(: ,i) x(:,l)]) ,title(’thetaCradl’),grid; subplot (222) plot(t(l:length(xd)), [xd(: ,2) y(: ,2) x(: ,2111,. . . title(’z[m] (R:x-d G:y B:xhat) ’1 ,grid; subplot (223) plot(t(l:length(xd)-I), [xd(l:length(xd)-l,3) sf*diff (y(: ,111 . . . x (1 : length~xd)-i ,311) ,title (’theta-D Crad/sj ’1 ,griä; subplot (224) plot(t(l:length(xd)-I), [xd(l:length(xd)-1,4) sf*diff (y(: ,211 . . . x (i : length(xd) -1,4) 1 ) ,title (’z-D Cm/sl’ ,grid;
pause
subplot (211) plot(t(l:length(xd)),u(: ,l)),title(’ul [Nm]’),grid; subplot (212) plot(t(l:length(xd)) ,u(: ,2)) ,title(’u2 CNml ’>,grid;
clg
D.4.2 Harmo . C
Laat de spindel in z en theta richting een harmonische beweging uitvoeren met opgegeven periode en amplitude. Hierbij wordt een Kalman-filter en een PD-regelaar gebruikt. Ook kan, mits gewenst, nog gebruik worden gemaakt van Computed torque termen voor wrijvingen, de zwaartekracht en traagheden.
verwacht Matlab file input-Ha.MAT met: matrix (1x3) die CT-termen aanhit zet de eerste term is voor de wrijving, de tweede voor de zwaartekracht en de derde voor de traagheden: We
duur van experiment: tm
amplitude van theta-beweging: A-thet a periode van theta-beweging: P-theta
sample-tijd: sf
amplitude van z-beweging: A 3 periode van z-beweging: P-Z
systeem matrices: A, B, C en syst-Ha.MAT met:
34 Hoofdstuk D: Programma's
regel matrix: filter matrix:
LO KO
Maakt Matlab file T-OUT.MAT aan, met: tijdreeks met theta en z gemeten en
tijdreeks met gewenst trajectorie: xd getransformeerd: Y
tijdreeks met geschatte toestand: X tijdreeks met ingangen: U
wait counts: wait */
#define NZ 3
#include <tce.h> #include <stdio.h> #include <math. h> #include <mat.h>
#include <exp-zm.def>
double t ,tm, sf ,pos-theta,pos-z ,theta-DD,z-DD ,A-theta,P-theta,A-z,P-z;
mat xd(4,i) ,u(2,1) ,2(3,1) ,x(4,1) ,y(2,1) ,alpha-d-D(1,1); mat We,A,B,C,LO,KO; double wait c33 ;
void ml-open("t-out", i) ; ts_save-aii(l) ; mi-put-vec(wait ,3,"wait") ; 1
epilog(void){ /*opslaan van de tijdreeksen in t-out.mat */
void run(void) C printf ( ' I /%2.3f\rlt,tm); /* tm op scherm weergeven */
exp-init(z,u,l/sf,wait); /* experiment initialiseren */ exp-zero (> ; / * ingangen gelijk aan nul maken */
/* Uitlezen van huidige coordinaten (z.p[Ol=theta en z.p[l]=alpha) */
exp-get 0 ; /* Coordinaten-transformatie: van alpha en theta naar
theta en z . Hierbij is z=O in het midden van de mogelijke beweging van de spindel gedefinieerd. */
x. p 101 =pos-theta=z .p [O] ; /* theta */ ~.pCi1~p0s~z=(z.pCOl-~.p~ll)*O.O32/(2*M_PI)+O. ii; /* z */
while ( t<tm ) c
/* Gewenste trajectories bepalen */ /* trajectorie voor theta, thetaP en thetaPP: +/
xd.p~01=pos~theta-A_theta*~cos~2*M~PI/P_theta+t)-l); xd.p[2]=A_theta*2*M-PI/P-theta*sin(2*M-PI/P-theta*t); theta-DD=A_theta*2*M-PI/P-theta*2*M-PI/P-theta* cos (2*M_PI/P_theta*t) ;
/* trajectorie voor z , ZP eli. zPP: xd.p [i] =pos-z-A-z* (COS (2*M_PI/P_z*t)-l) ; X~.~[~]=A-Z*~*M-PI/P-~*S~~(~*M-PI/P-~*~); z-DD=A-z*2*M-PI/P-z*2*M_PI/P~z*cos (2*M_PI/P_z*t) ;
*/
1
D.4 H a r m o 35
exp-get 0 ; /* uitlezen coordinaten */ /* Coordinaten transformatie : */
y.pCOl=z.p~01; y .p[ll=(z .pc01 -2 .p [i] ) *O .032/(2*M_PI) +O. 11 ;
x += l/sf*KO*(y-C*x);/*corrector; Kalman-filter */
x += l/sf*(A*x+B*u) ;/* predictor; Kalman-filter*/ u=LO* (xd-x) ; /* regelwet */
/* Computed Torque: */ /* alpha-d-D is de gewenste hoeksnelheid
van alpha. alpha-d-D . p [O] =xd. p [21-2*M-P1/0. 032*xd. p [al ; u. p CV! +=ge. p [GI *sign(xdj . p [Zj *W-theta+
We.p[l1*3.9*9.81*0.032/(2*M-P1)+ We.p[21*(0.0046*theta-DD+O.O153*z_DD) ;
We.p[ll*3.9*9.81*0.032/(2*M-P1)+ We.p[21*(0.0023*theta-DD-O.463O*z-DD) ;
u .p [i] +=We. p [O] *sign (alpha-d-D). p [O] *W-alpha-
*/
exp-put(); / * de berekende ingangen uitsturen */ printf ("%2.3f\rt*,t); /* weergeven verlopen tijd */
/* Bijhouden van de door ts-init geinitialiseerde tijdreeksen: */
t+=l/sf; /* ophogen van de tijd. */ ts-put-ali() ;
3
3 exp-f ini (1 ; /* afsluiten van het experiment */
void main(void) 1
/* Openen van de MATLAB-file input-Ha.mat en inlezen van gegevens
ml-open("input-Ha" ,O) ; bied-get-mat (t'We'l) ; tm=ml-get-scalar("tm", i) ; cf=ml_get_scalar("sf",500) ; A-theta=ml-get-scalar("A-theta" ,M_PI/2) ; P-theta=ml-get-scalar ("P-theta" ,i) ; A-z=ml-get-scalar ("A-z" ,O. 03) ; P-z=rnl-get-scalar("P-z", 1) ;
/* Openen van de MATLAB-file syst-HA.MAT en inlezen van gegevens
ml-open("syst-Ha" ,O) ; A=ml-get-mat ("A") ; B-1-get-mat ("B") ; Cd-get-mat ("C") ; LO=ml-get-mat ("LO") ; KO=ml-get-mat ("KO") ;
/* Initialiseren van de tijdreeksen ts-init(y, (int) tm*sf ,"y") ; ts-init (xd, (int) tm*sf ,"xd") ; ts-init (u, (int) tm*sf ,llul') ; ts-init (x, (int) tm*sf ,"x") ;
run0 ; /* starten van experiment in funktie run printf ("\r\n") ;
*/
*/
*/
*/
epilogo; /* opslaan van de tijdreeksen in t-out.mat */ 1
/* einde van Harm0.C */
36 Hoofdstuk D: Programma’s
D.5 Const
D.5.1 Const .M % C0nst.M zet gevraagde constante koppels op de moeren gedurende een % opgegeven tijd. Daarna worden de ingangen gelijk aan nul gemaakt en wordt % nog een opgegeven tijd gemeten. % Aangeraden wordt om in Matlab de file input-Co.MAT te laden, eventueel % enkele parameters te wijzigen en dan in Matlab C0nst.M te starten. % In input-Co.MAT staan de volgende grootheden: % totale duur van experiment in aantal :4 sâaple-rììoaìenten: nr % duur van bekrachtiging: na % sample-tijd: sf % grootte van ingang op motor O u0 % grootte van ingang op motor 1 u1 % % opm.1 Aan het eind van de beweging wordt het een en ander grafisch % weergegeven. % opm.2 De spindel moet voor het aanroepen van deze funktie in een positie % % maakt. % % C0nst.M maakt gebruik van Const.EXE.
worden gebracht die de gevraagde beweging in z richting mogelijk
save izpipnt-Co sf uC ui Iia nr ! const load t-out if uO”=O
t = (O:length(z(:,l))-l)/sf; clg, subplot(221) plot(t,z(: ,i)), grid, title(’z0 [radl ’1 plot(t,[O;sf*diff (z(:,l))I), grid, title(’dzO/dt [rad/sl’) plot(t,u(: ,111, grid, title(’u0’) 1 = length(z(:,l))-na; if 1>200, 1=200; end plot(t(2:l),sf*[diff(z(l:l,i)) diff(z(na-1+(1:1>,1>)1), grid, . . title(’dzO/dt [rad/s] ’1
end if uL”=O
if done, pause, end t = (O:length(z(: ,2))-1)/sf; clg, subplot (221) plot(t,z(: ,211, grid, title(’z1 [rad]’) piot(t, [O;sf*diff ( z ( : ,2))1), grid, title(’dzi/dt Crad/sl’) plot(t,u(: ,211, grid, title(’ui’) 1 = length(z(:,2))-na; if 1>200, 1=200; end piot(t(2:l>,sf*[diff(z(i:l,2)) diff(z(na-l+(1:1),2))]), grid, . .
end
D.5.2 Const. C
title(’dzl/dt [rad/~] ’>
p situ Tests in open lo tion
Expects Matlab file input-Co.MAT with scalars: sf - sample frequency (Hz) na - #active cycles nr - #cycles to registrate
D.6 Trajec 37
u0 - command value for alpha UI - command value for theta
Creates Matlab file T-OUT.MAT with: 2 - n*NZ : collected measurements U - n*2 : applied commands wait - 3*1 : wait counts */
#define NZ 3
#include <tce.h> #inclmie <s+_din.h> #include <math. h>
double sf,uO,ul; int na,nr;
double u C2l , z CNZl ; double wait 131 ;
void epilog(void) 1 mï-open("t-out", i) ; ts-save-all(1) ; ml-put-vec(wait ,3,'1wait") ; 3
void run(void) int i; exp-init(z,NZ,u,l/sf,wait); u[o] = uû; u[il = UI; exp-put 0 ; for ( i= O; i<na; i++ ) C ts-put-aiiO; exp-get(); 3 ULO] = uCi1 = O; exp-put(); for ( i=na; i<nr; i++ 1 C ts-put-allO; exp-get0; 3 exp-f ini 0 ; > void main(void1 C ml-open( "input-Co", 0 ) ; sf = ml-get-scaiar("sf I', 100) ; uo = ml-get-scalar("uO",O) ; UI = ml-get-scalar("uI",O); na = (int)ml-get-scaIar ("na", IO) ; nr = (int)ml-get-scaiar("nr1',200) ;
ts_initO(z ,NZ,nr, "z") ; ts-initû(u, 2 ,nr, "u") ;
run0 ; epilog0 ; 1
/* end of C0nst.C */
D.6 Trajec
D.6.1 Traj ec . M % Trajec.m laat de spindel een voorgeschreven trajectorie volgen % Aangeraden wordt om in Matlab de file input-Tr.MAT te laden, eventueel % enkele parameters te wijzigen en pas dan trajec.# te starten. % In input-Tr.MAT staan de volgende grootheden:
38 Hoofdstuk D: Pronramma’s
% duur van experiment in aantal sample- % %
momenten: sample-ti jd:
nr sf
% tijdreeks voor gewenst trajectorie (bv % gemaakt door make-xd.m): xd % %
X opm.l Aan het eind van de beweging wordt het een en ander grafisch % weergegeven. % opm.2 De spindel moet voor het aanroepen van deze funktie in een positie x % maakt. % % Trajec.M maakt gebruik van Trajec.EXE.
variabele die aangeeft of tijdreeksen van het experiment moeten worden
% bijgehouden (O=nee) : R
vcrden gebïackt die de gevraagde beweging in z richting mogelijk
save input-Tr xd sf nr R !trajec load t-out if (RI,
load t-out t=[o: i/sf :nr/sfl ’ ; xdh= Cl ; for i=l: (round(nr/length(xd))+l),
en&
subplot (221) plot(t(l:length(x)),[xdh(l:length(x),l) y(:,l) x(:,l)l),. ..
subplot (222) plot(t(l:length(x)), Cxdh(l:length(x) ,2) y(: ,2) x(: ,2)1), . . .
title(Jz[ml (R:x-d G:y B:xhat) J, ,grid; subplot (223) plot(t(l:length(x)-1) , [xdh(l:length(x)-1,3) sf*diff (y(: ,i)) . . . subplot (224) plot(t(l:length(x)-l), [xdh(l:length(x)-1,4) sf*diff (y(: ,2)) . . . pause
subplot (211) plot(t(l:length(x)),u(:,l)) ,title(’ul [Nml’),grid; subplot (212) plot(t(l:length(x)>,u(: ,211 ,title(’u2 CNmI’) ,grid;
xdh= [xdh; xd] ;
clg
title(’thetaCrad1’) ,grid;
x (i: length(x)-l,3)] ,title (’theta-D [rad/sl 1 ,grid;
x(1: length(x)-l,4)] 1, title ( ’z-D [m/sl ,grid;
clg
end
D.6.2 Trajec. C /* trajec.c -----_ ------
Trajec.m laat de spindel een voorgeschreven trajectorie volgen. uitvoeren met opgegeven periode en amplitude. Hierbij wordt een Kalman-filter en PD-regelaar gebruikt. verwacht Matlab-file input-Tr.MAT met:
duur van experiment in aantal sample- momenten: nr
sample-tijd: sf
gemaakt door make-xd.m): Xd tijdreeks voor gewenst trajectorie (bv
variabele die aangeeft of tijdreeksen
D.6 Traiec 39
van het experiment moeten worden bijgehouden (O=nee) : R
systeem matrices: A, B, C regel matrix: LO
en cyst-Ha.MAT met:
filter matrix: KO
Maakt Matlab file T-OUT.MAT aan, met: tijdreeks met theta en z gemeten en
tijdreeks met gewenst trajectorie: tijdreeks met geschatte toestand: tijdreeks met ingangen: wait counts:
getransformeerd: Y xd X
U
wait */
#define NZ 3
#include <tce.h> #include <stdio.h> #include <math.h> #include <mat. h>
double sf; int nr, R;
mat u(2,1),~(3,1) ,x(4,1) ,y(2,1); mat xd(4,1), A, B, C, LO, KO ; double wait C31;
void ml-open("t-out",i) ; ts-save-all(1); ml-put-vec(wait ,3,"wait") ; 3
epilog(void){/* opslaan van de tijdreeksen in t-out.mat */
void run(void) { int i; printf (" /%4d\r",nr) ; /* rm op scherm weergeven */ exp-init(z,u,l/sf,wait); /* experiment initiaiiseren */ exp-zero (1 ; /* ingangen gelijk aan nul maken */ /* Uitlezen van huidige coordinaten
(z.pCO]=theta en z.p[Il=alpha) */ exp-get 0 ;
/* Coordinaten-transformatie: van alpha en theta naar theta en z. Hierbij is z=O in het midden van de mogelijke beweging van de
spindel gedefinieerd. */ x .p C01=z .p [O] ; x.p[11=(z .p CO1 -2 .p Cl11 *O. 032/ (2*M_PI)+O. 11 ;
for ( i=O; i<nr; i++ ) c
x += l/sf*(A*x+B*u) ; /* predictor */ ts-get-allO ; /* ophalen van gewenste trajectorie */
exp-get 0 ; /* uitlezen coordinaten */ /* Coordinaten-transformatie: */
x += l/sf *KO* (y-C*x) ; /* corrector */ y .p [O] =z .prol ; y.p[Il=(z .p CO] -2 .p Cl]) *O .032/(2*M_PI)+O. 11 ;
u=LO* (xd-x) ; /* Regelwet printf ("%4d\r", i) ;
*/
exp-put 0 ; /* Bijhouden van de door ts-init geinitialiseerde
/* de berekende ingangen uitsturen */
ti jdreeksen: */ ts-put-all() ; 1
40 Hoofdstuk D: Promamma's
exp-f ini ( 1 ; 3
/* experiment afsluiten */
void main(void1
/* Openen van de MATLAB-file Input-Tr.MAT en inlezen
ml-open("input-Tr" ,O) ; */ van gegevens
sf = mi-get-scalar ("sf I', 100) ; nr = (int) d-get-scaiar("nr",O); R = (int) ml_get-scalar("R",O) ; ts-ioaäíxä, "xd"j ;
/* Openen van de MATLAB-file Syst-Tr.MAT en inlezen */ van gegevens
ml-open("cyst-Tr" ,O) ; A-l-get-mat ("A") ; B=ml-get-mat ("BI') ; C=mi-get-mat ("Cl') ; Lûd-get-mat ("LO") ; KO=ml-get-mat ("KO") ;
/* Initialiseren van de tijdreeksen if (RI C ts-init(u,nr,"u"); ts-init(x,nr,"x") ; ts-init(y,nr,"y") ; 3 run0 ; printf ("\r\n") ; epilog0 ; 3
/* end of Trajec.C */
*/