Dynamic symulation for two dimensional robot arm

Dynamic symulation for two dimensional robot arm

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). Technische Universiteit Eindhoven.

Document status and date: Published: 01/01/1987

Document Version:

Publisher’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 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.

DYNAMIC SYHULATION

FOR TWO DIMENSIONAL ROBOT ARM

Miroslaw Bober

Marek Florkowski

Stanislaw Staszic Uniwersity

Cracow, POLAND

Coach: ir. P.C.Mulders

September - October 1987

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

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 i t 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 i t in details so we made only the mainframe.

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

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

--§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.

--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 [ kgmJ _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 ₉₇ 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

--[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

dt

2 * WM2

=

ds

2 *

dt

* WM2

=

ds

2 *~*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.

-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

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 i t ' s sign and we add missing part with proper sign next.

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 i t 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.

--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 ₁+₁ '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 *dt 2 *wl + 1 +Kl1 *sign (*1 ) l/dt 2

<

L

UU 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 *dt}2 _{*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 *dt 2 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

=

c2*dl-bl *b2*dl+ l*a2 2 b2

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

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

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

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 ) / d t ) . 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

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

-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 i t ( using editor) and then run i t 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 )

-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

-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

All flags are stored in FLAG file. There is also dt variable in that file i t 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 i t s e l f ) . 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 i t 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 ).

--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 i t yourself in the analogical way as It takes about 7 minutes to run

2.7 TASK WITH PID REGULATOR

implemented but you can do i t is done in SIMINV.

this program.

ARMCONTR is perfect model for selecting optimal regulator We did not want to extend i t 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) l q r ) .

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 .:)

oo·. toO (j

1...---1...---...1

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

_;.'

_{.. J}

~

r

.J . J .. ': .. 1

0

c _,,\

₁₀

':'7

0

...

().

_l

-~()

.",

'

.

f:.

...

(). ':~

..

... () ..

4

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

_.(). 6

LinedI" () u

t.

( 11))

8

Li

rlEHH' I

out(a)

' .

\

...

_...

\

... ".

6

~

.

\,

....

_---

\

---

~\

\ ' \ \~ \

\,\

..

₄

_~

~

.

\\\~

"

_\,....

---

"'1 (:

..

f- ", \~,

-."

...

'-....

'.,.\..~

---

-...

,

.-.~

0

~ ...

n

I.,

_1,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 .

.

'

-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 dt Uk = dtE d

*

K + E

*

d _dtK d Up 1 D E

_*

d dt = dtK

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

--

---+/-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 i t

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]

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).

--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 k e y ' ) : 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;

--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;

--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;

--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:

--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;

--%***************************************************************%

*

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(' ang 1e 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); =' } :