Dynamic symulation for two dimensional robot arm

Bober, Miroslaw; Florkowski, Marek

Published: 01/01/1987

Document VersionPublisher’s PDF, also known as Version of Record (includes final page, issue and volume numbers)

Please check the document version of this publication:

• A submitted manuscript is the author's version of the article upon submission and before peer-review. There can be important differencesbetween the submitted version and the official published version of record. People interested in the research are advised to contact theauthor 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

Citation for published version (APA):Bober, M., & Florkowski, M. (1987). Dynamic symulation for two dimensional robot arm. (TH Eindhoven. Afd.Werktuigbouwkunde, Vakgroep Produktietechnologie : WPB; Vol. WPA0489). Eindhoven: TechnischeUniversiteit Eindhoven.

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 ?

Take down policyIf you believe that this document breaches copyright please contact us providing details, and we will remove access to the work immediatelyand investigate your claim.

Download date: 21. Apr. 2018

DYNAMIC SYHULATION FOR TWO DIMENSIONAL ROBOT ARM

Miroslaw Bober Marek Florkowski

Stanislaw Staszic Uniwersity Cracow, POLAND

Coach: ir. P.C.Mulders September - October 1987

WPA-Rapport 0489

ACKNOWLEDGMENTS

We would like to express thankfulness to our coach Mr Piet Mulders and Mr Henk Smit for warm harted greetings, introduction to robotics and time contributed for progress of our work.

We will remember nice atmosphere created by team: Gerard Kreffer, Artur Udo, John Janssen, Eric Retraint for long time.

Special thanks to Henk Smit and Gerard Kreffer for their valuable suggestions.

Our stay in The Netherlands has been well aranged by staf of the International Relations Departament -ire P. J. Corzilius, Mrs T. H. den Haan and Mrs J. M. H. van Amelsvoort.

We are grateful to J. B. Claassen family for their solicitude, care and time they spend showing us The Netherlands.

Miroslaw Bober

Marek Florkowski

PREFACE

The Mechanical Engineering Department where we have been during two months long practice is involved in construction of industrial robot arm. The linear robot arm has been developed built and tested. All results are described in literature <4>. There is a new aim now to extend existing construction to the second degree of freedom. Our task was to master existing arm to develop a model of extended one and to study possibilities of controlling it. In the same time work at mechanical design has been done by Gerard Kreffer.

We had to plan our work carefully because of short time of the practice.

1. Time to get to know existing construction 2. Works at model of two dimensional arm

and on software for simulation 3. Developing the hardware 4. Work on software description

1 week

4 weeks 1 week 2 weeks

We attached importance to the simulation program to make it as flexible as possible. It can be used as well for arm response simulation, calculation of extortion as for developing the best control law. That is why we spent four weeks working at it.

It is a pity that we could not verify results - arm is still in a design stage. We compared them with Gerard Kreffer's work however and they were nearly the same.

Time we spent working at hardware was surely too short to develop it in details so we made only the mainframe.

You do not corne across a general introduction to robotics in that book but it is easy to find it in literature.

II

TABLE OF CONTENTS

VARIABLES AND CONSTANTS LIST 1

CONSTANTS VALUES 3

I TWO DIMENSIONAL ARM DYNAMICS

1.1 Equations of dynamic model 4 1.2 Calculation of voltage - matrix notation 8 1.3 Calculation of response in state space . 10 1.4 Response calculation using differential

equations . 11

II SOFTWARE FOR ARM SIMULATION

2.1 Welcome to 'Armcontr' dynamic simulation . 14 2.2 Structure of the program. . 20 2.3 Flags and standard files. • 22 2.4 Simula36 - voltage computation . 23 2.5 Siminv - response simulation . 24 2.6 Po19999 - response simulation #2 . 25 2.7 Task with PID regulator . 25 2.8 Adaptive PID . 29 2.9 Verification . 30

III EXAMPLES

IV

VII

3.1 Option 1 . 31 3.2 Option 2 - path error response 31 3.3 Option 2 - steps response with POL9999 • 31 3.4 Option 3 - checking stability. . 31

PROGRAMS LISTINGS

APPENDIX .

A - *VOLT - Standard files B - *DAT - Standard files C - Plots for example

BIBLIOGRAPHY

III

· 32

• 50

51 55 59

· 71

VARIABLES AND CONSTANTS LIST

,. - means that this variable name is used only in program. corresponding variable name is placed in brackets.

a Ad al , al + t

At A2

A21

A22

b Bd B1

B2 B21

B22

Cl CII

da

dm do

dt D22

Ed Fd hi I Jl JAZ

j 1 1

ke kT Kit

K12

K21

L m Ho ml m2 HL P2 Ql Q4 Ra Rl R2 r1 r2 s S

- angle position state-space matrix in discrete time

- angle position at tl and tl+1 sample time - arm equation coefficient [1.15] - arm equation coefficient [1.30J - arm equation coefficient [1.30] - arm equation coefficient [1.30] - damping coefficient - state-space matrix in discrete time - arm equation coefficient [1.15] - arm equation coefficient [1.30] - arm equation coefficient [1.30] - arm equation coefficient [1.30] - arm equation coefficient [1.15] - arm equation coefficient [1.15] - distance between middle mass point extra load

and axis of rotation - distance between Hz middle mass and of rotation - distance between HI middle mass point and

axis of rotation - sampling time - arm equation coefficient [1.30] - state-space matrix· (Ad) - state space matrix· (Bd) - spindle pitch - current in the motor winding - spindle moment of inertia - moment of inertia of arm with the table on the axis of

rotation - moment of inertia of arm with the table on the motor

axis - constants from equations of moment of inertia - electro-motive force per 1000 r.p.m - torque per ampere - arm equation coefficient [1.15] - arm equation coefficient [1.15] - arm equation coefficient [1.30] - motor inductance - distance between middle mass point and turn axis - arm mass - middle part arm mass - motor mass - extra load mass - gear ratio

matrix of cost function in one dimention matrix of cost function in four dimentions

- motor resistance - matrix of cost function in

matrix of cost function in - linear motor resistance • - angle motor resistance • - linear position

one dimention four dimentions (Ra 1 )

(Ra 2 )

- matrix of linear velocity and acceleration

-- 1 --

§I

91 SO se sel Sl Sw,Ws tdl,2 TN Twc TF 1

Tf2

TJ 1 1

Db UKl Uk1 UK 2

Ukz Uu

DOl

Du2

W

W Wi WO we weI WI

Wm

WHI

WHZ

X X

- linear acceleration at tl sample time - linear velocity at tl sample time - matrix of linear position - linear zero time path error - linear error at sample tl time - linear position at tt sample time - mixed matrices - dynamic friction coefficients· (bl , b2) - torque on the motor axis - static friction torque - moment of friction on linear motor axis - moment of friction on angle motor axis - torque on the linear motor axis caused by linear

acceleration - torque on the linear motor axis caused by angle

acceleration of the spindle - torque on the linear motor axis caused by arm angle

velocity and acceleration - torque on the angle motor axis caused by angle

acceleration variation of moment of inertia - loss of the tension on the brushes - matrix of voltage correction for linear motor - voltage correction for linear motor at tt sample time - matrix of voltage correction for angle motor - voltage correction for angle motor at tl sample time - tension on the motor - matrix of voltage on the first motor - matrix of voltage on the second motor - arm angle velocity - matrix of angle velocity and acceleration - angle acceleration at tl sample time - matrix of angle position - zero time angle error - angle error at tt sample time - angle velocity at tl sample time - motor speed - linear motor speed - angle motor speed - state vector - distance between middle of the arm beam and

axis of rotation.

-- 2 --

CONSTANTS VALUES

CONSTANT UNIT CALCULATED VALUE

da [m] 0.92 dl [m] 0 dm [m] 0.60 do [m] 0.26 dof1 [V] 0.75 dof2 [V] 0.75 h1 [m/rad] 3.98E-3 j [kgm2] 23.83 j1 [ kgm J 0.9 Jl [kgm2 ] 2.384E-3 j2 [kg] 92.36 j2l [m] 0.83 ke 1 [Vs/rad] 0.226 ke 2 [Vs/rad] 0.171 kt1 [Nml A] 0.226 kt2 [Nml A] 0.171 kv 4.89 kv2 5 Ld1 [0 3162 71] Ld2 [0 3160 500] limh1 [V] 40 limh2 [V] 40 liml1 [V] -40 liml2 [V] -40 m1 [kg] 79.2 m2 [kg] 15 ml [kg] 0: 50 p2 97 r1=Rsl [ ohm] 0.4 r2=Rs2 [ohm] 0.93 td1 [Ns] 0.79 td2 [Ns] 0 ts1 [Nm] 0.446 ts2 [Nm] 3.5E-2 ub1 [V] 0 ub2 [V] 0 uofm1 (V] -2 uofm2 [V] -2

Chapter I

TWO DIMENSIONAL ARM DYNAMICS

1.1 EQUATIONS OF DYNAMIC MODEL

To make a dynamic simulation we have to develop equations of the arm in two degrees of freedom. We can do that by writing a torques balance on the motor axes. Motor torque is given by equation

[1.1]

[1.2]

where: TM - torque on the motor axis kT - torque per ampere I - current in the motor winding

R.*I + ke*w. + L*9! =Uu - Ub*sign(Uu) dt

where: Ra - motor resistance ke - electro-motive force per 1000 r.p.m w. - angle speed of the motor L - motors inductance Uu - tension on the motor Ub - loss of the tension on the brushes

The torques can therefore be given as

[1.3] TH = kT * (Uu - Ub*sign(Uu) - ke*w.)/R. [1.1+1.2]

Because of the small value part L*9! can be neglected. Equation [1.3] with subscripts 1 o~t 2 describes torques on the linear and angle motor respectively.

Let us consider the moments of forces caused by friction and acceleration on the linear motor axis.

We have following formulas

[1.4]

[1.5]

TFI =TWCl*sign(~) + bl*~ for the moment of friction

where: TWCI bl

- static friction torque - damping coefficient

TIll = (Mo+ML)*§*hl for the torque caused by linear acceleration

where: - arm mass - extra load mass - spindle pitch

-- 4 --

[1.6]

[1.7]

Tl12 = Jl*WMl=Jl*§/h1 for the torque caused by angle acceleration of the spindle

where: Jl - spindle moment of inertia WMI - linear motor speed

TI13 = (Mo+Ml)*m*w2 *ht angle velocity causes an extra linear acceleration and extra torque.We calculate it as if the arm is a point mass.

where: m - distance between middle mass point and turn axis

w - arm angle velocity Mo - arm mass

m is a function of position and external load mass

[1.8] m=mo (ML) + s

it is easy to find mo (Nt) using a simple model shown on fig. 1.2

[1.9]

where: mt - middle part arm mass m2 - motor mass Mo = m1 + m2 da - distance between middle mass point

of extra load and axis of rotation dm - distance between m2 middle mass and

axis of rotation do - distance between ml middle mass

point and axis of rotation

~ __ M __ L __ ~I~~I------------------------------m_l---------- _______________ ~-----__ __

arm middle I I mass point ) m do I (-) <-->.

da I dm (-------------------------->.(--~------------>

Now we can write

[1.10]

axis I of >.

rotation I

5 --

FIGURE 1.2

We may equate [1.3] and (1.4+1.5+1.6+1.7] to solve for UUl

[1.11]

[1.12]

since

[1.13]

we have

[1.14]

TH 1 = TJ 1 1 + TJ I 2 + TF 1 + TJ I 3

[(Mo+Ml)*hl+Jl/hl]*§+bl*~+TwCl*sign(~)+(Mo+ML)*w2*m*hl = =kTl *(UUI-Ubl *sign(UUl )-ke l*W.l )/Ra1

Uu 1 = f [(Mo +ML ) *hl +J1 Ihl ] I*§+bl *~+Twc 1 *siO'n (~) + + (Mo +ML ) *W2 *m*hl] I *Ra 1 /kT 1 +Ub 1 *sign (UUl ) +ke 1 *~/hl

It is convenient to express the arm dynamic equation as a simple equation which hide the details but show some of the structure.

[1.15]

where: Al = (Mo +ML ) *hl +Jl Ihl B1 = bl *Re 1 /kT 1 +ke I Ihl C1 = (Mo +ML ) *Ra 1 IkT 1 *hl CII = (Ml*da+m1*do-m2*dm)*Ral/kTl*hl K1t = Tw C 1 * Rli 1 I k T 1 K12 = Ubi

Now we calculate torques on the angle motor axis. We begin with moment of momentum conservation formula

[1.16]

[1.17]

dK T = -­dt

K = J * w

so we can express torque caused by angle acceleration an variation of moment of inertia J2 as

[1.18]

where:

dJ = --2 * WM2 dt

J2 moment of inertia of arm with table on motor axis

Moment of inertia of the arm with the table on the axis of rotation is given by formula (taken from <3> )

[1.19] JA2=ML*(x+0.665}2+(x-0.725)2+15*(x-0.935)2+76.36*x2 +12.55

[1.20]

[1.21]

where:

because

x - is a distance between middle of the arm beam and axis of rotation.

x= s + do and assuming do=O we have

Jz = [ML*(s+0.665)2+92.36*s2-29.5*s+26.189]/pi

where: Jz - moment of inertia of arm with table on the motor axis

pz - gear ratio

-- (; --

or

[1.22]

[1.23]

[1.24]

where: jl1 are constants you can easy find comparing [1.21] and [1.22].

dJ dJ ds dJ dt2 * WM2 = ds2 * dt * WM2 = ds2 *~*WM2

g~2 = [2*ML*(S+j2L)+2*j2*S+jl]/pl ds

Moment of friction is given by

[1.25]

Having compared the torques [1.23 + 1.25] we have

[1.26)

on the angle motor

[1.27] (kT 2 IRa 2 ) * (Uu 2 -Ub 2 * sign (Uu 2 ) - ke I *WM 2) ==

axis

= [Ml (S+j2L)2+j2*S2++jl*S+j]/pl**M2 +

[1. 3] and

+ [2*Ml*(S+jll)+2*j2*S+jl]/p~*~*WM2+TwC2*sign(wM2)+b2*WMI

Now we can calculate Uu2 as 2

[1.28] Uu 2 = (Ra 2 I kT 2 ) * { [Ml * (s + j 2 l ) 2 + j 2 * S 2 + j 1 * S + j ] / pa * *M 2 + 2

+ [2*Ml*{S+j2L)+2*j2*S+jlJ/p2*~*WM2+b2*WM2+TwC2*sign{WM2)] +

+ Ub2*sign(Uuz)+ke2*wH2

but

[1.29] WHZ =PZ*W

Now we can write

[1.30] UU2 = A2**+A21***s+A21***S2+B2*W+BJ1*~*w+BIJ*.*~*W+ + K21*sign(Uu2)+D22*sign(*)

where: Az = (Ml*jiL+j)*Ra2/kT2/P2 All! = (2*j2l *ML +jl ) *R. 2/kT 2/P2 A21 = (Ml+j2)*Ral/kT2/P2 BI = b2*Ra2/kT2+kez*p2 Bll = (2*Ml *jll+jl )*Ra2/kT2/P2 B22 = 2*A22 =2* (ML +j2 ) *Ra 2 IkT I/pi K21 = Ub 2

D21 = bl * Ra 2 / kT2

Having equations of the dynamic model [1.15] , [1.30] we can calculate either the voltages necessary to cause arm to follow desired path or response for given vo1tages.Notice that those equations are nonlinear and noncontinuous so we must perform linearisation first or solve as difference equations.

Next two paragraphs of this chapter will deal with that.

'7 --

1.2 CALCULATION OF VOLTAGES - MATRIX NOTATION

Arm control board records position of arm every sampling time so to make equations [1.15] , [1.30] useful for our purpose we perform transformation to discrete time.

[1.31]

[1.32]

[1.33]

[1.34]

where:

Al = (SI-1-SI )/dt

!ill = (AI+1-Al)/dt

Wt = (at+l-al)/dt

WI = (Wi + 1 -WI) /dt

St , St + 1 - linear sample

position at tt and tl + 1

time At - linear velocity at tl sample

time !ill - linear acceleration at tl

sample time dt - sample time at , at + 1 - angle position at tl and tl + 1

sample time respectively WI - angle velocity at ti sample

time WI - angle acceleration at tl

sample time

For convenience we use the following convention :

Xl = x(i) Xi - i-th element of matrix X

SI x N matrix with i rows and n columns

i

51:,i) = [ [] ~ is the i-th column of 5

S(i,:) = ~ l is the i-th row of S LC t , , , , • :JJ i

* ./ element-by-element operations

Q

Let us assume that:

so - is a matrix of linear position WO - is a matrix of angle position

so Wo

= [ SO(l) .•• SO(n) = [ a(l) ..••• a(n)

S - is a matrix of linear velocity and acceleration

S = ~11l •.• t,nD 8(1) .•. I(n)

W - is a matrix of angle velocity and acceleration

W = @Il) ... w,nD w(l) ••. w(l)

w,Ws - are mixed matrices

Sw = rsO(1)*(1)2 •••......• 50(n)*(n)~ l:.(1)2 .......•.......••.. w(nl:J

Ws = 1s0(1)2W(1). ......... sO(n)2*(n)1 ~0(1)W(1) •..•......•• So(n)w(n~

UUI - matrix of voltage on the first motor

Uu 1 = [ Uu 1 (1) ..•....••.•. UU 1 (n) ]

UU2 - matrix of voltage on the second motor

Uu 2 = [ Uu 2 (1) •.....•....• Uu 2 (n) ]

We can write equations [1.15] [1.30] as

[1.35] Uu 1 = [81 ,AI] *5+ [CI ,CI 1 ] *Sw. +

] ]

+ Kll.*sign(S(1,:»+Ub1*sign(UU1)

[1.36] UU2 = [B2,A2]*W+A21.*50.*W(2,:)+Au.*W(l,:)+ + B 2 1 • * S ( 1 , : ) . * W ( 1 , : ) + B 2 2 . * W s ( 2, : ) . * S ( 1, : ) + 02 2 . * sign (UlI 2 )

The last two matrix equations are very convenient for calculations Ul and U2 with MATLA8 (for more information on MATLAB see Appendix A ). We use them in program SIMULA36 which is described in chapter 2.4 . Notice that part Ubl*sign(UUl) cannot change the sign of UU1 (Ubi > 0), so we calculate UUI without loss of voltage on the brushes first, we examine it's sign and we add missing part with proper sign next.

a _

1.3 CALCULATION OF RESPONSE IN STATE SPACE

The inverse problem is to find out a response of the arm. We made two approaches to that problem. First using state space and second with differential equtions.

Let us begin with arm equations

[1.15]

[1.30]

UUl = Al*8+Bl*A+(Cl*W*S+Cll*W) + Kl1*sign(A) + +Kl a *sign (Uu 1 )

Uuz = Az*w+Azl*W*s+Azz*W*S2+Bz*w+B21***w+B2Z*S***w+ +K2*sign(Uuz)+022*sign(.'

We transform it to space discrete time

state like equations and to

[1.37 ]

[1.38J

! = -B!/Al**-[Cl/Al*W*S+Cl1/Al*w]*sign(mo+s) -Kll/A! *sign(A)-K12/Al *sign(Uul )+l/Al*UUl

'II = 1/(A2+A21*S+A2Z*S2)*[-B2-B21**-B22*S**)W­- Kit *sign(Uul )-022*sign(w)+Uu2]

When sampling time is short increase of s *! are small and we can use preceding values of s *! to equation [1.38]. Since the increase of w * are small too we can transform [1.37] in analogical way_

[1.39] til = -Bl/Al**1-[Cl/Al*W'-1*Sl+Cl1/Al*Wl] -

-Kl1/Al*sign(*1 )-K12/Al*sign(UUl )+l/Al*UUl 1 - 1 1-1

[1.40] WI = -1/ (A2 +Aa 1 *SI -1 +Az 2 *sf - 1 ) * [B2 +B2 1 **1 - 1 +

+ B22 *SI- 1 **1 - 1 ) *Wl +Ka 1 *sign (Uu 2 ) +02 2 *sign (.,,) -U1l2 ] 1 - 1 1-1

We received equations that are state equations in discrete time where the state matrices Ad Bd are functions of position and velocities.

[1.41] X = D] [1.42] X = [:] [1.43] Uu = @"D UUI

[1.44] XI" 1 = Ad * Xi + Bd * Uu I .. 1

where: Ad ,Bd state-space matrices in discrete time

[1.45]

Ad =

[1.46]

o

l/Al

[1.47]

U = juUl ~U2J

o

o

1 o

-Bl/Al .*dt+l

o

o

o

That idea is used in program SIMINV36.M to calculate the response of the object.Several tests have been done to check up if the assumptions made are correct.It is described more precisely in chapter [2.9}.

1.4 RESPONSE CALCULATION USING DIFFERENTIAL EQUATIONS

Another approach to the response simulation is to transform equation [1.15] [1.30] directly to discrete time and solve for 6 and w. Advantage of this method is that it is not necessary to make any approximation so it is correct as long as sampling time is relatively short.

-- 11 --

We can expres velocity and acceleration as

[1.48] [1. 49] *1 + 1 = (SI + 1 -SI ) /dt

[1. 50]

[1.51] w. + 1 = (WI + I -WI ) / d t

Having substituted [1.48 - 1.51] to [1.15] [1.30] and taking UUlt+l' and UU2 1 + 1 'as follows

[1.52]

[1.53]

Uu I • t l' = Uu 1 I t 1 - Kl 2 * sign (Uu 1 t .. 1 )

Uu 21 t I' = Uu 2 1+ 1 - Kll I *sign (UU 2 ttl )

we have

[1.54]

[1.55]

[1. 56]

[1.57]

UUlt+l' = {(Al+Bl*dt+Cl*dt2 *wf+l)*Sl+1-Al*(2*Sl-St-l)+

+Bl *dt*st +CII *dt2 *wl + 1 +Kl1 *sign (*1 ) l/dt2

< LUU 21 .. 1 • =

+Bz I * (SI + 1 -SI ) -B2 2 *SI- 1 *St + 1 +B2 2 *SI *SI + 1] *Wl + 1

-Az *Wi -Az 1 *WI *SI" 1 -Az Z *wi*sf + 1 +D2 2 *sign (WI) I/dt

{ [Az +Bll *dt+ (A2 1 +B2 1 ) *SI + 1 + (Az 2 +B2 2 ) *sf + 1 +

<

(AI +Bl *dt+Cl *dtZ *wl + 1 ) *SI + 1 +Cl1 *dt2 *wf + 1 -AI • (2*SI -SI - 1 ) +

+Bl *dt*St - [Uu 1 • -Kll ·sign (*1 )] *dtZ =0 1 .. 1

[A2 +B2 *dt-B2 1 ·SI + (A2 I +Bz 1 -Bll II *SI ) Sa + 1 + (Az 2 +Bz z ) *sf + 1] * I *wl+t-Az*Wt-A21*Wt*SI+1-Azz*Wl*Sl+t

-[UUll • -D22*sign(Wl)]*dt = 0 1 .. 1

We transform [1. 56] [1. 57] to simpler form

[1.58] f(al+bl *wf+t )*SI+1+dl *Wf+l+CI = 0 <

[1.59] La2+b2*SI+1+C2*sf+l )*wl+1+eZ*sl+1+f2*s2*dz = 0

where: al = Al+Bl*dt bl = Cl *dtZ

Cl = At *SI - 1 - (B1 *dt+2*Al ) *51-- (UU 1 • -KIt *sign(.) *dt2

dl = Cl I *dt2 1 .. 1

a2 = A2 +B2 *dt-A2 *51 b2 = A21-Bzz *SI +B21 Cz = A22+Bzz d2 = -Az*wt-(Uu2 '-D2z*sign(wl »*dt

-All 1 *WI 1 + 1 ez = f2 = -A22 *WI

- 1" -

using [1.58] we calculate S1+t assuming that a1+b1*wf + 1 () 0

[1.60]

having substituted [1.60] to [1.59] we can write ( for Wl+t

[1.61]

where: A = 2 b2 c2*dl-bl *b2*dl+ l*a2

B = 2 2 d f2 *dl -bl *dt *e2 +b. * 2

C = (2*Cl *C2 -al *b2 ) *dt +2*al *bl *a2 --bt *bz *Cl

D = (2*Cl *f2 -al *e2 ) *dl -Cl *bl *e2 + +2*al *bt *d2

E = (Ct *C2 -al *b2 ) *Cl +af *a2 F = (Cl *f2 -al *e2 ) *Cl +af *d2

Solving polynomian [1.61] for Wi+l we receive five roots. Four are complex and one is real. Using this one and [1.60] we calculate corresponding value of SI+1.

This is a recurrent formula, and two starting points are necessary to begin with. Those can be either two following positions or position and velocity at time zero.

Chapter 2

SOFTWARE FOR ARM SIMULATION

2.1 WELCOME TO ARMCONTR DYNAMIC SIMULATION

To instal armcontr : 1. instal MATLAB following instructions in PC-Mat lab

user's guide. 2. create new hard disc subdirectory and place file

MATLAB.BAT in it. 3. set proper MS-DOS search path (see guide). 4. make a copy of ARMCONTR diskette under new created

subdirectory.

To run ARMCONTR run matlab first end type armcontr. You have to wait a while until program is compiled ,you can check version then.This description refers to ver. 2.2.

Main menu is displayed and you may choose one option

********************************* THIS program can help you

*********************************

> 1 calculation of voltages that should be applied to motors [ place desired path in data.m or adat.mat, .. ] [ results will be in file volt.mat ]

> 2 calculation of response for given voltaoes lor path error [ place desired voltage in volt.m, avolt.m, •• ] [ results will be in file rdat.mat 1

> 3 RUNS 1 + 2 lor l+PID reoul. [ special option to check stability, OR ] [ to calculate voltages with error correction ] [ both:given and reconstructed path are plotted]

> 4 RUNS 2 + 1 [ special option to check stability 1 [ performs 2 first and applies results to 1 ] [both:oiven and reconstructed voltage are disp.l

-- J4

There is a detailed description of every option:

OPTION 1 Enables you to calculate voltages that should be applied to the motors. As a motor we consider driving amplifier and motor itself. All parameters of arm including amplifier and motor are in PARAH programme Desired path can be taken from standard files ADAT, BDAT, CDAT, DDAT or from program DATA.

In the last case you describe path by function placed in that programm and you can change number of points (usually settled for 100). You can also decide if you want velocity and acceleration to be calculated back or forward (~I = (SI-SI­I )/dt or ~1-1 = (SI-SI-1 )/dt). When you change functions describing path you should choose suboption with path calculation otherwise file DAT is not updated. Four suboptions are possible with / without limitation and with/ without nonlinearities. If limitations are ON calculated voltage is limited so that maximum motor current can not be exceeded. No other correction is made so path error may occur. For correction see option 3 B. If nonlinearities are OFF neither static and dynamic friction nor amplifier offset are taken under consideration.

At the end of this option calculated voltages are green plotted and you can also see red line function showing whether and where limitations were done. Values 1 and -1 correspond to up and down limitation respectively.

Subplots of voltage show: For linear motor:

U10 - voltage for acceleration and dynamic friction UIOF- voltage to minimize offset effect of amplifier 1 UIF - voltage for static friction U1C - voltage for centrifugal effect

For angle motor: U20 - voltage for acceleration and dynamic friction U20F- voltage to minimize offset effect of amplifier 2 U2F - voltage for static friction UIC - voltage for corioli effect

Subplots of voltage are shown without limitation. Voltages Ul, U2 are saved in VOLT file.

IIIIIII

1 2

Block diagram for option 1

if flag4 = 1 DATA calculates back if flag4 = 2 DATA calculates forward else file DAT is NOT updated

number of points = flag8

BDAT

3 4 5 > flag3

< if flag5=1 limit ON

FIGURE 2.1

OPTION 2 A > Enables to calculate response of arm for given

voltages using one of two programs SIMINV36 or POL9999. Voltage can be selected from standard files AVOLT, BVOLT, CVOLT, DVOLT. If limitations are ON voltage from file is limited. Suboption with nonlinearities perform in the same way as in option 1.

B > Helps to calculate response of arm for path error. You will be enquired about value of error and zero time position when error occurred. Real zero time position is zero time position plus error. Choosing suboption with nonlinearities is not recommended because of offset effect. At the and of programm values of error and normalised error are displayed. To normalise error we divide it by value of zero time error. Correction voltage is recorded in file CORRVOLT.

This option is very useful for choice the best regulator.

Block diagram for option 2A

1 2 4 (flagS > 5

if flagS =1 limit is ON flag7 > 1 2

zero time SIMINV36 position POL9999

IIIIIII IIIIIII

Block diagram for option 2B

zero time 1111 J error

I ,..-----V----.

CORRVOLT

zero time position

IIIIII

+/-

POL9999

-- 16 --

FIGURE 2.2

<

FIGURE 2.3

OPTION 3 A > (Without

programs SIMINV or in option

succeed each POL calculates 1. It is useful

PID regulator ) In this option two other. SIMULA calculates voltages and

response. You can choose input file as for:

1

- checking stability and accuracy ( Limitations OFF ). Notice that methods used are in discrete time so there may be some errors when sampling time is relatively long. All step edges should last at least 4 sampling times. Nonlinearities also cause errors because actual velocity sign is estimated. By comparing desired and reconstructed path you can find out if sampling time is short enough. Notice that argument of path functions in standard files is not time but number of sampling time so when sampling time is relatively long you have to create a new file.

- estimating what kind of errors may be caused by voltage limitations ( Limitations ON ). You can evaluate value of error which depends on desired path and limitations In suboption B you can use PID regulator to correct that error.

Block diagram for option 3A

BDAT

2 3 4 5 ) flag3

if flag 5=1 limit is ON

flag7 ) 1 2

zero time SIMINV36 position POL9999

IIIIIII IIIIIII

FIGURE 2.3

- 1 i -

B > ( With PIO regulator ) This option works like 3A but voltage from PIO regulator is added.

It is used for: calculation voltages with limitation with correction.

When limitation causes errors extra voltage from PlO is added to minimize it.

- observing arm behaviour with voltage limitation

- choosing optimal regulator

At the end of program normalised error in time is plotted. Execution lasts about 7 min. for 100 points

Block diagram for option 3B

1

zero time position

2

IIIIII

I

I

BOAT

3

+/-

1 LIMIT

I

POL9999

I I

ROAT

-- 18 --

4 5 > flag3

I CORRVOLT I

C~~~=}- +/-

I

I FIGURE 2.4

OPTION 4 This is another option to check stability_

SIMINV or POL9999 is performed first (input file can be chosen as in option 2 ) and SIMULA follows it.

Block diagram of option 4

flag7> 1 2

SIMINV36 POL9999

FIGURE 2.5

REMARK 1 You can change sampling time in every option. But remember

that standard files are defined in 'number of sampling' domain. So when you increase sampling time 10 times all velocities will increase 10 times and acceleration will increase 100 times.

2.2 STRUCTURE OF THE PROGRAM

Armcontr consists of 38 files. That are: 7 main programs

ARMCONTR.M SIMULA.M SIMINV36.M POL9999.M PARAM.M OATA.M LIMIT.M

3 help programs MAKEVOLT.M FL.M VOLTPLOT.M

1 example program PIOREG.M

8 standard files AVOLT.MAT BVOLT.MAT CVOLT.MAT OVOLT.MAT AOAT.MAT BOAT. MAT COAT.MAT OOAT.MAT

7 current files VOLT.MAT CORRVOLT.MAT OAT.MAT ROAT.MAT JUMP.MAT FLAG.MAT PAR.MAT

12 menus

Short description of each programm you can find at the its beginning.

SIMULA , SIMINV36 , POL9999 are more precisely described in 2.3 , 2.4 , 2.5 respectively.

ARMCONTR is a supervisor, he sets all flags ( see 2.3 ) and saves them in file FLAG.MAT

LIMIT is program that limits voltage. PARAM stores all parameters including PIO regulator. It

creates file PAR every time you run armcontr. DATA stores functions for path simulation. FL enables you to check option you were in. To do that exit

armcontr and type FL , all flags will be displayed. MAKEVOLT help you to create your standard files. You should

place desired function and destination file in it ( using editor) and then run it by typing MAKEVOLT.

Using VOLTPLOT you can plot volts from *volt files. To create *dat file use DATA prg. Insert path function and

destination file to DATA and run it. Do not forget to change back destination file for OAT.

You can also create your supervising program for special purposes ( see example in chapter 2.7 )

-. '?o -

BLOCK DIAGRAM OF ARMCONTR Ver 2.2

•

u..-...... D-.A :rFA_-=!I11111111

• •

if flag4 = 1 DATA calculates back if flag4 = 2 DATA calculates forward else file DAT is NOT updated

number of points = flagS

~ '---_A_~r-AT_--'I ..... 1_B_:Ar--T_-I1 ..... 1_C_:r-AT_ ..... 1 ! ..... _DD...,~_T_-II ~ : 1 .......... 2 .......... ·3 ........... ·4 .......... ·5 .. <'f'i~a~g~3~> .. 9

if flag6 =1 PARAM calcul. with nonlinear

1 2

if flagS =1 limit is ON

flag7 > 1

> SIMINV36

P I D

if flagS =1 limit is ON

zero time position

2

III1III III11I1

• •

CVOLT

4 < flag1 > S

...----v---. CORRVOLT

POL999

>-----------------------~ FIGURE 2.6

if flag9 = 0 PID regulator is OFF.

• • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • -if flag9 = 1 PID regulator is ON , input

between desired path and path reconstructed in if flag9 = 2 PID regulator is ON I input is

in i-1 sampling time.

is the difference i-1 sampl. time. the value of error

- ;{1 --

2.3 FLAGS AND STANDARD FILES

That what makes the program so flexible are flags. Below you will find description of their functions.

FLAG# VALUES DESCRIPTION

flagO option #: 1 option 1 2 option 2 3 option 3 4 option 4

flagl SIMINV36, POL9999 input file #: 1 VOLT 2 AVOLT 3 BVOLT 4 CVOLT S DVOLT 9 create file = 0

flag2 programs connection 0 NO 1 YES

flag3 SIMULA input file I 1 DAT 2 ADAT 3 BDAT 4 CDAT 5 DDAT 9 RDAT

flag4 OATA pointer 1 calculates back 2 calculates forward

flagS VOLTAGE LIMITATION 0 OFF 1 ON

flag6 NONLINEARITIES 0 OFF 1 ON

flag? SIMINV36/POL9999 switch 1 SIMINV36 2 POL9999

flagS NUMBER OF POINTS <500

flag9 PIO options = select PIO input 0 without 1 PID in option 3 2 PIO in option 2

All flags are stored in FLAG file. There is also dt variable in that file it indicates sampling time in milliseconds.

To change the flags without armcontr: CLEAR all variables, LOAD FLAG file, make necessary changes, SAVE FLAG.

Every time you start from main menu all flags are settled. For more details see chapter 4 ( Listings) and 2.4 ,

2.5 , 2.6.

Several standard files are created for your convience. You can make other standard files using VOLTMAKE and DATA. When you do that do not forget about Shanonn formula, maximum velocity and acceleration.

All standard files are generated for 100 points. If you want to work with unusual number of points you must create files of proper size and set flag8 for that number. But be careful with matrix sizes !. All standard files plots are in APPENDIX.

In file DDAT we placed the most difficult condition-maximum velocities accelerations and moments of inertia occurs in sampling time # 100.

2.4 SIMULA - VOLTAGE COMPUTATION

For voltage calculation we use formulas deduced in chapter 1.2. All matrices SO, 5, WO, W, Sw, Ws, are provided by DATA and stored in one of files DAT, ADAT, BDAT, CDAT, DDAT or RDAT ( the last one is made by SIMINV or POL and includes only matrices SO and WOo In that case flag3=9 and SIMULA generates 5, W, Sw, Ws itself). On flag3 depends which of them is used ( see Flags 4.3 ) When proper file is loaded time decoding is performed and all data is plotted. Parameters from PAR file are loaded next and calculations are made. To update perameters you should run PARAM first ( ARMCONTR does it for you ), especially when you have changed option with/without nonlinearities. All results are plotted at the end ( for subplot description see 4.1 option 1) , matrices U1, U2 are stored in VOLT file. Limitations are performed on calculated voltages Ul, U2 if flagS = 1.

-- ~,

2.5 SIMINV36 - RESPONSE SIMULATION

We employed state - space method described in 1.3 One of files *VOLT is loaded ( file # is determined by flag1-see flags description 4.3).

Let us begin with option without PIO regulator (flag9 = 0). If flag5 = 1 loaded voltage is limited. If flag2 = 1 (two programs working together ) proper *OAT is loaded for results comparison and zero time conditions else you are inquired about zero time position velocity and acceleration. Having completed calculation reconstructed path is plotted and comparison is made ( if SIMINV36 succeeded by SIMULA (=> flag2 = 1 and flagO -= 4 ) Reconstructed path is saved to ROAT file.

The deviation while performing with PIO are in arm input voltage :

There are two options with PIO regulator.

>If flag9 = 1 PID output voltages is added to voltages from file *VOLT. Limitations are made on current value of that sum if flag 5 = 1. Input of PID is a difference between reconstructed and desired path. We use it in option 3B.

>If flag9 = 2 PID output voltages is the only input voltage of the arm. Input of PID is real path error. Zero time error is loaded from JUMP file, it contains two constants se, we - for linear and angle error.

Correction voltages are stored in matrices UK1, UK2 they are saved to CORRVOLT file. You can visualize them using VOLTPLOT but you must exit armcontr first.

Plots of normalised errors ( flag9=1 ) or end time errors flag9=2 ) are displayed. It takes 7 minutes to execute SIMINV with regulator and nearly 7 without ( for 100 points ).

-- 24 --

2.6 POL9999 - RESPONSE SIMULATION # 2

For path determination we applied difference equations approach resolved in 1.4 One of files *VOLT is loaded see flags description 2.3).

filet is determined by flag1-

If flagS = 1 loaded voltage is limited. If flag2 = 1 ( two programs working together ) proper *DAT is loaded for results comparison and zero time conditions else you are inquired about zero time position velocity and acceleration. Having completed calculation reconstructed path is plotted and comparison is made

( when POL9999 follows SIMULA ). No option with PID regulator is

it yourself in the analogical way as It takes about 7 minutes to run

2.7 TASK WITH PID REGULATOR

implemented but you can do it is done in SIMINV. this program.

ARMCONTR is perfect model for selecting optimal regulator We did not want to extend it too much that is why we left some adaptations for you. Below you will find some examples and advises how to solve them.

You want to examine on actual position and biggest influence has [1.46])

PROBLEM 1 how optimal regulator parameters depends velocities. Let us speculate that the

s, ~, w (see state space matrices [1.45],

State matrices are computed in SIMINV. It is easy to create a short program ( with a name PIDREG ) that calculates optimal regulator parameters (using built in MATLAB function) lqr).

Now if it is interesting how parameters deviate in real path ( stored in one of standard files *DAT ) you can invoke REG from SIMINV and run option3. All you have to do is to write REG and put invocation line in SIMINV and plot results at the end, of course.

Another approach can be used if precise investigation has to be done. It is convenient to use just a part of SININV then and generate w, W, s in separate supervising program. We made that kind of analysis, because we were very interesting in results.

Listing of developed program and some of results you can find on the next page.

Uk1= Ld(l,:) * E E=[es eJ ew] P I D

Ld = f(Ad IBd ,Q,R) Uk2= Ld(2,:) * E

FIGURE 2.10

%***************************************************************% % * M&M * % % PIDREG.M % % % %***************************************************************%

clear;clc;clg: load par; e=-C1/A1: f=-B1/A1: g=-C11/AI:

Q=[ 1E7 0 o 0 o 0 o 0

R=[1 0;0 1]

si=O; a=1; while a==1, for 11=1:6,

o o o o

wi = (11-1)* 0.3; for i=1:10,

x(i)=i:

0: 0; 0; IE7]

sOi=-.35+.7*(i-1); sg=sign(wO+sOi); h=1/(A2+A21*sOi+A22*sOi*sOi); k=-B21*si-B22*sOi*si-B2; E=[O I 0 O;e*wi*wi*sg f g*wi*sg 0;0 0 h*k 0;0 0 1 0]; F=[O Oi1/A1 0; 0 h;O 0]; [K]=lqr(E,F,Q,R) ; P1(11,i)=K(1,1) ; P2(11,i)=K(1,2) ; P3(11,i)=K(I,3) ; P4(11,i)=K(I,4) ; P5(11,i)=K(2,1) ; P6(11,i)=K(2,2) ; P7 (11, i) =K (2 , 3) : P8(11,i)=K(2,4) ;

end; end;

clc; subp1ot(221) ; plot(x,P1(I,:) ,x,P1(2,:) ,x,PI(3,:) ,x,P1(4,:) ,x,P1(5,:)

,x,Pl(6,:»; title('Linear regul. I coef. '); subplot (222) ; plot (x, P2 (I, : ) , x, P2 (2, : ) , x, P2 (3, : ) , x, P2 (4, : ) , x, P2 (5, : )

,x,P2(6,:»; tit1e('Linear regul. P coef.'}; subplot(223); plot(x,P3(1,:) ,x,P3(2,:) ,x,P3(3,:) ,x,P3(4,:) ,x,P3(S,:)

,x,P3(6,:»; tit1e('Linear regul. D coef.');

subplot(224}; plot {x r P4 (1, : ) r x, P4 (2, : ) , x, P4 (3, : ) , x, P4 (4, : ) , x, P4 (5, : 0)

,x,P4(6,:}); title('Linear out (a) '); pause;clg; subplot(221); plot(x,P5(1,:),x,P5(2,:) ,x,P5(3,:),x,P5(4,:) ,x,P5(5,:)

,x,P5(6,:}); title('Angle regul. I coef.'); subplot(222); plot(x,P6(1,:) ,x,P6(2,:) ,x,P6(3,:) ,x,P6(4,:) ,x,P6(5,:)

, x, P6 (6, : ) ) ; title('Angle regul. P coef.'); subplot(223) ; plot (x, P7 (1, : ) , x, P7 (2, : ) , x, P7 (3, : ) , x, P7 (4, : ) , x, P7 (5, : )

,x,P7(6,:»; title('Angle regul. D coef.'); subplot (224) ; plot (x, P 8 (1, : ) , x, P 8 ( 2, : ) , x, P8 (3, : ) , x, P8 ( 4, : ) , x, PS (5, : )

,x,PS{6,:»; title('Angle at); pause; input(' EXIT YIN', 's'); if ans=='y' ,break;endi end; o AI'H: 1 e () II t. (8 e )

I (

·· .. 0. 1. I-

() 4 t." .... . . .1. ,,I I-

·

·

·

... g .-------01-------' .• (,,1 c: ~ () ,·.,0 .:) 1...------1...-----.....1

oo·. toO (j t.. 1 () "f .1 . .., " ,,\ ,

1 {;. ')n .. J\..."

t-J n ~:ll (0 v ( II!) 3:16:3. ?lB r411~11 f~ ~l

/' ,t"r

/ .'

.'

1()()O ,.' I- ",- -/"'-

".' ,/' "J 1 r ,"J ")") -4 ,.I' -: oot)l ... -".., I~~. J

J

" .. £i ()() ~

.• -""""

// JI"/

.

() ':~ j C j tOO, '')

0 c ~ r .J . J .. ': .. 1 0 c 10 ;.' .. J ,,\

':'7

0

... (). l

-~() .", ' . f:.

... (). ':~ ..

... () .. 4

... (l i:~ I • ...

_.(). 6

LinedI" () u t. ( 11)) 8 Li rlEHH' out(a)

I

' . \ ...... ...... \ ..... ". 6 ~ .

\, .... _--- \ ------- ~\ \'\

\~ \

\,\ .. 4 ~

~ .

\\\~ " \,.... --------- "'1 (: .. f- ", \~, -." ...... '-.... '.,.\..~ ----- -... ,

.-.~ 0 ~ .......

n I., 1,0 0 (;" 10 \( " )

FIGURE 2.8 ABC

EXAMPLE 2

It might be an interesting idea to use a digital filter to protect system from excitation. It allows to make proportional and divergentional action stronger~ We did not have enough time to investigate this topic .

. '

- ~R --

2.8 ADAPTIVE PID REGULATOR

Another interesting concept adjust its parameters while moving necessary is to invoke program like and modify Ldl t Ld2.

is to use regulator and to is in progress. All that is PIDREG ( see 2.7 ) in SIMINV

In that case some additional voltage may occur and cause nonstability.

[ 2.1 ]

so we have

[ 2.2 ]

[ 2.3 ]

[ 2.5 ]

Uk = E * [ kI kp kD ]

where: K = [ kJ kp kD ]T - regulator parameters

E = [Ie e 6 ] - error

d dtUk =

d dtE * K + E *

d dtK

d Up 1 D E * d

dt = dt K

where Up I D is additional voltage caused PIO parameters fluctuation

Up J D = I E * Q K dt dt

by

Let us consider E=const

[ 2.5 ] in time dt short enough to say that

[ 2.6 ] Up 1 D = E * dK

where dK is modification of K in dt time

Having calculated [ 2.6 ] we came to following conclusions:

- parameters of PID regulator should not be changed to quickly

- the bigger error value is the more slower they should change

- as long as we know values of E and dK we can calculate UPID and make proper correction

-- 29 --

--------------------+/-

J E * d dtK dt

s A w converter • s ~ w ) K • • • FIGURE 2.9 • •

This fascinating topic requires more detailed study.

2.9 VERIFICATION

The best way to examine correctness of our job would be work with real arm. Since it has not been ready yet we have to develop another means.

We made several steps to verify results obtained. First of all we wanted to verify mathematical model but the

only thing we could do was comparing our results with Gerard Kreffers labour results. Since there were pretty the same we considered that model is correct. When arm is ready some tests have to be done to prove if this is true. We share our masters opinion that there is usually somewhere a mistake but even so it can be easily corrected. We tried to write that report precisely to help in improvements. Some calculated parameters like static friction are likely inaccurate, some are still unknown (angle amplifier gain ... ). We left place for updated values (page 2).

When model is correct next question is if there is no mistake in solution of [1.15] [1.30] Since POL9999 and SIMINV36 provide us with the same results we are confirmed they are accurate.

Another step was performing option 3 in which two programs SIMULA and SIMINV/POL are run. Functional realised by that two programs following one the other is identity provided that assumptions from chapter 1 are fulfilled. You can check yourself that the difference between input and reconstructed path is reasonable.

At last in one dimensional case we obtained exactly the same results as Leon Pijs ( compare <4». For matrices

Ql = [ o 1E7 0 ] Rl = [ 1 ]

Q4 = [ 1E7 0 0 0; R3 = [ 1 0; 0 0 0 0; 0 1 1 0 0 0 0; 0 0 0 1E7]

Optimal regulator parameters in one dimensional case are Ld1=[ 0 3162 71] while in < 4 > you can find Ld=[ 0 2978 38.3]

-- 30

Chapter III

EXAMPLES

3.1 OPTION 1

We want to verify angle motor that had been chosen.ln order to do that we are interested in voltages on the motor in the most difficult conditions. We choose file DDAT. Sampling time is 10 ms Limitations are selected so that maximum motor current should not be excited. Subption with limitation is chosen.

Computed voltage and input file is shown in appendix B. As one can see there is no limitations so maximum current will not be exceeded.

3.2 OPTION 2 - PATH ERROR RESPONSE

Having computed optimal PID regulator we want to estimate arm response for 0.314 rad angle error. We know that most difficult situation is when arm is in extreme linear position and has maximal linear velocity.External load mass = 50 kg.

We place external load value in PARAM and execute ARMCONTR Values of error and zero time condition are entered from keyboard

Se=O ; we=0.314 ; so=0.35 ; ~0=1 ; §=O ; wo=O ; *0=0;

Parameters of regulator:

3.3

Ld= [0 o

3160 3162

71; 500]

Response of arm is shown in Appendix (page 60).

OPTION 2 - STEPS RESPONSE WITH POL9999

We want to simulate response for voltage steps stored in AVOLT. Suboption without limitations and nonlinearities. Plots of response are placed in Appendix (page62).

3.4 OPTION 3 - CHECKING STABILITY

To check stability of SIMINV we performed option 3 with nonlinearities and limitations. Results are shown in Appendix (page 64).

-- 31 --

Chapter IV

LISTINGS

Armcontr.M . . . .. .. 33 Simula.M · ....... 36 Siminv36.M ....... 39 Po19999.M · ..... 42 Pararn.M · ......... 45 Limit.M · . . .. . . .. . 47 Data.M · . . . . .. . . 47 FL.M .. . . . . . . . . . . 48 Makevolt.M · . . . .. .. 49 Voltplt.M . .. . . . . . 49

-- 32 --

%***************************************************************' % * M&M * , % ARM CON T R. M , % This program is a supervisor for all other programs , % DO NOT RUN THEM SEPARATELY!!!! , % , '***************************************************************' clcjclg; help menumm; while 500 dt=10;flag1=0;flag2=0;flag3=0:flag4=0;flag5=0;

flag6=0;flag7=0;flag8=100;flag9=0: a=10;

while a-=O & a-=1 & a-=2 & a-=3 & a-=4 clc; help menum; a=input('press proper key'):

end; if a==O,clc;break;end; flagO=a;clc; input(' Do you want to change sampling time

(now td=10 ms)? (YIN) ','s'); if ans==' y' ,

dt=input(' ENTER SAMPLING TIME [ms] '); end;

if flagO==3,input(' Do you want to use PID regulator?

if ans=='y',flag9=1:end: end; if a==3 I a==4,flag2=1;else flag2=0;end:

a=10;

(Y IN) ',' s • ) ;

if f1agO==1 : f1agO==3, % OPTION 1 OR 3 while a-=O & a-=1 & a-=2 & a-=3 & a-=4 & a-=5

clc; help menuv; help menuai a=input('choose one option ');

end; if a==1 I a==2,flag5=1;end; if a==1 I a==3,flag6=1;end; if a-=0,a=10;end; while a-=O & a-=1 & a-=2 & a-=3 & a-=4 « a-=5 & a-=9

clc; help menuv; if flag5==1 « flag6==1,help menunl;end; if flag5==1 « f1ag6==0,he1p menul;end; if flag5==0 & flag6==1,help menun;end; if flag5==0 & flag6==0,help menuw;end; help menuf; a=input('choose input file '):

end;

-- 33 --

f1ag3=a; if f1ag3==1,

if a-=O ,a=10;end; while a-=O & a-=1 & a-=2 & a-=3 & a-=4 & a-=5

c1c; help menubf; a=input('choose one option

end; if a==1 if a==2 if a==4

a==4, f1ag4=1;end; a==5, f1ag4=2;end; a==5, while 3

, ) ;

flag8=input('Enter a number of points '); if f1ag8 < 500 & flag8 > 5,break;end; input('ARE YOU CRAZY? ','s'); if ans=='y' & flag8>0 ,break;end

end; end;

end;

if f1agO==3, % OPOTION 3 f1ag1=1; if a-=0,a=10;end;

while a-=O & a-=1 & a-=2 clc; help menuc; a=input('choose one option ');

end; flag7=a;

end; if a-=O,

save flag flagO f1ag1 f1ag2 flag3 flag4 flag5 flag6 f1ag7 flag8 f1ag9 dt;

c1c;c1g; param; if flag4==1 flag4==2,data;end; simu1a; if flagO==3,

if flag7==1,siminv36;end; if f1ag7==2,po19999;end;

end; end;

end; if flagO==2 I

while a-=O & clc; help menuc;

flagO==4, a-=1 & a-=2 & a-=3

a=input('choose one option '); end;

OPTION 2 OR 4

if a==1 I a==3,flag7=1;e1se flag7=2;end; if a==3,flag9=2;se=input('Enter error of linear position [m] '):

we=input('Enter error of angle position [rad] '): save jump se we;end;

if a-=O,a=10;end; while a-=O & a-=1 & a-=2 & a-=3 & a-=4

clc; help menur; help menua; a=input('choose one option ');

end;

-- 34 --

if a==l I a==2 ,flag5=1;end; if a==l : a==3 ,flagG=l;end;

if a-=O,a=lO;end; while a-=O & a~=l & a~=2 & a-=3 & a-=4 & a-=5 & a-=9

clc; if flag9==O,

help menur; if flag5==1,if flagG==l,help menunl;else help menul;end:

else if flagG==l,help menun; else help menuw;end; end; help menufv; a=input('choose one option '); else a=9;end;

end;clc; flagl=a; if flagO==4,flag3=9;end; if a-=O,

save flag flagO flagl flag2 flag3 flag4 flag5 flagG flag7 flagS flag9 dt;

par am; if flag7==1 ,siminv36;end; if flag7==2 ,po19999;end; if flagO==4 ,simula;end;

end; end;

end;

- 35 --

% OPTION 4

%***************************************************************% % * M&M * % % S I M U L A. M % % This program calculates voltages that should be applayed % % to the motors % % % % Parameters of the arm are in program > PARAM.M , % , % Desired PATH is in program > DATA.M , % in matrices SO,S,W,WO % % remember TO RUN data.m after changing path !t!] , , , % Output (which is input voltage) is saved in > VOLT.M , % file in matrices U1,U2 , %***************************************************************' clear; load flag; 01=zeros(1,flagS) ;02=zeros(1,flagS);

if flag3==1 ,load dat;end; if flag3==2 ,load adat;end; if flag3==3 ,load bdat;end; if flag3==4 ,load cdat;end; if flag3==5 ,load ddat;end; if flag3==9 ,load rdat;SO(:,:)=SOR(:,:);WO(:,:)=WOR(:,:);

end;

S ( : , : ) = S R ( : , : ) ; W ( : , : ) =WR ( : , : ) ; Sw=[SO.*W(l,:).*W(l,:);W(l,:).*W(l,:}]; Ws=[SO.*SO.*W(2,:) ;SO.*W(l, :)];

S (1, : ) = S ( 1 , : ) . / d t ; W ( 1 , : ) =W (1, : ) . / d t ; S(2, :)=S(2,:) ./(dt*dt) ;W{2, :)=W(2,:) ./(dt*dt); Ws(l, :)=Ws(l,:) ./(dt*dt) ;Ws(2, :)=Ws(2,:) ./dt; Sw=Sw./(dt*dt) ; if flag3 -=9,

a=l; while a -=0,

clc;clg; help menud; a=input('choose one posibility ... '); if a==O,break;end; if a==S,a=9;b=1;else b=O;end; if a==1Ia==9 ,if b==1,subplot(221);end;

plot(SO');title('Linear path'); if b-=l,pause:end;

end; if a==2Ia==9 ,if b==1,subplot(222);end;

plot(S');title{' Lin.speed & accel.'); if b-=l,pause:end;

end; if a==3Ia==9 ,if b==l,subplot(223);end;

plot(WO');title('Angle path'); if b-=l,pause;end;

end; if a==4Ia==9 ,if b==1,subplot(224) ;end;

end; if a==5Ia==9

end; end;

plot(W');title(' Ang.speed & accel.'); pause;subplot;

polar(WO', (SO+0.5) ');grid; title('Polar plot of the path');pause;end;

-- 36 --

load pari %**************************************************************** % calculation of Ul %**************************************************************** U10=[Bl,Al]*S; U1F=K11.*sign(S(1,:»; U1C=[Cl,C11]*Sw.*sign{wO+SO); U1=(U10+U1C+U1F); U10F=uofm1+dof1*sign(U1); Ul=U1+U10Fi U1=U1./kv; U10=U10./kv; U1C=U1C./kv; U1F=U1F./kv; %**************************************************************** % calculation of U2 %**************************************************************** U20=[B2,A2]*W+A21.*SO.*W(2,:); U2C=A22. *Ws (1 , : ) +B21 . * S (1, : ) . *w (l , : ) +B22. *Ws (2, : ) • * S (1, : ) ; U2F=D22.*sign{W(1,:»; U2=U20+U2C+U2F; U20F=uofm2+dof2*sign(U2); U2=U2+U20Fi U2=U2./kv2; U20=U20./kv2i U2C=U2C./kv2; U2F=U2F./kv2; U20F=U20F./kv2i

if flag5==1 , limit;end; %*******************************~******* clciclgi input('Do you want plots or subplots of voltages? (P/S)

(N=no plots) ',' s ' ) : if ans·='n',

if ans=='p' ,a=l; else a=O;end; for i=1:5,

if i==l plot([Ol;Ul] ');title('Ul')igridiendi if i==2 ,if

a==O,subplot(221) iend;plot(Ul0) ititle('U10'):endi if i==3 ,if

a==O,subplot(222) ;end;plot(U1C);title('U1C');end; if i==4 ,if

a==O,subplot(223) :end;plot(U1F) ;title('U1F');end; if i==5 ,if

a==O,subplot(224) iend;plot(U10F)ititle{'U10F');endi xlabel ( · Time' ) ; ylabel{'Voltage')i if (a-=O & i-=5) i==l, pause:endi

endipause; subplot:

-- 37 --

for i=1:5, if i==l plot([02;U2] ') ;title('U2') ;grid;end; if i==2 ,if

a==O,subplot(221) ;end;plot(U20) ;title{'U20');end; if i==3 ,if

a==O,subplot(222);end;plot(U2F) ;title('U2F');end; if i==4 ,if

a==O,subplot(223);end;plot(U20F);title('U20F');end: if i==5 ,if a==O

subplot(224);end;plot(U2C);title('U2C')iend; xlabel('tirne'); ylabel('voltage'); if a-=O I i==l I i==5, pause;end;

end; subplot;

end; save volt Ul U2;

-- 38 --

%***************************************************************% * M&:M *

S I MIN V 36. M % %

% % % % % % % % % % % % %

This program calculates response of object for given voltages% %

Parameters of the arm are in program

Voltages applaied are in file

outputs ( that are linear and angle: path, velocity,

are saved in file in matrices

> PARAM.M

> VOLT.M

acceleration) > PATH.M > SOR,SR,WOR,WR

% % % % % % % % %

%***************************************************************% cleariclciclgi load flag; if flag1==1 ,load volt;endi if flag1==2 ,load avolt;end; if flag1==3 ,load bvolt:end; if flag1==4 ,load cvoltiend; if f1ag1==5 ,load dvoltiend; if flag1==9 ,U1=zeros(1:flagS} ;U2=zeros(1:f1agS);end; if flag9==2,load jumpiend; load par; if f1ag2==0 &: f1ag5==1 &: f1ag9==0,limit;end; e=-C1/A1; f=-B1/A1; g=-C11/A1; c1c; if f1ag2==1 &: f1ag3-=9,

if flag3==1 ,load dat:end; if f1ag3==2 ,load adat:end; if f1ag3==3 ,load bdat;end; if £lag3==4 ,load cdat;end; if £lag3==5 ,load ddat:end; S(1,:)=S(1,:) ./dt:S(2, :)=S(2,:) ./(dt*dt); W(l,: )=W(1,:) ./dt;W(2,: )=W(2,:).1 (dt*dt); Ws{1, :)=Ws(1,:) ./(dt*dt) ;Ws(2, :)=Ws(2,:) ./dti Sw=Sw./(dt*dt);

WOR(1)=WO(1);SOR(1)=SO(1); WR (1 t 1) =W ( 1,1) ; WR (2,1) =W (2,1) : SR(1,1)=S(1,1);SR(2,1)=S(2,1};

else input('does zero-time position velocity and acceleration = 0 ?

(Y IN) , , , s ' ) ;

end;

ifans=='n', disp('enter t=O position, velocity and acceleration'); WOR(1)=input('angle position = ? '); SOR(1)=input('1inear position = ? '); WR(1,1}=input(' ang1e velocity = ? '); SR(1,1)=input('linear velocity = ? '); WR(2,1)=inputC' ang l e acceleration = ? '); SR(2,l)=input('linear acceleration = ? ');

else SOR(1)=0;SR{1,1)=O;WR(1,1)=0; WR(2,1)=O;SR(2,1)=O;WOR(1)=O:

end; ,

-- 39 --

clg;clc; disp(' SIMINV36 in progress please wait •..

ca.300sek./IOOpoints !'); Ul=Ul.*kv; U2=U2.*kv2; Ul=Ul-uofml; Ul=U1-dofl.*sign(U1); U2=U2-uofm2; U2=U2-dof2.*sign(U2); XI=[SOR(l);SR(l,l);WR(l,l)]; we=we+WOR(l); se=se+SOR(l); for i=2:flag8

wi=WR(l,i-1); % last angle speed wj=WR(2,i-1) ; % last angle acceleration sOi=SOR(i-l); % last linear posision si=SR(l,i-l); % last linear speed h=1/(A2+A21*sOi+A22*sOi*sOi); k=-B2l*si-B22*sOi*si-B2; UI=[U1(i)-Kl1*sign(si);U2(i)-D22*sign(wi)];

if flag9==l, wOi=WOR(i-1); Ukl=-Ld1*[0;sOi-SO(i-l);si-S(l,i-l)]; Uk2=-Ld2*[0:wOi-WO(i-l};wi-W(1,i-l)]; UKl (i) =Ukl; UK2 (i) =Uk2; UI=UI+[Uk1;Uk2] ;

end; if flag9==2,

wOi=WOR(i-l); Uk1=-Ld1*[0;sOi-se:si]; Uk2=-Ld2*[0;wOi-we;wi]: UKl (i) =Ukl; UK2(i)=Uk2; UI=(Ukl;Uk2] ;

end; if flag5==1 & (flag9==1 I f1ag9==2),

Ll=[limll;lim12]; Lh=[limhl;limh2]; UI=UI.*(UI>L1 & UI<Lh)+Ll.*(UI<Ll)+Lh.*(UI>Lh);

end; E=[O 1 O;e*wi*wi f g*wi*wi;O 0 h*k]; F=[O 0;1/A1 0; 0 h]; [ED,FD]=c2d(E,F,dt*lE-3) ; XJ=ED*XI+FD*UI; SOR(i)=XJ(l); SR ( 1 , i ) =XJ ( 2) ; WR (1 , i ) =XJ ( 3) ; SR(2,i)=(SR(1,i)-SR(1,i-1»/dt/1E-3; WR(2,i)=JWR(1,i)-WR(1,i-l)}/dt/1E-3; WOR(i)=WOR(i-l)+(WR(1,i-1)+WR(1,i»/2*dt*lE-3; XI=XJ;

end;

-- 40 --

save corrvolt UKI UK2; save rdat SOR WOR SR WR; if flag2==1 & flagO-=4,

SOS=[SO;SOR] :SSl=[S{l,:) ;SR{l,:)] ;SS2=[S(2,:) :SR{2,:)]: WOS=[WO:WOR] ;WS1=[W{l,:) ;WR(l, :)] ;WS2=[W(2,:) ;WR(2, :)];

else SOS=SOR;SSl=SR{l,:) :SS2=SR(2,:) :WOS=WOR;WS1=WR(1,:);WS2=WR(2,:); end; a=l:

while a-=O; clc;clg: help menui; a=input('choose one posibility '): if a==O,break:end; if a==8,a=9;b=1; else b=O;end: if a==1Ia==9,if b==1,subplot(221);end;

plot(SOS');title('linear path');end: if a==2Ia==9,if b==1,subplot(222);

else if a==9,pause;end:end; plot(SSl');title{'linear vel.'):end;

if a==3Ia==9,if b==l,subplot(223); else if a==9,pause;end:end; plot(SS2'):title(' linear accel.');end;

if b==l, subplot (224) ; polar (WOS ' , (SOS+O. 5) , ) ; grid; title('Polar path');pause:end;

if a==4Ia==9,if b==1,clg;subplot(221): else if a==9,pause;end:end; plot(WOS');title('angle path'};end;

if a==5Ia==9,if b==1,subplot(222); else if a==9,pause;end;end; plot(WSl');title('angle vel.');end:

if a==6ta==9,if b==1,subplot(223); else if a==9,pause;end;end; plot(WS2');title('angle accel.'):end;

if a==7Ia==9,if b==1,subplot(224); else if a==9,pause;end;end; polar(WOS', (SOS+0.5) ');grid; title('Polar path') ;end;

pause;subplot; end:clc; if flag9==l,

input(' Do you want to see plot of normalised error? " 's'); if ans=='y' ,subplot(211) ;plot«SO-SOR)/max(SO»;

title(' Linear error'); subplot(212);plot({WO-WOR)/max(WO»;

title(' Angle error');

end; end; if flag9==2,

pause;

disp(' error of linear position disp(' error of angle position disp(' normalized linear error disp(' normalized angle error disp«we-WOR(lOO»/se);pause;

end;

-- 41 --

=');disp(se-SOR(lOO»; =') :disp{we-WOR(lOO}); =') ;disp«se-SOR{lOO) lise); =' } :

%***************************************************************% % * M&M * % % POL 9 9 9 9. M % % This program calculates response of object for given voltages % % % % Parameters of the arm are in program > PARAM.M % % % % Voltages applaied are in file > VOLT.M % % % % Outputs % % (that are linear and angle: path, velocity, acceleration) % % are saved in file ) PATH.M % % in matrices ) SOR,SR,WOR,WR % % % %***************************************************************% clear; clg;clc; load flag: if flagl==1 if flagl==2 if flagl==3 if flagl==4 if flagl==5

,load ,load ,load ,load ,load

volt;end; avolt;end; bvolt:end; cvolt;end; dvolt;end;

load par;clc; if flag2==1 & flag5==1,limit;end: if flag2==1 & flag3-=9,

if flag3==1 ,load dat;end; if flag3==2 ,load adat:end; if flag3==3 ,load bdat;end; if flag3==4 ,load cdat;end; if flag3==5 ,load ddat;end; S(1,:)=S(1,:) ./dt;S(2,:)=S(2,:) ./(dt*dt); W(1,: )=W(1,:) ./dt;W(2,: )=W(2,:} ./(dt*dt); Ws (1 , : ) =Ws ( 1 , : ) . I (d t * d t) ; Ws ( 2, : ) =Ws ( 2, : ) . / d t; Sw=Sw./(dt*dt); WOR(1)=WO(1);WOR(2)=WO(2); SOR{1)=SO(1);SOR(2)=SO(2); WR(1,1'=W(I,1);WR(1,2)=W(l,2) ; WR ( 2 , 1) =W ( 2 , 1) ; WR ( 2 , 2) =W ( 2 , 2) ; SR(l,l)=S(l,l) ;SR(l,2)=S(1,2); SR(2,1)=S(2,1) ;SR(2,2)=S(2,2);

else input('does zero-time position velocity and acceleration = 0 ?

(YIN) ','s');

end; end;

ifans=='n', disp('enter t=O position, velocity and acceleration'); WOR(l)=input('angle position = ? ');WOR(2,=WOR(1); SOR(l)=input('linear position = ? '):SOR(2)=SOR(1); WR(1,l)=input( 'angle velocity = ? '.) :WR(1,2)=WR(1,1); SR(I,l)=input('linear velocity = ? ');SR(1,2)=SR(1,1); WR(2,l)=input('angle acceleration = ? ');WR(2,2)=WR(2,1); SR(2,1)=input('linear acceleration = ? ');SR(2,2)=SR(2,1):

else WOR(1)=0;SOR(1)=0;WR(1,1)=0;SR(l,1'=0;WR(2,1)=O;SR(2,1)=0; WR(1,2)=O;SOR(2)=0;SR(l,2)=0:WOR(2}=0;WR(2,2)=0;SR(2,2)=0;

-- 42 --

clc;clg; disp ( , POL9999 in progress please wait •••

Ul=Ul. *kv; U2=U2.*kv2: Ul=Ul-uofml; Ul=Ul-dofl.*sign(Ul); U2=U2-uofm2; U2=U2-dof2.*sign(U2); dt=dt*lE-3; al=Al+Bl*dt: bl=Cl*dt*dt; dl=Cll*dt*dt: c2=A22+B22;

ca. 450sek./100points ! ');

for i=2:flag8-1, %*************************************************************** cl=Al*SOR(i-l)-(Bl*dt+2*Al)*SOR(i)-(Ul(i+l)­

-Kll*sign(SR(l,i»)*dt*dt; a2=A2+B2*dt-B21*SOR(i); b2=A21-S0R(i)*B22+B21; d2=-A2*WR(1,i)-(U2(i+l)-D22*sign(WR(1,i»)*dt: e2=-A21*WR(1,i); f2=-A22*WR(1,i); %*************************************************************** A=c2*dl*dl-bl*b2*dl+bl A 2*a2; B=f2*dl*dl-bl*dl*e2+d2*bl A 2; C=(2*cl*c2-al*b2)*dl+2*al*bl*a2-bl*b2*cl; D={2*cl*f2-al*e2)*dl-cl*bl*e2+2*al*bl*d2; E=(cl*c2-al*b2)*cl+al*al*a2; F={cl*f2-al*e2)*cl+al*al*d2: %*************************************************************** r=roots{[A BCD E F]); k=5; while 5

if imag(r(k»==O, WR(l,i+l)=r(k):break;end; k=k-l;

end; SOR(i+l)=-(dl*WR(l,i+l)*WR(l,i+l)+cl)/(al+bl*WR(l,i+l)*

*WR(l,i+l)}; SR(l,i+l)=(SOR(i+l)-SOR(i»/dt; WOR(i+l)=WOR(i)+(WR(1,i+l}+WR(1,i»/2*dt; WR(2,i+l)=(WR(1,i+l)-WR(1,i»/dt; SR(2,i+l)=(SR(l,i+l)-SR(1,i})/dt; end; save rdat SOR SR WR WOR; if flag2==1 & flagO-=4,

SOS=[SO;SOR] ;SS1=[S(1,:) ;SR(l, :)] ;SS2={S(2,:) ;SR(2, :)]; W 0 S = [W 0 ; W 0 R] ; W S 1 = {W ( 1, : ) ; WR ( 1 , : ) ] ; W S 2 = [W ( 2 I : ) ; WR ( .2 , : ) ] ;

else SOS=SOR;SS SR(1,:);SS2=SR(2,:);WOS=WOR;WS1=WR(l,:);WS2=WR(2,:); end;

-- 43 --

a=l; while a-=O;

clc;clg; help menui; a=input('choose one posibility '); if a==O,break;end; if a==8,a=9;b=1; else b=O;end; if a==1Ia==9,if b==1,subplot(221);end;

plot(SOS');title('linear path'):end; if a==2Ia==9,if b==1,subplot(222):

else if a==9,pauseiend:end; plot{SSl') ;title(' linear vel.');end:

if a==3Ia==9,if b==1,subplot(223): else if a==9,pause:end;end: plot(SS2'):title('linear accel.');end:

if b==1,subplot(224) :polar(WOS', (SOS+0.5) ') :grid: title('Polar path') :pause;end:

if a==4Ia==9,if b==1,clg;subplot(221); else if a==9,pause;end;end; plot(WOS'):title('angle path'):end;

if a==5Ia==9,if b==1,subplot(222); else if a==9,pause;end:end; plot(WS1'):title('angle vel.');end;

if a==6Ia==9,if b==1,subplot(223); else if a==9,pause;end;end; plot(WS2') ;title('angle accel.');end:

if a==7Ia==9,if b==1,subplot(224); else if a==9,pause;end;end: polar(WOS', (SOS+0.5) ');grid:title('Polarpath'): end;

pause;subplot; end;

-- 44 --

%***************************************************************% % * M&M * % % PAR A M. M , % , % This program stores all parameters of the arm , % It also calculates coefficients of the equations % % % % Outputs , % are saved in file ) PAR.MAT % % % %***************************************************************%

clear; load flag; % if flag6=1 then with nonlinearities else without

%**************************************************************** % PIO parameters %****************************************************************

Ld1=[0 1000 10] Ld2=[0 10 5];

%**************************************************************** % linear parameters %****************************************************************

J1=1.6*1.49E-3; m1=79.2; m2=15; ml=50; hl=3.98E-3; r1=0.4; kt1=0.226; kv=4.89; td1=flag6*0.79; ts1=flag6*0.446; da=0.92; dl=O; do=O; dm=0.60; uofm1=flag6*(-2}; dof1=flag6*0.75; ub1=0; liml1=-40; limh1=40;

45 --

% td=b % % ts1=Twc ,

%*************************************************************** % angle parameters % % We assume that moment of inertia of the arm is described by eq. % J2=jl*ml*(s+j21}A2+j2*s 4 2+jl*s+j %*************************************************************** j=23.83; jl=O.9; j2=92.36; j21=O.83; jl=1/97; r2=O.93; p2=97; kt2=O.171; kv2=5; ub2=O; uofm2=flag6*(-2) ; dof2=flag6*O.75; td2=flag6*O; ts2=flag6*3.5E-2; lim12=-40; limh2=40; %************************************************************** % coefficients %************************************************************** Al=(Jl/hl+(ml+m2+ml)*hl)*rl/ktl; Bl=ktl/hl+tdl*rl/ktl; Cl=(ml+m2+ml)*hl*rl/ktl; Cll=(ml* (da+dl)+ml*do-m2*dm) *hl*rl/ktl; Kll=tsl*rl/ktl; K12=ubl; wO=(ml*(da+dl)+ml*do-m2*dm)/(ml+m2+ml);

A2=(ml*j21 A 2+j)/p2*r2/kt2; A21=(2*j21*ml+jl)/p2*r2/kt2; A22=(ml+j2)/p2*r2/kt2; B2=(td2*r2/kt2+kt2)*p2; B21=A21; B22=2*A22; K2=ub2 ; D22=ts2*r2/kt2;

save par;

-- 46 --

%***************************************************************% % * M&M * % % LIM I T. M % % % % This program limits voltages in matricies U1 U2 % % to the levels liml1,limh1;liml2,limh2 from > PAR.MAT % % % % % %***************************************************************%

Mlim1=(limh1-liml1) 12; Mlim2=(limh2-limI2)/2; dlim1=(limh1+liml1) 12; dlim2=(limh2+liml2)/2; U1=U1-dlim1; U2=U2-dlim2; 01=[U1>Mlim1)-[U1<-Mliml]; 02=[U2>Mlim2J-[U2<-Mlim2]; 001=[01==0]; 002=[02==0]; U1=001.*U1+01.*Mlim1+dlim1; U2=002.*U2+02.*Mlim2+dlim2; end

%***************************************************** ••••• *****% % * M&M * % % D A T A. M % % % % This program simulates linear and angle path % % and calculates velocity and acceleration % % % % Outputs % % are saved in file > DAT.MAT % % in matrices > SO,WO,S,W % % % % •• *********************************************************.***%

load flag; % if flag4=2 then calculate back,else forward clc;clg; disp(' DATA in progress please wait ...• ca 120s. '); dtn=1E-3; %********************************************************

DATA SIMULATION %******************************************************** for i=1:flag8,

SO(i)=(i>10 & i<16)*(i-10)*0.015+0.075*(i>15); % linear path simulation

WO(i)=-(i>17 & i(26)*0.002*(i-17)-0.016*(i>25); % angle path simulation end;

-- 47 --

for i=2:f1ag8, if f1ag4==2,

else

end

% velocity calculation S{1,i)={50(i)-SO(i-l»/dtn i W(l,i)=(WO(i)-WO(i-l»/dtn; S(l,i-l)=(SO{i)-SO(i-l»/dtn; W(l,i-l)=(WO(i)-WO(i-l»/dtn;end;

if f1ag4==2, S(1,1)=S(1,2);WC1,1)=W(1,2); else

S(1,f1ag8)=5(1,f1ag8-1);W(1,f1ag8)=W(1,flag8-1);end; for i=2:f1ag8, % acceleration calculation

if f1ag4==2, S(2,i)=(5(1,i)-5(1,i-1»/dtn; W(2,i)=(W(1,i)-W(l,i-l})/dtn;

else 5(2,i-l)=(S(1,i)-S(1,i-1»/dtn;

end if f1ag4==2,

W(2,i-l)=(W(1,i)-W(1,i-1»/dtn;end:

S(2,1)=S(2,3) ;S(2,2)=5(2,3); W(2,l)=W(2,3);W(2,2)=W(2,3):

else 5(2,f1ag8-1)=5(2,f1ag8-2);S(2,f1ag8)=S(2,f1ag8-2); W(2,f1ag8-1)=W(2,f1ag8-2);W(2,f1ag8}=W(2,f1ag8-2):end;

%********************************************************* CALCULATION of 5w and Ws

%********************************************************* Sw=[(SO.*(W(l,:) .*W(l, :}»;W(l,:) .*W(l,:)]; Ws=(SO.*50.*W(2,:) ;50.*W(1, :)];

save dat SO S W Sw Ws WO;

%**************************************************************% % *M&M* % % FL % % % % Help program for flags reading % % % %**************************************************************%

load flag; clc; disp ( , disp(f1ag1) ; disp(f1ag2) ; disp(flag3) ; disp(f1ag4); disp(f1ag5); disp(f1ag6) ; disp (f1ag7) ; disp(flag8); disp(f1ag9);

f1ag1 ... f1ag9 = '):

-- 48 --

%***************************************************************' % % % % %

*M&M* MAKEVOLT

Help program to make volts file

% % % % %

%***************************************************************%

for i=l:lOO, Ul(i)=sin(i/20*pi)/2; U2(i)=sin(i/5*pi)/3; end; save bvolt Ul U2; end

%***************************************************************% *M&M*

VOLTPLOT % % % % % %

Help program to make plots of volts files and correction voltage

%***************************************************************%

a='n' ; while a=='n' clc;clg; disp ( , Choose one file number'); disp ( , , ) ; disp ( , vol t-O avolt-l bvolt-2 cvolt-3 dvolt-4

correction volt.-5'); n=input (' '); if n==O,load volt;end; if n==l,load avolt;end; if n==2,load bvolt;end; if n==3,load cvoltiend; if n==4,load dvolt;end; if n==5,load corrvolt;Ul(:,:)=UK1(:,:);U2(:,:)=UK2(:,:);end; subplot(211);plot(Ul);title('Ul');xlabel('Time');

ylabel('Voltage'); subplot(212);plot(U2);title('U2');xlabel('Time');

ylabel('Voltage'); pause; input('Exit YIN ','s'); if ans=='y',break;end; end; end;

-- 49 --

APPENDIX

AVOLT.MAT 51 BVOLT.MAT 52 CVOLT.MAT 53 DVOLT.MAT 54 ADAT.MAT 55 BDAT.MAT 56 CDAT.MAT 57 DDAT.MAT 58 EXAMPLE 1 59 EXAMPLE 2 61 EXAMPLE 3 63 EXAMPLE 4 65

-- 50 --

1-

O. (') .. v (.)

() ') ,.

1 t. ~! ~

t. I

i

d ! O. -4 l- I 9 I

! e I

I 0, I", • I:. ~ • !

0 ()

j

j "

O. .. ., f.

V O. 1 c .. :,i ~

()

1 t. 0, t -(':1

t~ e O. or;'

"i

() n \(

" , ;

j

i I I I , I i I

1 i I I i

I '''10 (: .. ,

J

! I I I I I I I I I I

i

I i I , !

'::) n 1t1l ·n"

Ul I I

AO 60 Ti HIe

I) ,., -- (~ .

.<·10 60 T1 llIe

AVOLT - standard file

Ul=l (i-5) - 1(i-30) U2=1 (i-15)*O.2

-- 51 --

·

--

·

80 to()

·

·

80 100

v o 1 t. a ~:J e

O. 5 ~--.~~~,~,,--~----------,----;~)j~,,-.--~,~--------~,--~--,~~--~ ! \ I \ I \

/ \. / \ 1/ \, I ' 'l \,

"\ /'~ I \ ./ \ ,,/ \ I \

o~ \ f \ I .-\ ! \ I \ I \!

\ / .".1 \./ \, Ii

\ , , , \.. ,/ ••. () 1:-; L.-____ ..I..-_,.,J"'"_'""'-o.,,.'_' _...J-____ ...J.-__ ..... ~~ ____ .l...-___ ~

... "() ~:~ 0 4 0 6 0 80 1 00

Time I 1'-1

(l 4 ~, " r------T,-----~'--~--~~------~,------~

-0, -4 '-----....L:-------'------L....---~~---~ () ~:?O 40 60 ao 100

Time

BVOLT - standard file

Ul = sin(i/20 * pi)/2 UZ = sin(i/5 * pi)/3

-- 52 --

V ()

1 t, (:1 ~;J e

v o 1 t. a 9 e

j, Ut I --- 1 I ,...,.._...r-

,..,--" O. g ~ /'.~

· ",," ",' ,.

/ O. 6

,. ~ / · /

I

O. l~ I

~ / -l f

() 1''1 • f .... ~/ ·

I o ().-------~~:. ..... to----l~""'-()----()""'-O----8 ..... 0--------'1 00

0.8 •

O. 6

0.4 •

Ti me

40 60 Ti me

CVOLT - standard file

Ul = 1 - exp(-i/lO) U2 = i/lOO

-- 53 --

..

..

..

.

80 100

v 0 1 t. a ~.~ e

\J 0 1 t. (":'\

~j e

j ,", ., ('t'•

Ul I

i

t I I

O. 8 i ! i

O. 6 1

-.J II O. 4 I

I () 0'" . (:. J ,

°0 I I

,') tl -40 (.:0 80 100 i ... w \J

T 1 ITI(')

0 II') ",( I..

I I I

-.. 0 t:; . ,. l- .

.n t l- .

j " _ ... , ,,' '- -

''', ."t,' .I-----....J....--.......I:::=====I::::::::===================:J

.- () ~::'O AO 60 80 100 T i ITt (.}

DVOLT - standard file

Ul = O.5*1(1-i) + O.7*1(i-50) U2 = 1(i-30)

-- 54 --

1..1 near' pat, h 0, 01 1"'"'10 ..... ::--------.-....1..----"7""1 0, ~1 1...1 I'}. 8 eed & i:l(~cel.

" /

O. 005 .. \\ 1/

~ ;' -

o !o- -·0, 005 ~ -\ /

\ / -0, OJ, o''-----''-..~-L~/---~

50 tOO

O. 01 :)at h

/ \ \ O. OOS \ /

\ 0 \ ·0.005 I \ I \! \

O. ,.OJ ':.,

O. t ()

-0, 1, / I

() ,''I I - .. ':'" ~-' , -~...'

-·0, 30 50 "

1

o=---

,.. ~ . . . . . . .{

i ;

l f I

-: i - t .~ !

\ /

. \

.. ~{ \" . i i.

:' \ : I I :

J : • I,

100

-0. OJ,o c' 0 lOO .. \

1"1 ~"t:"()'-------S""'O----~1~OO

POlar plot of the path O. 6 .-----------~~----~--------------~ e •••• 9 •••••

. ' "

O. 4 ~ "

. ~ .

-0. 4 " ','

...

. ~ # "' '; .. ' .... ',." ...... ,.; • • ..

" ' "" • I ~

.' ,. : ...

. , ' .. , :

. . : "

, . ,/

",. ~ • a "

t * • ,.. ~. "'"' •• , .. :.. ..

"

. "

.

,

, , , ,

" ..

. . ,

. . ....

'-. .' MMO, 6 I---_____ ._' . ...;. •• ~_:...__~ .• ;.;.. .•. _ •. _____ .....I

ADAT -- 55 --

o f." l.l near' at.t. G. 5 1 .. .1 Yl. 8 P e fHI & ae eel. ,;;I

// {I .... s~t

o. 4 O. 4 l- · O. 3 O. 3 ~

/~ o . ..., o .M, · . .:.. .t:.. ~

0.1 O. 1. - ·

°0 (ill aCCQler I-

50 j.OO 50 100

2 An~H e pat.h '.~J

.. / t: ..

'It.y''~~

./'

1.5 .. /" - 1..5 l- · /"" r"

1 //

1 ".' - l- · /'

O. 5 /' - O. 5 I- -

°0 O() ,\.,'],<, accel . .t

cO ,.~ H)O t" 0 .. l

POlar plot of t.he path t ~-----------.. -.~, ~----~"~"-",-,.-. ---------------,

"'~.;" . • ,t. I., ....... = ...... ~ :t ..

" . '. . . :; . ' .... : ....... , ....... .-•• .;... t • : ·-"'x.~" ... , ".~.." ..

-(), 8 f-

" . "\' '. ."'" ...' ': ," • " •••• ~ ' .. -:". .... .: '. • ".:' " 'oO -

.~ \ " '.

0.6 ..

:". .' '. : '..\ .' ". l ," .. ,,' .. ...". \ •• • o 4 . '. . .... . : . " '-

. ~ / .. ? ", ' ..... .':' ..... '.:' ~'::.' "~."'~' . '/. :,:/ .... .'., .~.:).:\ .. ,.' ...... . ... . O

."" .'.~ .. ~.. ..' .... ,"., ". '. ' ," " ,* .. ~ lOt· •••••••• -:,

.. c.. ~ ,.t ••• f .... : '.'~" • .. '.. '." ~ "~ • ; •• : ...... .. .. ". ,: .. " .... ' • "" ~ . : ~ ....... : ........... /:::::::..-:.>:.:-.. .. :~ ... :.: ...... :: :.

O . " " .':'.::;"('.~'::' ... ~~' : : ~ _ ••••••••• I ••••••••••• t •••• ' •••••• ~: ••• : ••••••••• ' •••••••••••••••••• , ••• , •••

-0. 2. . .. ,' ;:: :<::):EC/,'.'::L. , .!... ,: ..•••.• 4 .... · .~." ,,~ ~~ : : '. " .• 0 '0 .5.: ••.••.

. '.: "..'..... . .. . ..... '

-,·0. 4

. . ,

-0. 8 ~

'. .'

.. . ..

.. . •••••• :' :' : '., "0: .. I' ,., ,... •

.' '. ~ ••• : •• ~ -t .' '. • • ,. ',,, " ... : , •• 1' •• ": '. . . ....

:" •• ~ • ~ ~ 4 •• , ·f· . . ' . . . .

. " ,

. .' .. ' ... _. . ... ~' ....

5 ••• ~ ." ••••••• ~ , • . . '. . ..

• .. ','

' ..

.. .. .

.. ,..:... .. .' -·1 L....-_________ .-..:...O':'--:...._ .... ;.,. .. _ .• _. -----~

BDAT -- 56 -

100

O. 08

O. 06 - I O. 04 -O. 02

°0

0

- (). () () !:l -

.. -0. Ot --.. 0 OJ', . . ,. -

o 0"" - . "'0

Ll nf) aT' pa t. tl j.SO Li n. s pe (H:I & ,

1. ()() ~ ~ e

- e 50 - g

rs

- 0 ~ j .. .. -50

U ... § :: - ::

-100 ... ~ . .... 150 ! . "\0 tOO . .. 0 50 ..

An~Jl e path ""0 Ang. speed &

\ t ..

I E

- 1.0 ... g E

\ E ::.

- () n

i \ - .... to r- II ii

-·~~o 0 f

SO 100 50

POlar plot of the path O. 6 .-------------------------~~~~~------------------------~ I' "

• ,to •

. ' I", to,

. ....

O. 4 • I • ': ••••• '

0' ••• I ., ••

'. . . -

O. 2

, . ' . ,;

. '. . • : ..... : •• ,t •• :"

' .. . . .' .

' . . . ....

...... ' . . .' .' .. '" , .. .' :: ... ' . . .. ' .. "

.... '.; . . . . . . . . '.': ' ........ ". : :: :: .... '.: : ,: . . .... "': .. . : " ... : .... ::::~'::;";/ •. '~ ... :.' ... ,-':..... :

o ~ t ., I 1 ••••••••••• , ••• I , ,', •••• I •• t. I I • I I •••• I • I ......... , •••••• , ••••• --...

.. ~" ." .""."" ::.::' .:": ::.::'F:\:" "::'": ::::". " . " .. ':.

-0. 4

'. . . . . . . . t.: . : . : . . .' ..... . . . . '. ; :./. '.;' .. ~ .. : ..... :-:.' ". ' . .-

.. ,

.. ' .'

. . . .

. . . :

. : ' .. ..

' .

." I' ,0 '0 •

" • 'I ••

, •••• ~. " ••• t" •

. . . , . '.

"

',' ..

. . . -. ...

. '. -·0.6 L.....-______ •• _ •• ...;.,.:...:.-.....:..._ ..... .;.., ••• _ .• _. -----.......

CDAT -- 57 --

aceel.

-

. -

100

accel.

.

-

100

O. 35 r-__ ~I.~ .. 1~n~e~d~r'~p~i~3t~,I~·I ____ ~ 10 Li Fl. <, ,) eecl 8- decal.

0.34 - 8 u\( ,acE i

I I

6 I

f O. 33

4 . i O. 32 - -

.n" I t:~ I

"uV!.~

°0 50

O. 31 ~ -

O.30~------~~------~J~ 50 tOO

L S eed & ac eel. ~ O\l'1(?'Q t1 ('~(-t;;:>'\."")f

O. 6

O. 4

O 0") .r:..

100

POlar plOt of the path 1 r-----------------------~----___ ~----------------------------~ ....... ~.. . . .. ~ .. " : .. , ....

O. 8

O. G

O. 4

O • ., .r:..

o

·_·0 ~,

'-0,4

-0. 6

-0. 8

'. "

. . . ~

. ' . .

•• :.' • " ., .... : .... " I ~ ., : ... . " " .

.. .. .." .... .': .. '.. .. ~.. . " :., .. I ... ~ ~ 4 .. " • " • ",

, , . , . . . . " .... ...... . .. .. .. ' ....

,. :~ ...... t f " , •••• , .... i • ...

. ". ..

., ..

. .

, , \ . '

" , .

" .. ,," . .

" '

. . ..

1 . It ... ; ..... , ....... f •

- ~------------~~~--~.~. ------------~ DDAT

-- 58 --

v o 1 t a 9 e

V 0 1 t a 9 e

V 0 1 t a 9 e

U1 t 2 .---------r------.------.....-------.----

J ,i i

.. .. " ,f .. 0 .., J J.. .. .. , ..... I ........ " .. I .... , It, .. """" t t .... I'"" I ....... t ...... " .. t ........ t .. , ......... ~ I .... I ...... ., I"'" t .... "''''''' I .... " I""",,,,,,,,,,,,, .. . . . i

I 8 . . . " i

............. " ............... ,," •• " ............ " f' ." ..... " •• "." ................. , .. , ~ ....... , ~" .............................. , ......... " ......... '$' t ..... ..

. . . . i

f i I 6 ..... " .... t ... I ~ ..... I ..... I ; ....... 11 .. 1 ...... f .............. ; • t ...... t ...... t ...... , .......... .. ", .................................... ~ .............. , .... I . ..... I ... ..

: : : : f j

. ! 4 .. .. .. ..:

••••• ~ •• * ••• ~ .... ~ •••• ~~ ••••• ~.'.~ ..................... ~ ••• ,. .... " •••• "' ..... ~" •••••••• " ........... { .......... .

: : : : ~1 .. .. .. ":

J "", . . . .: ,,"u ...•......... " ..... : ...................... : ...................... : .. " .•... ~ .• " ...•• ~ .... ~ ......... ! ............ .

! !

°0~------~~~(~)------~~====~~~==~8~O~~--~ 40 60

1""/0 ,:.. lHO

15

10

5

°0 t:i 0 ., tOO Time

O. 05 UH~

0

-0 0'1 . "0 100

Time

V 0 1 t a 9 e

v o 1

O. 4

O. ~J

(). ~?

O. 1-

°0

Ui.C

50 Ti me

100

Uj.OF O. 05 ....... ..;..------;;;..;;-~-----,

t O~------------------~ a 9 e

-·0, 050L--------="=----~1~OO SO

v o 1 t a 9 e

v 0 1 t a 9 e

v o 1 t a 9 e

" t.. ••

10r---------.---------,---------~--------_T--------~ : :

7

6

5

<4

3

2

/ ~) ~ ••.•...•••.•••.•••. ;.. • ••.•...•••.••••• I •.••..•••.•.•.•.•.•• ' •••••.•••.•••.•••.••• ' ••••.•••••••.••••• 0(. -• • • 1 · ~ " ,~

~ • '0 .. ,I 8 1- •..•••.•...•••.•.•. : ••...•.•••..•..•••.. : •.•....••••. ..•••..• : .•••••••.•••.•••..•• : •.•.•••.•••.•.. J ...•

: t·. : /

I • I ") . . . . i , ............................................................. , .................... , ............ / ...... .

. . . . j t

• j 6 "* ........ , ...... r •• , ... ; It'''''' I.,. ~. r 1'1"' f. '" i. i •••• t ••• , •• I. II!"":'" *. f .. " ... II' t ....... Ii .. 11 • .:..,~.,.. •••••• , ...... . .. . .. .. ....,.-.. ,..,.­.......... . . . . ""-"'" 5 - ... " . , . ,. .... t ..... t ..... : ~ •••• , •• t " • \I •• tit •••• : • , .... i .... , ••• , ••• t • • : • ••• ;..~_~ ••••••••••• : ......... , ... , • , •••• I •••

._"" _..-r": .-

4 .. . -.-~. . ...................................................... -:-. ................................................... -.. ..... .. :..-.......... ~ ,. .. -- . --3 + ._,.... • •

'\. ........ ~ ............................ .,..," ~ ...... " ~ ... ' ........... 5 ~ " ... ~ .......... , ......... " ................. ~ .. #" ..... t ............... ~ .. ~ .. _

."..-.~ . . .. ~~ ..

~- . _.,. .. .. ..

2' _ .. ~ ....... ".,.:-~'7 ........... : ............. , • * .... I .... : .. ~ ......... ~ ••• , ••••••••• " ... , ....... , •• , ,. •••• ~ ......... , •••• I ..... .

_..,.,r- . . .. .

1 . . . .

_ .. ,.,.,. t., •• .o.o • .o ••• !" i''' ••• '' •• " ........ : ••• ,.' ~ •••• .o" ......... ~ .... .o • .o, •• .o ..... ,,. t ••• : ... .o •• , ••••• t.o.,. 1 ••••

°0~------~2~()---------4~iO--------~6~O--------~8~iO--------1~.OO

t i !lie

v o 1

U~!F O. 05 r------..;....;;.y . ..;......-----__.

t 0 1-------------1 a 9 e

to 50 100 -0. 05 0~---""":5::-':0:-----1:-=OO

t 1 ITI(-) t. i nlf~

O. 05 U'';IOF' to..

o ~------------------~

-0. 050~------~5~O~-------1~OO

time

v o l t a 9 e

60 --

U2C 4 ~--------~----------__.

3 I-

'.

°O~------..... ~~l~----..... --~ 50 100 time

6 e linear path O. 3 ~ ~------~~~~----~

O. 36 n O. 355

O. 35 -O.3450·--------~--------~ 50 tOO

o r

I ·-20 --40 -

- 6 () O~----~l..L.· O-----j--l, () 0

EXAMPLE 2

linear vel. 1----=...:...:...;.::...:~.....:..~:..-.-----.

O. 5

o V

-G. 50 50 100

.. ' -1 1....-__ ' • ....;,. :.:.... . .....;...: -...:.'._' '-' --"

-- 61 --

O 4 Jdth . ~----~~~--~----~

t.. O. 3

O. ~2

0.1

1 c· « « ~\ \

01 I \

'I \ O. 5

'0"',,',,' 'Pro ldz- [0 . ~~GG'S . ~.("',,;lJ

50 100 50 100

200 angle accel. Polar pat.h t ~~~.~.~.:~.~~.~ .. ~.~ __ . .. . : "

o ~ ____ --------------~

150

100 ·

50

.. ' ~". ". ". : ;' :.'. .': .... O. 5 !>-', ' •••••• ~.,: •. ' " ."-/, '" .. :;.:.': ...... : :' .... :: < ... ' .' . ':.

. " .. .' '.:: .... : ...... , .... \ O

..... : " r~'1 " • • . • • \ ~1i"-"' •••••• L:, ....... ; ••••• I ..

·,·0 5 :',: :::\::·}/rc>/··::: ::~ .. ~~ . . ~ ."". . ' '" '"' .. " .

• • :' ._ ,," : e. '. • ••••

--

-

t • ,,: ••

-~o ~--------~--------~ •. 0- ~lO 1 ()O -j, ~---'-'.~:.~:~"~"-'--~

-- 62 --

EXAMPLE 3 O 03 1 1 ne a r' pat. h lIne a r "81

, .: r---......;;.....:.....;,..;,.,;;....;;;~.r---L....::...:....:..:..---, (), 1 r--------"r----' --..,

/'-----< / 0.02 .. -

O. 025 f-

/ 0,015.. /

0, 03. ~

0.005 e /

o -' o

---

50 1.00

f! r--:-__ 1_1 ...:..n;.;;.e...;;(":l;.:..r-rd:..::(;...::C...::t~~l..:., __ _

1

o

-1.

.'~ -~O-~----~----~ 50 lOO

(-­O. 08 - I I O. 06 - I \ O. 04 ~ \

O. 02 ~

-·

·

· () _~/ ___ ~~_--~ o 50 100

O. 6 r---~P~O~l~dr~'~F~)d~t~,I~l __ -, .. : •. I,. : : .... : .

0,: ... :'>.:::::.\:: /,: ... .< .::::-" .. ~ o. t;.. ~ .'.. '.M·".'.." -

'. ' .. ':" ,"., :.';:. ::::::.,_. t •• :' ....... .

• • ." • :,' -", ' •• t." • I o I-" t •••••••• ", •• : r., ... ,", .. "" .J.

-0.2,··::::':-:,:»Ft<:;:/,,:) -0. -4 ':' .. :"!"~ to .....

'.:' ", .'

.- O. 6 1.....-__ • '-' • ....;., •• 0...-: .............. '-' .----

-- til --

O. 1r---~~~~~~ __ __

O. 08

O. 06

O. 04

O. 02

°o·~~----~c.·~o--------~ •. l 100

O. 4

O. 3

O .'') .c..

O. 1

°O~------~( .. ~O----~~d .:' 100

O. 15 r--__ ..:d:..:,f.:.,:;l.iI.:· l::...;e=--.:..v .::,H.:,.l..;.. ___ ....,

O. 1.

O. 05

50 100

-0, 6 1...-__ ' '..;.:.' :..;..,.' .....:.:......., . .;.;. .• -,.----'

-- 64 --

v ()

1 t a fJ e

(), 6

O. A

o . .. I.~ ...

o

.. ·0. 2

.. ·0. 4

-0. 6

-0. 8

u·l

-1. (', (..

EXAMPLE 4

1J1 I I I . ·

" ...

.~,-I'-",-

I . "-.~

''''\ ........ 1- ................... ; .. , ............ . · . , . . .. ~ ...... , ............ -. ~ .......... .

,;:r .., ,

......... , .... "'~.,. ............. .. '\

'. " ...

I- ..................... . , , .. ........... , .. ~ ..................... ". ~ ...................... ~ .............................. ~ .... -- .... .

'. \"

'.

'. \ " . . , r,' •

_"f'tJ"II"II'"II'!II~.tl.tllltlll"'l'~".I'IIII'!1tlltt' .......... , •• , •• ,., •••• t! •••••• " ••• , ••• , ••••

f \

I • ~. ~ ~ , .... , , t ........ t •••• : • 1 • If. t , •• 'I ••• ! •• 1 •• : ••• , ••••• , .r f t , •• I f II ': • I , • t •• , t , , • ~ •••• I • " : I •• t I 11 • t ••• t ••• 1 ••••

1-- •••••• ,.~., •••••••• , ••• , •••• t ....................... L .....• , ............................................. . . • 'f"

, "" ................... ; .................... ; .... ,:/ .............. ~ ........ ' ........... ; ............... ' ... . ", .

- . , '''''',' ~ ... ~ .. , .. , ...... ! . , .... , .... , ..... + ' • :.,~ ,

• J

.. . , .

.' . . , /,,,

· . . * ••••••• ., , ..... : ' It ••••• ' ............ , , .: • t 1 • It' , ..... I , .. , .... ..

•• _ .t "I .1.. to r I

I' I'"

tHJ I

80 100

65

l~ _____ t~}j~.() ______ ~

V () t." • ~ t-

o 1 t. o t-

.

T i ITlt)

O .) U1F • L. r------...;;..;;p;I~--__.

v O. 1 ~ ()

1 t. () ~ cl ~J t~ -·0. 1. ~

I I

.

--

() I'~

- • t.'. ()-----~ ..... i O----j ....... 00

TilT! (.)

-- 66 --

1 \ l

v o 1 t !I a 1 C .. ·J 0 t." I • .,\ t

e \ I \ I

() \., ,~ 0 100

IJI0F .... 0. ~:,!. ....-----.;;..;~----

() ,., V .... ..;) I-

o I · 1 t .... 0. -4 - · ':1

9 () c e .... .~) -- ·

m·O. 60"-----...... ------'

50 100 Ti me

lJ ()

1 t. (1

fJ e

O. 4 I , I J ,

,r..... ,v. ....

I .

o. t"J * t "'-.. ,r",,, .:" ,.. •• f • , I • t • , t t • t 1 • t t t • t , j • t t t t , '~\ , , .'~ t l fl' J r t t " f •• , , •• , , •• 1 , • , , , , • t t • , I •• '.~ " • t~',' f • t • ., ••• I tit t " I • I •• t j • " ••

}t. :'" "~"t t

.f , / •

\ ' ! ,

o ,

,< ! I

" "( '- • ,: ~ t

I ~ :~.

/ ~# : \~. ~ \~ I

: .: l .*, r- . -, ............ ~ .. , I ~ t', •........... , .. , . t't ' ••• , ••••••• , ••.••• \ . .f . ..•• t ••••••• " , .... \.~ •• , ••• , ••••••• , ., •• · . .. '\ , , . \ , . . , I • : It :

•• • t . , ~ : I

.. ·0. 4 . ; . ! ~ I ' \

_., fl" •••••• a , ....... : .: ••••• , .,,. .... , ••• I: , • : .~u a • J 1 • "' ••• I • I • t ... , I', .L, i • , '1 j 'II ••••• " I ..... J .•... J • '*' ..... ,..,.. t. , ~ t : t : t ;, . .' : .: · I · ~ } • ~ • ; f t

-0. 6 I 1 I ; j ! 1

_ •••• f • I , , ••••• 1 • I , , !} ..• I I , •••••• I , ••• I.: J ;:" ••• t, • I I • I • , • I ":..i I' i 1 t. It'" ; •• t t J •• : • i~,' •••••• ,. , ,t, •••••

-""W ~ " .; 1

-0. 8

-1

\, J""~ f... f " \-., * l ;:. f

, t 1,1 • '\.; ~ ~ •• , t Lt. • \ J • ...,

- ~ ... -'\ • ~ ~ • ~ .•• ~ . ~ .,'1 . ; .................... ~ ..... ; ~ , ••• \~ ........ ~ ..... -/ ... ~ ............................. " ............ : ... '0- ...... " ..... :1_ ",.. ,~ ",.

• \0 t \. I

\ \ i \ J "" " "I \ '-.. i

\ t \, / ., t \ t 1 .. ,

, ., , ........ '!'-•.• '.-~ .. • ,. ~ ..... ,. .... , .. "1' .. ;,.:~ . .. .. I···.. . ...... ~ ... , ...... t • ,,,,.' .... ," .......................... t' " •••••• ~, • or;'" , .... . I.... ... ' .. ","-

i i

40 60 80 100

time

67

u o 1 t. a ~J e

v

O.5.-_______ U~2-0------~ I

(\ (\ I \ } \ I \ l \

0. I \ I \ -\,/ \/ \\!

\ • . !

\) \) \) -0 Ii ""------------.....1 . ,.. 0 ('''0 ,) tOO

t. 1 HI~}

-0. :~ - -

(). 04 r:=:::;-_--;:::::=l=J ~!r' F_-;:::==:::;--,

v o,02l In. o 1 t

~ I I .

I • I • !

e(), 0;:: ~ U .. " O. ().4 L..,.!;;';;;;:;;;'::_--"':=::::::'_.....!;;'--;;;;j

o 50 100

IJ

\' 10··{} to" (

time

U '-/(' ,'u I

I

0-0. 35 r- - 0 1 t · .. 0. 4 ~ a ~J ··-0, 4!':' ~ e

.. ·0 " '-,t " r

I 1

1 - t

C1

- ~J e

-

II Ii

... () L~ t." . ,) ,.1 () ___ '--_ .... ~...L:\ 0---1_-'_....J

t 00

t. i IT! (0)

-- 68 --

f' ..... } '--___ --a. ____ ....1

\. 0 ~\O

time 100

0.015 r-___ l_l_n_e~a,r\~p~a~t~h----, I

O. 01.

O. 005

o -

_.(). ()OS ~ .

1 i ne a \" ace e 1 . t ~""'~~~~~':i~~~""'~

O. 5

o ~

. ~ ~ ::

!( .' ,. " " ,. -. . ~~ ..

;..~ ~. ~

-

? . "' ,/ '. / -.

~t \~ . .,./.' '~ ...... "::: .. -.: ..

() t.oo - .) ~--------~----------~ ... 0 (,';0

\. lOO

-- F,q --

(). () 6 .-----___ l_i_n_e_d..,.r_v_e_l_. ___ --,

O. 04 ..

o ~ , () () '''' \ uo. ."

. t ... ~ '. \

uuO. 04 f- '\ l '"', , ... ~" .

.

.... 0. 06 1....-___ ............... ___ --'

o 50 100

Polar path O. 6 ................... -........ ~~.-............ - ...

I • • : : I' ••

• '. I II • O. 4 ~ .... ' . .'. ": .. '. . .... • .' • .,' • I"' " • ,

" to ••• ' ,

,~, " ..... : ......... : .: .. ,' ........ '. O. I... .. •••• ".' •• • • '. .. •••• It. ." ••

•• '0' • ., .' : o •• ' :. '.' to' .' •••••• . ., .. ',.. .., O . '.' r....... . .

t--I • I '" •••••••• ; II; ••• : ••••• ~ ... _

. : ...... ':.. ...... :: ~;: :: . .'.::.' ... '.: ... . ... 0 f~'" • • ., ••••• " to I II

o I ... ~ ,'.... :': ':.: "0··. '0". i-.,. I" • ,,' • t • to • '! ', .. '

. ',' . ' , '.'. . ···0. 4 ~ ': .:··.-··i···,····. '\.' •

"0 t" • °1

,0

-·0. 6 '----. '-' :-: -AooO.'. -" -' --

o. ();:~ r---~-----'.....J.-""";"';"-----,

O. Ol~i ~, :2...-t-.... :'

f .. / : ./ \.. !

ll'l, '.~ j J~ /1\\ ;1\

\'" Ii \' \. fJt' \ \\ /1 \. / 1'. " i' \ \ ; • ". .. , I

\ .... 1/1 \ .... I \\,1 I \ I \ I \, l

O. Ot

O. O()~i

()

-0. OOf.i . . \ l

() OJ f. I ,.

"- . . " Ol.....--.......:..----~"---j.....l. 0 0

o '"'

. i'

!

\ .

\ ) ,

" . \tJ+ ;;

~,O

f

. , .

J

J

, 1 , , \ I,

~

. ,

--

?­: .

" . " .. ","

:!

too

- 70 --

'" O. 1. \ , '.

O. OS l\

\

····0. OS

o \ l. ,. 1\ : \" :

\\'1/' p' 1 ••

\ .... .... (), t S 10.... ____ "'-___ ___

o 50 100

"'O:. ..., . ~

-O.6~--·-'·~:~-·-··-·--~

BIBLIOGRAPHY

1. ROBOT ANALYSIS AND CONTROL H.Asada, J.-J. E. Slotine A Wiley Interscience publication 1986

ISBN 0-471-83029-1

2. INTRODUCTION TO ROBOTICS MECHANICS & CONTROL J. J. Craig Addison-Wesley Publishing Company 1986

ISBN 0-201-10326-5

3. Gerard Keffers works (still not printed)

4. OPTIMALE BAANPOSITIONERING VAN EEN LINEAIRE ROBOT ARM J.M.L. Pijls Afstudeenvers1ag WPA 0047 1987

5. ADAPTIVE FRICTION COMPENSATION IN DC MOTOR DRIVERS I.E.E.E. Int Cant. Robotics and Automation, San Francisco 1986

6. DIGITAL CONTROL SYSTEM Rolf Isermann Springer - Verlag 1981

ISBN )-540-10728-2

7. AUTOMATIC CONTROL SYSTEMS Benjamin C.KUO Prentice - Hall , INC 1975

ISBN 0-13-054973-8