Upload
dangkhanh
View
224
Download
0
Embed Size (px)
Citation preview
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-SII )/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