You are on page 1of 50

APPENDIX A:

Development of PR Manipulator's Geared Dynamical Equations


Using Lagrange's Equation

This appendix proves through analysis that low gearing substantially reduces the dynamics of a
motor's external load. It also shows that low gearing uncouples link dynamics in the PR
manipulator. Lagrange's equation applied to the PR manipulator and an example presented by
Ogata (1980), pp 130-131 are used to support this claim. Ogata's example is applied to the
manipulator's translation link and is presented in Figure A.1

N6

Tmotor N1
TLoad
θ1, V1, J1 N3 θ4, V4, J4

θ2, V2, J2 Belt


N2

N5
N4 θ3, V3, J3

Figure A.1: Motor gearing example

where

NT [equivalent gearing for all stages.]


Jeq [equivalent motor shaft inertia ]
Veq [equivalent viscous friction ]
J1 [motor's armature shaft inertia]
J4 [external inertia]
Tmotor [torque delivered by motor]
TLoad [external opposing torque from load and mechanism's
Coulomb and static friction]
D1 [motor's viscous friction constant]
D4 [load's viscous friction constant]
θ1 [motor's angular displacement]
θ4 [load's angular displacement]
N1 - N6 [number of teeth for each gear]

The torque equation for the first shaft (the motor shaft) is:

Tmotor = J 1θ1 + D1θ1 + T1 (A.1 )

124
where T1 is the load torque on gear 1 due to the rest of the gear-train. For the second shaft, the
torque equation is:

T2 = J 2θ2 + D2θ2 + T3 (A.2)

where T2 is the torque transmitted to gear 2 and T3 is the load torque on gear 3 due to the rest of
the gear-train. Since the work done by gear 1 is equal to that of gear 2 then:

T1θ 1 = T2θ 2
and

N 1θ 1 = N 2θ 2
θ 2 N1
=
θ 1 N2
T2 θ 1 N 2
= =
T1 θ 2 N1

N2
T2 = T1 (A.3)
N1

For the third shaft:

T4 = J 3θ3 + D3θ3 + T5 (A.4)

and

N4
T4 = T3 (A.5)
N3

For the fourth shaft:

T6 = J 4θ4 + D4θ4 + TL (A.6)

and

N6
T6 = T5 (A.7)
N5
If equations (A.1-A.7) are back substituted into one another then:
(
N
) NN
( ) NN N
( )
Tmotor = J 1θ1 + D1θ1 + 1 J 2θ2 + D2θ2 + 1 3 J 3θ3 + D3θv3 + 1 3 5 J 4θ4 + D4θ4 + TL
N2 N2N4 N2 N4 N6

125
Equation (A.8)

By writing all of the angles in terms of θ1 , or:

N5 N N
θ4 = θ3 , θ 3 = 3 , θ 2 = 1 θ1
N6 N4 N2
N N N
θ 4 = 1 3 5 θ1
N2 N4 N6
then equation (A.8) becomes:

  N 
2
NN 
2
 N1 N 3 N 5 
2

Tmotor =  J 1 +  1
 J 2 +  1 3  J 3 +   J 4 θ1
  N2   N2N4   N2N4 N6  
 
  N1 
2
 N1 N 3 
2
 N1 N 3 N 5 
2


+ D1 +   D2 +   D3 +   D4 θ1 (A.9)
  N2   N2N4   N2N4 N6  
 
NN N
+ 1 3 5 TL
N2N4 N6

The total gear ratio is rewritten as:

N N N 
NT =  1 3 5  (A.10)
 N2 N4 N6 

If each gear pair in NT has a smaller numerator than denominator, then NT will be a number
much less than unity. For this study, low gearing refers to a gear ratio that has a denominator
much greater than its numerator. By using Equation (A.10), then equation (A.9) becomes:

Tmotor = J eqθ + Deqθ + N T TLoad (A.11)


where

2 2 2
N  N N  N N N 
J eq = J1 +  1  J 2 +  1 3  J 3 +  1 3 5  J 4 (A.12)
 N2   N2 N4   N2 N4 N6 

2 2 2
N  N N  N N N 
Deq = D1 +  1  D2 +  1 3  D3 +  1 3 5  D4 (A.13)
 N2   N2 N4   N2 N4 N6 
If the gear-train damping and inertia terms are taken to be zero, then equation (A.11) becomes:

Tmotor = (J 1 + N T2 J 4 )θ1 + (D1 + N T2 D4 )θ1 + N T TL (A.14)

126
Equation (A.14) illustrates the dramatic effect gearing has on external inertial and torque loads.
If NT results in a small ratio, then the inertial and damping effects associated with the load can be
assumed negligible. The magnitude of the torque load TLoad , which is comprised of constant
friction terms that are generated in the mechanism as well as the torque developed from the
opposing load, is also reduced by NT. Thus, if sufficiently low gearing is applied to a motor, the
external inertial, viscous, and torque terms become insignificant. The only inertial and viscous
effects that the motor must overcome are internal to itself (J1 and D1 ).

Now consider solving the dynamical equations for the PR manipulator. The generalized
equation given by Craig (1989) is:

τ = M (θ )θ + V (θ , θ) + F (θ) (A.15)

where τ is the applied torque or force, θ is the position vector, M(θ) is the symmetric positive
definite mass/inertia matrix, V (θ , θ) represents the centrifugal and Coriolis forces, and
F (θ) represents terms due to friction effects. The derivation of the dynamic equations begins by
diagramming the system. The friction matrix in (A.15) is neglected for this diagram.

Body 3
l1
Body 1
Y
l2 m3
m1
X
A
θ m2,l1

τ2 τ1
Body 2

xOA

Figure A.2: Dynamics diagram of PR manipulator

The terms and variables are defined as:

θ [rotation angle]
xOA [translation displacement]
m1 [mass of length 1]
m2 [mass of length 2]
m3 [point mass of the non-rotating frame holder]
l1 [length of the rotating link]
l2 [distance to rotating link's center of gravity ]
IZZ2 [moment of inertia (l2)]
τ1 [torque applied to rotation link]
τ2 [force applied to translation link]

127
Stoten's (1992) algorithm was followed to determined the manipulator's dynamics. It is
presented as follows:

1) Determine the absolute velocity of each link.

2) Determine the total kinetic energy T, and potential energy, U in the machine. Form the
Lagrangian:

L= T-U (A.16)

3) Use the results of (A.15) in Lagrange's equation.

d  ∂L  ∂L
− = τi
dt  ∂x i  ∂xi
(A.17)

where xi is the generalized co-ordinate of each degree of freedom and τi is the generalized
external force applied at each co-ordinate xi .

The system operates in the X-Y plane, so gravity is not a factor, thus energy as related to the
potential U is eliminated from the Lagrangian. Kinetic energy equations are developed for each
body as labeled in Figure A.2.

Body 1 (Translational Link):

1 1 1
k1 = m1 x12 + I ZZ 1θ12 + m1 y12
2 2 2

where

y1 = 0
θ = 0
1

x1 = xOA

then

1
k1 = m1 xOA
2
(A.18)
2

Body 2 (Rotational Link):

1 1 1
k2 = m2 x 22 + I ZZ 2θ22 + m2 y 22
2 2 2

128
where

θ = θ2
x 2 = xOA + l 2 cos θ
y 2 = l 2 sin θ
x 2 = xOA − l2 sin θθ
y = l cos θθ
1 2

then

k2 =
1
2
( 1
) 1
(
m2 xOA − l2 sin θθ + m2 l 2 cos θθ + I ZZ 2θ 2
2

2
2

2
) (A.19)

Body 3 (Point mass):

k3 =
1
2
( 1
)
m3 xOA − l1 sin θθ + m3 l1 cos θθ
2

2
( )2
(A.20)

Thus, the total kinetic energy for the system is:

T = k1 + k 2 + k 3 =
1 2  1
( 2 1 ) (
2 1 2 
 2 m1 xOA  +  2 m2 xOA − l 2 sin θθ + 2 m2 l 2 cos θθ + 2 I ZZ 2θ  +) (A.21)

1
( )
2 1 (
 2 m3 xOA − l1 sin θθ + 2 m3 l1 cos θθ 
 2 )
and the Lagrangian is:

1
L = T − U = T =  m1 xOA 2 

1
(2 1
)
2 1
( 
+  m2 xOA − l2 sin θθ + m2 l 2 cos θθ + I ZZ 2θ 2  + )
2  2 2 2 
(A.22)
1
( )
2 1 (  2
 2 m3 x OA − l1 sin θθ + 2 m3 l1 cos θθ  )
Applying Lagrange's equation to each link produces:

d  ∂L  ∂L
− = τ1 (rotation torque) (A.23)
dt  ∂θ  ∂θ

d  ∂L  ∂L
− =τ2
dt  ∂xOA  ∂xOA
(translation force) (A.24)

129
where L is defined by equation (A.22).

Next consider gearing the displacement variables and writing them in terms of:

θ = N T1θ1 xOA = N T2 rθ 2
θ = N T1θ1 xOA = N T2 rθ2
θ = N T1θ1 xOA = N T2 rθ2

where θ1 is the angular position of the rotation motor, θ2 is the angular position of the
translation motor, N T(1, 2 ) are the respective gear ratios, and r is the translation link's equivalent
radius. Also consider gearing the torque and force terms in (A.23) and (A.24) and writing them
as:

τ g1
τ1 =
N T1
τ g2
τ2 =
N T2 r

where the subscript g stands for the torque generated by the motor.

Taking the partial and time derivatives of equation (A.22) with respect to θ = N T1θ1 yields:

∂L

∂N T1θ1
( )
= − m2 l 2 N T2 rθ2 sin (N T1θ1 ) + m2 l 22 (sin (N T1θ1 )) N T1θ1 + m2 l 22 (cos(N T1θ1 )) N T1θ1 + I ZZ 2 N T1θ1
2 2

( ) ( ) ( (
− m3l1 N T2 rθ2 sin N T1θ1 + m3l12 sin N T1θ1 )) N
2
θ + m3l12 (cos(N T1θ1 )) N T1θ1
T1 1
2

Equation (A.25)

and

130
d  ∂L 

dt  ∂N T1θ 

( ) ( )
 = −m2 l 2 N T2 rθ2 sin(N T1θ 1 )− m2 l 2 N T2 rθ2 (cos(N T1θ 1 ))N T1θ1 + m2 l 2 (sin (N T1θ 1 )) N T1θ1 +
2 2

( ) ( )
m2 l 22 2(sin(N T1θ 1 )cos(N T1θ 1 )) N T1θ1 − 2m2 l22 (sin(N T1θ 1 )cos(N T1θ 1 )) N T1θ1 + m2 l 22 (cos(N T1θ 1 )) N T1θ1 +
2

I N θ − m l (N rθ )sin (N θ )− m l (N rθ )(cos(N θ ))(N θ )+ m l (sin (N θ )) (N θ )+ 2 2


ZZ 2 T1 1 3 1 T2 2 T1 1 3 1 T2 2 T1 1 T1 1 3 1 T1 1 T1 1

m l 2(sin (N θ )cos(N θ ))(N θ )+ m l (cos(N θ )) (N θ )− 2m l (cos(N θ )sin (N θ ))(N θ )


2 2 2 2
3 1 T1 1 T1 1 T1 1 3 1 T1 1 T1 1 3 1 T1 1 T1 1 T1 1

Equation (A.26)

and

∂L
∂N T1θ1
( ) ( )
= − m2 N T2 rθ2 l2 (cos(N T1θ1 )) N T1θ1 + m2 l 22 (sin (N T1θ1 )cos(N T1θ1 )) N T1θ1 ( )2

( ( ) ( ))( ) (
− m2 l22 sin N T1θ1 cos N T1θ1 N T1θ1 − m3 N T2 rθ2 l1 cos N T1θ1 N T1θ1
2
) ( ( ))( ) (A.27)

+ m3l12 (sin(N θ )cos(N θ ))(N θ )


T1 1 T1 1 T1 1
2
− m3l12 (sin(N θ )cos(N θ ))(N θ )
T1 1 T1 1 T1 1
2

Substituting equations (A.26) and (A.27) into (A.23) yields:

τ g1
N T1
( ) ( ( )
= m2 l22 N T1 + m3l12 N T1 + I ZZ 2 N T1 θ1 + − m2 l 2 N T2 r sin N T1θ1 - m3l1 N T2 r sin N T1θ1 θ2 ( ))

Equation (A.28)

which represents the torque applied to joint A in Figure A.2.

Taking the partial and time derivatives of equation (A.22) with respect to xOA = N T2 rθ2 yields:

∂L

∂N T2 rθ 2
( ( )) (
= m1 N T2 rθ2 + m2 N T2 rθ2 − l 2 sin (N T1θ1 ) N T1θ1 + m3 N T2 rθ2 − l1 sin (N T1θ1 ) N T1θ1 ( ))
or

∂L
= (m1 + m2 + m3 )N T2 rθ2 − (sin (N T1θ1 ))(l1m3 + l2 m2 )N T1θ1 (A.28)
∂N T2 rθ2

131
d  ∂L 
 [ ( ) (
 = − m2 l2 sin N T1θ1 − m3l1 sin N T1θ1 N T1θ1 + (m1 + m2 + m3 ) N T2 rθ2
dt  ∂N T2 rθ2 
)]( ) ( )
(A.29)
( ( )
− m2 l 2 cos N T1θ1 + m3l1 cos N T1θ1 N T1θ1 ( ))( )
2

∂L
=0 (A.30)
∂xOA

Substituting equations (A.29) and (A.30) into (A.24) yields:

τ g1
N T2 r
[ ( ) ( )]( ) (
= − m2 l 2 N T1 sin N T1θ1 − m3l1 N T1 sin N T1θ1 θ1 + m1 N T2 r + m2 N T2 r + m3 N T2 r θ2 )( )
( ( )
− m2 l 2 N T21 cos N T1θ1 + m3l1 N T21 cos N T1θ1 θ1 ( ))( ) 2

Equation (A.31)

which represents the force applied to the translation link. The system presented in matrix form is:

τ g1  
=
(
m2 l22 + m3l12 + I ZZ 2 N T21 )( ) ( ( ) ( ))( )
− m2 l 2 sin N T1θ 1 + m3l1 sin N T1θ 1 N T1 N T2 r  θ1 
τ  
( ( )
 g 2  − m2 l 2 sin N T1θ 1 + m3l1 sin N T1θ 1 N T1 N T2 r ( ))( ) (
(m1 + m2 + m3 ) N T2 r 2 )    
 θ 2 
 0 
+
( ( )
 − m2 l2 cos N T1θ 1 + m3l1 cos N T1θ 1 θ 1
 ( ))( ) (N
2 2
T1 )
N T2 r 

Equation (A.32)

Equation (A.32) represents (after gearing) the torque each motor sees due to the PR manipulator's
link dynamics.

If the generalized equation of links dynamics for a robotic manipulator is expressed as:

τ = M (θ )θ + V (θ , θ) + F (θ)

then a friction vector similar to then one above could be added to equation (A.32). Gearing
would apply to the friction matrix similar to the method used in equation (A.14) where the TL
term is considered to include a constant Coulomb friction term. However, for this study it is
assumed that the links are frictionless, and friction terms are not included in the dynamically
equations. In Chapter 2 of this thesis, de Wit (1991) et al. claims that this is a reasonable
assumption, and presents a generalized motor friction model that accounts for external joint
friction.

132
In summary, it is obvious that equation (A.32) is dynamically coupled and highly nonlinear.
However, low gearing and proper material selection uncouples the system and substantially
reduces its nonlinearities. Thus, from equations (A.32) and (A.14) it can be seen that low
gearing eliminates the mechanism's dynamics, and leaves only the dynamics of the actuators for
consideration.

133
APPENDIX B

Derivation Of Brushed Commutated PM d.c. Motor Equations

The derivation of the electro-mechanical equation that describes the dynamics of a simple, non-
geared, PM brush commutated d.c. motor is developed in this appendix. Only viscous and
Coulombic friction are considered in the model. In Chapter 2, static friction, loading and gearing
are added to the results generated here.

The electrical equation of a PM d.c. motor is derived from the simple motor circuit illustrated in
Figure B.1 (Electro-Craft, 1980).

R L

Vapplied Iarmature Vb
Tg,ω

Figure B.1: PM d.c. brush commutated motor model

where

V= applied voltage Vb = Induced back emf voltage


I= armature current Tg = Internally generated torque
L= armature winding inductance ω = Motor output speed
R= armature resistance

The electrical relation between these variables is given by:

dI
V = L + RI + Vb (B.1)
dt
where Vb , the internally generated voltage, is proportional to the motor velocity ω and is given
by:

Vb = Kbω (B.2)

The motor back emf constant, Kb, is a measure of the voltage per unit speed generated when the
rotor is turning. The magnitude and polarity of Kb are functions of the shaft angular velocity, ω,
and direction of rotation respectively. Combining the above equations produces:

134
dI
V = L + RI + Kbω (B.3)
dt

which is known as the electrical equation of a d.c. motor.

The electro-mechanical equation of a motor is described by:

T g = Kt I (B.4)

where Tg is the total internally generated torque a motor must deliver in order to overcome
opposing torques. Kt is the motor torque constant which is a measure of the torque per unit
current produced by the motor. In a PM d.c. motor, torque is a linear function of the motor
current, I.

The following terms describe the motor's mechanical properties used in Tg:

J1 [motor's moment of inertia]


Tc [Coulomb, or constant friction torque in the motor
which is a function of polarity]
D1 [motor internal viscous friction (damping) which is a
function of the motor's velocity and polarity]

The opposing torque seen by the motor due to Coulomb and viscous friction is given by:

Topp = Tc sgn(ω) + D1ω (B.5)

Torque associated with rotating the motor's armature is:


Tarm = J1 (B.6)
dt

Combining equations (B.4-B.6) produces:


Tg = J1 + Tc sgn(ω ) + D1ω = Kt I (B.7)
dt

Equation (B.7) describes the mechanical dynamics of a d.c. motor. It neglects the static friction
or "stiction" term, and does not model disturbances or resonant modes of motor couplings.

After describing a motor in terms of its electrical and mechanical characteristics, the next step is
to combine them to form a transfer function. To simplify the derivation, nonlinear Coulomb
friction is not considered.

The derivation begins by considering the motor equations in terms of their Laplace transforms.

135
V(s)=(sL + R) I(s) + Kb ω(s) (B.8)

Tg(s) = Kt I(s) (B.9)

Tg(s) = (J1)sω(s) + D1ω(s) (B.10)

Combining equations (B.9) and (B.10) produces an expression for current:

1
I ( s) =
Kt
[sJ1 + D1 ]ω ( s) (B.11)

Combining (B.11) and (B.8) forms:

1
V ( s) =
Kt
[( sL + R)( sJ1 + D1 )]ω ( s) + Kbω ( s) (B.12)

Finally, the corresponding transfer function is:

ω ( s) Kt
Gm = = (B.13)
V ( s) [( sL + R)( sJ1 + D1 ) + Kb Kt ]

The transfer function for iron core permanent magnet DC motors has two real, negative poles.
These poles are the roots of the characteristic equation:

s2LJ1 + s(LD1 + RJ1) + RD1 + Kb Kt = 0 (B.14)

The motor transfer function can be written in terms of time constants τ1 and τ2 or

1 / Kb
Gm = (B.15)
[( sτ 1 + 1)( sτ 2 + 1)]
where the time constants are related to the poles of (B.15) by:

1 1
τ1 = − and τ2 = − (B.16)
p1 p2

The poles of (B.14) are:

( LD1 + RJ1 ) − 4 LJ1 ( RD1 + Kb Kt )


2
− ( LD1 + RJ1 ) + −
p1,2 = (B.17)
2 LJ1

136
In most PM d.c. motors, the inductance, L, and the viscous damping, D1, values are small
relative to the other terms in equation (B.17). If their product, LD1 ≈ 0, is considered to be true,
then the term under the radical in (B.17) is always greater than zero. Thus, the poles are negative
real, and are reduced to:

 (1 − 2 LKb Kt )
− ( RJ1 ) + RJ1  
 R 2 J1 
p1 =
2 LJ1

or
Kb Kt
p1 = − (B.18)
RJ1

and

 (1 − 2 LKb Kt ) 
− ( RJ1 ) − RJ1  
 R 2 J1  −2 RJ1
p2 = ≈
2 LJ1 2 LJ1
or
R
p2 = − (B.19)
L

By using the equations in (B.16), the motor's mechanical and electrical time constants can be
stated in terms of motor parameters:

RJ
τ1 = τ m = (Mechanical time constant) (B.20)
Kb Kt

L
τ2 =τe = (Electrical time constant) (B.21)
R

If τ2 is one tenth the magnitude of τ1 , then the motor can be modeled with good success as a
first order system. The transfer function written in terms of velocity for such a system is:

1 / Kb
Gm ( s) = (B.22)
( sτ m + 1)

137
APPENDIX C

Physical Dimensions and Parameters of the Model and Prototype Systems

1. Model System Data

The Physical Dimensions of the model system are listed in Table C.1. Refer to Figure
A.2 in Appendix A for manipulator orientation.

Table C.1: Model's physical dimensions

Variable Description Value Units


l1 Length of rotational link 6.9 in
l2 Length to l1 c.g. 3.45 in
xOC Distance from XY origin to
overhead camera position 17.6 in
xOA Length of translational link
in home position 10.7 in
m1 Mass of link 1 8.586e-2 (oz-s2)/in
m2 Mass of link 2 4.662e-3 (oz-s2)/in
m3 Mass of frame holder 1.165e-2 (oz-s2)/in
IZZ2 Moment of inertia for link 2 1.855e-2 oz-in-s2

The model's motor parameters are listed in Table C.2 (a) and (b). Most of the parameters are
nominal values taking from the manufacturer's data sheets. The values are experimentally
verified in Chapter 5 and restated here in tables C.2 (c) and (d).

Table C.2 (a): Model's (rotation) motor nominal parameters

Variable Description Value Units


J rotor inertia 3.9e-4 oz-in-s2
R armature resistance 8.4 Ohms
L inductance 6.17e-3 H
Kt torque constant 5.6 (oz-in)/A
Kb back emf constant .03953 (V-s)/rad
D1 viscous damping 1.07927e-4 (oz-in)/(rad/s)
Tc Coulomb friction .5376 oz-in
Ts static friction .8064 oz-in
Vref rated voltage 24 volts
Is peak current 5.6 amps
NT gear ratio 127.78:1 none

(Note: that an additional gear box with a ratio of 15:1 was coupled to this motor.)

138
Table C.2 (b): Model's (translation) motor nominal parameters

Variable Description Value Units


J rotor inertia 3.9e-4 oz-in-s2
R armature resistance 8.4 Ohms
L inductance 6.17e-3 H
Kt torque constant 5.6 (oz-in)/A
Kb back emf constant .03953 (V-s)/rad
D1 viscous damping 1.07927e-4 (oz-in)/(rad/s)
Tc Coulomb friction .5376 oz-in
Ts static friction .8064 oz-in
Vref rated voltage 24 volts
Is peak current 2.88 amps
NT gear ratio 127.78:1 none
r radius .23622 in.
Wt weight w/o gearhead 8.98 oz.

Frame size: Length= 2.203 in (56 mm) ; diameter=1.58 in (40.13 mm);


Shaft length 2.815 in (71.50 mm)

Table C.2 (c): Model's (rotation) motor experimentally verified parameters

Variable Description Value Units


J rotor inertia 3.9e-4 oz-in-s2
R armature resistance 8.5 Ohms
L inductance 6.17e-3 H
Kt torque constant 5.5984 (oz-in)
Kb back emf constant .03953 (V-s)/rad
D1 viscous damping 1.07927e-4 (oz-in)/(rad/s)
KD damping constant .0260 (oz-in)/(rad/s)
Tc Coulomb friction .56 oz-in
Ts static friction 1.5811 oz-in
Vref rated voltage 24 volts
Is break-out current .2824 amps
IP peak current 2.88 amps
NT gear ratio 1916.7:1 none
τm mechanical time constant 15.0 ms
τe electrical time constant .7258 ms
p1 1/τm 68.1351 Hz
p2 1/τe 1377.6 Hz
Kcr rotational angle sensor gain .26 rad/v

139
Table C.2 (d): Model's (translation) motor experimentally verified parameters

Variable Description Value Units


J rotor inertia 3.9e-4 oz-in-s2
R armature resistance 8.4 Ohms
L inductance 6.17e-3 H
Kt torque constant 5.6224 (oz-in)/A
Kb back emf constant .0397 (V-s)/rad
D1 viscous damping 1.07927e-4 (oz-in)/(rad/s)
KD damping constant .0266 (oz-in)/(rad/s)
Tc Coulomb friction .5376 oz-in
Ts static friction 1.4667 oz-in
Vref rated voltage 24 volts
Is break-out current .2609 amps
IP peak current 2.88 amps
NT gear ratio (translation link) 127.78:1 none
τm mechanical time constant 14.7 ms
τe electrical time constant .73452 ms
p1 1/τm 66.7581 Hz
p2 1/τe 1361.4 Hz
Kct translation sensor gain .7591 in./v

2. Prototype System Data

The Physical Dimensions of the prototype system are listed in Table C.3. Refer to Figure A.1 in
Appendix A for orientation.

Table C.3: Prototype's physical dimensions (Scale is roughly .2567:1)

Variable Description Value Units


l1 Length of rotational link 1.77165 in
l2 Length to l1 c.g. .88 in
xOC Distance from XY origin to
overhead camera position 5.118 in
xOA Length of translational link
in home position 3.34645 in
m1 Mass of link 1 .0114 (oz-s2)/in
m2 Mass of link 2 .0018 (oz-s2)/in
m3 Mass of frame holder .0016 (oz-s2)/in
IZZ2 Moment of inertia for link 2 .0064 oz-in-s2

The prototype's motor parameters are listed in Table C.4 (a) and (b). Most of the parameters are
nominal values taking from the manufacturer's data sheets. The values are experimentally
verified in Chapter 5 and restated in tables C.4 (c) and (d).

140
Table C.4 (a): Prototype's (Rotation) motor nominal parameters

Variable Description Value Units


-5
J rotor inertia 8.3711e oz-in-s2
R armature resistance 23.25 Ohms
L inductance (not measured)
Kt torque constant 3.6819 (oz-in)/A
Kb back emf constant .026 (V-s)/rad
D1 viscous damping 3.131e-5 (oz-in)/(rad/s)
Tc Coulomb friction .228 oz-in
Ts static friction .31 oz-in
Vref rated voltage 24 volts
Is peak current .915 amps
NT gear ratio 420.3 none

Table C.4 (b): Prototype's (Translation) motor nominal parameters

Variable Description Value Units


-5
J rotor inertia 8.3711e oz-in-s2
R armature resistance 23.25 Ohms
L inductance (not measured)
Kt torque constant 3.6819 (oz-in)/A
Kb back emf constant .026 (V-s)/rad
D1 viscous damping 3.131e-5 (oz-in)/(rad/s)
Tc Coulomb friction .228 oz-in
Ts static friction .31 oz-in
Vref rated voltage 24 volts
Is peak current .915 amps
NT gear ratio 420.3 none
r radius .334645 in.
Wt weight w gearhead 4.4 oz.

Frame size: Length= 32.5 mm; diameter=27.5 mm. Shaft length 51.0 mm

Table C.4 (c): Prototype's (Rotation) motor experimentally verified parameters

Variable Description Value Units


-5
J rotor inertia 8.3711e oz-in-s2
R armature resistance 23.25 Ohms
L inductance (not measured)
Kt torque constant 2.8466 (oz-in)/A
Kb back emf constant .0201 (V-s)/rad
D1 viscous damping 1.0e-5 (oz-in)/(rad/s)
KD damping constant .002461 (oz-in)/(rad/s)

141
Tc Coulomb friction .1407 oz-in
Ts static friction .1850 oz-in
Vref rated voltage 24 volts
IP peak current .915 amps
Is break-out current .078 amps
τm mechanical time constant 34.0 ms
p1 1/τm 29.3982 Hz
NT gear ratio 420.3 none
Kcr rotation angle sensor gain .2301 rad/v

Table C.4 (d): Prototype's (Translation) motor experimentally verified parameters

Variable Description Value Units


J rotor inertia 8.3711e-5 oz-in-s2
R armature resistance 23.25 Ohms
L inductance (not measured)
Kt torque constant 3.0736 (oz-in)/A
Kb back emf constant 0.0217 (V-s)/rad
D1 viscous damping 1.0e-5 (oz-in)/(rad/s)
KD damping constant .002868 (oz-in)/(rad/s)
Tc Coulomb friction .2370 oz-in
Ts static friction .3319 oz-in
Vref rated voltage 24 volts
Is break-out current .108 amps
IC running losses current .078 amps
τm mechanical time constant 29.2 ms
p1 1/τm 34.2728 Hz
NT gear ratio 420.3 none
r equivalent radius .334645 in.
Kct translation sensor gain .0655 in./v

142
The following motor data was used in section 4.3 of Chapter 4. It is the data for the "A"
prototype motor. These terms were not experimentally verified.

Table C.5 : Nominal motor parameters for Prototype A (data used in table 4.2)

Variable Description Value Units


J rotor inertia 2.3e-4 oz-in-s2
R armature resistance 4.33 Ohms
L inductance 2.34 mH
Kt torque constant 3.09 (oz-in)/A
Kb back emf constant .02187 (V-s)/rad
D1 viscous damping 1.9289e-4 (oz-in)/(rad/s)
Tc Coulomb friction .35 oz-in
Ts static friction (1.5*Tc) .525 oz-in
Vref rated voltage 24 volts
Is peak current 5.54 amps
NT gear ratio 127.78:1 none
r radius .23622 in.
Wt weight w gearhead 5.81 oz.

Frame size: Length= 2.378 in; diameter=1.175 in.

143
APPENDIX D

Simulation Code for Closed Loop Control of the "Model" System

The simulation model presented here is for the complete "Model" system. The Matlab code
used to generate the setpoint references from the trajectory planner algorithm is followed by the
Simulink model which is numerically integrated using the Runge-Kutta method. The Simulink
model includes a closed loop PID control option along with the feedforward inverse model. It is
presented here to provide the reader with a visual reference of the system under study.

The Matlab Code used to generate the trajectory references and to call the Simulink Model is:

% Matlab Filename: modrmt94.m


% Description: Simulation of "Model" system
% Author: Greg Gilbert
% Last Revised: 10/17/97
% Companion Simulink Filename: MDRFSM94.m
% Comments: This Simulink model includes and calcs a PID option with the inverse model

% Notes: This version of the traj planner uses a delt=dt of the RK integrator,
% or delta=dt=.0006 sec. Also note that this version takes numerical derivatives of the traj. planner

clear all

% Key System Dimensions of the PR manipulator

xc=17.6;
xoa=10.7;
l=6.9;

%%%%% Begin offline calculation of trajectory references%%%%%

% User entered setpoints in local xy-coordinate system.

xl=0 %These are the old coordinates


yl=0

x=1 %These are the new coordinates


y=1

thetal=-asin(yl/l); % theta last


xoal=xc-xl-l*cos(thetal); % xoa last

xp0=xoal+l*cos(thetal)+x; % x position for new point p (global)


yp0=l*sin(thetal)+y; % y position for new point p (global)

delspx=(xc-xp0); % delta x from point p to camera, set point ref x


delspy=(0 -yp0); % delta y from point p to camera, set point ref y

tref=1.5 % Set from experience

144
% Time, I.C., and dt of simulation

fixstep= .0006; % This is the dt step size


Tf=tref; % Final time for run
tol=1e-4; % Iteration tolerance
T=[0:fixstep:Tf]'; % Time array
L=length(T);

%% Generate setpoints offline and place in a matrix


%% to be sent to simulink model

tsp=T; % time setpoint equals Time of integration

for i=1:L

% set point programmer generator

tbar=tsp(i)/tref; %nondimensional time


spbar=3*tbar^2-2*tbar^3; %nondimensional cubic reference

xsp(i)=xp0 +spbar*delspx; %dimensional x setpt


ysp(i)=yp0 +spbar*delspy; %dimensional y setpt

%kinematic conversion

thetasp(i)=asin((ysp(i)-y)/l);
xoasp(i)=xsp(i)-l*cos(thetasp(i))-x;

end

%% Generate Derivative terms

delt=fixstep;

for k=2:L

vthetasp(k)=(thetasp(k)-thetasp(k-1))/delt;
athetasp(k)=(vthetasp(k)-vthetasp(k-1))/delt;

vxoasp(k)=(xoasp(k)-xoasp(k-1))/delt;
axoasp(k)=(vxoasp(k)-vxoasp(k-1))/delt;

end

%% End Trajectory Planner

%% Fill the U matrix with new setpoints

U=[thetasp',athetasp',vthetasp',xoasp',axoasp',vxoasp'];

Ut=[T,U]; % Input matrix to Simulink Model with time vector

%% Start Simulink Simulation

145
disp('Start clock')

[t,X,Y]=rk23('mdrfsm94',Tf,[],[tol,fixstep,fixstep], Ut); % Uses user defined step size

disp('Stop clock')

%% Print Desired Variables %%

The Complete Simulink Model called by the Matlab code is presented next.

+ .5 2
+ theta4
FF + D/A staturation Limit kamp1
PID Limiter Gain 1 theta4''
1 Rot
Vamp1
Thetad Xoa''_
To Workspace Rotational Motor
Traj Rotation Link Dynamics
tau1 (Rot Torque)
2 PID Theta
Thetad'' theta4_
Traj
2 theta4'
3 theta4''_
Limiter Kamp2
Thetad' Xoa''
Traj
Translational Motor Translation Link Dynamics
4 PID Tran Ff (trans link)
Xoad
Traj
Vamp2
5 To Workspace1
+
Xoad'' .5
+
Traj
FF + PID D/A staturation
6 Trans Limiter Gain2
Computed Torque
w/ Friction
Xoad' Compensation
Traj

Figure D.1: Model of complete system

The following figures are exploded views of the main blocks in Figure D.1. They are presented
in the following order:

1. link dynamics
2. servo motors
3. PID model
4. inverse model

The block diagrams for the rotational and translation links are

146
1 theta4

sin(u[1]) sin(theta4)

4.662e-3 M2 3.45 L cg 6.9 L 1.165e-2 M3 1.855e-2 Izz2

* M2Lcgsin(theta4) * M2Lcg^2 * M3L^2 * M3Lsin(theta4)1

+ + Sum1 3 Xoa'' + + + Sum 2 theta4''

* sumx(Xoa'') * sumx(theta4'')

- + Sum2

1 T1 (torque for rotational link)

Figure D.2: Rotational link with dimensions

1 theta4

sin(u[1]) sin(theta4) cos(u[1]) cos(theta4)

4.662e-3 M2 3.45 L cg 6.9 L 1.165e-2 M3 .08586 M1

+ + + Sum3

* M2Lcgsin(theta4) * M2Lcgcos(theta4) * M3Lcos(theta4) * M3Lsin(theta4)1

+ + Sum1 3 theta4'' + + Sum2 2 theta4' 4 Xoa''

* sumx(theta4'') * sumx(theta4') * sumx(Xoa'')

- - + Sum

T2
Translation Force 1 T2 (Force for Translation link)

Figure D.3: Translational link with values substituted

147
The block diagram representation of the translational motor is:
x''
2 alpha2 du/dt 1
Trans alpha2 to wksp1 Derivative xoa''
Torq -
+ a w wg x'
f(u) + f(u) 1/s f(u) .334645 y1b
-
-
Sum Kt/R 1/Jo Integrator1 N xoa', geared
Kt/(sL+R) R equiv
Sum2
1
Vamp 1/s
x
.0217 omega2 Integrator
f(u) omega to wksp
Kb +
D1
T2a y2b
+ .2370
Trans Torque Load xoa, geared
+ Tc Sign
T2gear Sum1
Trans Torque Load1 *
f(u)

Tfrxoa Product Stribeck Frict


Friction Torque

Figure D.4: Translational gearmotor

Note that the motor model includes an inductance term, making it a second order system. Also
note that the stiction term is modeled with the Stribeck effect. All torque terms are summed at a
mutual junction point. For all practical purposes, this is the most comprehensive model available
for a PM brushed commutated d.c. motor.

The following figure represents the common PID controller which functions by comparing the
desired position reference signal generated from the trajectory planner to a position feedback
signal. The error signal is differentiated and integrated and multiplied by the appropriate gains.

e2_cmd
error command
.03953

1 Kp (Kb)
+
in_1 -
1 f(u) du/dt + 1
+
K Kd Derivative + out_1
2 Sum1 Sum
in_2 1
2
s
Integrator Ki

Figure D.5: PID block terms

148
1 f(u)
a (1/N)(1/req)
Mux f(u) +
1
(JR/Kt)a+(Kb)w +
Out to PID
Mux1 Sum1 Sum Jcn
2 f(u) f(u) +
w (1/N)(1/req)1 .845(Fv)(R/Kt) +
+
f(u)
Sum2 xoaff
w2g Sign1 .9728(Fc)(R/Kt)
To Workspace1
omega 2 gear
f(u) *
Product fcomp2
Pulse deltaFs, (R/Kt)
Stiction Trq Friction Compensation
at start of run output variable for plotting

Figure D.6: Inverse model for translation axis

Note that the inverse model in Figure D.6 only represents the translation servo motor dynamics
and uses the cut-off stiction model rather than the Stribeck model. This model also neglects the
inductance term. Because of the low gearing and fast electrical response of the PM motors, this
inverse model is shown to be appropriate in the simulations.

149
APPENDIX E

Computed Torque Control Simulation Code

The following control code was used for simulating the model and prototype systems. This
controller does not use position feedback. The dimensions and motor parameters used in the
Matlab code and Simulink model are specified for the "model" system. The Matlab code pre-
computes the entire command reference signal using a sample period of 10 ms, and places it in
an array which is passed to the Simulink model. This model is numerically integrated using a
third order Runge-Kutta method at a sample period of .6 ms. The Simulink model only contains
the "real" world analog components of this system. The Matlab code presented here simulates
the digital controller.

% Matlab Filename: TRQ94MAT.M


% Description: Simulation of open loop "Model" system using the Computed Torque Method
% Author: Greg Gilbert
% Last Revised: 10/30/97
% Companion Simulink Filename: SYS94SIM.M
% Comments: Matlab pre-computes the inverse model. This file also compares experimental
% test data with simulated data

% Time, I.C., and dt of simulation

dt=.01 % Simulated sample period for computed torque control. Matches experimental sample period.
tref=3.6 % Nominal Setpoint time

% This is time array for dt setpoint programmer


Tf=tref; % Final time for run
tol=1e-4; % Iteration tolerance
T=[0:dt:Tf]'; % Time array
L=length(T)

%% Develop The Traj Planner and Model Ref Commands for Simulink

%% Simulation System-"Model" %%

% motor parameters

% Theta Motor
J_th=.00039;
R_th= 8.5;
Kt_th=5.6;
Kb_th=.03953;
Fv_th=1.07927e-4;
Fc_th= .56;
Fs_th=1.58111;
N1=127.78*15;

% Xoa Motor
J_x=.00039;

150
R_x= 8.4;
Kt_x=5.6;
Kb_x=.03953;
Fv_x=1.07927e-4;
Fc_x=.5376;
Fs_x=1.4667;
N2=127.78*4.2333418;

% mechanical system parameters


xc=17.6;
xoa=10.7;
l=6.9;

% Load Some of the Experimental Ref Data to be used as initial conditions for Simulations

load c:\thesis\testdata\data9413\posth.txt;
load c:\thesis\testdata\data9413\posx.txt;

% Conversion Gains and Operations for Ch1 Ch3 feedback


Kc1=.2667; %rad/v gain on rot pot, new gear train
Kc2=.7591; %in/v gain on tran pot, new measure 10-13-97

thetal=posth(1); %This should be the avg of the 1st sample a/d period
xoal=posx(1);

clear posth;
clear posx;

% Begin offline calculation of trajectory

%% User Entered Setpoints %%

x=1 %Move to x=1in, y=1in the local xy-coordinate system.


y=1

%%%%%%%%%%%%%%%%%%%%%
%%%% Set Point Programmer %%%%
%%%%%%%%%%%%%%%%%%%%%

% thetal=-asin(yl/l); % theta last


% xoal=xc-xl-l*cos(thetal); % xoa last

xp0=xoal+l*cos(thetal)+x; % x position for new point p (global)


yp0=l*sin(thetal)+y; % y position for new point p (global)

delspx=(xc-xp0); % delta x from point p to camera, set point ref x


delspy=(0 -yp0); % delta y from point p to camera, set point ref y

% Generate setpoints offline and place in a matrix


% to be sent to simulink model

tsp=T; % time setpoint equals Time of integration

for i=1:L

151
% set point programmer generator

tbar=tsp(i)/tref; %nondimensional time


spbar=3*tbar^2-2*tbar^3; %nondimensional trajectory path

xsp(i)=xp0 +spbar*delspx; %dimensional x setpt


ysp(i)=yp0 +spbar*delspy; %dimensional y setpt

%kinematic conversion

thetasp(i)=asin((ysp(i)-y)/l);
xoasp(i)=xsp(i)-l*cos(thetasp(i))-x;

end

delttheta=max(thetasp)-min(thetasp)
deltxoa=max(xoasp)-min(xoasp)

% Generate Derivative terms

for k=2:L

vthetasp(k)=(thetasp(k)-thetasp(k-1))/dt;
athetasp(k)=(vthetasp(k)-vthetasp(k-1))/dt;

vxoasp(k)=(xoasp(k)-xoasp(k-1))/dt;
axoasp(k)=(vxoasp(k)-vxoasp(k-1))/dt;

end

%%%%%%%%%%%%%%%%%%%%%%%
%%% Computed Torque Reference %%%%%
%%%%%%%%%%%%%%%%%%%%%%

% Gear vel and accel

vthetasp=N1*vthetasp;
athetasp=N1*athetasp;

vxoasp=N2*vxoasp;
axoasp=N2*axoasp;

for i=1:L

% Calculate sign function

if vthetasp(i)==0
sgn_th=0;
else
sgn_th=abs(vthetasp(i))/vthetasp(i) ;
end

if vxoasp(i)==0
sgn_x=0;
else

152
sgn_x=abs(vxoasp(i))/vxoasp(i) ;
end

% Calc Inverse Model Terms

%Theta Motor

u1a=((J_th*R_th)/Kt_th)*athetasp(i)+ Kb_th*vthetasp(i);
u2a=((Fv_th*R_th)/Kt_th)*vthetasp(i);
u3a=sgn_th*(Fc_th*R_th)/Kt_th;

if i<5
u4a=((Fs_th-Fc_x)*(R_th/Kt_th))*sgn_th;
else
u4a=0;
end

uths(i)=u1a+u2a+u3a+u4a;

%Xoa Motor

u1b=((J_x*R_x)/Kt_x)*axoasp(i)+ Kb_x*vxoasp(i);
u2b=((Fv_x*R_x)/Kt_x)*vxoasp(i);
u3b=sgn_x*(Fc_x*R_x)/Kt_x;

if i<4
u4b=((Fs_x-Fc_x)*(R_x/Kt_x))*sgn_x;
else
u4b=0;
end

uxs(i)=u1b+u2b+u3b+u4b;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Start of Simulink Simulation %%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Time vars for simulink simulation

fixstep= .0006 % This is the dt step size for Simulink Simulation


Tfsim=tref
tol=1e-4; % Iteration tolerance

% Fill the U matrix with new setpoints

uths=.5*uths;
uxs=.5*uxs;

U=[uths',uxs'];

Ut=[T,U]; % Input matrix with time vector

% Initial Conditions for ODE

153
theta0=thetasp(1);
xoa0=xoasp(1);
X0=[theta0,0,0,0,xoa0,0]';

%% Start Simulink Simulation

disp('Start clock..Simulink Run')

[t,X,Y]=rk23('sys94sim',Tfsim,X0,[tol,fixstep,fixstep], Ut); % Uses user defined step size

disp('Stop clock')

% Note on variables returned from integration: y1a(theta4dot), y2a(theta4) are from the rot motor;
% y1b(xoadot), and y2b(xoa) are from the translation motor

th0=y2a(1)
thdot0=omega1(1)
thdotdot=alpha1(1)
x0=y2b(1)
xdot0=omega2(1)
xdotdot=alpha2(1)

% Produce Global position coordinates for attenuated arm


% and error terms

dxoasim=y2b;
theta4=y2a;

m=length(theta4);

for k=1:m,
Yg(k,1)=l*sin(theta4(k,1));
Xg(k,1)= dxoasim(k,1) + l*cos(theta4(k,1));
end

minYg=min(Yg)
minXg=min(Xg)

Xnew=xc-min(Xg(:,1))
Ynew=0-minYg

errX_pu=(x-Xnew)/x
errY_pu=(y-Ynew)/y

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%% End of Simulation code %%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Load Experimental Data and Compare to Simulations

load c:\thesis\testdata\data9413\ch0.txt; %Theta Dac Control Signal


load c:\thesis\testdata\data9413\ch1.txt; %Theta position feedback signal

154
load c:\thesis\testdata\data9413\ch2.txt; %Xoa control signal
load c:\thesis\testdata\data9413\ch3.txt; %Xoa psotion feedback signal
load c:\thesis\testdata\data9413\uth.txt; % Theta mod ref command in volts
load c:\thesis\testdata\data9413\ux.txt; % Xoa mod ref command in volts
load c:\thesis\testdata\data9413\posth.txt; % Theta Pos mod ref command in rads
load c:\thesis\testdata\data9413\velth.txt; % Theta Vel mod ref command in rads/s
load c:\thesis\testdata\data9413\posx.txt; % Xoa Pos mod ref command in inches
load c:\thesis\testdata\data9413\velx.txt; % Xoa Vel mod ref command in inches/s
load c:\thesis\testdata\data9413\ax.txt; % Xoa accel mod ref command in inches/s
load c:\thesis\testdata\data9413\ath.txt; % theta accel mod ref command in inches/s

% Time and Delta T used for Experimental Traj Planner Calc to creat time array

dt_exp=.01;
ls=length(uth);
Ts=dt*ls;
ts=[0:dt_exp:Ts-dt_exp];

% Conversion Gains
Kc1=.2667; %rad/v gain on rot pot, New gear train
Kc2=.7591; %in/v gain on tran pot, new measure 10-13-97

ch1k=Kc1*ch1; % Dimensioned rot feedback


ch3k=Kc2*ch3; % Dimensioned tran feedback

% Time Array for A/D Channel Data


dt_ch=.01 %the actual a/d sample rate
l1=length(ch0);
Tfs=dt_ch*l1;
t1=[0:dt_ch:Tfs-dt_ch];

% Add Xoal initial condition to ch3k


for i=1:l1
ch3k(i)=ch3k(i)+xoa;
end

%%%%%%%%%%%
%% Plot Results %%
%%%%%%%%%%%

disp('')
disp(' Compare Trajectory Calculations between Matlab and C code')

figure(1),plot(tsp,thetasp,'r.',ts,posth),grid,title('Pos theta sp (Matlab vs C code)')


figure(2),plot(tsp,vthetasp,'r.',ts,velth),grid,title('Geared Vel Theta sp (Matlab vs C code)')
figure(3),plot(tsp,athetasp,'r.',ts,ath),grid,title('Geared Accel Theta sp (Matlab vs C code)')

figure(4),plot(tsp,xoasp,'r.',ts,posx),grid,title('Pos xoa sp (Matlab vs C code)')


figure(5),plot(tsp,vxoasp,'r.',ts,velx),grid,title('Geared Vel Xoa sp (Matlab vs C code)')
figure(6),plot(tsp,axoasp,'r.',ts,ax),grid,title('Geared Accel Xoa sp (Matlab vs C code)'),zoom on

uths=2*uths;
uxs=2*uxs;

figure(7),plot(tsp,uths,'r.',ts,uth,'y.'),grid,title('Uth Matlab vs C code '),zoom on

155
figure(8),plot(tsp,uxs,'r.',ts,ux,'y.'),grid,title('Uxoa Matlab vs C code '),zoom on

disp('')
disp(' Compare Simulink System Response to Exper: Mod Ref and Actual System Resp')

uth=.5*uth;
ux=.5*ux;

figure(9),plot(ts,uth,'y',t1,ch0,'r'),grid,title('Uth C code Ref vs DAC Ref Output '),zoom on


figure(10),plot(ts,ux,'y',t1,ch2,'r'),grid,title('Uxoa C code Ref vs DAC Ref Output'),zoom on

figure(11),plot(t1,ch0,t1,ch1),grid,zoom on,
title('Exp:Theta (Ch0) Cmd vs Pos (Ch1) Feedback'),xlabel('t,sec'),ylabel('Volts')

figure(12),plot(t1,ch2,t1,ch3),grid,zoom on,
title('Exp: Xoa (Ch2) Cmd U vs Pos (Ch3) Feedback'),xlabel('t,sec'),ylabel('Volts')

figure(13),plot(ts,posth,t1,ch1k,'r'),grid,
title('Exp: Pos theta Traj Plan vs Exp'),xlabel('t,sec'),ylabel('rads'),zoom on

figure(14),plot(t1,ch1k,t,y2a,'r'),grid,
title('Exp: Theta Pos Fdbk vs Simulink Model Fdbk '),xlabel('t,sec'),ylabel('rads'),zoom on

figure(15),plot(ts,posx,t1,ch3k,'r'),grid,
title('Exp: Pos Xoa Traj Plan vs Exp'),xlabel('t,sec'),ylabel('inches'),zoom on

figure(16),plot(t,y2b,t1,ch3k,'r'),grid,
title('Exp: Xoa Pos Fdbk vs Simulink Model'),xlabel('t,sec'),ylabel('inches'),zoom on

figure(17),plot(t,y2b,tsp,xoasp,'r'),grid,
title('Exp: Xoa Simulink Model Pos ref vs Pos Fbk'),xlabel('t,sec'),ylabel('inches'),zoom on

The Simulink model called by the Matlab code is presented below.

156
Vamp1
To Workspace

theta4
1 2
Uth Cmd Limiter_ kamp1 theta4''
Xoa''_
Rotational Motor Rotation Link Dynamics
tau1 (Rot Torque)

theta4_
2 2 theta4'
Ux Cmd Limiter theta4''_
Kamp2
Xoa''
Translational Motor Translation Link Dynamics
Ff (trans link)

Vamp2
To Workspace1

Figure E.1: Computed torque simulink model

Note that this model is numerically integrated using a third order Runge-Kutta method at a
sample period of .0006 seconds. The digital trajectory planner and the inverse model used a
sample period of .01 seconds.

157
APPENDIX F

Computed Torque Control "C" Code

The following control code was used in a PC based controller. It was written and compiled in
Borland 3.1 C for DOS. Calls are made to function routines written by ComputerBoards Inc. that
perform the A/D, D/A signal conversion. Although this code shows parameters for the prototype
system, the same program was used to run the model system. Only the physical dimensions,
setpoints and motor parameters were changed for the model version. Both systems were tuned
by accurately identifying their motor parameters, with the static and Coulomb friction terms
being the most important. This controller does not use feedback error correction.

/**************************************************************************

TRQJOHN.C Open Loop Computed Torque Control Law

This program runs the prototype system

Written by: Greg Gilbert


Last Revision Date : 2/11/98

Program Description:

This is an Open Loop program that produces a polynomial trajectory path for a
two link two motor planar robot. User provides position reference
in the local coordinate system. Program uses the traj path to compute the so called
"Computed Torque Control Law" (Craig, 1989)." 2 control channels
and 2 position feedback channels are read in while
an 8254 counter timer paces the A/D input and D/A output. Sample Rate is 100 hz.

**************************************************************************/

/* Include files */
#include <stdio.h>
#include <conio.h>
#include <dos.h>
#include <time.h>
#include <cb.h>
#include <math.h>
#include <stdlib.h>

void main ()
{
/* Variable Declarations for Control and A/D Boards*/

/* Setpoint References in the Picture Coordinate System */

float x=1*(35/25.4)/10; /* Ref in in. */


float y=1*(35/25.4)/10; /* Ref in in. */

/* Time Var Declaration */


float time=3.6; /* Setpt Program Reference Time, # * time constant */

158
float dt=.01; /* sec; Stated free run DAC Sample Period */
int N; /*Number of Loops through Control Array*/

float t=0; /* Dimensioned time used with setpoint programmer */


float tref=0; /* Reference time, to be adjusted */
float store; /* Used for temp storing of vars */
clock_t start, end; /* Clock vars */

/* Counter vars declare */

int CounterNum;
int RegName;
int Config;
int LoadValue;
unsigned Countn;

/* Motor Parameters */

/* Theta Motor */
float J_th=8.3711e-5;
float R_th=23.25;

float Kb_th=.02;
float Kt_th=141.623*Kb_th;

float Fv_th=1.0e-5;
float Fc_th=.147; /*(.19 to sustain bkout) Torque Coulombic Friction */
float Fs_th=.22437; /* Torque break out */
float N1_th=420.3; /* Gearing gain */
float N4_th=1; /* (No) Extra Gearing for Rotation */

/* Xoa Motor */
float J_x=8.3711e-5;
float R_x=23.25;

float Kb_x=.0217;
float Kt_x=141.623*Kb_x;

float Fv_x=1.0e-5;
float Fc_x=.237;
float Fs_x=.3319;
float N1_x=420.3;
float req=2.988235; /* inverse of 1/.3346456=1/[(17/2mm)*(1/25.4)] equiv radius, inches */

/* Mechanical System Constants */

float xc=5.118; /* Distance to camera position, in. */


float xoa1=3.34645; /* Length of translational link to arb. center point, in. */
float l=1.77165; /* Length of rot link in in. */
float Kc1=.2301; /* rad/V, New 1 turn pot measured 2/2/98 */
float Kc2=.0655; /* in/V, (measured 10-27-97) gain of translation link pos sensor */

/* A/D Single Channel Mode Variable Declaration */

int i=0;

159
int j=0;
int BoardNum1 = 1;
int ULStat;
int UDStat;
int Gain = BIP5VOLTS;

unsigned ch0fbk; /* ch0fbk is temp var which passes value to *cmd1 */


unsigned ch1fbk; /* during channel by channel input read */
unsigned ch2fbk;
unsigned ch3fbk;

unsigned *cmd1;
unsigned *thfbk;
unsigned *cmd2;
unsigned *xfbk;

unsigned DataVal0; /* DataVal0 is temp var which is passed data from *cmd1 */
unsigned DataVal1;
unsigned DataVal2;
unsigned DataVal3;

float EngUnits0=0; /* EngUnits is temp var which passes data to *CH0 */


float EngUnits1=0;
float EngUnits2=0;
float EngUnits3=0;

float *CH0; /* Final Data Array written to disk */


float *CH1;
float *CH2;
float *CH3;
/* Vars to sum I.C. A/D read */
float posfbksum=0;
float posfbksumx=0;

/* D/A Variable Declaration */

int BoardNum0 = 0;
float EngUnits=0;
unsigned DataVal;
long Rate_DA, Count_DA;
int Options_DA;

/* Temp DAC output Array */

unsigned dac_out[2]; /* Data array for cbscanout */

/* Theta and Xoa control output variables */

float motact=0; /* 4094 = 1 pu= 5V, 0=-5V, 2047=0V, control output to analog amp */
float motactx=0; /* 4094 = 1 pu= 5V, 0=-5V, 2047=0V, control output to analog amp */
unsigned DataValue; /* Used for conversion for motact to counts */
unsigned DataValuex;

/* Model Ref Control Terms*/

160
float sgn_th=1; /* Variable to use for sign function on theta */
float sgn_x=1; /* Variable to use for sign function on Xoa */

float u1a=0; /* control out Theta terms for model ref*/


float u2a=0;
float u3a=0;
float u4a=0;

float u1b=0; /* control out Xoa terms for model ref*/


float u2b=0;
float u3b=0;
float u4b=0;

/* Setpoint Programmer variables */

float xsp=0;
float ysp=0;
float thetal=0; /* The last theta, or starting angle */
float theta=0; /* New theta */
float xoal=0; /* The last xoa, or starting translation position */

float tbar=0; /* Nondimensionalized time */


float spbar=0; /* Nondimensionalized setpoint */

float xpo=0; /* x position for new point p (global) */


float ypo=0; /* y position for new point p (global */

float delspx=0; /* delta x from point p to to camera set point ref x */


float delspy=0; /* delta y from point p to to camera set point ref y */

float RevLevel = (float)CURRENTREVNUM;

/* Initialize arrays for traj reference terms to feed to Model Ref */


double *pos1; /* Pos traj term for theta */
double *vel1; /* Vel traj term for theta */
double *a1; /* acceleration traj term for theta */
double *uth; /* theta model ref command */

double *pos2; /* Pos traj term for Xoa */


double *vel2; /* Vel traj term for Xoa */
double *a2; /* acceleration traj term for Xoa */
double *ux; /* Xoa model ref command */

unsigned *U0; /* Theta command output array */


unsigned *U1; /* Xoa command output array */

/* Declare variables for writing data to disk */

FILE *stream0; /* output file for ADData */


FILE *stream1; /* output file for ADData */
FILE *stream2; /* output file for ADData */
FILE *stream3; /* output file for ADData */
FILE *stream4; /* output file for ADData */
FILE *stream5; /* output file for ADData */
FILE *stream6; /* output file for ADData */

161
FILE *stream7; /* output file for ADData */
FILE *stream8; /* output file for ADData */
FILE *stream9; /* output file for ADData */
FILE *stream10; /* output file for ADData */
FILE *stream11; /* output file for ADData */

stream0 = fopen("ch0.txt", "w+"); /* Theta control signal */


stream1 = fopen("ch1.txt", "w+"); /* Theta position feedback signal */
stream2 = fopen("ch2.txt", "w+"); /* Xoa control signal */
stream3 = fopen("ch3.txt", "w+"); /* Xoa postion feedback signal */
stream4 = fopen("uth.txt", "w+"); /* Theta Model Ref Command in Volts */
stream5 = fopen("ux.txt", "w+"); /* Xoa Model Ref Command in Volts */
stream6 = fopen("posth.txt", "w+"); /* Theta Pos Model Ref Command in rads */
stream7 = fopen("velth.txt", "w+"); /* Theta Vel Model Ref Command in rads/s */
stream8 = fopen("posx.txt", "w+"); /* Xoa Pos Model Ref Command in inches */
stream9 = fopen("velx.txt", "w+"); /* Xoa Vel Model Ref Command in inches/s */
stream10 = fopen("ath.txt", "w+"); /* Theta accel Model Ref Command */
stream11 = fopen("ax.txt", "w+"); /* Xoa accel Model Ref Command in inches/s */

/* Declare UL Revision Level */


ULStat = cbDeclareRevision(&RevLevel);
UDStat = cbDeclareRevision(&RevLevel);

/* Calculate DAC and A/D run time values */

/* DAC Time Control Calcs */

store=time/dt; /* The desired number of loops through control law */


N=store; /* Pass to integer var */

printf("The stated run time is: %f sec\n",time);


printf("The stated delta t (dt) is: %f sec\n", dt);
printf("Loops through control (N) is : %i \n",N);

/* Initialize size for all pointed arrays based on calcs above*/


pos1=malloc((N+1)*sizeof(double));
if (!pos1) /* Make sure it is a valid pointer */
{
printf("\nout of memory for pos1 Variable\n");
exit(1);
}

vel1=malloc((N+1)*sizeof(double));
if (!vel1) /* Make sure it is a valid pointer */
{
printf("\nout of memory for vel1 Variable\n");
exit(1);
}

a1=malloc((N+1)*sizeof(double));
if (!a1) /* Make sure it is a valid pointer */
{
printf("\nout of memory for a1 Variable\n");

162
exit(1);
}

uth=malloc((N+1)*sizeof(double));
if (!uth) /* Make sure it is a valid pointer */
{
printf("\nout of memory for uth Variable\n");
exit(1);
}

pos2=malloc((N+1)*sizeof(double));
if (!pos2) /* Make sure it is a valid pointer */
{
printf("\nout of memory for pos2 Variable\n");
exit(1);
}

vel2=malloc((N+1)*sizeof(double));
if (!vel2) /* Make sure it is a valid pointer */
{
printf("\nout of memory for vel2 Variable\n");
exit(1);
}

a2=malloc((N+1)*sizeof(double));
if (!a2) /* Make sure it is a valid pointer */
{
printf("\nout of memory for a2 Variable\n");
exit(1);
}

ux=malloc((N+1)*sizeof(double));
if (!ux) /* Make sure it is a valid pointer */
{
printf("\nout of memory for ux Variable\n");
exit(1);
}

U0 = malloc((N+1)*sizeof(unsigned));
if (!U0) /* Make sure it is a valid pointer */
{
printf("\nout of memory for U0 variable\n");
exit(1);
}

U1 = malloc((N+1)*sizeof(unsigned));
if (!U1) /* Make sure it is a valid pointer */
{
printf("\nout of memory for U1 variable\n");
exit(1);
}

/* Declare size of the Data collection arrays */

cmd1 = malloc((N+1)*sizeof(unsigned));

163
if (!cmd1) /* Make sure it is a valid pointer */
{
printf("\nout of memory for cmd1 Variable\n");
exit(1);
}

thfbk = malloc((N+1)*sizeof(unsigned));
if (!thfbk) /* Make sure it is a valid pointer */
{
printf("\nout of memory for thfbk variable\n");
exit(1);
}

cmd2 = malloc((N+1)*sizeof(unsigned));
if (!cmd2) /* Make sure it is a valid pointer */
{
printf("\nout of memory for cmd2 Variable\n");
exit(1);
}

xfbk = malloc((N+1)*sizeof(unsigned));
if (!xfbk) /* Make sure it is a valid pointer */
{
printf("\nout of memory for xfbk variable\n");
exit(1);
}

CH0 = malloc((N+1)*sizeof(float));
if (!CH0) /* Make sure it is a valid pointer */
{
printf("\nout of memory for CH0 Variable\n");
exit(1);
}

CH1 = malloc((N+1)*sizeof(float));
if (!CH1) /* Make sure it is a valid pointer */
{
printf("\nout of memory for CH1 variable\n");
exit(1);
}

CH2 = malloc((N+1)*sizeof(float));
if (!CH2) /* Make sure it is a valid pointer */
{
printf("\nout of memory for CH2 Variable\n");
exit(1);
}

CH3 = malloc((N+1)*sizeof(float));
if (!CH3) /* Make sure it is a valid pointer */
{
printf("\nout of memory for CH3 variable\n");
exit(1);
}

164
/* Initialize the pointed arrays */
i=0;

/* Theta Terms */
*(pos1 + i)=0.0;
*(vel1 + i)=0.0;
*(a1 + i)=0.0;
*(uth+ i)=0.0;
*(uth+ i+1)=0.0;

/* Xoa Terms */
*(pos2 + i)=0.0;
*(vel2 + i)=0.0;
*(a2 + i)=0.0;
*(ux+ i)=0.0;
*(ux+ i+1)=0.0;

/* Initiate error handling


Parameters:
PRINTALL :all warnings and errors encountered will be printed
STOPFATAL :if any fatal error is encountered, the program will stop */
ULStat = cbErrHandling (PRINTFATAL, STOPFATAL);
UDStat = cbErrHandling (PRINTFATAL, STOPFATAL);

/*************************************************************/
/**** END Variable Declarations ********************/
/*************************************************************/

/* Preset Translational link Gear Teeth */

/* EngUnits=-1.3;
ULStat = cbFromEngUnits(0, Gain, EngUnits, &DataValue);
ULStat = cbAOut (0, 0, Gain, DataValue);

delay(0);

EngUnits=0;
ULStat = cbFromEngUnits(0, Gain, EngUnits, &DataValue);
ULStat = cbAOut (0, 0, Gain, DataValue); */

/* Read in present Analog coordinates theta and xoa as the starting point */
/* Loop through and average before taking points for I.C. */

for (i=1;i<2500;i++)
{
UDStat = cbAIn(1, 1, BIP5VOLTS, &ch1fbk); /* Get position feedback on theta*/

UDStat = cbToEngUnits (1,BIP5VOLTS, ch1fbk, &EngUnits1);/* Convert Pos fbk to Eng units */

posfbksum=posfbksum+EngUnits1;

UDStat = cbAIn(1, 3, BIP5VOLTS, &ch3fbk); /* Get position feedback */


UDStat = cbToEngUnits (1,BIP5VOLTS, ch3fbk, &EngUnits3);/* Convert Pos fbk to Eng units */

165
posfbksumx=posfbksumx+EngUnits3;
}

/* Perform calculations for the initial setpoints */


/* Setup Initial Conditions, delta references */

thetal=posfbksum/i; /* The last theta, or starting angle */


xoal=posfbksumx/i; /* The last xoa, or starting translation position */

printf("Starting theta volts position is: %f\n",thetal);


printf("Starting xoa volts position is: %f\n",xoal);

thetal=thetal*(Kc1); /* Convert voltage value to radians */


xoal=xoal*(Kc2) + xoa1; /* Convert voltage value to inches with offset*/

printf("Starting theta (radians) position is: %f\n",thetal);


printf("Starting xoa (inches) position is: %f\n",xoal);

/* Setpoint Programmer I.C. */

xpo=xoal + l*cos(thetal) + x;
ypo=l*sin(thetal) + y;

if (y==0)
{
ypo=0;
}

delspx=(xc-xpo); /* delta x from point p to camera, set point ref */


delspy=(0-ypo); /* delta y from point p to camera, set point ref */

tref=time; /*Reference time, to be adjusted */

/* Compute Offline setpoints for Trajectory Planner and Model Ref*/

for (i=0;i<N;i++)
{

tbar=t/tref; /*Nondimensional time */


spbar=(3*tbar*tbar)-(2*tbar*tbar*tbar); /*Nondimensional SP */

xsp=xpo + spbar*delspx; /* Dimensional x setpt */


ysp=ypo + spbar*delspy; /* Dimensional y setpt */

/* Kinematic Conversion on position Theta and Xoa */

*(pos1+ i)=asin((ysp-y)/l); /* theta, angle */


theta=*(pos1+ i);
*(pos2+ i)=xsp-l*cos(theta)-x; /* Xoa, in */

t=t+dt; /* Update dimensional time */


}

/* Generate Derivative Terms */

166
for (i=0;i<N;i++)
{
*(vel1+ i+1)=(*(pos1+ i+1)-*(pos1+ i))/dt; /* theta dot */

*(vel2+ i+1)=(*(pos2+ i+1)-*(pos2+ i))/dt; /* xoa dot */

*(a1+ i+1)=(*(vel1+ i+1)-(*(vel1+ i)))/dt; /* theta dotdot */

*(a2+ i+1)=(*(vel2+ i+1)-(*(vel2+ i)))/dt; /* xoa dotdot */

}
/* End Trajectory Planner */

/* Compute Model Reference/Torque Control Law */

for (i=0;i<N;i++)
{

/* Gear Velocities and Accelerations */

*(vel1+ i)=(N1_th)*(N4_th)*(*(vel1+ i)); /* Thetag V */


*(vel2+ i)=(N1_x)*(req)*(*(vel2+ i)); /* Xoag V */

*(a1+ i)=(N1_th)*(N4_th)*(*(a1+ i)); /* Thetag a */


*(a2+ i)=(N1_x)*(req)*(*(a2+ i)); /* Xoag a */

/* Calculate sign function of velocity for Coulomb Frict */


if (*(vel1+ i)==0)
sgn_th=0;
else
sgn_th=fabs(*(vel1+ i))/(*(vel1+ i)); /* sign for theta */

if (*(vel2+ i)==0)
sgn_x=0;
else
sgn_x=fabs(*(vel2+ i))/(*(vel2+ i)); /* sign for xoa */

/* Calculate Inverse Model */

/* Theta Motor */
u1a=((J_th*R_th)/Kt_th)*(*(a1+ i)) + Kb_th*(*(vel1+ i)); /* inverse dynamics */
u2a=((Fv_th*R_th)/Kt_th)*(*(vel1+ i)); /* viscous damping */
u3a=sgn_th*((Fc_th*R_th)/Kt_th); /* Coulomb Friction */

if (i<4)
u4a=((Fs_th-Fc_th)*(R_th/Kt_th))*(sgn_th); /* Step for stiction for 2 dt's */
else
u4a=0;

*(uth+ i)=u1a + u2a + u3a + u4a; /* command out put, Xoa full scale */

/* Xoa Motor */
u1b=((J_x*R_x)/Kt_x)*(*(a2+ i)) + Kb_x*(*(vel2+ i)); /* inverse dynamics */
u2b=((Fv_x*R_x)/Kt_x)*(*(vel2+ i)); /* viscous damping */

167
u3b=sgn_x*((Fc_x*R_x)/Kt_x); /* Coulomb Friction */

if (i<4)
u4b=((Fs_x-Fc_x)*(R_x/Kt_x))*sgn_x; /* Step for stiction for 2 dt's */
else
u4b=0;

*(ux+ i)=u1b + u2b + u3b + u4b; /* command out put, Theta full scale */

/* Save Torque/Model Reference Vars to disk */

printf("Saving Trajectory Planner and Model Ref Commands for Th and X\n");
for (i=0; i<N; i++)
{
fprintf(stream4, "%f\n", *(uth + i));
fprintf(stream5, "%f\n", *(ux + i));
fprintf(stream6, "%f\n", *(pos1 + i));
fprintf(stream7, "%f\n", *(vel1 + i));
fprintf(stream8, "%f\n", *(pos2 + i));
fprintf(stream9, "%f\n", *(vel2 + i));
fprintf(stream10, "%f\n", *(a1 + i));
fprintf(stream11, "%f\n", *(a2 + i));

/* close the open stream files */


fclose(stream4);
fclose(stream5);
fclose(stream7);
fclose(stream8);
fclose(stream9);
fclose(stream10);
fclose(stream11);

/* Print to screen important Trajectory Planning Info */

printf ("\nDimensional Total Time is: %f ",t);


printf ("\nThe Last Count for i is: %i",i);
printf ("\nThe Last Theta U command (volts) is: %f",uth[N-1]);
printf ("\nThe Last Xoa U command (volts) is: %f\n",ux[N-1]);

/* Convert Voltage command arrays to binary Counts, *.5 for amp gain */

for(j=0;j<N;j++)

{
motact=(*(uth+j))*.5; /* reduce by .5 & Store theta output to temp float var */
motactx=(*(ux+j))*.5; /* reduce by .5 & Store X output to temp float var */

/* Convert Voltage to counts */

168
ULStat = cbFromEngUnits(BoardNum0, BIP5VOLTS, motact, &DataValue);
ULStat = cbFromEngUnits(BoardNum0, BIP5VOLTS, motactx, &DataValuex);

U0[j]=DataValue;
U1[j]=DataValuex;

/* End of Conversion Loop */

/* Free Memory of Traj Planner variables */

free(pos1);
free(vel1);
free(a1);
free(pos2);
free(vel2);
free(a2);
free(uth);
free(ux);

/****************************************************************/
/* D to A Configuration */
/****************************************************************/

/* Set Control variables for cbAoutscan */

Rate_DA = NOTUSED;
Count_DA = 2;
Options_DA = 0;

/*************************************************************
***** 8254 Counter Configurations *************************
**************************************************************/

/* Config Counter
Parameters:
BoardNum# :the number used by CB.CFG to describe this board
CounterNum :the counter to be setup
Config :the operation mode of counter to be configured */

CounterNum = 3; /* Default vaule =1 */


Config = RATEGENERATOR;
ULStat = cbC8254Config (BoardNum1, CounterNum , Config);

/* Initialize cbCLoad() variables


Parameters:
BoardNum# :the number used by CB.CFG to describe this board
RegName :the counter to be loading with the starting value
LoadValue :the starting value to place in the counter */

LoadValue = 39500; /* 37635; Note the resolution of


counter tick is 2.5e-7 secs */

169
RegName = LOADREG3;

/***************************************************************
******* Begin Control DAC out sequence *************************
****************************************************************/

printf("\nPress any key to start and go on-line...\n");


getch();

/* Send Out Control Signal */

start = clock(); /* 1 CLK_TCK is 18.2 Hz */

for(j=0;j<N;j++)

UDStat = cbAIn(BoardNum1, 0, BIP5VOLTS, &ch0fbk);


UDStat = cbAIn(BoardNum1, 1, BIP5VOLTS, &ch1fbk);
UDStat = cbAIn(BoardNum1, 2, BIP5VOLTS, &ch2fbk);
UDStat = cbAIn(BoardNum1, 3, BIP5VOLTS, &ch3fbk);

*(cmd1+j)=ch0fbk;
*(thfbk+j)=ch1fbk;
*(cmd2+j)=ch2fbk;
*(xfbk+j)=ch3fbk;

ULStat = cbCLoad (BoardNum1, RegName, LoadValue);


ULStat = cbCIn (BoardNum1, CounterNum, &Countn);

dac_out[0]=U0[j];
dac_out[1]=U1[j];
ULStat=cbAOutScan(BoardNum0,0,1,Count_DA,&Rate_DA,Gain,dac_out,Options_DA);

while (Countn>350)
{
ULStat = cbCIn (BoardNum1, CounterNum, &Countn);

end = clock();

/* Send zero reference to ensure DACs are down */

ULStat = cbAOut (BoardNum0, 0, BIP5VOLTS, 2047);


ULStat = cbAOut (BoardNum0, 1, BIP5VOLTS, 2047);

printf("\nThe time for Run according to DOS System clock was: %f\n", (end - start) / CLK_TCK);

/* Free control arrays from memory */


free(U0);
free(U1);

170
/* Convert Points in Array to Engineering Units (Volts) and Save */

for (i = 0; i < N; i++) /* loop through the ADData array and convert */
{
DataVal0=*(cmd1 +i);
UDStat = cbToEngUnits(BoardNum1,BIP5VOLTS,DataVal0,&EngUnits0);
*(CH0 +i)=EngUnits0;

DataVal1=*(thfbk +i);
UDStat = cbToEngUnits(BoardNum1,BIP5VOLTS,DataVal1,&EngUnits1);
*(CH1 +i)=EngUnits1;

DataVal2=*(cmd2 +i);
UDStat = cbToEngUnits(BoardNum1,BIP5VOLTS,DataVal2,&EngUnits2);
*(CH2 +i)=EngUnits2;

DataVal3=*(xfbk +i);
UDStat = cbToEngUnits(BoardNum1,BIP5VOLTS,DataVal3,&EngUnits3);
*(CH3 +i)=EngUnits3;

/* Save Data to Disk */

fprintf(stream0,"%f\n",*(CH0+i));
fprintf(stream1,"%f\n",*(CH1+i));
fprintf(stream2,"%f\n",*(CH2+i));
fprintf(stream3,"%f\n",*(CH3+i));

/* Perform calculations for the ending setpoints */

thetal=CH1[N-1]; /* The last theta, or starting angle */


xoal=CH3[N-1]; /* The last xoa, or starting translation position */

thetal=thetal*(Kc1); /* Convert voltage value to radians */


xoal=xoal*(Kc2) + xoa1; /* Convert voltage value to inches */

printf("\nThe final theta (radians) position is: %f\n",thetal);


printf("\nThe final xoa (inches) position is: %f\n",xoal);

/* close the files */


fclose(stream0);
fclose(stream1);
fclose(stream2);
fclose(stream3);

/* Free Arrays */
free(cmd1);
free(thfbk);
free(cmd2);
free(xfbk);

free(CH0);

171
free(CH1);
free(CH2);
free(CH3);

}
/*******************************************************
************* END OF PROGRAM ************************
********************************************************/

172
VITA

Gregory S. Gilbert holds a BA in economics from the University of Virginia and a BS in


mechanical engineering from Virginia Tech which he earned in 1993. This thesis was written to
fulfill the requirements of Virginia Tech's MS degree in mechanical engineering. He was last
employed by the General Electric Company where he worked as a technical trainer in the
company's Drive Systems business.

173

You might also like