Inverse kinematika lasrobot KUKA 161 : programma HP 9825

(1)

Inverse kinematika lasrobot KUKA 161 : programma HP 9825

Citation for published version (APA):

Koumans, P. W. (1988). Inverse kinematika lasrobot KUKA 161 : programma HP 9825. (TH Eindhoven. Afd. Werktuigbouwkunde, Vakgroep Produktietechnologie : WPB; Vol. WPA0610). Technische Universiteit Eindhoven.

Document status and date: Gepubliceerd: 01/01/1988

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 be important differences between the submitted version and the official published version of record. People interested in the research are advised to contact the author for the final version of the publication, or visit the DOI 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 page numbers.

Link to publication

General rights

Copyright and moral rights for the publications made accessible in the public portal are retained by the authors and/or other copyright owners and 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, please follow below link for the End User Agreement:

www.tue.nl/taverne

Take down policy

If you believe that this document breaches copyright please contact us at:

openaccess@tue.nl

providing details and we will investigate your claim.

(2)

lasrobot KUKA 161

Programma HP 9825

Ir. P.W. Koumans \vPA-rapport 0610

(3)

Inleiding

Beschrijving programma inverse kinematika

Iasrobot type KUKA 161

P.W.Koumans, vakgroep WPA 24-6-1988

Bij het opste11en van een te lassen produkt t.o.v. een

lasrobot wordt men gekonfronteerd me~ de vraag wat de

optimale positie van de Iasnaad is. De robot heeft zijn beperkingen in het bereik van de verschillende draaipunten. Verder kan een wat ongunstig gelegen lasnaad maken dat

bepaalde assen van de robot ongunstige versnellingen en snelheden moeten krijgen, hetgeen ten koste gaat van de positje-nauwkeurigheid. Deze hangt verder ook af van

hoekstanden van de verschillende assen. Het daarom van belang om een optimale positie van de lasnaad van te voren te kunnen vastleggen. Er is een eenvoudig programma op de HP 9825

geschreven waarmee men het lassen van de robot kan sirnuleren en kan bepalen wat de hoekstanden van de verschillande

draaipunten is afhankeIijk van de positie van de lastoorts op de Iasnaad. Doordat de Iasnaad in een aantal gelijke delen wordt verdeeld zijn hierrnee ook de hoeksnelheden en de hoekversnellingen te bepalen.

Uitgangspunten

De ligging van de lasnaad t.o.v. de robot wordt als bekend verondersteld, evenals de stand van de lastoorts t.o.v. de lasnaad op de lasplaats. Die stand van de toorts is af-hankelijk van het type lasnaad. Uit deze gegevens en de afmetingen van de robot zijn de hoekstanden van alle draaipunten van de lasrobot af te leiden.

Beschrijving van de configuratie

In fig.1 zijn de delen van de robot vernoemd. De robot bestaat uit een onderstuk, draaibaar om draaipunt 1. De rotatie-as van dit draaipunt valt samen met de Z-as van een globaal assenstelsel. De onderarm is met draaipunt 2

verbonden aan de onder arm en met draaipunt 3 aan de bovenarm.

De bovenarm bestaat uit twee delen, die met draa~~~nt 5

t.o.v. elkaar om hun lengte-as kunnen draaien. De der draait met draaipunt 4 om de onderarm. ook de toortshou-der is in twee delen verdeeld, die met draaipunt 6 t.o.v. elkaar om hun lengte-as kunnen draaien.

(4)

1. het robotvlak, dat het onderstuk, de onderarm en de bovenarm bevat,

2. het tussenvIak, bevattende de bovenarm en de toortshouder, 3. het Iasvlak, bevattende de toortshouder en de 1asnaad. De Iasnaad mag hier ook worden gezien als de momentane raakIijn aan een 1askromree op het momentane laspunt.

Invoergevens

De vo1gende gegevens z1Jn a1 ingevoerd, gebaseerd op de

afmetingen van de robot KUKA 161:

1. De z-coordinaat van draaipunt 2; 2. de 1engte van de

onderarm; 3. de 1engte van de bovenarm.

Ingevoerd moet worden:

4. Het aantal punten van de 1asnaad dat bekeken moet worden. De lasnaad wordt verondersteld recht te zijn en wordt nu onderverdee1d in een aantal gelijke delen, aangegeven door punten.

5. Het beginpunt van de lasnaad in X-coord., Y-coord. en Z-coord. van het globale assenste1sel.

6. Het eindpunt van de 1asnaad in X-coord., Y-coord. en Z-coord.

7. De coordinaten van het draaipunt 4 van de toortshouder in

de stand bij het beginpunt van de 1asnaad in X, Y, en Z.

Berekeningsmethode

Bepa1ing punten van de lasnaad

Het aantal punten wordt geste1d op I.

Voor ieder punt (J) geIdt:

X(J)

=

X~J-1)+ (X(I)-X{l»)/(I-1); analoog voor Y(J) en Z(J)

Hoek draaipunt 1 (fig 2)

Deze voIgt uit: tan D(l) = (Y(4)/X(4»

Hoek draaipunt 3 (fig 3)

M

=

.f(X(4) 2+Y(4) 2); N

=

.f( (Z(4)-H) 2+M2)

cos (D(3» = (K2+L2-N2)/(2KL)

Hoek draaipunt :2 (fig 3)

D(2)

= asin«Lsin(D(3»/N)

+ atan«Z(4)-H)/M) + 900

Een vIak door drie punten

Een sub-routine geeft de formule voor een vIak door drie punten.

Als deze punten 1, 2 en 3 zijn met in het X- , Y- en Z-vIak

resp. als coordinaten

x,

y, z, dan geldt:

1

x

y

z

1 xl yl zl

1 x2 y2 z2 = 0

(5)

Of x1y2z3 - x1y3z2 - x2y1z3 + x2y3z1 + x3y1z2 - x3y2z1

- x(y2z3 - y3z2 - y1z3 + y3z1 + ylz2 - y2z1)

+ y(x2z3 - x3z2 - xlz3 + x3z1 + xlz2 - x2z1)

- z(x2y3 - x3y2 - x1y3 + x3y1 + xly2 - x2y1)

=

0

Het robotvlak

Dit is bepaald door de draaipunten 1, 2, 3 en 4. V~~r de

eenvoud worden de punten 1, 2 en 3 genomen. Het tussenvlak

~it vlak is bepaald door de draaipunten 3, 4 en het rnornentane

laspunt. Het lasvlak

Hiervan worden het draaipunt 4 en twee laspunten genornen.

Bij €en rechte lasbaan en een vaste stand van de toorts

t.o.v. de lasbaan is dit lasvlak een vlak in een vaste stand.

Hoek draaipunt 5

Deze hoek is de hoek tussen de elkaar snijdende robotvlak en tussenvlak.

De hoek tussen de snijdende vlakken ax + by + cz + d = 0

px + qy + rz + s = 0 volgt uit:

acos«ap + bq + cr)/(/(a2 ₊ _b2 ₊ _c _{2 )} _(/(p2 ₊ _q2 ₊ _{r 2}_»

Hoek draaipunt 6

Deze hoek is de hoek tussen het tussenvlak en het lasvlak. Hoek draaipunt 4

Deze hoek is de hoek tussen de rechte door de draaipunten 3 en 4 en de rechte door het draaipunt 4 en het mornentane laspunt.

De afstand tussen het draaipunt 4 en het laspunt volgt uit:

/ ( (x ( 4 ) -x (5) ) 2+ (y ( 4 ) -y ( 5) ) 2+ ( z ( 4 ) -z ( 5) ) 2)

Op analoge wijze is de afstand tussen het laspunt en

draaipunt 3 te bepalen. Hiermee is driehoek draaipunt 3, 4 en laspunt vastgelegd. Met de cosinusformule is nu de gevraagde hoek te bepalen.

Uitvoergegevens

Er wordt op het beeldscherrn een tabel gegeven met voor de verschillende draaipunten de hoeken die gelden voor de berekende punten van de lasnaad.

Verder wordt de beginstand van de robot en de lasnaad getoond in een X-Y-Z-assenstelsel. De beginstand wordt in getrokken

(6)

lijnen weerqegeven. De standen van de robot bij de verschil-lenden punten van de lasnaad wordt met een streep- stippel-lijn getoond.

Op het beeldscherm kan verder het verloop van de hoek van draaipunt 1 worden weerqegeven. Dezelfde geqevens kunnen

wOld~n geplot (fig.4). Hierbij kunnen de hoekverdraaiinqen

van aIle draaipunten grafisch worden weerqegeven en bovendien de hoeksnelheden en hoekversnellingen (fig.S). Bij de

bepaling van de snelheden en hoekversnellinqen wordt

aangenomen dat de lassnelheid constant is. Het rapport bevat een uitdraai van het proqramma geschreven in de

(7)

draaipunt 3 _ bovenarm _ _

_-

onderarm -draaipunt 2 - -onderstuk - - - - -draaipunt 1- - __

y

j

_.

/

2-... ... ...

-

-;:t~ ...

Y

fig. 2

-

-\

_ _ robotvlak " / tussenvlak " lasvlak

-::"

_ • __ draaipunt 4-.,.

\

-

- -

--

/

\

-- ---r

d!:a2:i£un-.!= ~ ' ...

\

,

\

x

lasnaad'

1 -\

L-

---

-fig.1

...3

---

----

-1'1

I

l.

I

_'t'

I

:-z('i)

H

I

/

X

I

- _I

I /

/'1(&f.)

- -

-r-\

~..JI fig. 3

(8)

KUKA

01 -g

-6

-3

"

:3 6

9

12 15

18

161

Fig.4

begtn

l_noad

ac-

120. ee

y-

-38. ee

z-

".5e

.snd

1 cwnaad

ac-

118. ee

y-

38. ee

z-

38. ee

begt n

t.oort.e

ac-

138. ee

y-

-20. ee

z-

11 e.

ee

02

03

04

05 DB

136

116

76

19

86

136

112

17

18

98

136

108

78

11

93

135 le6

78

17

97

133

104

79

16 lel

131

103

80

15 1&14

129

103

80

14 le7

126

103

81

13 lle

124

104

81

12

113

120

106

81

11

116

(9)

e

E t--!I ""J!

o

C ttl o

-IS) CD

....

I E E t--t--I I ."

o

t:

.,

o

-E E t--r... I "'0 0 0

c:

• a

-

_•

+l Ol

c

• ...

IS) ~ I \5) r;-.. I

1

Ii \

~

\

I /\ \

IS) IS) & & & & & & &

N ~ ID M M ID ~ N .... I 1 I -I hoekverdraa1ins draaipunten in sr.

6~

\ \ \

\

\ \

\

\5) \5) & & & &

~ 00 ... ... ~ U') I I l no.k.n.lhed.n in sr/la8tiJd/l~~ & U')

-i

,

••

~

\

4~

I

/

\ 1\

\

/

\/\

/

\

IS) _& _& _& _& _& _&

U') _~

-

...

~ U')

r--I l l hoekv.r.n.11insan in 9r/lastiJd~2/100 & m

-& ~ ~ig.5

(10)

!

..

I

0: % "---KUKA-robot-161---P. K. 06-1988 " 1: i. It - - - •• 2: % "---inverse kinematika---" 3: 7.

4: % "dr.pntl(H) 77.S,lengte onderarm (K) 80,lengte bovenarm (L) 80" S: 77.S}H;80}K;80}L

6: ent "aantal punten rechte lasnaad?",I 7: sfg 14

8: dim X(S,IJ,Y(5,IJ,Z[S,IJ,O[6,IJ~P[3,IJ,Q(3,IJ,R[3,IJ,S[3,IJ

9: dim U[3J,V[3J,W[3J

10: ent "beginpunt lasnaad,x=?",X[S,IJ,"y=?",YtS,IJ,"z=?",Z[S,lJ 11: ent "eindpunt lasnaad, x=?",X(S,Il,"y=?",Y[S,IJ,"z=?",Z[S,IJ 12: ent "P4 toortshouder beginstand,x=?",X[4,lJ,"y=?",Yt4,lJ 13: 14: 15: 16: 17: 18: 19: 20: 21 : 22: 23: 24: 25: 26: 27: 28: 29: 30: 31 = 32: 33: 34: 35: 36: 37: 38: 39: 40: 41 : 42: 43: 44: 4S: 46: 47: 48: 49: 50: 51 : 52: ent "z=?",Z[4,lJ fmt 1,10x,3cl0 wrt 10.1,"X","Y","Z" fmt S,3f9.2 wrt 10.S,"begin lasnaad",X[S,lJ,Y[S,lJ,Z[S,lJ;wait 100 wrt 10.S,"eind lasnaad ",X[S,IJ,Y[S,I1,Z[S,IJ;walt 100 wrt 10.S,"toortshouder ",X(4,lJ,Y[4,lJ,Z[4,lJ;wait 100 ent "veranderingen geg?,ja=l",G

if G=l;gto -11 for J=l. to 1-1 X[S,JJ+(X[S,I1-X[S,lJ'/(I-l)}X[5,J+1J Y[S,JJ+(Y[S,IJ-Y[S,IJ)/CI-l)}Y[S,J+1J Z[S,JJ+(Z(S,IJ-Z[5,lJ)/CI-l)}Z[S,J+1J X(4,J)+X(S,2J-X[S,IJ}X[4,J+l1 Y[4,JJ+Y[S,2J-Y(S,lJ}Y[4,J+ll Z[4,J1+Z[S,2J-Z[S,11}Z[4,J+l1 next J for J=1 to I

atn(Y[4,JJ/X[4,JJ)}O[I,J1;% .. hoek draaipunt 1" if Dtl,J1>160 or DCl,JJ<-160;dsp "draaipunt 1 buiten bereik";stp

\(X[4,JJA_{2+Y(4,J]A2)}M;\«Z[4,JJ-H)A2+M}A_2)}N

ac::s(O(-"2+L'·'2-N"'2)/21<U}DC3~J];7. " hoek draaipunt 3" if D[3~JJ<4S;dsp "draaipunt 3 buiten bereik";stp

if D[3,JJ>16S;dsp "hoek dr.pnt 3 groter dan 16S gr.";stp

asn(Lsin(DC3~J1)/N)+90+atn«Z[4,JJ-H)/M)}OC2,JJ;%" hoek draaipunt 2" if D[2,JJ<:20 or D[2,JJ>230;dsp "draaipunt 2 buiten bereik";stp

l<c::os(D[2~J]-90)c::os(D[1,J])}X[3,JJ;7. II x-coord dr.pnt 3"

l<c::os(D[2,J]-90)sin(D[1,J])}Y[3~JJ;'Y. " v-coord dr.pnt 3" H+Ksin(D[2,JJ-90)}Z[3,JJ;7. ., z-coord dr.pnt 3"

'Yo II ROBOTVLAK P[ 1, J ]x+Q[ 1, J 1y+RCi, J lz+SC 1, J 1=0"

0}U[1J;O)V[IJ;O}W[11 0)U[2];O}V[2];H}Wt21 X[4.J]}U[3J;Y[4,J]}V[3J;Z[4,JJ}W[31 gsb "vlak" r1}P[1~J];r2}Q[1~JJ;r3}R[1,JJ;r4}S[1~JJ 7. " TUSSENVLAI< Pt2, J Jx+QC2, J Jy+RC2, J Jz+SC2, J 1=0" X[3~J1}U[IJ;Y[3,JJ}V[IJ;Z[3,J]}W[11 X[4,JJ}U[2J;Y[4,JJ}Vt2J;Zt4,J]}Wt2J X[S,JJ}U[3];Y[S,J]}V[31;ZtS,JJ}W[3J gsb " v l a k"

(11)

53: rl)P[2,JJ;r2}Q[2,JJ;r3}R[2,Jl;r4}S[2,Jl 54:

55: 56:

57:

% ., LASVLAK (vast vIak) PC3,JJx+Q[3,JJy+R[3,Jlz+SC3,JJ=0"

58: 59: 60: 61 : 62: X[4,lJ}U[lJ;Y[4,1]}VC11;Z[4,lJ}W[ll X[S,lJ}UC21;Y[S,lJ}V[2J;Z[5,lJ}W[2J X(S,IJ}U[3J;YCS,IJ)V[3J;Z[S,IJ}W[3J gsb "vI ak" rl)P[3,JJ;r2}Q[3,JJ;r3}R[3,JJ;r4)SC3,JJ

% II hoek tussen robotvlak en tussen vlak"

P[1,JJP[2,Jl+QC1,J]Q[2,JJ+R[1,JJR[2,JJ}rS

\(PC1,JJA_2+QC1,J1A_{2+RC1,J1A2).\(P[2,J1A2+Q[2,JJ}A_2+RC2,JJA_2)}r6

63: acs (r5/r6) }OC5, J J; % " hoek draai punt S" % " hoektussen tussenvlak en lasvlak"

P[2,JJPC3,JJ+QC2,J1QC3,JJ+R[2,J1RC3,JJ}rS \(PC2,J1A2+Q[2,J1A_{2+RC2,J]A2'*'(P[3,JJ}A_{2+QC3,JJ"'2+RC3,JJ}A_2»r6 64: 65: 66: 67: 68: 69: 7(): 71 : 72:

ac:s(rS/r6)}0[6,Jl;% " hoek draaipunt 6"

% I I hoek draaipl..tnt 4"

(X[3,Jl-X[4,J])A2+(Y[3,J]-Y[4,JJ)A2+(Z(3,JJ-ZC4,Jl)"'2)r5;% " (x(4, J ]-X [5, J J) ···2+ (Y[4, J J-Y[5, J J )'"--2+ (Zt4,J J-Z[S, J J) "'2}r6; % " (XeS, J J-X(3, J J) A2+ (Y[5, JJ-Y[3, J 1) · ... ·2+ (Z [S, Jl-Z(3, J J) ... 2}r7; % " \rS}r5;\r6}r6;\r7}r7 PQII P-laspunttl Laspunt-Q" 73: ac:s ( (rS+r6-r7) 12\ <r5*r6) ) } 0[4, J J; % " next J hoek draaipunt 4" 74: 75: 76: 77: 78: 79: fmt 1,5x,c:4,z wrt 10.1,"01","02","03","D4","D5","06" fmt 2,I;wrt 10.2 fmt 3,6f9.0 for J=l to I 80: wrt 10.3~OC1,JJ,0[2.J],D[3,J],D[4,J].0[5,JJ,DC6,JJ;wait 100 81: ne>:t .J 82: if G=2;gto 143

83: ent "grafiek hoekverdraaiingen? l=ja",G 84: if G#l;gto +33

85: wtb 10,27, u *ad2h 1i 2j-180I180mln 1 Oo30p lr 1 vC" 86: wait 1500 87: wtb 10,27, u*,"i\21<";wtb 10,27, "*aA";wait 500 88: for J=l to I 89: wrt 10,D(1,JJ,10J/I;wait 100 90: ne:.:t J 91: wttl 10,27, "*1Dl" 92: wtb 10,27, "*a3~::";wtb 10,27, u*aA";wait 500 93: for J=l to I 94: wrt 10.D(2,Jl,10J/I;wait 100 95: ne:-:t J 96: wtb 10,27,"*102"

97: wt.b 10,27, u*a4f<u;wtb 10,27, u*.3A";wait SOO 98: for J=l to I 99: wrt 10,0[3.JJ,10J/I;wait 100 1 00: ne~·: t J 101: wtb 10,27, "*1D3" 102: wtb 10,27, "*a.5~<";wtb 10,27, "*aA";wait 500 103: for J=l to I 104: wrt 10,DC4.JJ,10J/I;wait 100 105: neNt J 106: wtb 10,27,"*104" 107: wtb 10,27,"*a6K";wtb 10,27,"*aA";wait 500

(12)

08: for J=1 to I 09: wrt 10~0[5,JJ,10J/I;wait 100 .10: next J , 11: wt b 10, 27, "

*

1 05" .12: wtb 10,27,"'a7K";wtb 10,27,"*aA";wait 500 ,13: for J=l to I ~14: wrt 10,0[6,JJ,10J/I;wait 100 ~ 15: neNt J l16: wtb 10,27," *1 06"

l17: ent "weergave robotsta.nden? 2= ja", G l18: if G#2;gto +24 l19: wtb 10,27,"*dAn 120: wtb 10,27, "*ad2hli2j4k-l00130(lm-60n140Q" l21: wait 1500jwtb 10,27, "*aA";wait 500 L22: 7}A;42}B L23: wrt 10,0,120;wrt 10,O,0;wrt 10,150cos(A),-150sin(A) L24: wrt 10,0,O;wrtl0,-150cos(42),-150sin(42)

125: wtb 10,27, "*pA";wtb 10,27, "*alK"p-Jtb 10,27, "*aA";~udt 500 126: wrt 10,O,O;wrt 10,O,H 127: wrt 10~X[3,I]cos(A)-Y[3,1]cos(B),Z[3,1]-X[3,lJsin(A)-YC3,1Jsin(B) 128: wrt 10,X(4,IJcosCA)-Y[4,lJcos(B),Z[4,lJ-X[4,IJsinCA)-Y[4,1Jain(B) 129: wrt 10.X[5,lJcos(A)-Y[5,lJcos(B'~Z[5,lJ-X[5,IJsin(A)-Y[5,1Jsin(B) 130: wtb 10,27,"*a2kA";wait 500 131: wrt 10,X[5,lJcos(A)-Y[S,lJcosCB),Z[S,lJ-X[S,lJsin(A)-Y[S,1Jain(S) 132: wrt iO,X(S,IJcosCAJ-Y[5,IJcos(B),Z[S,IJ-X[S,IJsinCA)-Y[S,IJsinCB) 133: for J=2 to I 134: wttl 10,27, "*a8kA";wait 500 135: \~r· t 1 (l, (1, H 136: wrt 1(1,X[3,JJcos(Al-Y[3,JlcosCB),Z[3,JJ-X(3,JJsin(A)-Y[3,Jlsin(B) 137: wrt 10,X[4,JJcosCA)-Y[4~J]cos(B',Z[4,JJ-X[4,J]sin(A)-Y[4,JJsinCB) 138: wrt 10,X[5,JJcos(A)-Y[S,JJcos(B),Z[S,JJ-X[S,JJsin(A)-Y[S,J]sin(B) 139: ne:-~t J 140: wt.b 10,27, UtaB";wtb :1.0,27, !lU" 141: gto 75 142: emt ""lotten?,3=ja",G 143: if G#3;gto +91

144: dsp "e;;taand A4,links onder en continue";stp 145: scl -140,530,-210,155;7)A;42}B;line 3,2.5;pen 146: pIt 0.150;plt O,O;plt lS0cosCA),-150sinCA),-1 147: pIt O.O,-2;plt -150cos(8),-150sinCB),-1

148: line ;plt O,O;pIt O,H

149: pIt X[3,llcos(AJ-Y[3,IJcosCB),Z[3,IJ-X[3,IJsln(A)-YC3,lJsinCB) ISO: pIt X[4,IJcos(AJ-Y[4,lJcos(S),Z[4,IJ-X[4,llsinCA)-Y[4,lJsinCB) lSI: pit X[5,lJcos(AJ-Y[S,lJcos(B),Z[5,11-X[S,IJsin(AJ-Y[S,IJsin(B)

IS2: line 4;plt X[5,IJcos(AJ-Y[5,IJcosCB),Z[5,IJ-X[5,IJsinCAJ-Y[S,IJsin(8),-1 153: line 6

154: for J=2 to I

155: pIt O,H;plt X[3,JJcosCA)-Y[3,JJcos(B),Z[3,JJ-X[3,JJsinCA)-Y[3,JJsinCB)

1S6: plt X[4,JJcos(A)-Y[4,JJcos(BJ~Z[4,JJ-X[4,J]sin(A)-Y[4,JJsinCB)

157: pIt X[5,JJcos(A)-Y[5,JJcosCB),Z[5,JJ-X[5,JJsin(A)-YC5,JJsin(B),-1 158: ne)<t ,J

(13)

159: pit -100,80,1 160: wrt 70S,"si.3S,.5";lbl "I<UKA 161" 161: pit -40,-80,1 162: wrt 705,"si.18,.26" 163: wtb 70S, "LB" 164: fmt 4,c13," :<=",f7.2," y=",f7.2," z=",f7.2 16S: wrt 70S.4,"begin lasnaad",X[S,IJ,Y[S,1J,Z(5,IJ 166: wrt 70S.4,"eind lasnaad",X[S,IJ,YCS,IJ,ZCS,IJ 167: wrt 70S.4,"begin toorts",X[4,lJ,YC4,IJ,Z[4,lJ 168: wtb 70S,3 169: pIt -110,-110,1 170: wtb 70S, "LB" 171: fmt 1,5x,c4,z 172: wrt 70S. 1, "01", "02","03","04", "OS", "D6" 173: fmt 2,/;wrt 70S.2 174: fmt 3,6f9.0 17S: for J=1 to I 176: wrt 70S.3,OC1,JJ,0[2,JJ,0[3,JJ,D[4,JJ,D[S,JJ,DC6,JJ 177: ne){t J 178: wtb 70S,3

179: ent "plot grafiek hoeken?,4=ja",G 180: if G#4;end

181: \«X[S,IJ-X[S,1])A2+(Y[S,IJ-Y[S,IJ)A2+(Z[S,Il-Z[S,1])A2)}rl0 182: scI -180,670,-22S,60

183: pIt 180,0;pIt 180,80;plt -180,80;pIt -180,0,-1 184: wrt 70S,"si.18,.28" 18S: fxd O;xax 0,30,-180,180,1 186: for C=1 to 6 187: for J=l to I 188: pIt 0[C,JJ,80eJ-l)/CI-l);next J 189: pen;ipit -S,-5,1 1 90: i f C= 1 ; 1 b 1 "1"; lin e 2 191: if C=2;lbI "2";line 3 192: if C=3; Ibl "3"; line 4 193: if C=4;Ibi "4";line S 194: if C=5.1bl "S";line 6 195: if C=6;lbl "6";line 1.96: ne>:t C 197: pIt -190,S;csiz 1,1.9,1,90

198: lbl "lengte lasnaad =",rl0, "mm";csiz 1.1,1.9,1,0

199: pIt -120,-18;lbl "hoekverdraaiing draaipunten in gr.";csiz 200: scI -90,335,-120, 16S;wrt 70S,"si.18,.28" 201: pIt 90~0;plt 90,80;plt -90,80;plt -90,0 202: fxd O;xax O,20,-90,90,1;line 203: for C=l to 6 204: for J=2 to I 20S: pIt 100(DCC,JJ-D[C,J-1J)/(I-l),80(J-l.S)/(I-l);next J 206: pen;iplt -3,-5,1

(14)

207: 208: 21)9: 210: 211: 212: if if if if if if C=l;lbl C=2;lbl C=3;lbl C=4;lbl C=5;lbl C=6;lbl 213: next C "1";line 2 "2";line 3 "3";11ne 4 "4";line 5 "5";line 6 "6";line 214: pIt -95,5;csiz 1,1.9,1,90

215: Ibl "lengte lasnaad =",rl0, "mm";csiz 1.1,1.9,1,0

216: pIt -55,-18;lbl "hoeksnelheden in gr/lastijd/l00";csiz

217: scI -90,335,-15,270;wrt 705~"si.18,.28" 218: pIt 90,0;plt 90,80;plt -90~80;plt -90,0 219: fxd O;xax 0,20,-90,90,I;line 220: for C=1 to 6 221: for J=3 to I 222: pIt 100(DtC,Jl-2DCC,J-l1+D(C,J-21),80(J-2)/CI-l);next J 223: pen;iplt -3,-5,1 224: if C=I;lbl "1";line 2 225: if C=2;lbl "2";line 3 226: if C=3;lbl "3";line 4 227: if C=4;lbl "4";line 5 228: if C=5;lbl "5";line 6 229: if C=6;lbl "6";11ne 230: ne:·:t C 231: pit -95,5;csi2 1,1.9,1,90

232: Ibl "lengte lasnaad=",rl0, "mm";csiz 1.1,1.9,1,0

233: pIt -65,-18;lbl "hoekversnellingen in gr/lastijdA_2/100";csiz

234: end 235: "vI ak": 236: U(IJV(21W(31-U(lJV[3JW(2J-Ut2JV(lJWt31+U(2JV(31WC11+U(3lVCIJWC2]}r4 237: r4-U(3JV(2JW(1])r4 238: -V(2]W[3J+V(3JW(2J+V[lJW[3J-V[3JW(11-V[IJW(21+V[2JW[1J}rl 239: U(2JW[3J-U(3JW[2J-U[IJW[3J+U[3JW[lJ+U[1]WC21-UC21W[1)r2 240: -U[2JV[3]+U[31V(2]+UtlJV[3J-U[3JV[lJ-U[lJV[21+U[2JV[1]}r3;ret 241: end