CONTROL OF A LEADSCREW DRIVEN FLEXIBLE ROBOT ARM by Nabil Georges Chalhoub A.:. A dissertation submitted in partial fulfillment of the requirements for the degree of Doctor of Philosophy (Mechanical Engineering) in The University of Michigan 1986 Doctoral Committee: Assistant Professor A. Galip Ulsoy, Chairman Professor Milton Chace Professor N. Harris McClamroch Professor Richard Scott

er\.T\ KA_ i~A - 1

ABSTRACT CONTROL OF A LEADSCREW DRIVEN FLEXIBLE ROBOT ARM by Nabil Georges Chalhoub Chairman: A. Galip Ulsoy High performance requirements in robotics have led to the consideration of structural flexibility in robot arms. Here the dynamics of a spherical coordinate robot arm, whose last link is very flexible, is studied for the purpose of control. The assumed modes method is employed to represent the deflection of any point on the flexible link, and the Lagrangian approach is used to obtain the unconstrained equations of motion. The robot arm, considered in this work, has two joints driven by a leadscrew transmission mechanisms. The kinematic constraints associated with the nonbackdrivable leadscrews are also considered. An integral plus state feedback controller is derived based on a linearized version of the rigid body model of the robot arm. The controller is then implemented on the rigid and flexible model. The rationale is to simulate the controllers currently used in industrial robots and to assess the interrelationships

between the robot arm structural flexibility and the controller design. The simulation results illustrate the differences between leadscrew driven and unconstrained axes of the robot; they indicate the trade-off between speed and accuracy; and show potential instability mechanisms due to the interaction between the controller and the robot structural flexibility. The integral plus state feedback controller, derived for the rigid body model of the robot arm, is extended to include the flexible motion. The objective is to introduce additional damping into the flexible motion. This is done by using additional sensors to measure the compliant link vibrations and feed them back to the controller. The effect of control and observation spillover is examined, and found not to present a serious practical problem. The simulation results show that additional damping in the flexible motion can be achieved by including the flexible motion in the control action. Experimental evaluation showed excellent agreement with the results of the digital simulations.

To my parents ii

ACKNOWLEDGMENTS I would like to thank all the members of my dissertation committee for their interest in my work and their helpful comments. I would also like to extend my appreciation and gratitude to my advisor, A. Galip Ulsoy, for his sincere and excellent guidance. A special thanks goes to the Robotics division of the Center for Research in Integrated Manufacturing (CRIM) at the University of Michigan for providing me with financial support throughout the duration of my thesis work. The help of their technical staff, John Caldwell, Jerry Turney and Rob Giles, is very much appreciated. I would also like to extend my thanks to Mary Ann Pruder and Marianne Moylan for typing my dissertation and all the related technical reports. Finally, a special thanks goes to my parents for their encouragement and their unfailing support. iii

TABLE OF CONTENTS DEDICATION...............,............................~o.............. ii ACKNOWLEDGEMENTS.................................................... iii LIST OF FIGURES.............................v.......... Vii LIST OF APPENDICES.......................................... xiii CHAPTER 1. INTRODUCTION............................................ 1.lo Motivation................. 1 1.2. Previous Work in the Dynamic Modeling of Robots............. 2 1.3. Overview of Robot Controller Design.................................... 4 1.4. Dissertation Overview............................................... 7 2. DYNAMIC MODELING OF THE ROBOT ARM......................... 10 2.1. Modeling...................................................... 10 2.2. Equations of Motion............................................... 13 2.3. Summary.................................................. 20 3. LEADSCREW DYNAMIC MODELING AND CONSTRAINTS.................................................. 21 3.1. Overview of the Static Model of a Leadscrew....................... 21 3.2. Generalization of the Static Model........................................ 23 iv

3.3. Coulomb Friction Effect....................................................... 26 3.4. Self-Locking Condition Effect.............................................. 27 3.5. Constrained Equations of Motion........................................... 28 3.6. Summary................................................................ 30 4. RIGID BODY CONTROLLER.................................................. 31 4.1. Design of the Integral Plus State Feedback Controller.......... 31 4.2. Results and Discussion........................ 35 4.3. Summary and Conclusions............................................... 65 5. DESIGN AND IMPLEMENTATION OF THE RIGID AND FLEXIBLE MOTION CONTROLLER.................................... 67 5.1. Description of the Three Lumped Mass System..................... 67 5.2. Controller Design for the Lumped Model............................... 69 5.3. Results and Discussion of the System with Three Lumped Masses................................................................... 74 5.4. Linearization of the Rigid and Flexible Equations of Motion............................................................................ 83 5.5. Derivation of the Rigid and Flexible Motion Controller........ 85 5.6. Results and Discussion of the Rigid and Flexible Motion Controller................................................... 92 5.7. Summary................................................................................ 114 v

6. EXPERIMENTAL RESULTS AND COMPARISON TO SIMULATION RESULTS....................................... 121 6.1. Experimental Set Up......................................................... 121 6.2. Experimental Evaluation of Model Parameters...................... 126 6.3. Design and Performance of the Rigid Body Controller.......... 127 6.4. Design and Results of the Rigid and Flexible Motion Controller.............................................. 135 6.5. Conclusions........................................................................... 140 6.6. Summary.........................0.. 141 7. SUMMARY AND CONCLUSIONS................................................. 142 7.1. Content of the Thesis..........0...................................... 142 7.2. Major Contributions............................................................ 144 7.3. Prospective Research Topics............................................... 145 APPENDICES............................................................*.................... 146 BIBLIOGRAPHY................................................. 245 vi

LIST OF FIGURES Figure 1-1. A schematic of the physical system. 8............................ 8 2-1. Arm geometry and coordinates.............................. 12 2-2. Free body diagram of the robot arm............................................... 19 3-1. Free body diagram of an unrolled thread...................................... 22 3-2. Transition of the leadscrew housing motion from lower to upper thread depending on the sign of the normal force, N........ 24 3-3. Effect of coulomb friction on an oscillatory response...................... 29 4-1. Block diagram of the integral plus state feedback controller......... 35 4-2. r response obtained from the rigid body controller in the base run................................................................................. 39 4-3. 0 response obtained from the rigid body controller in the base run.......................................................................... 40 4-4. d response obtained from the rigid body controller in the base run................................................................................ 41 4-5. Flexible motion coordinate qii(t) in response to the rigid body controller in the base run............................................................. 42 4-6. Flexible motion coordinate qs(t) in response to the rigid body controller in the base run............................................................. 43 4-7. Flexible motion coordinate q21(t) in response to the rigid body controller in the base run...................................................... 45 vil

4-8. Flexible motion coordinate q(t) in response to the rigid body controller in the base run............................................... 46 4-9. Control signal for the prismatic joint obtained from the rigid body controller in the base run............................................ 47 4-10. Control signal for the second joint obtained from the rigid body controller in the base run.............................................................. 48 4-11. Control signal for the base joint obtained from the rigid body controller in the base run.........................................4............. 49 4-12. Total vertical deflection, V, obtained from the rigid body controller in the base run.............................................................. 50 4-13. Total horizontal deflection, W, obtained from the rigid body controller in the base run.............................................................. 51 4-14. q response obtained from the rigid body controller in the instability run............................................................................. 53 4-15. Control signal for the second joint obtained from the rigid body controller in the instability run..................................................... 54 4-16. Control signal for the base joint obtained from the rigid body controller in the instability run..................................................... 55 4-17. Total vertical deflection, V, obtained from the rigid body controller in the instability run..................................................... 56 4-18. Total horizontal deflection, W, obtained from the rigid body controller in the instability run..................................................... 57 4-19. Flexible motion coordinate, q11(t), in response to the rigid body controller in the speed accuracy trade-off..................................... 58 4-20. Flexible motion coordinate, q4(t), in response to the rigid body controller in the speed accuracy trade-off..................................... 50 viii

4-21. 0 response obtained from the rigid body controller in the speed accuracy trade-off.................................................. 60 4-22. b response obtained from the rigid body controller in the speed accuracy trade-off.......................................................... 61 4-23. Control signal for the second joint obtained from the rigid body controller in the speed accuracy trade-off................................ 62 4-24. Control signal for the base joint obtained from the rigid body controller in the speed accuracy trade-off.................................. 63 4-25. 0 response obtained from the rigid body controller with the payload increased to 78.59 g......................................... 64 5-1. A schematic of the three lumped mass model................................ 68 5-2. First mass response obtained from the rigid body controller.......... 75 5-3. Second mass response obtained from the rigid body controller.............................................................................. 76 5-4. Third mass response obtained from the rigid body controller........ 77 5-5. First mass response obtained from the rigid and flexible motion controller in the reduced order model case................................... 78 5-6. Second mass response obtained from the rigid and flexible motion controller in the reduced order model case................................. 79 5-7. First mass response obtained from the rigid and flexible motion controller in the control spillover case......................................... 80 5-8. Second mass response obtained from the rigid and flexible motion controller in the control spillover case....................................... 81 5-9. Third mass response obtained from the rigid and flexible motion controller in the control spillover case......................................... 82 ix

5-10. r response obtained from the rigid and flexible motion controller in the reduced order model case................................... 93 5-11. 0 response obtained from the rigid and flexible motion controller in the reduced order model case................................... 5-12. q response obtained from the rigid and flexible motion controller in the reduced order model case................................... 95 5-13. Flexible motion coordinate qn(t) in response to the rigid and flexible motion controller in the reduced order model case.......... 96 5-14. Flexible motion coordinate q2l(t) in response to the rigid and flexible motion controller in the reduced order model case........... 97 5-15. Control signal for the prismatic joint obtained from the rigid and flexible motion controller in the reduced order model case.......... 98 5-16. Control signal for the second joint obtained from the rigid and flexible motion controller in the reduced order model case.......... 99 5-17. Control signal for the base joint obtained from the rigid and flexible motion controller in the reduced order model case.......... 100 5-18. Flexible motion coordinate q11(t) in response to the rigid and flexible motion controller in the control spillover case.............. 102 5-19. Flexible motion coordinate q12(t) in response to the rigid and flexible motion controller in the control spillover case................. 103 5-20. Flexible motion coordinate q2(t) in response to the rigid and flexible motion controller in the control spillover case................. 104 5-21. Flexible motion coordinate q=(t) in response to the rigid and flexible motion controller in the control spillover case................. 105 5-22. Control signal for the second joint obtained from the rigid and flexible motion controller in the control spillover case................. 106 x

5-23. Control signal for the base joint obtained from the rigid and flexible motion controller in the control spillover case.................. 107 5-24. Flexible motion coordinate ql(t) in response to the rigid and flexible motion controller in the control and observation spillover...................................................... 109 5-25. Flexible motion coordinate q l(t) in response to the rigid and flexible motion controller in the control and observation spillover.................................................... 110 5-26. Flexible motion coordinate q2(t) in response to the rigid and flexible motion controller in the control and observation spillover.....................111 5-27. Flexible motion coordinate q=(t) in response to the rigid and flexible motion controller in the control and observation spillover............................... 112 5-28. Flexible motion coordinate q1i(t) in response to the rigid and flexible motion controller in the control and observation spillover with structural damping included................................... 115 5-29. Flexible motion coordinate q12(t) in response to the rigid and flexible motion controller in the control and observation spillover with structural damping included................................... 116 5-30. Flexible motion coordinate q2(t) in response to the rigid and flexible motion controller in the control and observation spillover with structural damping included.................................. 117 5-31. Flexible motion coordinate q2(t) in response to the rigid and flexible motion controller in the control and observation spillover with structural damping included.................................. 118 6-1. A schematic of the experimental set up......................................... 122 6-2. 0 response obtained from the rigid body controller in the experimental work......................................................................... 130 xi

6-3. Control signal for the second joint obtained from the rigid body controller in the experimental work.............................................. 131 6-4. Total vertical deflection in response to the rigid body controller in the experimental work............................................... 132 6-5. 0 response showing the effect of friction in the experimental work.......................................................................... 133 6-6. Control signal for the second joint showing the effect of friction in the experimental work............................................. 134 6-7. 0 response obtained from the rigid and flexible motion controller in the experimental work.............................................. 137 6-8. Control signal for the second joint obtained from the rigid and flexible motion controller in the experimental work..................... 138 6-9. Total vertical deflection in response to the rigid and flexible motion controller in the experimental work..................... 139 xiii

LIST OF APPENDICES Appendix A. The Unconstrained Equations of Motion......................................... 147 B. The Constrained Equations of Motion for 0 and r........................... 157 C. The Rigid Body Equations of Motion of the Robot Arm................ 161 D. Rigid Body Controller Gain Matrices.............................................. 170 E. Schematic of the Double Integrator Circuit.................................... 174 F. Listing of the Computer Code for the Digital Simulation............... 176 G. Listing of the Computer Code for the Experimental Work............. 211 xiii

CHAPTER 1 INTRODUCTION This chapter provides an overview of the research in robot arm modeling, dynamics, and control. The motivation for high precision and high speed robot operation is discussed and some of the problems are identified. The current state-of-the art in robot arm dynamic modeling and control is reviewed, including studies of robot arm flexibility. Finally, the objectives of this study are set forth, and the organization of the dissertation is summarized. 1.1. Motivation Until the 1980's, the use of robots was primarily limited to industrial and research environments which are hazardous or unpleasant for human workers. In recent years, the evolution of the mechanical arm from teleoperator and crane to present day industrial and space robots has increased their implementation in almost every industrial area. However, the poor endpoint positional accuracy of current robots have restricted their applications to tasks that are error tolerant. Many tasks, such as assembly of high precision parts require positional tolerances on the order of 0.0254mm (0.001"). Existing manipulators can perform tasks 1

2. with a repeatable position on the order of 0.254mm (0.01"). The operation of high-precision robots is severely limited by the dynamic deflection which persists for a period of time after a move is completed. The settling time required for this residual vibration delays subsequent operations, thus conflicting with the demand for increased productivity. These conflicting requirements have rendered the robotic assembly task to be a challenging research problem. The performance problems in robotics are related to the transmission mechanisms (i.e. backlash, friction and compliance), the structural deformation (both static and dynamic), and the massive design of the robot arm. 1.2. Previous Work in the Dynamic Modeling of Robots The automation of assembly tasks will be greatly enhanced if robots can operate at higher speeds with greater positioning accuracy. These goals cannot be achieved with the existing massive robot designs, which make them slow and heavy. Many robot arms are made to be massive for increased rigidity. To improve their structural performance, payloads and operating speeds are kept fairly low. In other words, the rigid body assumption is justified by conceeding low performance. For higher operating speeds, mechanisms should be made light-weight to reduce the driving torque requirements and enable the robot to respond faster. However, lighter members are likely to elastically deform, thus making it a necessity to take into consideration the dynamic effects of the distributed link

3 flexibility. This is because high speed operation leads to high inertial forces which in turn cause vibration and deteriorate accuracy. This difficulty may be eased by fabricating the moving members of manipulators in fiber reinforced composite materials which can result in high structural stiffness and strength with low mass [1]; however the basic problem remains. In an attempt to improve the overall performance of manipulators, some researchers have treated the aforementioned problems separately. A direct drive arm is used by Asada [2] to solve the backlash and friction problems. The compliance problem in the drive mechanism is discussed by Kuntze and Jacubusch [31. Zalucky and Hardt 141 used a micromanipulator to correct for the link static deflection. The first step in improving the performance of robots is to obtain a reasonably accurate dynamic model. The general practice has been based on rigid body analysis where all the robot links are assumed to be rigid [5-9]. This is understandable since even the rigid body model is complex, nonstationary and highly nonlinear. However, when the robot is operated at high speed, the vibration induced by the large inertial and external forces can no longer be neglected and the rigid body assumption becomes invalid. Until recently, the inclusion of the link's flexibility in the dynamic model of the robot arm is done by solving for the rigid body motion and the flexible motion separately [10]. Generally the flexible motion is small, and its effect on the rigid body motion is neglected. Therefore, only the effect of the rigid body motion on the flexible motion is considered. This is done by solving the rigid

4 body equations for inertial forces which in turn are introduced as excitation sources to the elastic problem. This approach is adequate for modeling fairly rigid structures. However, an accurate dynamic model for a very flexible structure would require all the coupling terms between the flexible and the rigid body motions to be retained. This is done by using coupled reference position and elastic deformation models. Some researchers have used finite element techniques to describe the elastic deformation [11-161. Others have used global methods such as the assumed modes method, Rayleigh-Ritz method, etc. [17-181. These approximate techniques can be used to yield a set of equations which represent the combined rigid and flexible motion. 1.3. Overview of Robot Controller Design Robots are inherently very complex structures. Their rigid body dynamic models exhibit a nonlinear and nonstationary behavior. In today's robots, the desired end-effector position is converted by real-time kinematic computation into the equivalent angles that each joint must assume [191. The joints are then simultaneously driven to their assigned angles by separate conventional servo loop controllers. However, the implementation of conventional linear control techniques have led to poor performance because of both the inherent geometric nonlinearities of these systems, and the dependence of the system dynamics on the characteristic of the manipulated objects. Therefore, a sophisticated controller design is needed to ensure the robot desired performance.

6 In the control of rigid robots, adaptive, nonlinear feedback, and optimal control techniques have been investigated. Adaptive control theory 120-251 has been proposed as a promising. solution to the nonlinearity and nonstationarity problems. The main task of the adaptive controller is to adjust the feedback gains of the manipulator so that its closed loop performance characteristics closely match the desired ones. However, the large required computation time has restricted their applications to simulation studies. Nonlinear feedback control, or the "computed torque" method [26], has led to better performance over conventional control techniques in computer simulations. The controller is based on an idealized model of the manipulator. This is a severe drawback because when the "idealized" control law is used with an actual plant, the validity of the scheme is subject to doubt and the robustness of the controller becomes of great importance. A time-optimal control strategy is studied in [27] to enable faster movement of the robot arm. Kahn and Roth [28] showed that this technique results in a two-point boundary value problem. As a consequence, the computation must be repeated for each new set of initial and final conditions used in the numerical solution. In addition, the numerical algorithm yields an optimal solution that is a function of time and does not account for any unexpected disturbances which may act on the system. These drawbacks have rendered the implementation of the time-optimal control technique difficult, if not impossible, for real time application in feedback control of robots. However, this technique has turned out to be a powerful tool for off-line trajectory planning.

6 In an attempt to relax these difficulties, a suboptimal control strategy is often used in digital simulations. In this technique, linearized version of the dynamic model of the manipulator is obtained for which an analytical optimal control solution can be found. Many of these sophisticated control techniques suffer from excessive computation time requirements which make them unattractive for real time applications using current microprocessor technology [291. Besides the difficult issues encountered in rigid robots, a new problem is created when robot compliance is included in the dynamic model. It emanates from the distributed nature of the link mass and elasticity. An infinite number of degrees of freedom are required to specify the position of every point on the elastic link. However, due to physical limitations, only a finite number of sensors and actuators can be mounted on the flexible body. Thus resulting in the problem of controlling a large dimensional system with a smaller dimensional controller [30-31]. Meckl and Seering [32-33] proposed two types of forcing functions to achieve fast response with negligible residual vibrations. The first type allows vibration to occur during the move but would stop the motion in such a way as to eliminate any residual vibration. This consists of a bang-bang action with multiple switching points. The second type avoids the excitation of the resonant modes of the structure as it moves. This is done by using a forcing function with no frequency components matching the system natural frequencies. This work is still confined to research laboratories; it can be used off-line to generate the trajectory of the robot arm that satisfies a criterion considering both the response time and

7 the residual vibration. By and large, the research in the closed loop control of flexible manipulators can be divided into two categories. The first uses additional sensors to measure the flexible motion (i.e. assumes all state variables to be available) [34-371. This enables the inclusion of the flexible motion in the control action, thus achieving better positional accuracy with the existing joint motors. The second category employs a micromanipulator along with additional sensors to compensate for static and dynamic structural deflections [4], [38-39]. This concept gives the control system designer more capabilities to improve the robot arm performance at additional hardware cost. 1.4. Dissertation Overview The purpose of this work is to improve the end-effector positional accuracy by measuring and feeding back its transverse vibration. The spherical coordinate robot arm considered in this study is schematically illustrated in Fig. 1-1. Two types of controllers are implemented. The first is a conventional linear controller based on the rigid body model. The rationale is to simulate the controllers currently used in industrial robots and to assess the interrelationships between the robot arm flexibility and the controller design. The second control system uses the measured flexible motion to introduce additional damping in the flexible structure.

8 motor drive for payload leadscrew second beam crboss second flange / am leadscrew motor drive for the first beam cylindrical rotating base crs seto o cross section of cross section of the second beam the first beam Figure 1 -1. A schematic of the physical system.

9 In the next chapter, the dynamic modeling of the robot arm including the transverse elastic displacements of the last link is presented in detail. The derivation of the constraint equations imposed by the leadscrews are derived in Chapter 3. Chapter 4 includes the design of the rigid body controller and the results of the investigation of the interrelationships between the structural flexibility and the controller design. In Chapter 5, the rigid and flexible motion controller is designed. The separate and combined effects of control and observation spillover are shown. Chapter 6 presents the experimental apparatus along with the corresponding results. A comparison and discussion of the theoretical and experimental results is also made. Finally, Chapter 7 summarizes the work done, presents the main conclusions, and states some prospective research topics.

CHAPTER 2 DYNAMIC MODELING OF THE ROBOT ARM Typically, the dynamic modeling of a flexible robot arm considers the flexible motion to be small and neglects its effect on the rigid body motion. This technique gives an accurate model for fairly rigid structures. However, in the case of very flexible manipulators, all the coupling terms between the rigid and the flexible motions need to be retained. This is done by using coupled reference position and elastic deformation models. In this chapter, the dynamics of a spherical coordinate robot arm, whose last link is flexible, is studied for the purpose of control. The assumed modes method is employed to represent the deflection of any point on the flexible link. Lagrange's method represents a very systematic approach to obtain the equations of motion of such a complicated system in the simplest manner possible. 2.1. Modeling The physical system considered in this work is a spherical coordinate robot, which has two revolute joints and one prismatic joint (see Fig. 1-1). It consists of an arm connected to a gear driven rotating base. The arm is made of two beams 10

11 such that the second beam can move axially into the first. The entire arm is free to rotate around the horizontal axis passing though the pivot point o and parallel to the unit vector K (see Fig. 2-1). The r and 8 coordinates have leadscrew transmission mechanisms, whereas power is transmitted to the 0 coordinate through a gear train. All axes are driven by dc servo motors. The second beam consists of a thin, lightweight and very flexible aluminum rod. The rationale is to exaggerate the flexibility problem which leads to a better understanding of the interaction between the structural flexibility and the controller design. This will also narrow the gap between the natural frequencies of the elastic modes and the servo loop frequencies, thus reducing the numerical computation problem associated with stiff systems and the problem of sampling rapidly enough to characterize the high frequencies of the flexible arm. The first and second beams of the robot arm have length L and L2 respectively as shown in Fig. 2-1. A reference inertial frame (I, J, K) fixed at point o, is shown along with a non-inertial, body fixed, rotating reference frame (i,, k). The equations are derived in terms of the latter to keep the mass moment of inertia constant throughout the rigid body motion of the robot. The robot arm can rotate with angular velocity o around the vertical axis passing through J and 0 around the horizontal axis along k. The second beam has one additional degree of freedom r which allows it to move axially into the first beam. The payload at the end-of-arm is modeled by a concentrated mass mp at the end of the second beam.

12 0 K k Figure 2-1. Arm geometry and coordinates.

13 The general modeling problem for articulated flexible manipulators has been studied by others [11-16]. The constraint equations for a slot joint which allows for a combined translational and rotational motions is discussed in [40]. In this study, the prismatic joint constraint is handled by considering the portion of the second beam protruding from the first beam to have a flexible cantilever beam like behavior, whereas the first beam and the part of the second beam located inside the first beam are considered to undergo rigid body motion only. Referring to Fig. 1-1, this modeling assumption is justified by the structural properties of the first beam which is stiff in flexure. The protruding portion of the second beam is relatively flexible. It is more rigid in compression than in flexure. Therefore, the longitudinal vibrations of the flexible portion of the second beam are neglected and only transverse vibrations are considered in addition to the rigid body motion. Since the elastic deformation is small, the variations of the mass moment of inertia due to the flexible motion are assumed to be negligible. 2.2. Equations of Motion Lagrange's equation [41-42] is implemented to obtain the dynamic model of this complicated system. This method considers the system as a whole rather than its individual components, thus eliminating the need to calculate interacting forces. It formulates the problem in terms of two scalar functions, namely, work and kinetic energy. The position vector of the mass center for the rigid portion of the robot arm becomes important in the derivation of its kinetic energy expression. For the

14 first beam, one has, R I= | - a |i (2-1) where the star superscript indicates a value related to the center of mass. L is the total length of the first beam. a is the length of the portion of the first beam lying to the left of the pivot point 0 (see Fig. 2-1). The position vector of the mass center of the part of the second beam located inside the first beam is R2 = z (2-2) where z' = L1 + 2 /, Ll is the length of the portion of the first beam lying 2 2 to the right of the pivot point 0, L2 is the total length of the second beam, and r is the length of the part of the second beam protruding from the first beam. The position vector of any point on the protruding part of the second beam is, Rg-y' i + Vj+ Wk (2-3) where y' = L1 + y and y' - r o. The assumed modes method [411 is used to obtain the expressions for the transverse vibrations v and W. They are written as a linear combination of admissible functions, 4, (y), of spatial coordinates, multiplied by time dependent generalized coordinates q, (t). V = 4', (v)q1, () (2-4) (2-4) I=l An approximation to the dynamics of infinite dimensional systems is ofteni based on the fact that low frequencies alone are adequate to describe the flexible

16 behavior of the system. Therefore, n is selected to be 2, and the admissible functions are chosen to be the first two eigenfunctions of a clamped-free beam. Referring to Fig. 1-1, these boundary conditions are chosen because the second beam slides inside the first beam and is supported by a teflon sleeve which only permits axial motion. The clamped-free beam eigenfunctions f and f2 have the following general form which is described in [43], [44].,= cos (Y - cos - - a, inh -- - in '- (2-5) where the values of e, and a, for each mode are given in [431. Note that an additional approximation has been introduced in that the solution is not separable in space and time when r(t) is not constant. However, this forms a useful approximation as long as r(t) satisfies the Euler-Bernoulli beam assumptions. That is r must always be much greater than the beam cross sectional dimensions, the deflection terms v and w don't exceed one tenth of r, and rotary inertia and shear deformation effects are ignored. Substituting (2-4) and (2-5) into (2-1) to (2-3) gives the complete form for the position vectors. These results are then used to obtain the velocity terms from: dR, (2-6) dt. where n is the rotation vector of the (i, j k) basis and R, is the position vector of any particle along link i.

16 The velocity terms are then used to develop the kinetic energy expressions for each component of the robot-arm including the payload, mp. The latter is considered to be a point mass, and does not include rotation around its own mass center. The total kinetic energy will be T.= TrT, (R )dm +m=R 2. R - 2(2-7) *~r l T., + " ~ where m is the number of links in the robot arm, T,, is the kinetic energy of link i and T, is the kinetic energy of the payload. The total potential energy consists of the strain energy of the second beam 1451 and the potential energy associated with the rigid body motion. The strain energy includes terms due to flexural rigidity. o2 EI(y) as'(' ]t) dy + 1 jEI(y) a2W( t d (2-8) and terms due to the axial force: 2 T( aV,) ] d + f JT(y,) (yt)| dy (2-9) 2 '[ y 'j 0ay where T(y,t) is the axial force in the second beam which arises due to gravity and inertial forces. The latter consist of centripetal acceleration and flexible motion effects. However, the variation of the axial force due to the flexible motion is neglected and one obtains,

17 r2 T(y,t) — pA2({cos2 + ~)(Llr + ) 1 - ~2 ^ -r LIr + — (2-10) + m, (L + r) [^cos2 + + [m + pA 4r - )] sin A compressive T(y,t) would tend to decrease the transverse stiffness of the beam while a tensile T(y,t) would have the opposite effect. The total potential energy will be: L' tL Vt = Im4|~2 - a + mldi + mli(LI + r - L2)+ L + r- - + m, (L + r ) }gsin9 + pA 2gcosOf V(y)dy + m, V L + gcosO o (2-11) +~a2v(yt) d 1 aEf jO (y, t) d 1 f EI(y)~ -d2V{| dy + - El (y) y2 0 ]1 + 2f ( y') OV y,t) ]' + 2 fj(u8 yt)|a V(S)] dy + OjT(y W(y,t)t) dy where m, and di are respectively the mass and the distance from the (i, j, k) origin to the mass center of the leadscrew that drives the second beam and the mH1 is its housing's mass. The coefficients in the kinetic and potential energy expressions, given in equations (2-7) and (2-11), involve the integration of complicated functions with respect to the spatial coordinate y. Some of the terms encountered in the kinetic

18 energy expression have the following form, f f f f4', (y)dy fly1o (y)dy o 0 o0 0)f yt + () r f f 01 (y)dy fyf4. (y)dy 0 o where i 1,2 and = 1,2. The integration terms appearing in the potential energy expression can be written in the following general form, [ d2, (y) ] fi d (y) = - 1,2 If dy2 d2 - fo o 1,2 where 4, (v) is the eigenfunction previously defined in equation (2-5). These complex functions are integrated numerically using Gaussian quadrature of high order (40 points) [46]. The virtual work principle is implemented to obtain an expression for the generalized forces. If the forces exerted on the robot arm are divided into conservative forces, which are directly related to the potential energy, vt, and nonconservative forces which are not, then the virtual work expression could be written as, [41] W- 6WW + 6W,, (2-12) where the subscipts c and denote terms due to the enconservative and nonconservative forces respectively. 5Wc is related to the potential energy V as follows:

19 aw, — v, = -, az,' (2-13) where z, is the it' generalized coordinate that can be either one of the rigid body degrees of freedom r, 0 and 0 or one of the flexible motion coordinates q1,, q,2, q9 and q=. The free body diagram of the robot arm, which shows the nonconservative forces, is depicted in Fig. 2-2. The expression for 6aw. can be easily obtained. 6W. = Q,, =- R R1 Z 0o + Fci 612 I L1+-L2 + TaJ6x1 F, j 6- R 1 - (2-14) = Fc r - aF 60M + T3ra where Q,., is the it nonconservative generalized force. 6a is the virtual rotation vector of f. R is the reaction force at the pivot point. F,, F, and T3 are the F,... ' R Figure 2-2. Free body diagram of the robot arm.

20 joint driving force and torques. Now that the kinetic energy, potential energy and the virtual work expressions are derived, the equations of motion can be readily obtained by implementing the following form of Lagrange's equation d fTt T OTt 0Vt d + aT, at, Qart (2-15) The resulting equations of motion are seven highly nonlinear, coupled, second order ordinary differential equations. The reader is referred to Appendix A for the listing of the unconstrained equations. 2.3. Summary A brief description of the physical system is given along with the definition of the coordinate axes and the generalized coordinates. The assumed modes method is implemented to model the flexible motion of the last link. Expressions for the virtual work, kinetic and potential energies are derived. Lagrange's equation is employed to obtain the unconstrained equations of motion. Some of these equations can be greatly simplified once the role played by the leadscrews and the effect of the rigid body motion on the flexible motion and vice-versa, are understood. These leadscrew constraints are considered in the next chapter.

CHAPTER 3 LEADSCREW DYNAMIC MODELING AND CONSTRAINTS The robot arm considered in this work has two joints driven by leadscrew transmission mechanisms which are used to transmit power by converting angular motion to linear motion. In this chapter, the kinematic constraints associated with a leadscrew are introduced to investigate the behavior of a leadscrew driven flexible robot arm in the presence of coulomb friction and the self locking condition (i.e., the nonbackdrivability of the leadscrew). 3.1. Overview of the Static Model of a Leadscrew A detailed derivation of the static model of a leadscrew can be found in [471. A brief summary of the modeling procedure is included to provide the required background for the next section. A free body diagram of an unrolled single thread of a leadscrew is shown in Fig. 3-1. Fa is the resultant of all the forces exerted on the leadscrew in the axial direction. N is the normal force and P is the force required to raise or lower the axial load, FA, on the thread surface. The friction force, f/, is considered to be of the simplest form of dry coulomb friction, 21

22 Fax Ijjlsgn(i) h ndm.H Figure 3-1. Free body diagram of an unrolled thread. f -- I NI sgn(z) (3-1) where j is the coefficient of friction and z represents the velocity along the rough surface of the leadscrew thread. The system is in equilibrium under the action of these forces. Therefore, the minimum force, P, required to raise or lower the axial load, F,, can be obtained from the summation of forces in the vertical and horizontal directions. The expression for P to raise the load has the following form, Fa[. + Ps P= (3-2a) where is the static coefficient of friction. To lower the load, P becomes where ~ is the static coefficient of friction. To lower the load, P becomes

23 P=F (3-2b) 1+1-I I Jm I where I is the lead of the leadscrew and d, is its mean diameter. The expression given in (3-2) represent the force required to overcome static friction and the geometric effect. This force, P, can be directly related to the torque, T, applied on the leadscrew by, T d. (3-3) 2 In the case where < < f dj in (3-2b), the force required to lower the axial load becomes negative. This causes the screw to spin without external effort. However, most leadscrews are nonbackdrivable in the absence of the applied force, P. This results in satisfaction of the following inequality, p5 > tan = - d (3-4) where 01 is the thread helix angle. The inequality in (3-4) is often referred to as the self-locking condition. 3.2. Generalization of the Static Model The static model of the leadscrew derived in [47] is useful for determining the torque required to overcome static friction and geometric effects for the load configuration given in Fig. 3-1. This model needs to be modified if different configurations are considered.

24 N Fa ~ - I y^^ ^2~ teadscrew ^ \I ^housing N Fax surface of the surface of te iower thread OI lower thread upper thread Figure 3-2. Transition of the leadscrew housing motion from lower to upper thread depending on the sign of the normal force, N. In this work, the leadscrew is assumed to satisfy the self-locking condition. The friction force has the same form as in (3-1) and the axial force, F,, can be either tensile or compressive. As a consequence, the normal force, N, can now be positive or negative. This sign change in N can be physically interpreted as having the leadscrew housing sliding on the upper thread's surface if it was originally on the lower surface or vice versa. This is illustrated in Fig. 3-2. The equations of motion for both the upper and lower thread surfaces can be combined in the following general form, -Fr sin ~1- I~ 171 |gn(z) + P cos t = mf z (3-5) where m, = 0 is a fictitious mass, and z is the coordinate along the thread sur

26 face. It can be directly related to the rigid body degree of freedom driven by the corresponding leadscrew through simple geometric relations. The expression for the normal force, N, is obtained from the summation of forces in the vertical direction, N = F, cos O1 + P sin A1 (3-6) In this work, the axial load, F,, consists of inertial and gravitational terms. It can be written as F. =- F.(Z, i, g) (3-7) where xzr = I, 0,, qll, q12 q21, q2, is the generalized coordinate vector and g is the gravitational acceleration. Substituting (3-6) and (3-7) into (3-5), the general form of the equation of motion becomes, - f[z,x,,g,abs(z,,z2,g)] (3-8) where abs (z,,x,g) function represents the absolute value of the normal force N in (3-5). The term z, can be either r or a since only two of the rigid body degrees of freedom r and 8 are driven by leadscrew transmission mechanisms. The f,'s are very complex, highly nonlinear functions which do not have closed form solutions. Instead, they are solved numerically on a digital computer. However, most numerical algorithms require that the equations be written in either of the following forms, z = f (,,, t) (3-9) or reduced to the first order form

26 f (, t) (3-10) where now zr -- Ir,, 12,, 21, q, r, 0,, Q, 2, 21, q q]. However, (3-8) cannot be written in either of these two forms due to the presence of the acceleration vector z on the right hand side. This difficulty would not exist had it not been for the absolute value term of the normal force, N, in equation (3-5). To overcome this problem, separate equations are written to describe the motion on the upper and lower thread surfaces of the leadscrew. By writing the force balance equations from the appropriate free body diagram in Fig. 3-2, the lower thread equation of motion can be obtained, m; z = -Fan [sin p(z + Pcos sin(z) + Pcos - t sinsgn(z)] (3-11) Similar procedures are followed to derive the equation of motion on the upper thread of the leadscrew, m/ = Fax [sinoi - p cos 0iltn(z) + P[cos, + it sin Oijgn()] (3-12) This results in having two equations of motion for each degree of freedom driven by a leadscrew mechanism. 3.3. Coulomb Friction Effect The effect of friction can be clearly identified at the beginning of the motion or when the response of the degree of freedom driven by a leadscrew is oscillatory. If the leadscrew is to rotate as soon as the input torque is applied, then the latter must be large enough to overcome the combined effects of static friction and geometry. Otherwise, the leadscrew would remain at a standstill, thus

27 causing a delay in the system response. In the case of an oscillatory response, the time at which the maximum overshoot occurs corresponds to the position where the system comes to a complete halt (i.e. velocity is zero). This results in a sudden jump in the required driving torque caused by the sharp increase in the magnitude of the friction force due to the transition from a dynamic to a static coefficient of friction. This may reshape the entire response if the leadscrew is driven by a controller. The latter, being unaware of this sudden increase in the required driving torque, would supply an input torque that is not large enough to overcome the resistive torque of the static friction. As a result, the system would remain at rest waiting for the control effort to build up. Under these conditions, the system is said to fall in the "stopping region." This is illustrated in Fig. 3-3. For some special cases, analytical expressions for the upper and lower bound of that region are developed in [481. 3.4. Self-Locking Condition Effect In this work, a self-locking condition is assumed; that is, once the leadscrew is subjected to an axial load, it will not rotate unless the applied input torque is large enough to overcome a certain resistive torque, TR. In the case of compressive axial force, the latter would have the following form, F d, (it, + tan 'pi) TR 2 (1-s ta (a3-13 a) for raising the load, and

28 F, d, (P - tan^l) TR =- 2 (3-13b) for lowering the load. The self-locking condition plays an important role in a multi degree of freedom system where the axial force exerted on the leadscrew is partially due to coupling terms between the degree of freedom driven by the leadscrew and the rest of the system degrees of freedom. In the absence of an input torque, the leadscrew which is nonbackdrivable, will not rotate regardless of the magnitude of the axial force. This renders the degree of freedom driven by the leadscrew transmission mechanism completely decoupled from the rest of the system degrees of freedom. However, this nice property is lost with the application of an input torque. Since the latter is directly related to the axial force which in turn depends on the coupling terms. To recapitulate, the self-locking assumption of the leadscrew transmission mechanism leads to the following, 1. The degree of freedom driven by the leadscrew is completely decoupled from the rest of the system degrees of freedom in the absence of the input torque. 2. Once the input torque is applied, the coupling terms between the system degrees of freedom are retained through the axial force. 3.5. Constrained Equations of Motion The gear train that drives the o motion or the base joint is assumed to be ideal (i.e. no backlash or friction). Therefore, the physical system does not impose any constraint on d. In this work, only the leadscrew constraints are con

29 e rtation c.0 8. b.C0 4.00 8.0C 12.0O Time. t!second Figure 3-3. Effect of coulomb friction on an oscillatory response sidered. As a result, the equations of motion for r and 0, derived in Chapter 2 and listed in Appendix A, need to be modified. Newton's method is employed to compute the axial forces exerted on the leadscrews that drive r and 0. The latter, which consist of inertial and gravitational terms, have the general form given in equation (3-7). The constrained r and e equations of motion are obtained by substituting Fu by its value in equations (3-11) and (3-12). The interested reader is

30 referred to Appendix B for the listing of the constrained equations. In an attempt to check the consistency of these equations, a dynamic model based on the rigid body assumption of the robot arm is derived in appendix C. It shows that the rigid body model can be obtained as a special case from the general equations of motion presented in appendices A and B. This provides a partial confirmation of the validity of the modeling procedure. 3.6. Summary A brief description of the static model of a leadscrew has been presented along with some modifications to allow for the handling of the dynamic case. Effects of coulomb friction and the self-locking condition on the behavior of a leadscrew driven rigid body degree of freedom are discussed. Finally, the constrained equations of motion for r and 0 are derived. Now that the derivation of the dynamic model of the flexible robot arm is complete, the interrelationships between the structural flexibility and the controller design will be investigated in the next chapter.

CHAPTER 4 RIGID BODY CONTROLLER This chapter presents the derivation of an integral plus state feedback controller based on a linearized version of the rigid body model of the robot arm. The controller is then implemented on the rigid and flexible model derived in Chapters 2 and 3. The rationale is to simulate the controllers currently used in industrial robots and to assess the interrelationships between the robot arm structural flexibility and the controller design. 4.1. Design Of The Integral Plus State Feedback Controller In most existing industrial robots, the nonlinear terms in the equations of motion are not considered in the design of the control system. This design practice is valid as long as the robot arm is restricted to slow motion. However, when manipulators are operated at high speed, the effect of geometric nonlinearities and the dependence of system dynamics on the characteristic of the manipulated objects become significant, thus leading to a degradation of the overall robot arm performance. 31

32 A conventional linear controller is implemented in this work to simulate the controllers currently used in industrial robots. It is designed based on a linearized version of the rigid body equations of motion derived in Appendix C. Throughout this work, the payload is kept constant and the rigid body degrees of freedom are restricted to the vicinity of the equilibrium state. This is done to reduce the effects of the variations in the manipulated objects and the nonlinear terms on the linear controller. Thus leading to better assessment of the inlterrelationships between the robot arm structural flexibility and the controller design. The first step in designing the controller is to express the rigid body equations of motion in terms of state variables which are defined as follows: Y1 r YV2= y3 - () y4r y= e y,= The general form of the nonlinear state equations, based on the rigid body equations of motion given in Appendix C, can be written as, -i=f (,) (4-2) where y is the state vector and u is the control vector. The function f is continuously differentiable in the y,'s. It is expanded in a Taylor series about the equilibrium state vr = Irr,,e,O0,0,0 to give, /,(yU)- f(y,,u )+ (y, - y,,)+ - N - s Ij=1 goy k ~aj. (uk - ue ) + I higher order terms ] Define 6u = uk - ue and 6y = y, - ye. Substitute (4-3) into (4-2) and retain the

33 first order terms only, to obtain, S= A 6y+ BDu (4-4) Equation (4-4) represents a first order linear approximation of (4-2). The matrices A and B can be obtained from, Of 1Of1 of I Of _ri _ o'V, o.. _ _ 0., d - 01/6e Ou1 - 9u, A= - - and B - - (4-5) a/f ale af a af a 1 49e ye.A, dUi aUs 3y u Note that each rigid body degree of freedom driven by a leadscrew has two equations representing its motion on the lower and upper thread. This results in two sets of linear equations. The A and B matrices are listed in Appendix D Next the design of a linear controller for the rigid body axes r, 0, and o is considered. In modeling such a controller, the dynamics associated with joint sensors and actuators are neglected. It is assumed that r, e and 0 and their time derivatives are available, and that the control torques T1, T2, and T, can be applied to drive each axis. A simple linear controller is used. It is a multiple input - multiple output integral plus state feedback controller whose control vector, u, can bewritten as, = -K y + K'(R- Cy)dt (4-6) where KS is the 3 x 6 state feedback gain matrix, K' is the 3 x 3 integral gain matrix and Rr = [R, R2, R8, R, 0, 0 is the desired reference input. The term Cy

34 represents the system output where the C matrix has the following form I 0 0 0 0 0 0100 C- 0 1 0 0 o o 0 1-0 0 By defining three additional state variables as, J7 = (y - Rl)dt y8 = f(Y2- R2)dt v = f(s - R~)dt (4-7) the control vector, u, becomes = -KZ (4-8) where Z' -= Yl,2,ysy4,y6,y8,Y7,s,syd. The 3 x 9 gain matrix K now has the following form: K =K K: (4-9) The gains are chosen based on the linear model described in (4-4) and (4-5). The selected gain matrix is of the following form, is ks 0 ks 0 0 O l0 0 K-= ks k o O k 0 O k 0O o 0o k 0 0 ks 0 0 kj This controller decouples the linearized system, thus allowing us to carry out a design procedure for each axis independently to achieve a damping ratio of 4 = I and desired servo-loop frequencies of w,, w, and w, for the r, i, and t axes respectively [49]. The formulations needed to evaluate the gain matrices of the lower and upper thread motions are given in Appendix D. This simple controller is then

35 I I L- J Figure 4-1. Block diagram of the integral plus state feedback controller applied to the robot arm as modeled by both the rigid body and the combined rigid body and flexible equations of motion. This is illustrated in the block diagram in Fig. 4-1. The computer code written for the digital simulation is listed in Appendix F. The simulation results are presented in the next section. 4.2. Results and Discussion The dynamic equations of the flexible robot arm along with the equations obtained from the controller lead to a set of seventeen complex, coupled, highly nonlinear stiff equations. The difficulty in handling stiff problems is that most conventional numerical algorithms for solving ordinary differential equations require a time increment equal in measure to the minimum time constant of the system while the problem range which is the difference between initial and final

36 value of time is equal in measure to the maximum system time contant. As a result, the problem cannot be run to completion in a reasonable number of steps. Gear's method [50], used here, is well suited to handle stiff systems since it automatically changes the step size depending upon the region where the solution is relatively active. The purpose of the simulation studies is to investigate the inter-relationships the effect of constraints due to transmission mechanisms, forced excitation due to inertial forces, and potential instability mechanisms such as resonance. The standard set of physical system parameters used in the computer program are listed in Table 1o The results obtained for the rigid and flexible motion are shown in Fig. 4-2 to 4-13. The first three plots show the critically damped behavior of the rigid body degrees of freedom r, 6, and &0 This illustrates the good performance of the linear controller in the vicinity of the equilibrium point. The plot in Fig. 4-5 represents the motion of the flexible coordinate q,,(t). In the transient response, one notes the following: (1) The excitation of the structural vibration is due to the effect of the rigid body motion. This excitation decreases with time due to the damping introduced by the rigid body controller, and to the diminishing effect of the inertial forces associated with the rigid body motion as it approaches the steady state.

37 TABLE 1 Standard Set of Physical System VALUE Parameters Mass of the first beam (ml) 0.698 Kg Mass of the second beam (m) 0.0429 Kg Mass of the Payload (m, ) 0.05 Kg Cross sectional area of the second beam (A2) 0.00003167 m2 Length of the first beam (L 1) 0.358 m Length of the second beam (L2) 0.5 m Gravitational acceleration (g) 9.81 m /ec2 Aluminum density (p) 2707 Kg /m Flexural rigidity (El) 5.67 Pa Reference position for r 0.4 m Reference position for 0 0 rad Reference position for 0 0 rad Desired reference position for r 0.5 m Desired reference position for 0 0.5 rad Desired reference position for 0.5 rad Servo natural frequency for r (w,, ) 4 rad/sec Servo natural frequency for 0 (w3.) 4 rad/sec Servo natural frequency for $ (w,#) 8 rad/sec

38 (2) The natural frequency of the flexible mode decreases with time due to the increase of the span of the flexible beam and to the vanishing effect of the rigid body inertial forces applied to the robot arm as modeled by both the rigid body and the combined rigid body and flexible equations of motion. (3) The steady state response is a small amplitude sustained oscillation around a negative value. The latter represents the static deflection due to gravity while the sustained oscillation shows the decoupled response of the elastic motion from the rigid body motion at steady state due to the effect of the self locking condition of the leadscrew. The motion of q12(t) is illustrated in Fig. 4-6. It has one additional feature. The magnitude of the vibratory motion is on the order of 10om while the one for qi1(t) is on the order of Io-m. This is due to the large amount of energy required to excite the higher modes. Coupling with the first mode is also evident. Figures 4-7 and 4-8, which illustrate the flexible motion represented by q1,(t) and q2(t), indicate a very important aspect of this particular robot design. The transient responses show the large excitation induced by the rigid body motion, and then die out with time. These interesting results can be interpreted as follows: (1) Gravity doesn't affect the flexible motion in the q21(t) and q=(t) directions. Therefore, their motions are expected to either die out to zero with time or oscillate around zero (i.e. no static deflection).

39 r disrlacement [meterl C1 ~:. r.____________ UO 1.60 3.20 4.80 Time. t secnd! Figure 4-2. r response obtained from the rigid body controller in the base run.

40 e rotatio n Iradiani =b.00 1.60 3,20 4.8C Time. t [scondl Figure 4-3. e response obtained from the rigid body controller in the base run.

41 rotation [radianr b00o 1.60e 3.20 4.80 Tme t [second] Figure 4-4. * response obtained from.the rigid body controller In the base run.

42 Disolaoement 11Wu[meterl.'0 -.6 3.20 4.80 Time, tsecondl Figure 4-5. Flexible motion coordinate q11(t) In response to the rigid body controller In the base run.

43 Disolaoement Q1 2lmete) r IC I0 i.60 3.20 4.ac?A.,,.^. O0 1.60 3.20 4.8: Time. t secondl Figure 4-6. Flexible motion coordinate q12(t) in response to the rigid body controller in the base run.

44 (2) Since there is no constraint on the rigid body motion in the o direction, the flexible motion in the q21(t) and q22(t) directions act as a disturbance source on the rigid body motion in the H direction. These disturbances are compensated for by the rigid body controller. Figures 4*-9 to 4-11 represent the control torques T1, T2, and T3. The oscillations in the control signals for the base and second joints show the effect of the flexible motion on the rigid body motion. The control torques T1 and T2 for the r and a axes respectively are set to zero once the steady state is reached. This is due to the nonbackdrivable characteristic of the leadscrew transmission mechanism which satisfies the self locking conditions. The total deflection of the end effector in the vertical and the horizontal directions (i.e. v and W in equation (2 -4)) are shown in Figs. 4-12 and 4-13 and found to be dominated by the first mode and nearly identical in shape to Figs. 4-5 and 4-7. Additional runs are made with modifications to the standard set of parameters to study the behavior of the system in the following areas: (1) Instability mechanisms: The flexural rigidity, El, is reduced to 0.44Pa. This can be achieved by constructing the flexible beam of polymers. As a result, the natural frequencies of the flexible beam are lowered to values that are close to the servo loop frequencies. The main results are shown in Fig. 4-14 to 4-18. The important features of this run are: (a) In Figs. 4-14 to 4-16, the 4 response has small overshoot. The control torques for the base and second joints are very oscillatory. This illus

45 Disolacement q21 Wt) meter Ci To I 0.00 1.60 3.20 4.80 Time. t [second] Figure 4-7. Flexible motion coordinate q21(t) in response to the rigid body controller in the base run.

46 I C122MI3et '0.00 1.60 3.20 4.8C Time. t [second Figure 4-8. Flexible motion coordinate q22(t) in response to the rigid body controller In the base run.

47 Control toraue ItI) [N-MI c0 == 'too 1.0 3.20 4.80 lmeL. t seond Figure 4-9. Control signal for the prismatic joint obtained from the rigid body controller In the base run. rigid body controller in the base nun.

48 Control toraue I2LIUNil,, '0.00 L60 3.20 4o8C Tme. t fsecondi Figure 4-10. Control signal for the second joint obtained from the rigid body controller in the base run.

49 Conitrol toraue B 13(t) N-M Ue 'o00 1.6 3.20 4.80 Time. t Isecond Figure 4-11. Control signal for the base joint obtained from the rigid body controller in the base run.

60 V defiectikn lmetedr I - OO 1.606D 3.20 4.8! Time, I [secondl Figure 4-12. Total vertical deflection, v, obtained from the rigid body controller in the base run.

61 I.00 1.60 3,20 4.SCFigure 413. Total horizonta deflection w, obtained from the rigid body controller he base run.4. Time. t [secondi Figure 4-13. Total horizontal deflection, w, obtained from the rigid body controller In the base run.

62 trates the effect of the flexible motion on the rigid body motion. (b) The flexible motion frequency is reduced and the magnitude of the oscillation in both the vertical and the horizontal directions increased. (see Fig. 4-17 and 4-18). (c) Due to the small value of El and to the proximity of the servo-loop frequency wa to the fundamental beam natural frequency, the amplitude of these oscillations grow with time. This is clearly shown in Fig. 4-18. Such proximity is not likely to occur in practice, but would lead to very poor performance when it does. Note that the integration is interrupted at 1.24 seconds since this case could not be run to completion with a reasonable computation time due to the divergence of the solution. (2) Speed accuracy trade off: The servo loop natural frequencies for r, e, and A, are raised from w, = 4,w= 4 and w = 8 rad/sec in the base run, to, = 8, w =8, and w = 16 rad/sec. Comparing the numerical results obtained with those of the base run, an increase on the order of 2 is observed in the amplitudes of the flexible motion coordinates. This is illustrated in Fig. 4-19 and 4-20 where the- response of the first elastic mode in the horizontal and vertical directions are given. Unlike the base run, the responses of 6 and ( (see Fig. 4-21 and 4-22) show the effects of the flexible motion on the rigid body motion. This is also reflected in Fig. 4-23 and 4-24 where the control torques for the base and second joints become more oscillatory then

63 A rotation fradiani 1i c.00 0.80 L60 2.40 Time. t second] Figure 4-14. * response obtained from the rigid body controller in the instability run.

64 Control toraue ime t oT.OO 0.8e0 1.60 2.40 Tme. t lsecondl Figure 4-15. Control signal for the second joint obtained from the rigid body controller in the instability run.

55 Control torque TIPQ IN-Ml ~WLPI 4 0.00 0.80 160 2.40 Time. t second Figure 4-16. Control signal for the base joint obtained from the rigid body controller in the instability run.

66 V deflection Imeterl s 0.00 0o80 160 2.40 Time. t second] Figure 4-17. Total vertical deflection, v, obtained from the rigid body controller in the instability run.

67 W deflection lmeterl,.. '' I CD 0.00 0.80 1.60 2.40 Time. t Isecond Figure 4-18. Total horizontal deflection, w, obtained from the rigid body controller in the instability run.

68 Diseolacement,11(If Imeterl -:l '0.0 0.00 0.o40 0.80 1.20 Time, I second) Figure 4-19. Flexible motion coordinate q11(t) In response to the rigid body controller in the speed-accuracy trade-off.

59 00 Disolacement g21(t) meter] I 0.00 0.40 0.80 1.20 Time. t Lsecondl Figure 4-20. Flexible motion coordinate q21(t) in response to the rigid body controller in the speed-accuracy trade-off.

60 e rotation radian] IT.'o 0. Id ~ I ': 1 bo00 0.40 0.80 1.20 Time. t Isecondl Figure 4-21. 8 response obtained from the rigid body controller in the speed-accuracy trade-off.

61 rotation kadiraa 0 I! _ %.oo 0.40 0.80 1.20 Time. t [secon Figure 4-22. * response obtained from the rigid body controller in the speed-accuracy trade-off.

62 Contrl torque I2(UtIMI c~ 0.00 000 0.00 0.40 0.80 1.20 Time. t secondl Figure 4-23. Control signal for the second joint obtained from the rigid body controller in the speed-accuracy trade-off.

63 Control toraue 13 LN-Mo I. 8 0.00 0.40 0.80 1.20 TJme. t econd Figure 4-24. Control signal for the base joint obtained from the rigid body controller in the speed-accuracy trade-off.

64 e rotation 1radian) Ca C -'.00 -0.80 1,60 2.40 Time t fse&nd1 Figure 4-25. e response obtained from the rigid body controller with the payload increased to 78.5 g.

65 their counterparts in the base run. (3) Effect of eadscrew on the controller design: The payload mass is increased from 50g in the base run to 78.5g and the gains of the rigid body controller are retuned to compensate for this new payload mass. The latter increases the flexible motion inertial terms, causing large fluctuations in the magnitude of the normal force, N, and even in some cases changing its sign. Recall from the leadscrew constraint derivation that changes in the sign of N result in switching of the motion from the lower to the upper thread or vice versa. This will, depending on the type of motion, either increase or decrease sharply the required torque to rotate the leadscrew which may be under tension or compression. Thus causing a deterioration in the controller performance in the vicinity of the transition period. This is clearly shown in Fig. 4-25 where the notch in the e response is caused during the transition of the motion from one surface to another. 4.3. Summary and Conclusions An integral plus state variable feedback controller, based on the linearized version of the rigid body model of the robot arm, is derived. This simple controller design is considered to be representative of the robot controllers currently in use. The dynamic model and the controller design are used as the basis for the simulation studies. The latter demonstrates the potential mechanisms by which the robot structural dynamics and controller design can interact.

66 The effects of constraints, due to the leadscrew transmission mechanisms, give the robot arm a cantilever beam-like behavior in the r and e directions which leads to some sustained vibratory motion of q,,(t) and q,(t) at steady state. The absence of the constraint in the 0 direction allows the strain energy to be dissipated by the rigid body controller. The simulation results demonstrate the following additional features: (i) There is a potential for instability if the servo-loop frequencies approach the natural frequency of the robot arm, and (ii) High speed operation deteriorates the accuracy of the system, and results in a trade-off between speed and accuracy. These results are very useful for designing and evaluating potential approaches to the flexible robot arm control problem. The latter will be discussed and treated in the next chapter.

CHAPTER 5 DESIGN AND IMPLEMENTATION OF THE RIGID AND FLEXIBLE MOTION CONTROLLER The interrelationships between the structural flexibility and the controller design, obtained in Chapter 4, provide guidance in developing the multiple-input multiple-output controller for the rigid and flexible motion of the robot arm. Towards achieving this goal, a simple case, involving the controller design of a three lumped mass system, is treated. The latter simulates the control problem of a compliant beam whose flexible motion is approximated by two elastic modes. The rationale is to start with the control of a relatively simple system as a preliminary step towards the general control problem of the flexible robot arm. 5.1. Description of the Three Lumped Mass System In an attempt to gain insight into the control problem of the flexible robot arm, a simple system consisting of three lumped masses connected by massless springs is considered (see Fig. 5-1). This system can be viewed as a lumped model of an articulated compliant beam. The rationale is to scale down the complexity of the dynamic model of the robot arm, to get better insight into the controller 67

68 F1(t) X1 1 mI x3 Figure 5-1. A schematic of the three lumped mass model.

69 performance, while working with a simple linear model, and to reduce the cost of computer runs. The first mass reflects the rotational rigid body degree of freedom of the joint whereas the second and third masses represent the first and second elastic modes of transverse vibration. No structural damping is considered in the discrete model and the springs represent the structural stiffness of the beam. The three lumped masses are supported in the vertical direction to include the effect of gravity. The control force is applied to the first mass only. Denoting the displacements of the first, second and third masses by 21, Z2 and zs respectively. The equations of motion can be written as miz 1 = k1(z2 - z) + mIg + F1 (5-la) m2 2 =-k1(22- 21) + k2(z3 - 2) + m2 (-lb) m3:s = -k2(z3 - 22) + mSg (5-1c) where,1 and k2 are the structural stiffness terms and g is the gravitational acceleration. 5.2. Controller Design for the Lumped Model Two types of controllers are used for the three lumped mass model. The first is based solely on the motion of the first mass. This gives a good indication of the effect of the rigid body controller on the flexible system which is represented by the second and third masses. The second controller is designed based on the motions of the first and second masses, thus, including the flexible motion into the control action. The third mass, representing the second elastic mode, is

70 intentionally not considered in the controller design to study the effect of control spillover. In both controller designs, an integral plus state feedback controller is implemented. The gains of the rigid body controller are computed based on the equation of motion for the first mass, mzX I- F + mlg (5-2) This equation is identical to (5-la) with the flexibility term k(z2 - zx) set to zero. The control force, F1, has the following form, ^F = -[k ]| - k f(z' - R,)dt (5-3) By substituting (5-3) into (5-2), the gains are computed to achieve a damping ratio of C = 0.53 and a desired natural frequency for the first mass. The controller is then applied to the three lumped mass model. The rigid and flexible motion controller is designed based on the linear equations of motion of the first and second masses, mi 1 = F1 + mlg + k1(T2 - Z1) (5-4a) m2z 2- m2 - k 1(2 - z ) (5-4b) These equations are similar to (5-la) and (5-lb) with the coupling term k2(z3 - Z2) set to zero. Define the state variables to be, Y1 =1 z 2 2 Ya= zl 4 - 2 Y& fJ(l -Ri)dt the equations of motion become,

71 0 0 100 0 0 0 0 I 0 = _ O 0 0 Ms + |L F1+ g (5-5) m= m1 mi k] k] ~ -- 00 0 M4 0 m2 mT2 1 0 000 M0 0 in matrix notation, this can be written as, y- Ay+ BF, + W (5-6) where the vector, W, shows the effect of gravity, Note that with this notation, the integral action becomes embedded in the state equations. Thus, the integral plus state variable feedback controller can simply be written as, F1 -Ky (5-7) where the gain matrix KTr Ik ks k{ ks kJ k ]. Substitute (5-7) into (5-6) to get, y Ay+ BF + W- (A - BK)y + W (5-8) The eigenvalues of (A - BK) matrix can be arbitrarily assigned by the state feedback F, = -Ky, as long as the system given by (5-6) is controllable. First, the state equations are converted to the controllable canonical form. This is done by transforming the state vector y to a new state vector z using a constant transformation matrix T so that y= TZ (5-9) The differentiation of this equation yields,

72 p TZ (5-10) Substituting these values into (5-6) to get -Z T-ATZ+ T-IBF, + T-'W =A'Z+ B'F + TW (51) where A — T-AT and B' T-B. Let t, be the ith column of T, i.e. T- t ipt2.....,] J(5-12a) Use the formulation given in [511, [52], [53], 154] to compute the, 's t =B At, - t,, - alt, (5-12b) At2 - t - an-t Atl -a, t, where the a, 's are the coefficients of the characteristic polynomial of A. This choice of T leads, to the following general form of the matrices A' and B, 0 1 0 0 O 0 0 0 1 0 0 0. A'- T-1AT - O O O 0| and B'- T-'B 0 (5-13) 0 0 0 0 1 0 a5 ~~a4 -af 3 ~^ " 1 -

73 In the new state equations, the control signal F1 becomes F1 -K'Z -K'T-y= -Ky (5-14) where K'r- [k, k;,, k;, k;, k. Use (5-14) in (5-11), Z= (A'- B'K)Z + T-W (5-15) the closed loop matrix can be written as, 0 1 0 0 0 o o 1 0 0 A'- B'K' O O 0 1 0 (5-16) O 0 0 0 1 -ask-;) (-a- k) ( — Ak;) (-a-;) (-a -k The gains are computed by matching the entries of (A'- BK') with those of the desired matrix A'd. The latter has the following form, o 1 0 0 0 o o 1 0 O A d 0 0 0 1 0 (5-17) 0o 0 0 1 -ads -ad4 -ad3 -ad2 -adl where ad,'s are the i^ coefficient of the desired characteristic polynomial. The desired eigenvalues are assigned according to settling time and percent overshoot considerations. The gain matrix, K, of the new state vector is directly related to the gain matrix, K, of the original state vector through equation (5-14). This leads to, K = K'T (5-18) The merits of the two controllers, derived in this section, are compared by applying them on the system of three lumped masses.

74 6.3. Results and Discussion of the System with Three Lumped Masses The gains for the rigid body controller are computed to achieve a damping ratio f = 0.53 and a natural frequency, w = 15 rad /sec This controller is applied to the three lumped masses and the results are illustrated in Fig. 5-2 to 5-4. The response of the first mass, x,, is very oscillatory due to the effect of coupling with the second and third masses. Note that the three lumped mass model (see Fig. 5-1) represents a conservative system. That is, once the system is excited, it should oscillate indefinitely. However, the decay in the.2 and z, responses, in Fig. 5-3 and 5-4, is caused by the damping induced by the rigid body controller. For best evaluation of the rigid and flexible motion controller, a reduced model, consisting of the first and second mass, is initially used. The exclusion of the third mass eliminates the effect of control spillover. Figures 5-5 and 5-6 represent the responses of the first and second mass respectively. The overshoot is due to the assignment of the desired damping ratio = 0.53. The oscillation seen in Fig. 5-2 and 5-3 is completely eliminated due to the additional damping introduced by the rigid and flexible motion controller. Finally, to show the effect of control spillover, the rigid and flexible motion controller is applied on the three lumped masses. This is illustrated in Fig. 5-7 to 5-9. The responses of xz and Z2, which represent the displacements of the first and second mass, become more oscillatory then in Fig. 5-5 and 5-6. This deterioration in the response is expected due to the effect of control spillover. However, the performance of this controller, even in the presence of control

76 X1 dispeme [meter] 'o 0 'C.oo 4.00 8.00 12.00 Time. t [sec Figure 5-2. First mass response obtained from the rigid body controller.

76 X2 dspacement [meter] r0 co Cb.00 4.00 8.00 12.M0 Time. I* *l Figure 5-3. Second mass response obtained from the rigid body controller.

77 X3splacement [meter] ^. 0 Ct ~ ~~o~ 1:.00 4.00 8.00 12.00 Time. t [second b Figure 5-4. Third mass response obtained from the rigid body controller.

78 X1 dspacement [meter] -T 00 C.Oo 4.00 8.00 12.OC Time. t Isecond Figure 5-5. First mass response obtained from the rigid and flexible motion controller In the reduced order model case.

79 X2 displacement [meter] ~0.00 4.00 8.00 12.00 Tlme. t (second Figure 5-6. Second mass response obtained from the rigid and flexible motion controller In the reduced order model case.

80 X1 dspacement [meter] =b.oo 4.00 8.00 12o00 Time. t secon Figure 5-7. First mass response obtained from the rigid and flexible motion controller in the control spillover case.

81 X2 displacement [meter] CD cm C C~ 1 b.o0 4.00 8.00 12.00 Time. t lsecond] Figure 5-8. Second mass response obtained from the rigid and flexible motion controller in the control spillover case.

82 x3 dsplacement [meter] Co _.00 4.00 8.00 12.00 ime. t [seconda Figure 5-9. Third mass response obtained from the rigid and flexible motion controller in the control spillover case.

83 spillover, is still superior to the rigid body controller (see Fig. 5-2 to 5-4). The oscillatory motion is approximately reduced to half. At steady state, the deviation of the second and third mass displacements from the desired value is caused by the static deflection of the springs. 5.4. Linearization of the Rigid and Flexible Equations of Motion The first step in designing the rigid and flexible motion controller for the robot, is to obtain a linearized version of the equations describing the combined rigid and flexible motions. The latter have the following general form, Mx(z)z + F(z,z)- F' (T) (5-19) where zr = [r,,,q,qjq2, q2,q22 and TT= I T,,T3TJ. The inertia matrix M(z) is not diagonal. Therefore, the linearization procedure, outlined in section 4-1. cannot be applied directly. An intermediate step, involving the algebraic computation of the inverse inertia matrix, is required to transform (5-19) to the form given in equation (4-2). To avoid this difficulty, another procedure, which is also based on Taylor series expansion, is implemented. Suppose T. (t),zx (t),zi (t) and Z (t) satisfies the second order differential equation given in (5-19) for t > 0. Define z(t), (t) + 6z(t) (5-20) 7t) = T(t) + MT(O) Substitute (5-20) into (5-19), to get M(z, + z)[ "Z +_6] + F(Z,3 + ), Z + z)= F' (T + a) (5-21)

84 Expand by Taylor series the nonlinear terms around z, and T,. This yields,. M(:,).. or(,i,) M(x )+ M Z +: + F(,Z. )+-.F..) [ (o) z _ " ].," ~z ). (5-22) F, ) - (- F' (Ts) + ST+ higher order terms Rearranging the terms, {. - QOM(,Xa ).. OM( ). M(z, )z' + F(Z';,,Z )- Ft ( Ts) + m(Z, )- + a(. ) + Z - (5-23) F(,,; ) OFC(, i, ) F' (T) + F( z - 6z +.. - -- 6: =.T T+ higher order terms az O 9 9T T Assuming that the solution, x,, is at equilibrium (i.e. Z, = z, = 0 ) and retaining only first order terms, equation (5-23) becomes aMF(z,,O) OF(,3,O) OF' (T7,):~M(x )&' 6 + o~ - _ 6(5-24) which can be rearranged to OF(n,,O) M1 ( O.F,O) aFI MR.) = -bzox_ 6 - M- + M-(, ) (T, (5-25).... az ~T. where M~-1(,) is the inverse of the inertia matrix M(z) evaluated at Z,. In matrix form, equation (5-25) can be written as, - [O 0 7x7 17x 7 07x 1 I OF(z,,O). F(,,O) _i + OF' - [~M1(:) Ox -M-'(z,) ---~ --- ]11, ~....... -- ]M (Z ) aT] _"z ox L The linearized form of the equations of motion for the rigid and flexible motion can be readily obtained by evaluating equation (5-26).

86 5.5. Derivation of the Rigid and Flexible Motion Controller This section, is a very important one, in that it contains a detailed discussion of how the flexible motion is treated in the design of the rigid and flexible motion controller. Any compliant link would undergo static and dynamic deflections. There are two approaches with which the static deflection problem can be handled. In the first approach, the static deflection is computed theoretically and then taken into consideration in specifying the desired end effector final position to the controller. In the second approach, additional sensors and actuators are used. The static deflection is measured and corrected for by the additional actuator referred to as a "straightness servo" in 14]. In this work, no attempt is made to compensate for the static deflection, as this can be treated as a separate problem. The control objective is to introduce additional damping into the flexible motion. This is done by using additional sensors to measure the compliant link dynamic vibrations and feed them back to the controller. Theoretically, any flexible system has an infinite number of elastic modes. Due to physical limitations, a limited number of sensors and actuators can be applied, thus restricting the controller design to the few critical modes. Note that the outputs of the sensors would contain information about the unmodeled as well as the modeled modes. This is referred to as observation spillover. Similarly, the control action would affect both the modeled and unmodeled modes leading to the control spillover phenomenon. Based on the work done by Balas in [30], it is expected that the higher unmodeled modes can cause detrimental effect on the system response. It

86 can even lead to instability. To examine the effect of observation and control spillover, only the first elastic mode in the vertical and horizontal directions are considered in the controller design. The second mode is considered to be a representative of the higher unmodeled modes. The rigid and flexible motion controller is designed based on the linearized equations of motion given in (5-26). An integral plus state feedback controller is implemented. The integral action is applied on the joint angles to insure that they reach their desired values with zero steady state error. Define the state vector, y, and the control vector, u, to be, yg= u=6T (5-27) Then equation (5-26) can be written in the following general form, Ay + Bu (5-28) This represents a linear, time invariant, multi-input multi-output system. Three additional state variables are introduced to facilitate the implementation of the integral plus state feedback controller, y - = f(r - RI)dt y12 = f(- R2)dt yis = ( f- R3)dt This embeds the integral action into the state equations. Thus allowing the control action to be expressed as, u= -KF (5-29) where K' is the feedback gain matrix of the rigid and flexible motion controller.

87 Substitute (5-29) into (5-28), to get = (A - BK)y (5-30) The eigenvalues of the closed loop matrix, (A- BKF), can be arbitrarily assigned by the controller, defined in (5-29), as long as the system given by (5-28) is controllable 155]. Note that for a given set of eigenvalues, the solution for the gain matrix is not unique. The eigenvalues determine the stability and the speed of the response where the closed loop eigenvectors reshape the transient response. The non-uniqueness of the state feedback gain matrix, KF, in pole assignment provides the designer the freedom to partially select the closed loop eigenvectors [561, [57]. Standard procedures employ a transformation matrix to convert the linearized system to controllable canonical form for arbitrary eigenvalues assignment. Note that the set of eigenvectors are assigned by the particular choice of the transformation matrix. One such systematic method 1531 is used to compute the gain matrix K'. First, a state feedback is introduced to make the system controllable by a single component of the u vector. This converts the multi-input system to single input one. Second, the method established for single-input systems, which was used earlier in the three lumped mass case, is employed to assign the desired closed loop eigenvalues. Note that the gain matrix is not unique for a given set of eigenvalues. The methodology, described here, does not make any attempt to use the freedom given by the nonuniqueness of the state feedback gain to

88 partially assign the closed loop eigenvectors. The latter, which determines the shape of the transient response, ends up being arbitrarily assigned; thus, leading, in most cases, to poor transient response. This method is employed on the system given by (5-28). Although, the steady state response followed exactly the reference input, the transient response was very oscillatory. Most of the control effort was carried out by a single component of the control vector, u, thus rendering this particular control design to be completely unacceptable from a practical point of view. Being aware of these difficulties, the control problem is now approached from a physical point of view. No transformation matrix is used. Therefore, all the state variables have physical interpretations. The main task of the controller is to move the robot arm from one position to another with the least amount of vibration possible. From the results of the rigid body controller (see Fig. 4-2 to 4-13), the rigid body degrees of freedom r, a and ( exhibit a critically damped behavior with zero steady state error. However, the transverse deflection in both the vertical and the horizontal directions are oscillatory. In light of this, the rigid and flexible motion controller is obtained by expanding the rigid body controller with the emphasis on introducing additional damping into the flexible system. To do this, the terms in equation (5-26) which represents the open loop case, have to be physically interpreted. M-'(zxT)F0) consists of all the stiffness terms whereas M-l(z, ) aF ',) contains all the damping terms. By evaluating equation (5-28) and including the integral action one obtains,

89 0 0 00 00 0 1 0 0 0 00 1 0 0 0 00 0 0 1 0 00 0 00 v\ 0 0 0 0 0 0 0 1001 000 1 0 00 0o o o o I o o o 0 o 1 4 0 0 0 0 0 00001 I 000 V a - O O O O I V O, 1 _ An7 An 0 A74 0 0 0 O o o o o oo il 0 0~ 0 ^0 0 AG j 00000 00 0 o 00 VIr 0 00 0 I 0 0 0 0 0 1O O 1 0 0 0 0 0 0 0 0 0 0 0 0 0 is9 0 0 1 0 0 j 000 0 o 0o 0 0 0I A0 lo Ao 0 O O 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 Be1 B82 0 + B71 B72 0 U2 (5-31) 0 0 Bu B91 B92 0 U 3 0 0 Bo103 0 0 0 O O o 0 0 0 0 0 0 where the A,, and B,, terms represent the open loop terms. This yields Ml(z ). )= 0. That is no structural damping is considered. This is conOx- 0 sistent with the robot arm dynamic modeling assumption. Applying the rigid

00 body controller, u= -K y where K' represents the feedback gain matrix of the rigid body controller. [ 10 0 k0 k 0 0 0 k0 0 0 the closed loop matrix (A - BK) becomes, o o o o o 1 o 0 o o o o o o o o o o I o o o I o o o o o o o I o o 1o o 0 o o o o o 00 0 o o o oo1 o o o A61 — a A62- a 0 A4 O I -a, -a, 0 0 0 -I -a -1 0 (A- BK') A71 - a A72- a 0 A74 0 I -al -a, 0 0 0 - -a1 0 0 0 -al 0 A 8 0 0 -a1 0 I 0 0 -a1 A91-aA A2-aa 0 A O 0 -al -a1 0 0 0 J -ao -aO 0 0 0 -aO 0 A10,6 0 0 -a, 0 0 0 0 -aO 1 0 0o 0 o o0 I 0 0 0 0 1 000 o 0 0 o00 Io 0 0 0 0 1o 0 o o ooo 0o o 0 0 (5-33) where a1 terms represent the entries altered or created by the rigid body controller. The portion of the closed loop matrix that corresponds to M-(z, ) (0 - x

91 has nonzero terms. The latter represent the damping introduced by the rigid body controller in both the rigid and flexible motions. This is consistent with the decaying oscillation obtained in the transverse deflection for both the vertical and horizontal directions (see Fig. 4-5 to 4-8). Since the main emphasis of the rigid and flexible motion controller is to introduce additional damping into the flexible system, then the gain matrix of the rigid body controller is modified to give hk; kV 0 O k 0 0 kf9 0 k{,H 0 0O Kr =- k1 k2 O 00 k k 0 k- 0 0 k1,12 0 (5-34) O O kS 0 0o k o kr0o 1 0 0 k3 The corresponding closed loop matrix can be written as follows, o o o o o I i o o o o I o o o 0 0 000o I 1 00 o o 0 o o o I o 1 o o o o o o o oo o o o o o I 0 o o o o o 1 i o 10 0 A61-al A2-al 0 A4 I -al - 0 -a2 0 -a2 -0a - A71-a A72-al 0 A^O74 -al -a1 0 -a2 0 | -al -ta 0 (A - BK") 0 0 -a, 0 As6 I o -a1 o -a, I 0 O -C1 A91 - al A2- a 0 As4 0 - -al 0 - a2 0 | -al -al 0 0 0 -a1 0 A10,6 0 0 -a1 0 -a2 0 0 -a1 1 0 00 0 0 00 0100 0 0 1 000 000000 0 00 0 0 100 10 0 0 0 01 00 (5-35)

92 where A,, 's terms are obtained from the linearized version of the rigid and flexible motion equations. al terms represent the entries altered or created by the rigid body controller and a2 terms represent the entries created to induce more damping into the flexible system. Note that the selection of the gains kf, ki and kJlo should be done very carefully, since they simultaneously introduce new terms in the rigid and flexible motion equations. Thus, the larger these gains are, the more damping of the transverse deflections can be achieved and the greater is the effect of the flexible motion on the rigid body motion. This causes the latter to become oscillatory. Therefore a compromise should be made in introducing as much damping as possible while keeping the effect of the flexible motion on the rigid body motion to a minimum. 5.6. Results and Discussion of the Rigid and Flexible Motion Controller The rigid and flexible motion controller is first applied on a reduced order model where only one elastic mode is considered to represent the flexible motion in each the horizontal and the vertical directions. The rationale is to eliminate the effect of observation and control spillover. The results are illustrated in Fig. 5-10 to 5-17. The first three plots show the expected critically damped response of the rigid body degrees of freedom r,O and 0. They are identical to their counterparts in the base run of the rigid body controller (see Fig.4-2 to 4-4). The first mode in the vertical and the horizontal directions are presented in Fig. 5-13 and 5-14. The oscillations are due to the inertial forces emanating from the fast and

93 r displacement [meter (u o) 1.60 3.20 4.8 ~u.00 1.60 3,20 4.80 Time. t Isemon Figure 5-10. r response obtained from the rigid and flexible motion controller in the reduced order model case.

94 e rotation kradian CD,0.M00 1t60 3.20 4.80 Time. t fsecond Figure 5-11. e response obtained from the rigid and flexible motion controller in the reduced order model case.

96, rotation Iradianl! %.00 1.60 3.20 4.90 Time. t [secondl Figure 5-12. response obtained from the rigid and flexible motion controller in the reduced order model case.

96 'fernement l' 160.meter20 Figure 5-13. Fl 0.00exible motion coordinate q4.(t)in response to the rigid and flexible motion controller in the reduced order model case.

07 Dslacement g21(t lmeter] CS. fo. I ~ I I 0.00 1.60 20 4.80 Time. t flsecondl Figure 5-14. Flexible motion coordinate q21(t) in response to the rigid and flexible motion controller in the reduced order model case.

98 Control toraue 0.00 1.60 3.20 4.80 ~Mo 0.00 160 3.20 4.10 Time. t fsecond} Figure 5-15. Control signal for the prismatic joint obtained from the rigid and flexible motion controller in the reduced order model case.

99 t[0 Control torque I2(1) IN-MI case. '0.00 1.60 3.20 4.80 Time. t [second] Figure 5-16. Control signal for the second joint obtained from the rigid and flexible motion controller in the reduced order model case.

100 Control toraue T.1 It) N-M camseg I 0.00 1.60 3.20 4.80 Time. t [secondl Figure 5-17. Control signal for the base Joint obtained from the rigid and flexible motion controller in the reduced order model case.

101 sudden rigid body motions. However, these vibrations have rapidly died out, thus reflecting the additional amount of damping introduced by including the flexible motion into the control action. Finally, Fig. 5-15 to 5-17 show the applied control torques T,, T2 and T3. The oscillations in the control signals show the effect of the flexible motion on the rigid body motion. Additional runs are made to study the following effects: (1) Control spillover: The rigid and flexible motion controller is now applied on the full rigid and flexible motion model. (i.e. two elastic modes are used to represent the transverse vibration in both the vertical and horizontal directions). This is done to assess the effect of control spillover on the second uncontrolled elastic mode. The main results are shown in Fig. 5-18 to 5-23. The first four plots represent the first and second elastic modes in the vertical and horizontal directions respectively. Even though the responses corresponding to the first mode become more oscillatory then the previous case, the overall performance of the flexible motion coordinates show a reduction of approximately 50% in magnitude when compared with their counterparts in the base run of the rigid body controller. The large oscillation in the control torques T2 and T,, shown in Fig. 5-22 and 5-23, reflects the greater effect of the flexible motion on the rigid body motion. (2) Control and observation spillover: This run simulates an actual situation where both control and observation spillover can be present. In the experimental set up, accelerometers mounted at the end effector, are implemented to measure the tip acceleration. The latter is then integrated twice to yield

102 sO1 1emeted -I~CA c00.80 1.60 4 0O0O 0.80 1040 Figure 5-18. Flexible motion coordinate 11(t) in response to the ngid and flexible motion controller In the control spillover case.

103 C Q12~ [meter/ To iJL ^Figure 5-1. Flexible motion coordinate q12(t) in response to the rigid and flexible motion controller In the control Pllover case.

104!oo I.B3 I6O 2_O40 TsmeI femecdl Figure 5-20. Flexible motion coordinate q21(t) in response to the rigid and flexible motion controller In the control spillover case. e)

106 Disgoacement 22l( meterd N CT -c_ I 0. 10 0.80 1.60 2.40 Time. second Figure 5-21. Flexible motion coordinate q22(t) In response to the rigid and flexible motion controller in the control spillover case.

108 to I — 0.00 0.80 160 2.40 Time. t seconrd Figure 5-22. Control signal for the second Joint obtained from the rigid and flexible motion controller in the control spillover case.

107 Controt toraue CD ^ b -00 0.80 1.60 2.40 Time. t secondl Figure 5-23. Control signal for the base joint obtained from the rigid and flexible motion controller In the control spillover case.

108 the transverse deflections V and w and their time derivatives v and W. Assuming that the flexible motion is entirely due to the first mode, then V and W can be written as V = +1q11(t) W =' 1q21(t) (5-36) The flexible motion coordinates q11(t) and q21(t) become, V W n(t) = and 21(t) (5-37) where D, is the eigenfunction of a clamped free beam, previously defined in equation (2-5), evaluated at the end effector. In digital simulation, the observation spillover is introduced as follows, ()= 1(r)q11(t ) + (-)q12(t) V(r) (5-38) (() (r)q21(t) + ')2(r)q (t) W(r) (53) Thus the (t)and (t)measurements that arefed bac to the controller Thus the qii(t) and q^{2t) measurements, that are fed back to the controller, are no longer representatives of the first mode only. Instead, they contain informations about both the first and second elastic modes. Figures 5-24 to 5-27 present the main results of this run. In Fig. 5-24 and 5-26, the responses of the first mode in the vertical and horizontal directions are almost identical to those in the control spillover case. However, the uncontrolled second mode exhibits more oscillatory behavior in the horizontal direction while it becomes unstable in the vertical direction, This instability is obtained due to the combined effect of control and observation spillover on the second mode.

109 sacement;11(t) [meter I.00 0.80 60 2.40 Time. t seco nd Figure 5-24. Flexible motion coordinate ql1(t) in response to the rigid and flexible motion controller In the control and observation spillover.

110 Disaacement;11[t) [fme ter] cm observation spillover, lime. I (second) Figure 5-25. Flexible motion coordinate q12(t) in response to the rigid and flexible motion controller in the control and observation spillover.

921t imeter rV ~ 1 0.00 0.80 1.60 2.40 Time. t second Figure 5-26. Flexible motion coordinate q21(t) in response to the rigid and flexible motion controller in the control and observation spillover.

112 Dispacement q22t) meter OI00 0o80 1.60 2.4C Time.t [second Figure 5-27. Flexible motion coordinate q22(t) in response to the rigid and flexible motion controller in the control and observation spillover.

113 Balas discusses the stability issue under these conditions in [30]. (3) Control and Observation spillover in the presence of structural damping. Any physical system possesses a certain amount of structural damping. The latter is introduced into the dynamic model of the flexible robot arm by using Rayleigh's dissipation function, F, which can be written in the following general form, F= 2 S q (5-40) 2 1 Sm where c,? is the damping coefficient. Considering only diagonal terms (i.e., -= 0 for r 7 8) equation (5-40) becomes, F = (cq1i + 22 + + cQ) + c4q2) (5-41) The values of C, 's are determined experimentally and found to be 0.103 kg rad/sec which corresponds to a damping ratio, ~ = 0.0145. Viscous damping forces can be derived from Rayleigh's dissipation function, F, using the following formulation, Q — a, (5-42) where z, is the it generalized coordinate. Dividing the nonconservative forces Q,, in equation (2-15) into those dissipative type and those of impressed upon the system by external forces, Lagrange's equation become d |T a } T +V + a F y^ ~, -~ ++ = Q1,cd (5-43) dt aZ, ~ aZ, Ox, Ox,

114 where Q@.cD denotes generalized forces due only to external forces exerted on the system. The rigid and flexible motion controller is then applied to the modified robot arm model described in equation (5-43). The results are shown in Fig. 5-28 to 5-31. These plots are identical to their counterparts in the undamped model with control and observation spillover case except for the response of the second mode in the vertical deflection, ql2(). which becomes stable. 5.7. Summary This chapter provides the design of the rigid and flexible motion controller for the robot arm. First the control of a three lumped mass system. which represents a discrete model for a single compliant link, is developed. The rationale is to start with the control of relatively simple system and progress gradually towards the general nonlinear problem. Two types of controllers are employed in the lumped.,model study. The first, simulating the rigid body controller, is based entirely on the motion of the first mass. In the design of the second controller, the dynamics of both the first and second masses are considered. The third mass is deliberately left out to study the effect of control spillover. The main results obtained, can be stated as follows, (1) The rigid body controller provides some damping of the flexible motion.

116 Displacemgnt Q1 filmeterl - CO E0. '0.00 0.0 1.60 2.40 Fiure 5-28. exblemotion coordinate q(t)in response to the riid Figure 5-28. Flexible motion coordinate qil(t) in response to the rigid and flexible motion controller in the control and observation spillover with structural damping Included.

116 Disldacement g12(f lmeterl TO 0.00 0.80 L60 240 Time. t (second Figure 5-29. Flexible motion coordinate q12(t) In response to the rigid and flexible motion controller In the control and observation spillover with structural damping included.

117 Dislacement 2l() m rmete~ f0t N.00 0.80 1.60 2.40 Time. t {second Figure 5-30. Flexible motion coordinate q21(t) in response to the rigid and flexible motion controller in the control and observation spillover with structural damping included.

118 Disolaernent = l So. '000 0.80 1060 2.40 Ilm LseonIz Figure 5-31. Flexible motion coordinate q22(t) in response to the rigid and flexible motion controller in the control and observation spillover with structural damping included.

119 (2) In the absence of observation and control spillover, the rigid and flexible motion controller completely eliminates the vibratory motion. (3) In the presence of control spillover, the rigid and flexible motion controller reduces the oscillation observed in the rigid body controller case by almost half. The rigid and flexible motion controller is designed based on a linearized version of the general equations of motion. Only the first mode of the transverse deflection in both the vertical and horizontal directions are considered in the control action. Higher modes are left out to study the effect of control and observation spillover. The controller objective is to add more damping to the flexible motion while maintaining a good rigid body motion performance. The following conclusions can be drawn: (1) In the ideal case where there is no observation or control spillover, the flexible motion recovers rapidly from the sudden excitation induced by the rigid body motion inertial forces. (2) In the presence of control spillover, the overall performance of the flexible motion show a reduction of approximately 50% in magnitude when compared with their counterparts in the base run of the rigid body controller. (3) In the presence of observation and control spillover, identical results are obtained for the flexible motion as in the control spillover case except for the response of the uncontrolled second mode in the verti

120 cal direction which becomes unstable. (4) In the presence of structural damping with observation and control spillover; the response of the uncontrolled second elastic mode in the vertical direction becomes stable. A comment is in order here. One should note that even in the presence of observation and control spillover, the rigid and flexible motion controller provides an additional damping capable of reducing by approximately 50% the magnitude of the flexible motion oscillation detected in the rigid body controller. There is a need for experimental validation of the dynamic model of the robot arm, as well as evaluation of the rigid body controller versus rigid and flexible motion controller. This will be the subject of the next chapter.

CHAPTER 6 EXPERIMENTAL RESULTS AND COMPARISON TO SIMULATION RESULTS The flexible robot arm control problem has been investigated using analytical methods and digital simulation. There is a need for experimental evaluation of both the dynamic moQdel of the robot arm and the controller design. This chapter experimentally compares the rigid and flexible motion controller with the rigid body controller to evaluate the merit of measuring and feeding back the flexible motion. 6.1. Experimental Set Up The three degree of freedom spherical coordinate laboratory robot, described in section 2.1, is used for the experimental work. It is interfaced to an IBM/PC microcomputer. A Tecmar Lab Tender interface board which has built-in 8 bit analog to digital and digital to analog converters is used. The schematic of the experimental set up is given in Fig. 6-1. Three DC servo motors, driven by power amplifiers, are used as actuators. The control torques are transmitted to the links by leadscrews for the r and e axes and a gear train for the ~ axis. One 121

122 DC motor dscr roew Loptic= i _ 4 " - ' i { _r 4 *J18o2 Add x screw. opticatl I (1M Pi,,o^ ^ r ~/ f' / //\\ ^ filter Figre. A Schematic of the expemental

123 tachometer generator and one optical encoder 158], [59] is mounted on each joint to measure the position and velocity of its corresponding rigid body degree of freedom. The optical encoder signals are read by the computer through digital counters. Each joint has a different size of counter associated with it according to their range of operation. A 20 bit counter is used to record the number of counts generated by the optical encoder monitoring the < rotation whereas 12 bit and 20 bit counters are used to record the U and r motions respectively. The contents of any counter can be strobed into a tri-state buffer. The latter can be accessed by the computer through differential drivers. Differential receivers are also used to reduce the noise on the control lines originating with the microcomputer. The part of the second beam protruding from the first beam, which is considered to be flexible in the digital simulation, is made of a thin aluminum rod. The payload is kept constant. It consists of a mounting stud and two Kistler piezotron accelerometers (model 8606A100). The latter are used to measure the end effector transverse vibration in both the vertical and the horizontal directions. The stability conditions for such systems where the sensors are not colocated with the actuators are discussed in [60]. The acceleration signals are passed through a Kistler piezotron coupler (model 5120). The latter has a built in Kistler low pass filter (model 5318A22) with a break frequency set to 60 HZ. This attenuates the higher mode signals, thus reducing the effect of observation spillover. The dc component of the coupler's output which represents the end effector acceleration due to the rigid body motion needs to be filtered out. This

124 is done by passing the acceleration signals through two cascaded high pass Krohn-Hite filters (model 3322) with a break frequency set to 3 HZ. Finally, the total deflection of the end effector and its time rate are obtained by integrating twice the accelerometer signals using analog double integrators [61], [62]. These integrators are used to devote most of the sampling period to the computation of the control action. The schematic drawing of the circuit board is given in Appendix E. The flexible motion signals are digitized by the 8 bit analog to digital converter whose resolutions are 0.2176 mm/count for the total deflection, V, and 16.314 mm/sec/count for V. The prismatic joint poses a very challenging problem. As the second beam slides inside or outside the first beam, the length of the flexible part varies. Thus, causing the beam natural frequencies to change significantly. This problem is not considered in this study. Therefore, the length of the part of the second beam protruding from the first beam, r, is kept constant. Due to hardware difficulties, the base joint governing the 0 rotation was not operational, thus restricting the experimental work to the second joint only. That is only the rigid body coordinate 6 and the flexible motion in the vertical direction, v, can be controlled. The horizontal deflection, w, would also be affected by the controller due to its coupling with V. The 0 rotation is measured by an optical encoder with 625 counts per revolution (Optisyn model 77-4-003-625AA). The latter is enhanced by connecting the encoder shaft to a gear train with gear ratio of 8:1 to yield a resolution of 31.25 counts per degree of 0.

125 In designing the second beam, the following constraints are taken into consideration: (1) The capability of the small dc servo motor mounted on the second joint. (2) The computation time required for the control algorithm. (3) The speed of the microcomputer used. To satisfy the first constraint, the second beam should be lightweight. The control algorithm or the speed of the microcomputer impose a very stringent constraint on the design of the second beam. In the experimental work, the flexible motion is considered to be dominated by the first elastic mode. In order to avoid aliasing [63], the sampling theorem must be satisfied. That is the sampling period must be at most half the harmonic motion period. Thus, the fundamental frequency, which is fixed once a certain design for the second beam is adopted, would set the maximum sampling period. However, for high fundamental frequency, the period, which is inversely proportional to the natural frequency, becomes very small. Depending on the speed of the microcomputer or the control algorithm used, the upper bound of the sampling period imposed by the design of the second beam, may or may not be large enough to perform the control loop. Therefore, in this work, the second beam is designed to have a low fundamental frequency. This would give ample time to try different control algorithms. For the experimental set up employed here, the length of the second beam is chosen to be O.Sm and its diameter is 0.635cm so that its fundamental frequency would

126 be approximately 6 HZ. The control algorithm used is an integral plus state feedback controller whose control loop requires a small amount of computation. The sampling period used is 0.024 sec. and is well below the upper bound imposed by the sampling theorem. 6.2. Experimental Evaluation of Model Parameters In contrast to the digital simulation, the dynamics of the actuators and sensors are considered in the experimental work. The robot arm is treated as a pure inertia loading on the motor shaft. The armature controlled DC motor used in the experimental set up is very common in servo control systems. The derivation of its dynamic model is well documented in the literature [49]. The corresponding transfer function can be written as follows, _ K~ (61) U T8 + where u is the input voltage to the motor, r is the time constant, and K is the overall gain of the motor, the power amplifier and the tachometer generator. The parameters K and r are determined experimentally from open loop runs. For a constant step input u(t) = A volt and initial condition 6 (t = 0) = o, the response of the system described by equation (6-1) can be given by 6(t) KA(1 - e)(6-2) At steady state, e,, is equal to KA. The gain, K, can now be easily determined since e,, corresponds to the steady state value of the open loop response of the system to a step input. The time constant, r, corresponds to the time at which

127 the open loop response reaches KA(1 - e-). Several open loop runs are made, where different step inputs are given to the system and the sensors values arc stored in the computer. The average values for K and r are found to be 2.284 degree/sec/volt and 0.15 sec respectively. 6.3. Design and Performance of the Rigid Body Controller The model described by equation (6-1) is first expressed in terms of state variables which are defined as follows: yl I Y2 (6-3) ysf(O- R2)dt where R2 is the desired position of e. The state variable y3 is introduced to imbed the error signal into the state equations. The latter would have the following form, 0 1 0 0 02 = | ~ 0 — '0 uY + 0 (6-4) ) cn be e ssd as In matrix form, equation (6-4) can be expressed as = Ay+ Bu + R (65) Using the procedure outlined in [641, the state equations are converted to difference equations which can be written as,

128 y (tk + 1) Py (t ) + Qu(tk ) (6-6) tk -k.T where T is the sampling period. P and Q are obtained from p__ eAATr+A(2++ AT2 A2T3 ADT T(+ P CA Ir 1- + AIT + +.....+ (i + 1) ) (6-7) Q feAd IT+ I AT+ A T+ A TO+' ^ + - i+ -) +..0 Q =I e { 2! 3! (i + 1)!. Two different values of i 100 and i = 300, are considered in the series. No significant differences in the results are observed, thus, for i = 100 the series has converged. Next the design of the rigid body controller is considered. An integral plus state feedback controller is used. The control signal u(t ) becomes, yl(t, ) u(t) [k, 2 k3] V2(tk) =-KKy(tk) (6-8) Yl3(t )J where K r is the transpose of the controller gain matrix. The closed loop difference equations are obtained by substituting (6-8) into (6-6) v(tk+) = (P- QK)y (t)+ R (6-9) The gains are selected to yield a damping ratio = 1 and desired servo loop frequency w,. The controller software used in the experimental studies is listed in Appendix G. The rigid body controller is applied on the robot arm to rotate it from its initial position (o) = 0, which corresponds to the horizontal position, to a certain

129 desired position, R2. Several runs are made. It is found that for R2 greater that 20 degrees the effect of nonlinearity becomes significant and the response is oscillatory. Typical results of the rigid body controller are shown in Fig. 62 to 6-4 for R2= -20 degrees. The first plot shows the e response. It has a small overshoot which persists for a while due to the effect of friction. Figures 6-3 and 6-4 represent the control voltage applied to the motor and the total vertical deflection of the end effector. The maximum deflection observed is approximately ~ 3.5mm. The control signal shows some saturation. This is desirable since it serves the purpose of driving the robot arm at its highest speed. The pulse like pattern observed in Fig. 6-3 and 6-4 illustrate the poor resolution obtained from the 8 bit analog to digital and digital to analog converters. An additional run is performed to study the effect of friction. This is done by driving the robot arm beyond its linear range and the response becomes oscillatory with a large amount of overshoot. This is illustrated in Fig. 6-5. At the peak of the oscillation, e is zero. This results in a sudden increase in the required driving torque due to the change from dynamic to static coefficient of friction. The control signal, illustrated in Fig. 6-6, is unable to respond quickly to the sharp variation in the resistive torque would cause the robot arm to come to a complete halt waiting for the control effort to build up. This is consistent with what is observed in the simulation result in Fig. 3-3.

130 8 rotation [degrees] n \ 0.00 4.00 8.00 1200 ime- [second Figure 6-2. 0 response obtained from the rigid body controller in the experimental work.

131 Control signal [volts] 8 ioe '0.00 4.00 8.00 12.00 Time. t Lfeconds Figure 6-3. Control signal for the second joint obtained from the rigid body controller in the experimental work.

132 V(t) displacement [mm] oeco 0 Figure 6-4. Total vertical deflection In response to the rigid body controller In the experimental work.

133 e rotation [degrees] i \ _. ~ C= 0.00 4.00 8.00 12.00 me. t secondl Figure 6-5. 0 response showing the effect of friction in the experimental work.

134 Control signal (volts] '0.00 4.00 8.00 12.00 Time. secondl Figure 6-6. Control signal for the second joint showing the effect of friction in the experimental work.

136 8.4. Design and Results of the Rigid and Flexible Motion Controller In the experimental work, the observation and control spillover are always present. The control action affects all elastic modes whereas the accelerometer signals contain information from all modes. A low pass filter with a break frequency set to 60 HZ attenuates the effect of higher modes. However, it does not eliminate the observation spillover problem entirely. Fortunately, the physical system exhibits some light structural damping. As seen in Chapter 5, this provides a margin of stability for the uncontrolled modes. The objective of the rigid and flexible motion controller is to introduce more damping into the flexible motion. This is done by following the same controller design approach used in Chapter 5. That is expanding the rigid body controller to include the feedback signal of the derivative of the flexible motion coordinate, q,(t). The control signal becomes l(tk ) u(tk ) = - k, k4] V kS(t) = (6-10) y4(tk ) where the yv(t,) in the augmented state vector y represents q,(t). The latter is obtained by assuming that the flexible motion is dominated by the first elastic mode. Therefore, from the assumed modes method, the total vertical deflection, V, can now be written as Vi= tl(y)qi d(t) (6-11) its time derivative V, is

136 V = 41(y) 11(t) + 11(y)q 1(t) (6-12) In the experimental study, the length of the flexible beam is kept constant. This yields 1(v)= o (6-13) Thus leading to V= 4(v)q u(t) (6-14) Since the accelerometers are located at the end effector, then V q (t) = r) (6-15) where j(r) is the value of the assumed mode shape evaluated at the end effector. V is the time rate of the total deflection. It is obtained by integrating the accelerometer signal once using the hardware integrator. Equation (6-15) provides a means to compute the newly introduced state variable y, = q11(t) required for the rigid and flexible motion controller. Only the gain k4 needs to be selected since the rest of the gains k,,k2 and k3 retain the same values that they hold in the rigid body controller. The gain k, is selected by trial and error. A large value for k, would increase the effect of the flexible motion on the rigid body motion. This would have an adverse effect on the response of the rigid body coordinate 0. Therefore a compromise is reached where the gain k4 is selected to be large enough to damp out the flexible motion within an acceptable tolerance while keeping the effect of the flexible motion on the rigid body motion to minimum. The rigid and flexible motion controller is applied to the robot arm and the

137 e rotation [degrees] 8 I _ ( N I I I +. _ 0.00 4.00 8.00 t2.00 Time. t (second) Figure 6-7. 0 response obtained from the rigid and flexible motion controller In the experimental work.

138 Control signal [volts] d \ m Tv '.00 4.00 8.00 12.00 Time. t fsecon Figure 6-8. Control signal for the second joint obtained from the rigid and flexible motion controller in the experimental work.

139 V(t) displacement [mm] Ce 0 Time. t secn Figure 6-9. Total vertical deflection in response to the rigid and flexible motion controller in the experimental work.

140 results are shown in Fig. 6-7 to 6-9. The first plot represents the overdamped 8 response. The applied control voltage, shown in Fig. 6-8, has two important features: (1) The control signal fluctuates sharply during the period where e approaches its desired value. In response to these oscillations, the DC motor introduces a series of pulses into the system. This results in reduction of the amplitude of the vertical deflection. Thus leading to additional damping in the flexible motion. (2) A small position error exists after t ==3 sec. The control effort is unable to overcome the resistive torque, induced by the friction and geometry effects of the leadscrew, and brings the e response to a standstill. The integral action, which tries to correct for the position error, starts building up the control action. This phenomenon is clearly observed in the control signal plot (Fig. 6-8). Finally, Fig. 6-9 shows a reduction in the maximum deflection of the end effector of approximately 75% as compared to the rigid body controller results shown in Fig. 6-4. 6.5. Conclusions In experiments, the rigid and flexible motion controller reduced the transverse deflection in the vertical direction by approximately 75% over the rigid body controller even in the presence of observation and control spillover.

141 The effect of friction, observed in Figs. 6-5 and 6-6, is very consistent with the results obtained in the digital simulation. Note that in the digital simulation, an approximate reduction of 50% in the vertical deflection is obtained versus an approximate 75% reduction in the experimental work. This may in part be due to the fact that in the experimental work only the second joint is used while in the digital simulation all three joints are moved simultaneously. 66.6 Summary This chapter provides a description of the experimental set up. A set of difference equations, which represents a simple linear model of the servo positioner, are obtained. Several open-loop runs are performed to measure the open loop characteristics of the motor, the power amplifier and the tachometer generator. Both a rigid body controller and rigid and flexible motion controller are implemented. A comparison between their performances is made. Also the effect of friction is experimentally demonstrated.

CHAPTER 7 SUMMARY AND CONCLUSIONS This chapter provides a brief summary of the dissertation content. The main conclusions are outlined and some prospective research topics, from unsolved issues encountered in the present research, are stated. 7?1. Summary An overview of the research done in robot arm modeling, dynamics, and control is presented. In this work, the dynamics of a spherical coordinate robot arm, whose last link is very flexible, is studied for the purpose of control. The assumed modes method is employed to represent the deflection of any point on the flexible link. All the coupling terms between the rigid and the flexible motions are retained. This is done by using coupled reference position and elastic deformation models. The kinetic and potential energy expressions are developed. Finally, Lagrange's method is implemented to obtain the unconstrained equations of motion. The robot arm considered in this work has two joints driven by leadscrew transmission mechanisms. The kinematic constraints associated with a leadscrew 142

143 are introduced to investigate the behavior of a leadscrew driven flexible robot arm in the presence of coulomb friction and the self locking condition (i.e. the leadscrew is nonbackdrivable). An integral plus state feedback controller is derived based on a linearized version of the rigid body model of the robot arm. The controller is then implemented on the rigid and flexible model. The rationale is to simulate the controllers currently used in industrial robots and to assess the interrelationships between the robot arm structural flexibility and the controller design. In an attempt to gain insight into the control problem of the flexible robot arm, a simple case, involving the controller design of a three lumped mass system is considered. The latter simulates the control problem of a compliant beam whose flexible motion is approximated by two elastic modes. This allows the study of the controller performance while working with a simple linear model. The objective in designing the rigid and flexible motion controller is to introduce additional damping into the flexible motion. The integral plus state feedback controller, derived for the rigid body model of the robot arm, is extended to include the flexible motion. This is done by using additional sensors to measure the compliant link dynamic vibrations and feed them back to the controller. The effect of control and observation spillover is examined by considering one elastic mode in both the vertical and horizontal directions in the controller design. The second mode in each direction is considered to be representative of the higher unmodeled modes.

144 Finally, an experimental evaluation is performed on both the rigid and flexible motion controller and the rigid body controller to assess the merit of measuring and feeding back the flexible motion. 7.2. Major Contributions The major contributions of this work are: (1) The kinematic constraints associated with a leadscrew are developed and the behavior of a leadscrew driven flexible robot arm in the presence of coulomb friction and the self locking condition is investigated. (2) The investigation of the interrelationships between the robot arm structural flexibility and the controller design has led to the following conclusions: (a) Drive characteristics influence coupling between flexible and rigid body motions. (b) Rigid body controllers can provide some damping into the flexible motion, and the reduction of arm vibration depends on drive characteristics. (c) Interactions between structural design and controller design should be considered. (3) The inclusion of the flexible motion in the control action results in additional damping into the flexible motion and the important issue of observation and control spillover is found to present no serious practical problem.

146 (4) The dynamic model of the leadscrew driven flexible robot arm and controller design are evaluated experimentally and found to be in good agreement with the digital simulation results. 7.3. Prospective Research Topics The following issues, which were encountered in this work, need to be addressed in future research: I - The problem of developing a general procedure to obtain a dynamic model of a prismatic joint in a flexible robot arm should be addressed. 2 - Using additional sensors to measure the flexible motion, the merits of introducing additional damping into the flexible motion by different types of controllers need to be examined. 3 - The issue of using additional sensors and actuators should be studied.

APPENDICES 146

147 APPENDIX A THE UNCONSTRAINED EQUATIONS OF MOTION The resulting seven nonlinear, coupled, second order ordinary differential equations of the unconstrained motion are listed in this appendix. The equation of motion for r is, r r 0 for Fc -0 and -pA2r + pA(L2- r)r + pA2 rr + r2+ (rfco60+ rq cos0- rOsin0)[.78q21 +.43 q + rP cosG(,78q 21 +.43 22) - (rO + r ).78q 1 +.43 12 - r(.78q11 +.43q12) + 0.61(r q1 + 2rq llq ) + 0.67(r q11q12 + + rq11g12)+ 0O5(rqllq 11 + rgql + rgq1q 1n) + 0.6.5( rqq 12+ q + q ) +0. + 2r12 ) - 0 6(12 + + 1211 + r12q 11) + 0.5(r1q 112 + 12q 12 + r 12q 12) + 8L 1(.7811 +.43q12) + L 1(.78q11 +.43q12) + (O r + Or)(.35q11 -.25q12) + br(.35q11 -.25q2) - (' rsinO + r rsinO + e rbcosO)(.5q 11q2 +.65q11q22 -.65q2q21 +.5q1n22) - rsin [.5(q 1q21 + q q21) +.65(q 1q22 + q 11ql) -.65( 12q21 + q 12q21).5(q2q,= + q(2qz22 + 0.61(r q2 + 2rq21q21) + 0.67(rq21q22 + rq21.22 + rq21lq) + 0.18(r q + 2rq22q2) + (( rsinO + rsin s + rcosS)[.5sqq, +.65q2i q2 -.65q 2,l +.5q22q, + rsin 0.5(q2i q, + q 2Iqll)+ 0.65(q21q12 + q21q12) - 0.65(q,11 + 22q11) + O5(q2212 + 222) - (L(4' cosO - 8sins) [.78q21 +.4321] - L 1 cos8(.78q 21 +.43q22) - ( cos6 - 0sin) [.35rq21 -.25rg2]

148 - co.35( g 2+ rq21) -.25(rq 22 + r 22)] + 0.5(21;+ r ^ + rq ) + 0.6rq Q22 + rq 21Q = + rq 21) - 0.65(;q gi 21 + rq 21 + 22rq fl) + 0.5(q 22 22 + r + rq =q2 ) + mr { + 2(0 cosO - 0 Using)(q21 - q22) ~ IpA2 gI L2 1 + ' ] -pA,2 (L2- [);[|l2 L2 2 2 L'-'t' +z r L, —T- + 2 ~.~{ L2 2r2 + jL i# +2 rJ ] pA 2(L2 - r)2 2e ) pA. +; 2cos2(q 22 + q) + 2 + q 2) + 2r cosO(.78 21 +.43q 2) -2r.78q +.43 12) - 2) 0 Ocs(q 11 q21 + q 2q 22) + rq 1 q 11 + 1.3rq q 12 - 1.3rq12q11 + rq 12+ Q1l + 2 + 2L I[78ill +.43q] + 2(.35rq 11- 25rq2 + 1.14rg + 0.182rq 2) - 2 sin (.5rq 121 +.65rq 1q 2-.65;q 12q21 +.5rq 12q + q 1q 21 + q 12 2) + 6 (L + 2L1r + r2)- 2iL ssinO(.78q1 +.43q22)- 4b8 rsinO(.57q21 +.0912q=) 2o 2,2 + ( siO(q 2 + q )+ rq 2 21 + 1.3rq 212- 1o3rq =q + rq 22 + q2 + q22 + 2~siDn(.5rq2l1q1 +.65rq q12 -.65rq 2q +.Srq22q12 + q21 q + q2q 12) - 2L 1 cosO(.78q21 + 43q22) - 20 cos8(.35rq21 -.25rq + 1.14rq21 + 0.182rq ) + sin" g(q 2 + q 2)- 2 sinkcosOL (.78,, +.43q 12) - 4rsinOcosO(.57q 1, +.091q ) + (L: + 2L r + r2) cos2) - {4(q- l - q 2) + 262(LI + r) -4 sin#(q - q=) - 4>cos0(q21 - q) - 4?sinOcosO(q11 -' q2) + 2(L1 + r)Icos28} + mHar - ( Acos^ + 02)mH,(L 1 + r - L ) = Fc - (m 2 + m, + mH,)gsins - pA 2gcosO(0.78q 1 + 0.43q 12) +E [18.5(2 + q~ ) + 728.28(qL2 + 2 )] for F 4 + - '271

149 The equation of motion for Bis, B=0 0o for F, =0 and I, ot 2 22 2 2 IA- )l+I PA - + il 2r mI (L i+ r - L2) o e o o I, e2- )A2( 2 -+ = (2(Or + r0)(q~ + qg)+ 20(2q1q11 + 2qq12) -2(rr + r2)(o78gq, +.43q1) - 2rr(.78q 1 + *43q12) 21^ rcos2 + Q rcos0 - S rsinm](iiga + q 1222) - ~ reo~s0(_llq + qq2s1 + q12q 2 + 12l22) + 2L [o78r ql +.78rq1g + 43r q12 +.43rq12 + A78rqn1 +.78rq +.43rql +.43rq 12 + 2(rr + r2)(.35q1 -.2512) + 2rr(.35q1l -.25q2) + 4rr (.57q11 +.09q12) + 2r 57q 1 +.09q 2) + 2 r L?r + Lir + 20(L2r + 2Llrr+ r2r) - 21(r sinG + ~ cosO)(.78rgq2 +.43rq22) + 2L sin [78(rq 2 + rq:1) +.43(rgq + rq =) - 4( r's7inI + -0r29cs + 2rr sin)(.57q21 +.0922) - 2 rsinS(.57q21+ OO9 g2) a+ T j8O(^ + 2 - 2q 112) +8(2711g11 + 2q2q12 -2q11q12 - 2q11q) - 4r (11 - g12) - 4r(11 - q12) - 8( CS0 - B0 sino)qll 21 - 11122- q12q21 + Q12z2] - 8cose[q1q2 +1121 - - q 1122 - q212q21+ 12q22 + q12qJ + 4r(q{1 - q 2) + 4(L1 + r)(ql - 12) + 20( 1 + r)2 + 4(L 1 + r)r - 4 sin + (L + r) sin + (L, + r) Ocos0 (q - q22) - 4(L + r) sin(q2 - q ) + pA4L - r)f L1 - 2 + - cososin + 12 ) cosI s ro inO+ (I +- q2) I ) sin (L1 2 r ' l2

160 - pA~ (-29coshsinO(rqg21 + rq2) - 2rO rsinG(.78q2l + 43q^) + 24 resine(gq1q21 + q1 2q22) - 2~ rrco6(.Sq llq 2 +.65q 1122 -.65q 1221 + 5q 12 22) - 20 rcos6(q 11 q21 + 12q22) - 26rL 1 cosO(.78q21 +.43q2) - 208 r2cos6(.57q2l +.9q22) + 2~ rsinocos(q2 + qg) + 2rr cos(.5q 21 q +.65q 212 22 q 11 + 5q 22q 12) + 2 rcosO(q 21 q + q22q12) + 2L 1r4 sinD(.78q21 +.43q2) + 2L 1 rsinO(.78q21 +.43q') + 2rrQ sin6(.35q2 -.25q p) +.43g 2)- 22r2(cos- sin2)(.57ql +.o092) - 2cosesin0 I L r + L r2 + 3 221~~~~~ 3 3 r- L - 8 2cosOsin6(q2 + q22 - 2q 2 q 22) - 4r sin(q 21 - q) + 8 esin(q 11q 21 - q 22- q221 + q12q22) - 80 CosO(q lq - q1 22 - q12q21 + q1222) - 48(L I + r)~ cos(q 21 - q ) + 82sin cos(q21 + q2 - 2q21q2) + 8 cos8(q 21 q11 2- q 12- q 22q 11 + q 22 12) + 4(L1 + r)6sin(q 21 - q22) + 8gSincoso(q 12 + q - 211q 12) - 4 (L+ r)(cos2 - sin2)(q 11 - q2) - 2(L i + r)2coss -=aF, - m mi2 al + ml d J + mHm(L i+ r - L2) + m+ r - 2 + m,(L + r) gcos - pA2sin.78rq11 + 0.43rq12 -m, gsinO[2q 1 2q2] for Fa 3 0

161 The equation of motion for ~ is, miL - a + 2 + c + M (LI + r - L2) cos2 - 20 Ocos~sinO) 2 2 -_ (COS L r- r PA2 ~. ~PA2 4 2s - 2 ~ sinecose(L2 - r)s + 2rxcos2Omnu(L, + r - L2) 6 2 + pA { [o rcos2- + ~ rcos - 24 rcossin]( + q) + rcos22(2g 2g21 + 2q 22) + [r rcos8 + r2cos - rresin] (O.78q, + 0.43Lg) + rrcos(O.78q2 + 0.43q) - [ rcos6 + OrcosO - 0rsin] (q lq21 + q12q 22) - rcose ( qq 9 + q 11q21 + q12q2 + q 1222) - r 2sin +r rsinO + rrcos] (O.5qglq 2 + 0.65q 1122 - 0.65q12q21 + O.5q12q2) - rrsins0.5(qqli +q 2 + 1 1)+0.65(q11q2 + qlq22) - 0.65(q1q21 + q12 q1) + 0.5(12q22 + q12q22) - (rsin + rcosO)[qi2l + 1222 -rsin[Qq2i+ 11q21.2 r ] - ~. -2 1 + q 1222 + qi2q - L[ rsi + sin8 + rsi + os (0.78q21 + 0.43q22) - OrL IsinO(0.78q21 + 0.43q 22) - ( r2sinO + 2rrOsinO + r 2 cose)[o.57q 2 + O.9qz]2 - r2si.5742 + 0.092) + 2 + 2 sin + s in + rs osO] (q + q 2) 2 - + d rsin2D(2q2lq2 + 2q 2zq) + r 2sins + rrsin' + rrcos] (0.5q2lql + 0.65q21l12 -0.65q22,11 +.5q22q12) + rrsin[O. 5(2iql + q21q11) + 0.65(q2112 + q21q12) -0.65(q 22q 11 + 22q 11) + 0.5(q 22 12+ 222) + rsins(q2111 + q 21q11 + q 22q + q 22q 12) + [sine + rOcos ](q2lqll + q=ql2)+ L,1sinO(O.78rq21 + 0.43rq=, + 0.78rq2 + 0.43rq2) - L icos[ 0.78(r ( + q) + 0.43(r q22 + rq) + 0.78(rq21 + rq) + 0.43(rq2 + rq'=) - [coss + rrcosO - rresinO](0.35q21 - 0.25q22) - rrcos0(O.35g2 - 0.25q22)

162 - [2r;co6 - r 2sin](0.57 21 + 0.09 22) - r2cos(0.57q + 0.09 ) + (ti 2 r + rsin2 + 2 Orsin cos)(g + q ) + 2 rsin2( q 11,, + 2qq 12) - 2L,1 [ rsinscosO + erco620 - ersin28 + rsinkcos] (0.78q1, + 0.43 12) - 20 rL,sinOcosO(0.78gqi + 0.43q ) - 2[' r2sinOcosO + 2rr' sinOcoste + > r'2cos28 - 1 r2inJ] (0.57q11 +.09q12) - 24 r2sinOcos8(0.S57q11 + 0.09q 2) + (Lr + 2L rr+ r 2r co + r + L r 2 (L, Co6 2 sin6cos) + mr 4[c eos28 - 20 OcosesinD](q2j + q22 - 2q21q2) + 40 cos28(2q21q2 + 2q^q22- 2q21q22- 2q q22) + 2[rcos - rsin](q21 -) q) + 2rcosO(q2 - q) - 4 [cos - 61sinO] (qg,q2 -- 12q21 + q 2q22) - 4cosO(q11q221 + qq1q-21 - i11q 2- q11q22 - q212q + q12q + q12q2) - 40cos(q 11 21 - 11l22 - q12q21 + q12q^2) 4sinO(q 1121 + q1121 - q 11q22- q11q22 - q12 21 -1221 + 12 +22 q+ 12^) - 2[Osin6(Li + r) + ersin8 + cosO(L + r+ (- q- q - 2sinO(L + r)[ q - q22 +4(' sin2G + 2i OsinOosO)[.q2 + q - 2q2122] + 82 sinO2(q 21q + q q22 - 2q22 - q q22) + 40cosO(q'2111 - q21 12- q22q11 + q 2212) + 4sinO(q21 11 + q2111 - 21 12 - q21q12- 2211 - q22q11 + Q 22 12+ 22q 12)- 2[rcos8 - (L1 + r)sin(21 - 22) - 2(L1 + r)cos(q21 - q22) + 4( sin2 + 24 cosOsinO)(qg 4 - 2q11qi2) + 8 sin2 (q11q11 + q 12q12- 1 1 12- q 11 q.)- 4[4 sinOcosO(L + r) + *c06s2(L + r) - OSinD2(L + r) + rsinecos8](i - q ) - sinscos(L + r)(1l - q12) + 4 (L + r)2cos28 + 2(L + r)4 rcos28 - 2 L I + r)2cosasin) + (mH2 a+ I2) = TI)

163 The equation of motion for q,,(t) is, p2 {^((r2 + rr)(q11 - 1.3q12) + rr(q1 - 1.3q12) + 2rq11 + 2rq1 + 1.56L,(e r + Or) + 1.14(' r2 + 2brr) - 2(' rsindqg + 2orcos1q + 4 rsin.qgl + 1 rsinq21) - 2?rq1 + 1.56rr+ 2 rcosq1- 1.52r2q11- 1.06r2q12- r;l 1- 1.3rrql2- 1.56rL I -0.7rr + 2.6~ rrsingOq - 24 rsiniq21 - 22 rsinO2qii + 1.562L rsin~cos0 + 1.142r2 sinOcosO} 8( - 2)+ 4(L + r) + 4l- 8() + sins + OcosO)[ql - q] - 8 sin(q 21 - 22) - 8(q 11 - q 12) + 4 r + 8 bcos#(q f 1 - q 2) - 8(q21 - q22) sinO - 8sin28(q, - q ) + 4sin4 cosO(L + r) = - 0.68m' gcost - 2m, gcosO - 12.62 Elq,'2 2+ r 4.65 7.38 -pA~j2 +cos2L + + 2 J | qj - ql2} + pA.2cos28 + )[(3.08L1 + 1.13r)q1 - (6.96L1 + 3r)q12].[m (L + r)(cos + + (m + pA2r)gsin - q2 + pA gsinO 3.08q 11 - 6.96 12

164 The equation of motion for q2(t) is, p.4 (r2 + rr)(0.65q11 + 0.5q12) + rr(O.65 11 +.5q 12) + r12 + rq12 + 0.43L,(O r + r) + O.09(2Grr + e r2) - (' rsinOq= + 9 OrcosGq2 + 4 rsin8q2 + ~ rsinO2) - 0rq 12 + 0.43rr0 + rf OcosOq 2- 0.53r2q 2 - 4.34rq 12 + 0.65rrql - O.5rrq12 - 0.43rOL + 0.25rrO - 1.3E rrsinOq2l - i rsinOg2 - r sin2q 12 + 0.43r sinocosL 1 + O.O9gr 2sinOcoss + mr {4(q 12 q 1) - 2l[ (L1 + r) + 8 - 4(+ sine + cosO)(q22 - q21) - 4 sinO(q - q21) -4(q12 - q1 ) 1)- 2rB + 4 BcosO(q 22- q21) - 4( sinD(q - q )- 4q sin28(q: - q i1) - 2osinmcosO(L + r) = -1.43m2' gcosO + 2mr gcos - 485 Elq12 2 2o + rI2 7.38 32.42 - p42( cos2 + 2Lir + 2- - qll + ql2J 2 r + + p(4 (2cos2Q + b2) [6.96L + 3r)ql1 + (23.77 1 + 9.73r)q 12 - [m (LI + r)(2cos2 + ) + (m, + pAr)sinO] - -8 q11 + 4 q12} + pA^2 ^sin - 6.961 1 + 23.77 q1

166 The equation of motion for q,,(t) is, pA240.r2q2, + rr q2 + rrq2) - 0.65(r2q2 + rr q2+ ) + rr2) + r2 +. rsinOq 1l + O rcosOq 1 + rsinmq 1I + rsingO 11 - 0.78 [r L cosO + r< L Icos - r OL Isin] - 0.57 [2rr cose + r cos- r2 sinO] - 2rco2q21 - 0.78rr co68 + r + cosOql + sinO(O.Srrq - 0.65rrql2 + rq l) + 0.78r6L 1sin6 + 0.57r 2 sine - rsin2gq2 - 0.76r2q 2 - 0.53r q - r 0.5rr2 - 0.65rrq2 - sin(0.5rrq, + 0.65rrq12) + 0.78rL 10 cosO + 0.35rro cosO} + mp {4( 21 - q 2) + 4(; sinG + OcosO)[ 11 - q12 + 4sin(q 11 - 12) - 2r4 ^cos + (L1 + r); cosO - (L1 + r)90 sinG] - 4 cos20(q21 - 22) - 2r cosO + 4X GcosO(q11 - q12) + 40sir^O(q11 - q12) + 2L I + r)q sinO - 4f sin2(q - 22) = 2362 - pA s + r + q4 1 - - + pA6(&cos5o + ~ E-q21 pA72(q38cos2G p2)fL S X [ 21 - - q. + pA s )[(3.08L1 + 1.13r) -21 (6.96L1 + 3r)qJ r r Qn - 1 r r + pA i+ r)((3.08q2l 6.96q+ + pA ^gsin0(3.08q 21 - 6.96q 2)

166 The equation of motion for q(t) is, pA 20.65(r21 + rr q + 21)+ O.rr q +r q22 + rr22) + rq + q 22 + ' rsinq 12 + Orcos6q 12 + 4 rsin^q 12 + 4 rsinig 12 - 0.43L 1(r cos6 + r4 cos0 - r, OsinO) - 0.09(2rr; cosO + r24 cos6 - r2X OsinO) - rX2cos2q22 - 0.43rr cosO + r OosOq 1 + 0.43rOL 1 sinO + O.09r280 sinO - r42sin2q22 - 0.53;2q21 - 4.34 q22 + 0.65rrq21 - 0.5rr22 + 2 sinO(1.3rrq 1 + rq12) + 0.43rL 1 cosO - 0.25rrf cosO + m 4(q 22- q21) + 4(0 sinG + OcosO)(q 12- q 1l)+ 4 sinO(q12- q1l) + 2[ro cosO + (L I + r)4f cos# - (L1 + r)4 UsinG] - 4 cos28(q22 - q21) + 2r;cos + 4I Bcos6(q2- q11) + 40 sinO(q12- q,)- 28(LI + r) sinG - 4esinO(q- q2)} 485.519 Elq pA2 os2 + 2)r 7.38 32.42 q ~ ~p o O^ + - ^ - ~ 21 + ~ + p.4A2cos2 + )[- (6.96L1 + 3r)q21 + (23.77L1 + 9.73r)qg] ) + ({t +II 7.38: + r2 —2 - [m (LI + r)+ r)( cos + 0+ (m pA2r)gsin - 21 + -.2 q= + p.4 9sin[ - 6.96q21 + 23.77q22

167 APPENDIX B THE CONSTRAINED EQUATIONS OF MOTION FOR 0 AND r Only the listing of the affected equations is shown here (i.e. r and e equations). The constrained equation of motion for r on the lower thread is m2+m, + mH)gsine- m L l + r - j + mp(li+ r) + mH(L + r - L )] (2 + cos2) + 20cos [(0.78qi + 0.43g)m + 2m,( q - - 28[m' (0.78q11 + 0.43q12) + 2m, (q + cose [ml (0.78q21 + 0.4322) 2 In 30 i * ~~ r 12 + m (q - q22)] + (2sinOcos — )[m[ (0.78q11 + 0.43q12) + 2m, (q - 12)] (tantli + psgn(r)) +( Id J,(1 - tan l) + (m2 + m + m)( + tani)} r + { 2pA 2rtcos8(0.78q21 + 0.43 22) - 2rpA 2(0.78q 1l + 0.43q 12)) (tanli + A) -= ( - tanig n(r)) where ml is the mass of the part of the second beam protruding from the first beam and u is the dynamic coefficient of friction.

168 The constrained equation of motion for r on the upper thread is, (m2+ m, + mH)gsine- m2Li + r- j| +mp(LI+r) + m^(LI+ r - L2)](2 20 + i) + 2*osO[(0.78q21 0.+431q)m2 + 2m, (q21- q22)] - 2m (0.78g11 + 0.43q2) + 2m, (qu - + + 0cos [m2 (0.78q + 0.43q ) + 2m, (q21 - q22)] + (^sinecose - M ) '[m (0.78q 1 + 0-43q12) + 2m,(q1 - q 12) (tanV - lsgn(r)) + (Id- 4(l - ptant ) + (m2 + m, + (+ tan l)} r + 2pA2rcosO(0.78q + 0.43q ) - 2rgpA2(O.78q11 + 0.43q 2)} ( + tanl,1) = d (2T + 1tan gn(r)) An intermediate result for the constrained equation of motion for e is presented here to define the quantity R.,, R = mH2g - mL + m - + md + m2 Li + r - 2 — + mH1(Ll + r - L2) ] a 2 | 12 +c+ m L r2 + + 12 + m -L a2 +mH(LIl+r- L2)+ m,(Li+r)2 12 2 — pA 0.78r L iq1 + 1.56L rq 1 + 0.43r L q2 + 0.86rLqL + 0.78rLgq at + 0.43rL q I - 0.43(r2q11 + rr q + rrq) -.68(q12 + rrq + rq12) + 0.57(2rr11 + r2 q1) + 0.09(2rrq12 + r 2)} - a ((r + L,)( 11- q 2)- r (q 1 -q 2) q - a q' q~12) - r~(q -q + (0.+ (0.57r2)q + (0.43rL + O.Oor2)q + (0.78rL1 + 0.35 )q21 + (3Lr - )] + m sin + r)( - q) +(0.43L, r -0.25rr)q + Li + r)(q- q22) J~~~~e a )Bl~ ~

169 + -sin [(0.78rLi + 0.57r2)q + (0.43rL, + O.O9r2)qj + 2m,. (q2- q)(Ll + r) + 2 sm28[(0.78rL + 0.57r2)ql a a + (0.43rL L+ O.09r2)q] + esin2u(q,, - q 2)(L, + r),, pA2 *2 078L a 1o s[(0.78rL + 0.57r)qi + (0.43 rL + 0.09rqg I a 2m,.2 2pA42cos0 F 2mP 2cos28(L1 + r)(qli - q12) + 2r(q 11qi2 + q12q22) + r(O.Sqiiq2 + O.Sq12q2 a a L + 0.65ql2q 1- 0.65q Iq2) +.F lcos[ qlqi -,q=- ql2q + ql222] 0 pA 2c68 4 m, ' cose + r(q11qm+ q(12q ) + 1 ) + (q 1121 - q12q21 - q1122 + q 12 22) a a a a + m s7inScos(qy + qg) + 4 2sin-cos [g + q - 2qllql where mf and mf are the masses of the parts of the first beam lying to the left and to the right of the pivot point 0 respectively. The constrained equation of motion for e on the upper thread is, R^,tan3L+g8gnO)+ {2a [mu1Li-+r-L2)+m4LI r-~[ -W Uadl~lrrln~+( -2t8L2 w idr 2l sint1of ti of - +, (Lq + 8 q, Q (q - vq2qI + qp-q:-A qr - ao a L + r, tiii+ +q 2i2i2)J(tauM + ) + ( - n(ta i) L 2 2L ' I 1 12 +- 2 2 kV 2 2 + 2 (qg + q1)+ a (q~ {q + q1-2 CO1OL2) (tan~, + #)' where yI is the mass moment of inertia of the second joint leadscrew around its axis.

160 The constrained equation of motion for e on the lower thread is, R.,(tan'p -,pgn(6)) +( - [mH^( + r- L2) + r m 2 + m,(L + r '- [qllIll- q11l12 + q12q12- 912ill] - 2 [o.5r(2 + g2) + r(q 11 + l2)]) ( + tani) + - (1 + utano'l gn(i)) - I mH 1 F2 2 1L I L -aj 1*t2 2 2 + L + mH,(L + r -L2) + m.(LI+r)l + (g + 12+ a ( + 2 - 2a12) + ta ) + - an + 4- (q 11 + q - 2qnq12) (p + tanjbl)+..- a jIda aw~

161 APPENDIX C THE RIGID BODY EQUATIONS OF MOTION OF THE ROBOT ARM In an attempt to check the work done for the general case, the equations of the rigid body motion of the robot arm are derived. The system is now greatly simplified since it has only three degrees of freedom (i.e. r, 6 and ( ). The robot arm is treated as an assembly of rigid bodies whose mass centers become important in the derivation ofrtheir kinetic energies (see Fig. 2-1). For the first beam, one has, R; = a (C-1) where the star superscript indicates a value related to the center of mass. L is the total length of the first beam and a is the length of the portion of the first beam lying to the left of the pivot point 0. The position vectors of the mass center of the second beam and the payload are, Re i - r- hi (C-2), = (Li + r (C-3) where Li is the length of the portion of the first beam lying to the right of the

162 pivot point 0. r is the length of the part of the second beam protruding from the first beam and L2 is the total length of the second beam. The position vectors of the mass center of the leadscrew and its housing that produce the translational motion of the second beam are, R'- = d i (C-4 RH1 (L.+ r-L2)i ((-5) where d, is the distance from the origin of ( i, j, k) to the mass center of the leadscrew. The leadscrew housing is assumed to be a point mass located at the beginning of the second beam. KINETIC ENERGY The velocity terms can be derived from, dR R = --- +2 x R, dt = (sinO i + cos0 j) + 0 k (C-6) where Q is the rotation vector of the ( i, j, k) basis and R, is the position vector of the mass center for the its body. The velocity terms are then used to develop the kinetic energy for each part of the robot arm including the payload, mp. The latter is considered to be a point mass, and does not include rotation around its own mass center. Since the first beam and the leadscrew that drives the second beam undergo fixed point rotation, then their kinetic energies can be defined by:

163 T- = n- (Ho +H,1) (C-7) where H, and AU are the angular moments of momentum of the first beam and the leadscrew respectively around point 0. The kinetic energy of the second beam has rotational and translational parts, T2= m2R'2 R'2+ I fl H (C-8) 2 2 2 where H2 is the angular moment of momentum around the mass center of the second beam. Since the payload and the leadscrew housing are considered to be point masses and do not undergo rotation about their own mass centers, then their kinetic energies would simply be written as, Ts- m Rp +. mH R (C-9) where mH, and mp are the masses of the leadscrew housing and the payload respectively. The kinetic energy of the leadscrew and its housing that are used to rotate the first beam around the pivot point 0 can be obtained from 1 oH' 1 IIH'o, (C-o) where H'~2, H~'H are the angular moments of momentum of the leadscrew and its housing respectively around point 0. The expression for fi can be written as n, = (C-ll) The total kinetic energy would then be

164 T= -T, (C-12) a4 POTENTIAL ENERGY The datum line is chosen to be aligned with the I unit vector. It is horizontal and passes through the point 0. The potential energy associated with the rigid body motion arises from the gravitational acceleration, g, and is: IL t L Vt( mIV2 -aj + m1ld+ m i(L + r-L2) + m2L i + r- + m(L1 + r)g si (C-13) VIRTUAL WORK PRINCIPLE The virtual work principle is employed to obtain the generalized forces. If the forces exerted on the robot arm can be divided into conservative forces, which are directly related to the potential energy expression V, and nonconservative forces which are not, then the virtual work term can be written as [421: 6W = 6W + 6WVC (C-14) where the subscripts c and nc denoteterms due to conservative and nonconservative forces respectively. 56W is related to V, as follows: 6W, = -6V, = - --, (C-15) where z, is the itk generalized coordinate which, in this case, can be any of the rigid body degrees of freedom r, e and ~. rv,, can be written in the following

165 form, 6Wc = SQC 6z., (C-16) S-i where Q,,c represent nonconservative generalized forces. 6Wc expression can be easily derived from the free body diagram depicted in Fig. 2-2. 6Wc = R' ER X + Fc i 6 R2 (L+r-L + T3J' 6X + Fs ER (....... -'- I- _4 (C-17) - F. 6r - aF, 6i + Te6a where ~ is the virtual rotation vector of n. Comparison of (C-16) and (C-17) yields, Qrsc - Fc = arc (C-18) Q *"c - T8 LAGRANGE'S EQUATIONS Now that the expressions for the nonconservative generalized forces, the total kinetic and potential energies are derived; the equations of motion can be obtained using the following form of Lagrange's equation, d I( Tt T aTt V (19) Substitute (C-12), (C-13) and (C-18) into (C-19) to get the following: (m+ +m + m )r -( cos2 + 2)[mHL(L + r - L2) + m2Li+ r- | + mp(Li+ r) =Fc - gsinO(m2+ mp + m-)

169 The unconstrained equation of motion for 0 is, mLt + m[L 22 L2 m22 + 2 +m2+12 2 |,2 +mi- - -a +c+mH(l +r-L2)+ rm|Li+r- 2 + 12 + m, (L + r) ) + 2;( m..(L I + r - L2) + m2 L + r 2 + M. (L. + r) + OsinScosa I ~ + i - a +c m2Li ++ r - L2)2 L12 m2^ L2 +m2 LI +- + 12 +m,(Li+ r)2 =- f, - 9cos6\m]2 - a + mM1d: + mHi(Ll+ r-L2)+ m2 LI+ r+ mr(L + r) (C'2J) where c is the mass moment of inertia of the prismatic joint leadscrewv with respect to the j and k directions. Finally the unconstrained equation of motion for o can be written as follows, 2 12 2 1L2 22 2+ m - aj + + ( + r-L + mc L + r 2 + 12 + mp (LI + r) cos20 + (rnH2a2 + II2) - 20 0 cosO sinO I + il- i a( L+mHL r-LL+cm L r 2 + )2 - - + mr ( L 2 + -r) + c + r+ m L + r- -2 +m (L + r) =Ts (C-22)

167 CONSTRAINED EQUATIONS OF MOTION The only constraints, that are considered in this study, are the ones imposed by the leadscrew transmission mechanisms. Therefore, only the r and 8 equations will be affected. The constrained equations of motion can be obtained by applying the formulation derived in Chapter 3 on (C-20) and (C-21). The modified r equation would have the following form for upper thread motion [ -(m2 + mp~ mH)gsinO + ( cos 28 + 2)[m (L +r) + m L + m r- 2 2TI + muH(Ll + r - 2)](tani - psgn(r))+ d - ( + Ttantgn(r)) (= i+ m + + m)(tan, + + ) + Jt.(l - -tal) r (C-23) and for lower thread motion {-(m+ + m p mHgi + m(gs+(cos2O +?)[m (L + r)+m2 Li + r- -2 + mK(Li + r - L2)]}(tan + pgn(r)) + 2T ( - tan n(r)) = (m2 + mP + mH)(tanol + ) + id J1(l - ptanl) r (C-24) where L, is the helical angle of the thread, # is the dynamic coefficient of friction, Ti is the applied control torque, d, and I are the mean diameter and the lead of the leadscrew respectively. J,1 is the mass moment of inertia of the prismatic joint leadscrew around its axis.

168 The constrained equation of motion for 0 on the lower thread is {mH2g s- mL + m + + r d +m- 2)+,2L+-L)+mL,+ --- + m,(L +r- + c + 12 + ml +- - 2o + mH (LI + r- L )2 m + 12 +./, + 12 in" r + m2 L + r ---\- 2]s a mH(L,+ r-L2+mH+ mr2 m + r — | + m,( (LJ+ )( tanP+ 2T2 ma 1 r2 m L 2 + ~- (1+ 1tanV1n()) -., + - ic+ - 'n a [ 12 '12 a a 12 -2 1 m2Lm122L2 + M2!'12 + 211+- +m2 L(L+r-ta-'+-) + M J12(1 - )~ (C-25) The constrained equation of motion for e on the upper thread is: +1 mL +sicos + mL m Lj - m(l+)12 2 ~ c+ m +ml - -+l 2re m2 2 L 2r)2(tan - r. MH1+- L ( + -L ML+ m ir — + m,(LI + r)(ta) + lL) +a 2T2 (1- Itant-/!legn(6)) =j |jsinglcosg a [-2 + 2T2 (1 - taan sn()) = { m, + mc L + - m LI~ dm sinccosi, a 12 12 + mg (L + r - L2 + M2LI + r - + M(+ mal+u + m1(,+- L)a 12 2 (C-25) +coe (O 1 2 ' + m2l+ r- + mH(L+ r-(L2+ MrRLm+ d) t+#sgnIb a 12 2

169 + ml - aL +r — +2 m (L + r-L)2 + m(L + r)2(tn + ) + d J,(l - tan, l)} ' (C-26) where mf and mR are the ses of the parts of the first beam lying to the left and to the right of the pivot point 0 respectively. Ji, is the mass moment of inertia of the second joint leadscrew around its axis.

170 APPENDIX D RIGID BODY CONTROLLER GAIN MATRICES Evaluation of equation (4-5) leads to the following general form for the A and B matrices, 0 0 0 1 00 0 0 0 0 0 0010 0 0 0 0 00001 0 0 A ~ 0 A42 and B A= 0 A420 0 0 0 0 B41 0 0 A1 A1 62 0 0 00 0 B0 0 0 0 0000 0 0 Bu Upper thread motion for r and B yields the following entries for A and B matrices, - (m + mp + mH1)gcos 8, (tant1P - u8gn(r)) A42 ~=~ (m2 + mP + mrl)(tanQl + -) + Id Jl,(l - tanol) A61 = - a (mn, + m2 + m )(tanOl + #sgn(8))a2e - a mH(L + r -L2) + m LI + r, -2 + mr (La + r, )] m - _ 2 - m r + m, ld 2 2 a 2 2 + mn,(LI + r, -L 2)+ mLir- + m (L + r )] X (tn )(t + )) / x (itan0I + j)(tanij +;tsgn(u))j /

171 gsinf, M mPLI L2 A ={g ' [m Li +mdli + rmH(Ll+ r -L2)+ m2Ll + r m- 2 m +, (Li + r, (tani + pgn() / 2 2(1 + ptan'ljgn(r)) B41 = ["~-~4 dm (Im2 + mp + mHn)(tanil + p) + ld-,(I -,tanl) 2(1 - ptantl8gn(0)) Bs. ~ a2e dm where mH2, 1 m 1L12 mL 2 L t 2+ - cO +.. + + (L + -L2)2+ + m,( -a)2 sint1'costil a1 12 - 2 + m-L, + 2+ m, (L + r, (tant, + r) + mni + r ( tan+- ) Any variable with a subscript e should be evaluated at the equilibrium point. Lower thread motion for r and 0 gives the following entries for A and B matrices. - (m2 + m, + m.H)gcosO, (tani, + p.gn(r)) ~A42X =~~~4 (m2 + mP + mHl)(tanl + A) + -4 /, (l - tan)l) A6 = g(- 'a (mH, + n2 + m, )(tanl -,g1n(9))a2, - [m i(L + r, - L2) + 12i I gcos I am L m LL + r + m (L i+ r, a mLI am + d + m2I +2 a 2 2r 2 ) 1 mH(Ll + r - L2) + m2 L +L r - + mr(Li + r) X (tan l + p)(tanl- ssgn())) / C2

172 pgsin#e M nL I,2 A =62 e+ mdll + mH,1(LI + r - L2) 2L re 2 a mB m (L + ) (tan1 - gn()) / 2)) 2(1 - ptan+jign(r)) B41 = - d.+ M 2l + )(tan ) + d J,(1 - tanl) 2(1 + ptanrtlgn(6)) Bs2a a2e dM Since p is driven by a gear train, then one value for B^ is obtained, 1 B — --- B.~ where - 12 + mI 1 aJ + c+ mHJLI + re L2) +m2l + re_, 1,, 2 2 + 2 + m (L, + re cos- - + m-2a + J2 The gain matrix of the rigid body controller is, = ks ks o ks 0 o 0 0 10 0 k 0 0 0 0 kO 0 133 0^80 0.W

173 where 5s _wwdI + 2Pd1dl Wd1 B41 KS = 1Wd2 + 2Pd2Ed2 Wd2 + A a B62 KMs = w + 2PdsdS~Wds K12 = B-B B41 KS A61 K21 - —. B62 S 2f2dlWdl + Pdl K14 == ~5 ~ S 22d2Wd2 + Pd2 26 - ~.. ~ Ks- 2 ds3Vds + Pd3 B62 K, Pd2 Wd2 K22 = -- B52.g PdXWd2 where; and Wd, are the desired damping ratio and servo loop frequency. Pd, is the pole assigned for the integral controller.

174 APPENDIX E SCHEMATIC OF THE DOUBLE INTEGRATOR CIRCUIT

11.6K 1 1.5K ~~~~I +112V I4 REISET 4.99K? l ~ 1^ -12V V 12.4K-IN7i3/6.2V r1.00 -< 2V KESET IN753/6.2V X12V 0^ 6K I___~12V 0.01-B +1R 2. +OR ^ ^ 0.0^1 ^2^ ~^Q0.01p o 12V \iiAAA _1^ / AQ^I.\ O ^ *~\^AVV^ r^ 4.99K 680K 12AK +OR 4t —, ivv rl (J ( ~^q I I I 'N753 J.~0 1 1 -F i t0.0^ 75.OK ^ ^ K 7O.OK 60K 75.OK: 2.4~ri ~ 'L-ll r.9R.9R

176 APPENDIX F LISTING OF THE COMPUTER CODE FOR THE DIGITAL SIMULATION

177 C********' MAIN PROGRAM IDENTIFICATION ** C C C THIS PROGRAM IS WRITTEN TO SIMULATE THE RIGID AND FLEXIBLE C MOTION OF A ROBOT ARM. SEVEN DEGREES OF FREEDOM ARE USED 1N C THIS STUDY. THE FIRST THREE DEGREES OF FREEDOMDENOTED Y(1) C TROUGH Y(3), ARE USED TO SIMULATE THE RIGID BODY MOTION, C WHILE THE LAST FOUR DEGREES OF FREEDOMY(4) THROUGH Y(7), ARE C USED TO SIMULATE THE FLEXIBLE MOTION WHICH IS TRUNCATED AFTER C ITS SECOND MODE. THE SEVEN HIGHLY NONLINEAR SECOND ORDER C ORDINARY DIFFERENTIAL EQUATIONS OF MOTION ALONG WITH THE STATE C FEEDBACK CONTROLLER EQUATIONS ARE TRANSFORMED TO SEVENTEEN FIRST C ORDER ORDINARY DIFFERENTIAL EQUATIONS. THE LATTERS ARE THEN C SOLVED SIMULTANEOUSLY USING GEAR'S METHOD. C C C C C ******** FUNCTION IDENTIFICATION **** C C C THIS FUNCTION EVALUATES THE SGN FUNCTION C OF THE ABSOLUTE VALUE OF ITS ARGUMENT. C C C C FUNCTION ASGN(X) REAL*8 X,ASGNEPSI ASGN=O.ODO EPS1=0.0000000000001DO IF(DABS(X).GT.EPS1) GO TO 100 ASGN=O.ODO GO TO 101 100 ASGN 1.000 101 RETURN END C C C C C THIS FUNCTION EVALUATES THE SGN FUNCTION OF C ITS ARGUMENT. C C C * * ~ ~ **~** * S * * ** S * S * * J * * * * * ** * * * * * * * * ** * * * * * * * * * * * * * * * * C C FUNCTION SGN(X) REAL*8 X,SGNEPS SGN=O.ODO EPS=0.0000000000001DO IF(X.GT.EPS) GO TO 151 IF(DABS(X).LE.EPS)GO TO 153 SGN=- 1.000 GO TO 152 153 SGN0.000

178 G O O 152 151 SGNs1.ODO 152 RETURN ENO C C C C C C-eOoe*eo* MAIN PROGRAM BODY ~ ~*< C C REAL*8 TOUTEPS.DMH.Y(17).TO,KSlINKS14NIK11N REAL*8 WNR.WNTETA,WNFETA,POLE1,POLE2,POLE3.ZETA1 REAL*8 ZETA2,ZETA3,MU.MH1.,MlM2.MPL1, L2.KSY1 REAL*8 LO.GPIN.PIIN.B1P.B2P,3. IK11PIK22P IK22NKS12N REAL*8 IK33.KSllPKS22P.KS33.KS14PK K25PoKS36KS22NKS25N REAL*8 C1oC2,C3JL1,L,R1,R2.R3,KS12P.KS21PKS21N.TT3,TT4 REAL'8 A51PA2E.TH,M1L.M1R.LT.IL2,CONST2.CONST3 REAL'8 RDMAXTDMAXRDPERTDPER,ER1PERER2PERTA1o TA2.A52P REAL*8 ATT1IATT2,TT1 MUSR11.A42N.BIN.A51NA52N.A2,ROWEI REAL*8 TW2.B2N.A42P.CONSToTW3,S REAL*8 AAAI,AA2.AA3,AA4,INERT,ML.DL.TLIMIT REAL*8 ATN1.ATN2,AB A1 ABN2.PHEE1.PHEE2.V,W C C COMMON /GEAR9/ NSTEP,NFENJE COMMON /GAINKP/ KS lP.KS14P. K 1P,KS22PoKS25P, I IK22P.KS33 oKS36. IK33,KS12P.KS21P COMMON /GAINKN/ KSllNKS14N,IK9lN,KS22NKS25N, I IK22N.KS12N,KS21N COMMON /CONTL/ C1,C2.C3 COMMON /DATA/ MIM2,MP.MHI.LI,L2.MU,G,KSY1.M1LMIR.LToIL2 1,R1.R2,R3.LO.JL1.L,DM.TT3.TT4 COMMON /DATA1/ ATT,1ATT2,MUS,CONST2,CONST3 I,ML.DL.INERTI.TT1,TA1,TA2 COMMON /SINGUL/ SA2,ROWEI COMMON /DT/ RDMAX,TDMAX.RDPERTDPER,ERIPERER2PER.CONST C C REAL*4 Y1(2010).Y2(2010), Y3(2010) Y4(2010) Y5(2010).Y6(201) REAL*4 Y7(2010),Y8(2010) Y9(2010) Y10(2010), Y1(2010) REAL*4 Y13(2010),Y 14(2010), 5(2010) Y6(2010),7(2010) REAL*4 TI(2010),Y12(2010),X(2010) AT,YA(2010).Z(2010),ATTT REAL*4 D1(2010),D2(2010) C C DATA YESNO/3HYES,2HNO/ C WRITE(7,10) 10 FORMAT('1'.3X. 'TIME'.9X, 'r, 14X, 'THETA'. 11X, 'PHY' 1X, 1 '011(T)'.O1X. '012(T)'9X,'021(T)') C C READ(8s 71)R1.R2,R3.Y( 1).Y(2) READ(8,71)Y(3).Y(4).Y(5).Y(6).Y(7) 71 FORMAT(5F10.5) 72 FORMAT(6F10.5) READ( 8 72 )WNNWNTETAoWNFETA o POLE 1.POLE2.POLE3

179 READ(B,72)ZEAltAI. A2.ZLEA3.MU.MUS,G READ(8.603)M2 L2. A2.ROW, EI 603 FORMAT(F15.8,F7.3,F15.10,2F10.2) C C *** DATA ** C TT4~0.000 TT30.000 TT 1 0.000 TA10.000 TA2 0.000 TH=O.000 N=17DO IOD000 TO0.000 HOOOO.01DO C C ** INITIAL CONDITION OF THE STATE VECTOR. *** C CONSTY( 1 )+( (R1-Y( 1) )/2.000) CONST2Y(2)+( (R2-Y(2))/2.000) CONST3Y(3)+( (R3-Y(3))/2.ODO) TLIMIT0O.1D00 Y(8)=0.000 Y(9)=0.000 Y( 12)O=.ODO v(13)=0.00D Y(14)=O.O00 Y (15)=0.000 Y( 16)=0.000 Y( 17)0.000 C C C *** DESIRED SYSTEM CHARACTERISTICS. *** C TOUT=0. 0001DO EPS. 00001DO MF 22 INDEX 1 MIR0. 45400 M1LO. 2436300 MI MIL+M1R I L20.00107400 C C *** PHYSICAL SYSTEM PARAMETERS. *** C ATN11.875104100 ATN24.694091100 ABN 1 0.734095500 ABN2 1.0184664400 MH1=0.071DO MP =0. 05D00 L 1=0.23300 KSY 1 0.0699700 LOO. 12500 JL 10 0.00000044641D0 DM=O.0072 100 L0. 001587500

180 INERT I0. 000694DO ML O.0687DO DL 0.087D0 LTL I+LO C C *** ERROR CRITERION ** C R1 IL1+R1 ATT1R1-Y( 1) ATT2sR2-Y(2) ER iPERO.000012700 ER2PRERER1PER/R1 C C PHEE IOCOSH(ATN1 )-DCOS(ATN )-ABN1*(DSINH(ATN1 )-OSIN(ATN1 )) PHEE2wOCOSH(ATN2)-DCOS(ATN2)-ABN2*(DSINH(ATN2)-SIN(ATN2) ) A~M2+MP+MH 1 PININERTI +MH1*( (L1+CONST-L2)**2)+(M1*(LT**2) )/2. ODO+ I (M2*(L2**2))/12o.00+MI*(((LT/2.ODO)-LO)**2)+M2* I ((LI+CONST-(L2/2.ODO))**2)+MP*((L1+CONST)**2) PIIN(M1R*L1 )/2ODO+M2* (L 1+CONST-(L2/2.ODO))+ I MP*(LI+CONST)+(ML*DL)+MH1*(LI+CONST-L2)-((LO*M1L)/2.0DO) AA1=(MHLO)/(DSIN(KSY 1)*DCOS(KSY 1)) AA2=(PIN*(DTAN(KSY )+MU) )/LO TW2,( 12.566D*L 1 )/(L*DM) TW3aM2* (L I+CONST-(L2/2. 000) )+MP* (L 1+CONST) I +MH1*(L1+CONST-L2) C A2EAA1+TW2*LO*( 1.000-(MU*DTAN(KSY1)))+AA2 AA3A* (OTAN(KSY1 )+MU)+TW2*( 1.ODO-MU*TAN(KSY1 ) ) AA4-MHI-((PIlN*DCOS(CONST2))/LO) C B31 o000/(PIN*((DCOS(CONST2))**2)+(MH1*(LO**2)+IL2)) IK33=(POLE3*(WN ETA**2) )/B3 KS33 ( (WNFETA**2)+2.0O*POLE3*ZETA3*WNFETA )/B3 KS36=(2.O.0*ZETA3*WNFETA+POLE3)/B3 C C A42P=-(G*A*OCOS(CONST2)*(DTAN(KSY1 )+MU))/AA3 B1P(2.000*( 1.ODO-(MU*OTAN(KSY1 ))) )/(AA3*DM) A51P( (-( (G*COS(CONST2) )/LO)*A*(DTAN(KSY )+MU)*A2E)-(((! 2.00/LO)*((OTAN(KSY1 )+MU)**2))*TW3*AA4*G))/(A2E**2) A52Px+(PIIN*G*DSIN(CONST2)*(DTAN(KSY1)+MU))/(LO*A2E) B2Ps(2.000* (.ODOMU*DTAN(KSY1)))/(OM*A2E) C IK11P=(POLE1*(WNR**2))/B1P IK22P=(POLE2*(WNTETA**2))/B2P C C KS11P=((WNR**2)+2.000*POLE1*ZETA1*WNR)/B1P KS22P=( (WNTETA**2)+2.000*POLE2*ZETA2*WNTETA+A52P)/B2P C C KS12PA42P/B 1P KS21P=A51P/B2P C C KS14P=(2.ODO*ZETA1*WNR+POLE1 )/B1P KS25P,(2.000*ZETA2*WNTETA+POLE2)/B2P C

181 c A42Na-(G*A*DCOS(CONST2)*(DTAN(KSY1)-MU))/AA3 BIN=(2.000* ( 1.O00+(MU*DTAN(KSY ))))/(AA3*DM) ASIN-((-((G*OCOS(CONST2))/LO)*A*(DTAN(KSY1)-MU)*A2E)-(( 1 2.0DO/LO)*(((DTAN(KSY1))**2)-(MU**2))*TW3*AA4*G))/(A2E**2) A52N-+(PIIN*G*DSIN(CONST2)*(DTAN(KSY1)-MU))/(LO*A2E) B2N (2.0DO*( 1.ODO+MU*DTAN(KSY)))/(DM*A2E) C IK 1IN(POLEi *(WNR**2))/BtN IK22N-(POLE2*(WNTETA**2))/B2N C C KSI IN((WNR**2)+2.0DO*POLE *ZETA1*WNR)/B1N KS22N-((WNTETA**2)+2.0DO*POLE2*ZETA2*WNTETA+A52N)/B2N C C KS12N~A42N/B1N KS2tNSA51N/B2N C C KS14N-(2.000*ZETA1*WNR+POLE1 )/B1N KS25N (2.ODO*ZETA2*WNTETA+POLE2)/B2N C 15 If(TH.LT.0.5DO)GO TO 343 INOEXX2 C *- CALL GEAR SUBROUTINE TO SOLVE THE EQUATIONS OF MOTION. ** C 343 CALL DGEAR (N.TO,HY,TOUT,EPS,MF.INDEX) C C C THE CONDITION FOR THE VARIABLE S IS INCLUDED TO POINT C OUT THE SINGULARITY OF THE INERTIA MATRIX WHENEVER IT C OCCURS AND THEN TO HALT THE EXECUTION OF THE PROGRAM. C C IF(S.EO.0.ODO) GO TO 101 GO TO 102 101 WRITE(7.103) S 103 FORMAT( OX,'S', F6.4) GO TO 34 102 WRITE(7,11) TOUT.Y(1),Y(2).Y(3),Y(4),Y(5).Y(6) 11 FORMAT(IX,F7.42X.5(E14.2,E4.8X) 14.86X) C C S8* STORE THE NUMERICAL RESULTS FOR PLOTING. ** C TH TH+ 1.0 DO Y1(I)~Y( ) Y2(1 )Y(2) Y3(I )Y(3) Y4(I )Y(4) Y5( )sY(5) Y6( )Y(6) Y7( )=Y(7) Y8(I)=Y(8) Y9( )Y(9) Y 10( )Y(10) Y11( )=Y(1i) Y12(I )=Y(12) Y13(I )Y(13)

182 Y14(1)sY(14) Y15(I )C1 Y16(I )C2 Y 17(1 )C3 VPHEE Y (4)+PHEE2*Y(5) W=PHEE1*Y(6)+PHEE2*Y(7) 01(1)=V 02 ( I)w X( I )(LI+Y( 1) )DCOS(Y(2) )*DCOS(Y(3) )-V*DSIN(Y(2) )*DCOS(Y(3)) I +W*DSIN(Y(3)) YA(I )=(LI+Y( ))*DSIN(Y(2) )+V*DCOS(Y(2)) Z(I )(L+Y( 1 ) )DCOS(Y ( 2)) *DSIN(Y(3))-V*DSIN(Y(2))*DSIN(Y(3)) 1 -*oDCOS(Y(3)) C C C Z(I) SHOULD BE CONSIDERED NEGATIVE, HOWEVER THE COORDINATE C SYSTEM ADOPTED BY THE PLOTTING ROUTINE IS THE LEFT HANDED C SYSTEM (i.e.X IS HORIZONTAL AND POINTING TO THE RIGHT.,Y C IS VERTICAL AND POINTING UPWARD. Z IS PERPENDICULAR TO C THE PLANE OF THE SCREEN AND POINTING TOWARDS THE SCREEN.) C FOR THIS REASON Z(I) IS CONSIDERED POSITIVE IN ORDEER TO C MAKE IT AGREE WITH THE PHYSICAL SYSTEM. C TI (I )TOUT WRITE(6,208)TOUT 208 FORMAT(2X,'TOUT',F7.4) IF (INDEX.EQ.O) GO TO 13 WRITE(7,12) INDEX 12 FORMAT(//.17X,'ERROR RETURN WITH INDEX=' 17) GO TO 34 13 TOUT=TOUT+O.0037DO IF(TOUT.GT.TLIMIT)GO TO 206 GO TO 207 206 TLIMITsTLIMIT+O.01DO WRITE(6 203) 203 FORMAT(//.2X,'WOULD YOU LIKE TO HALT THE EXECUTION? (Y/N)') READ(5,204)ATTT 204 FORMAT(A4) IF(ATTT.EQ.YES)GO TO 205 207 IF(TOUT.LE.6.0DO) GO TO 15 C C '* OUTPUT THE REST OF THE NUMERICAL RESULTS. ** C 205 WRITE(7.446) 446 FORMAT(////,7X, 'TIME', 1X,'022(T)',13X.'r DOT',12X, 1 'THETA DOT',.iX,'PHY DOT',11X,'011(T) DOT',9X. 1 '012(T) DOT') C C DO 444 ILL=I,I WRITE(7,445)TI (ILLL), Y7( ILL). Y8( ILLY9(ILL), ILL) (ILL) 1,Y12(ILL) 445 FORMAT( 5X. F7.4,5X,6( E 14.8,5X)) 444 CONTINUE C C WRITE(7,556) 556 FORMAT(////,7X,'TIME',SX '21(T) DOT',9X,'022(T) DOT,'8X, 1 'r CONTROLLER'. 5X 'THETA CONTROLLER'.4X, 'PHY CONTROLLER')

183 DO 555 IT-1.I WRITE(7,592)TI( IT).Y133(IT).Y14(IT),Y15(IT).Y16(T),Y17(IT) 592 FORMAT(5X.F7.4.5X,5(E 4.8,5X)) 555 CONTINUE C C WRITE(7,435)B 1P.B2P.B 1NB2N,B3,IK11PIK22PIK33, I KS11P,KS22P,KS11N.KS22N,KS14P,KS25PKS14N.KS25N.KS36. I KS12PKS21P,KS12N,KS2IN.KS33 435 FORMAT(///.2X 'B1P-', E14.8.3X.'B2P',E14.8,3X.'BIN'.E14.8. I /.2X,'B2N',E14.83X.'B3~'.E14.83X3X.'IKI1P',.E4.8. I /.2X.'IK22P='.E14.8,3X,'IK33='.E14.8.3X.'KS11P='.E14.8, 1 /.2X.'KS22P.'.E14.8.3X.'KS11N=',E14.8,3X.'KS22N='.E14.8. 1 /,2X,'KS14P~'.E14.8.3X.'KS25P='.E14.8.3X,'KS14N='.E14.8. 1 /.2X,'KS25N'.E14.8.3X.'KS36='.E14,8.3X.'KS12P=',E14.8, 1 /.2X'KS21P='oE14.8.3X. 'KS12N=',E14.8.3X,'KS21N='.E14.8. I /o2X.'KS33='.E14,8) C WRITE(7,93)RDMAX TDMAX RDPER, TDPER 93 FORMAT(2X.'RDMAX=',F2.1010.4X'TDMAX=',F20.10./, 1 2X,'ROPER',F20. 10.4X,'TDPER=',F20. 10) C WRITE(7,587) 587 FORMAT(///,15X,'" POSITION OF THE END EFFECTOR."',///) C C WRITE(7,588) 588 FORMAT(///.7X, 'TIME'. 10X, 'X COORD.',O X, 'Y COORD. '.OX, I 'Z COORD.',///) C C DO 589 IK=1,I WRITE(7,590)TI(IK),X(IK).YA(IK),Z(IK) 590 FORMAT( 5X, F7. 4,6X,3(E14.8, 4X)) 589 CONTINUE C WRITE(7,600) 600 FORMAT(//,3X,'TIME',6X,'V DEFLECTION',7X,'W DEFLECTION') DO 601 IKL=1,I WRITE(7,602)TI(IKL),D(IKL),D2(IKL) 602 FORMAT(2X, F7.4 3X, 2(E14.8,5X)) 601 CONTINUE C C *** MAKE THE 2-0 PLOTS. *** C CALL PLTF(I,Y1,Y2.Y3,Y4,Y5.Y6,Y7,Y8.Y9,Y10,.Yl. I Y12,Y13.Y14,Y15,Y16,Y17.D1,D2,TI) C C **' MAKE THE 3-D PLOT. '** C WRITE(6,111) 111 FORMAT(2X,'WOULD YOU LIKE TO MAKE A 3-D PLOT? (YES/NO)') READ(5,112)AT1 112 FORMAT(A4) - IF(AT1.EQ.NO)GO TO 34 CALL DIM3(I,X,YA,Z) CT 34 WRITE(7.16) NSTEP,NFE,NJE 16 FORMAT(//,'PROBLEM COMPLETED IN',110,2X, 1 'STEPS',//,21X,I10.2X,'F EVALUATIONS',//

184 I 21X.I10.2X.'J EVALUATIONS'.//) STOP END C C SUBROUTINE DIFFUN (N.TIME,Y.YDOT) C C tCe~~6~oo~ PROGRAM IDENTIFICATION ***** C C THIS SUBROUTINE DEFINES THE DIFFERENTIAL EQUATIONS OF MOTION C C C********* VARIABLE IDENTIFICATION *55***~ C C C M1'MASS OF THE FIRST BEAM C M2=MASS OF THE SECOND BEAM C MP=MASS OF THE PAYLOAD C MH1=MASS OF THE HOUSING OF THE LEADSCREW USED C TO MOVE THE SECOND BEAM IN THE r DIRECTION. C MHI2MASS OF THE HOUSING OF THE LEADSCREW USED C TO DRIVE THE ROBOT ARM IN THE TETA DIRECTION. C CisTRANSLATIONAL CONTROLLER FORCE C C2~ROTATIONAL CONTROLLER TORQUE AROUND K VECTOR C C3~ROTATIONAL CONTROLLER TORQUE AROUND THE VERTICAL AXIS C L1LENGTH OF THE FIRST BEAM C L2=LENGTH OF THE SECOND BEAM C ZETAI1DAMPING RATIO IN THE r DIRECTION C ZETA2=DAMPING RATIO IN THE TETA DIRECTION C ZETA3=DAMPING RATIO IN THE FETA DIRECTION C WNR=NATURAL FREQUENCY FOR r C WNTETA=NATURAL FREQUENCY FOR TETA C WNFETA=NATURAL FREQUENCY FOR FETA C R1=REFERENCE INPUT IN THE r DIRECTION C R2=REFERENCE INPUT IN THE TETA DIRECTION C R3=REFERENCE INPUT IN THE FETA DIRECTION C IKitiGAIN FOR THE INTEGRAL CONTROLLER C KSMIaGAIN FOR THE STATE FEEDBACK CONTROLLER. C Y( )=DISPLACEMENT IN THE r DIRECTION C Y(2)=DISPLACEMENT IN THE TETA DIRECTION C Y(3)=DISPLACEMENT IN THE FETA DIRECTION C Y(4)=Q11(T) DISPLACEMENT. C Y(5)=Q12(T) DISPLACEMENT. C Y(6)=Q21(T) DISPLACEMENT. C Y(7)=Q22(T) DISPLACEMENT. C Y(15) TO Y(17) ARE THE INTEGRAL OF THE ERROR BETWEEN C THE DESIRED AND THE COMPUTED POSITIONS AND VELOCITIES. C A2=CROSS SECTIONAL AREA OF THE SECOND BEAM C ROWDENSITY OF THE UNIFORM SECOND BEAM C C C C ~*8-***** STORAGE BLOCK ***~

186 c c REAL*8 M1.M2.MP.L1.L2.R1,R2.R3,Y(N YD(N),YDOT(N),A4.B4 REAL*8 KSY1,MHI.,Ct.C2,C3C3,ACC3BCC.MUS.KS N.KS 14N REAL*8 LO.G.MU.KS1 P,KS14P,KS22P,KS25P.IK1IN.KS22N.N1.N2 REAL'8 ACC7.,dL,LKS2SN,DM.KS33,KS36,IK22N.KS12N.KS21N REAL*8 IK11P IK22P.IK33,ACC9,M1L,M1R.LT,IL2.ATT,ATT2 REAL*8 ER1PERER2PER,RDMAX.TDMAXRDPER.TDPER.SGNASGN REALS TT1.TT3.TT4,ABC1,ACC12,ACC13,ACC14.CONST2.CONST3 REAL'8 CONST.TA1.TA2,ERR1.ERR2.KS12P.KS21P.ML.DL.INERTI C C REAL'8 TIME,CENTACDIST,PALL(7)oSPART30,PART31.PART32 INTEGER IV(7),IJ.IN,IM,IYIUJUUH.K,BC,DC.PART33.PART34 REAL*8 PBY(7) oA2,ROW,A13.A14 A15,ROW7.ROW8 PART35 PART36 REAL*8 ROW1,ROW2,ROW3,ROW4,ROW5,ROW6,PART37,PART38,PART39 REAL*8 M2P,AL1,AL2.AL10AL11.EI,ROW10,ROW9oPART40,PART41 REAL*8 PART50,PART51,8(7,7),T(7,7), F(7) D(7). YVECT(7) REAL*8 PTB(7, 7),PTF(7),PTD(7) PTBY(7). A(7 7) PART60. PART61 REAL*8 PART1.PART2.PART3,PART4,PARTS,PART62.PART63,PART64 REAL*8 PART6,PART7,PART8,PART9,PART65,TMIN1.TMIN2 C C COMMON /SINGUL/ S,A2,ROW, E COMMON /GAINKP/ KSI1P.KS14PIK11P.KS22P,KS25P. 1 IK22P.KS.KS33 KS3 IK33.KS12P.KS2P COMMON /GAINKN/ KS11N,KS14N,IK 1N,KS22N,KS25N. I IK2NKS2N,K2NKS21N COMMON /CONTL/ C1,C2.C3 COMMON /DATA/ MI M2,MP,MH1,LI1,L2,MUG,KSY1.MIL.,M1R RLTIL2 I.R1,R2.R3,LO.JL1,L,DMTT3.TT4 COMMON /DATA1/ ATT1,ATT2,MUS.CONST2,CONST3 I,ML,DLINERTI.TT1.TA1,TA2 COMMON /DT/ RDMAX,TDMAX,RDPER,TDPER,ER1PER,ER2PER.CONST C C TT1 IS INTRODUCED TO DETECT THE FIRST TIME THE INTEGRATION C IS CARRIED OUT. C IF(TT1.GTo0.5DO)GO TO 200 YDOT(8)=O.ODO YDOT(9)=O.ODO YDOT(10)=0.000 YDOT( 11)=0.ODO YDOT( 12)=OODO YDOT(13)=0.0DO YDOT(14)=0.000 C 1 *0.000 C20. 000 C3-0o000 C C 200 M=7 A13=0.7600 A14=0.53D00 A 15=4.34DO AL 10.78D00 AL2O0. 4300 AL10 12. 362D0 AL 11485.51900 C

186 C INITIALIZATION OF THE T MATRIX. C DO 222 1-1,7 DO 222 dJ1,7 T( I J)0.000 222 CONTINUE C C C C C* ~*000 *e* * 0000 * * **** * **0 * - ** * * * * * * * 0* 0000 * * * * * *** C C C*****4 *** 50~SORT BLOCK 0***0 * C C C CENTAC=(Y( 10)**2)*((OCOS(Y(2)))* *2)+(Y(9)**2) OIST=(L*Y( 1 ) +((Y( 1)**2)/2.000) BCL I+Y( )-(L2/2.0DO) ROW 1 ROW*A2 ROW2uL1-(L2/200D0)+(Y( 1)/2.000) ROW3L2-Y( 1 ) ROW4=LI+Y( 1) ROW5=(L1'*2)*Y( 1)+L1*(Y( 1)**2)+(Y( 1)*3)/3.0DO ROW6 ROW 1/2.000 ROW7 MP* ROW4 ROW8=((ROWI*Y(1))+MP)*G*DSIN(Y(2)) ROW9=ROWI*G*DSIN(Y(2)) ROWIO=ROW1*(Y( 10)**2)*OSIN(Y(2) )*DCOS(Y(2)) N2P ROW*Y( 1 ) C A4 =M2+MP+MH 1 ACC9 MH /( (DS I N(KSY 1 ) ) * (COS(KSY 1 ) ) ) ACC7=( 12.566D*L 1)/(L*DM) B4 M2*BC+MP*ROW4 BCC=B4+MH1*(LI+Y( )-L2) ACC3( (M1R*L1)/2.ODO)+MP*((L+Y( 1))+2*(LI+Y( 1 )-(L2/2.ODO)) i +(ML*DL)+MH1*(LI+Y(1)-L2)-((M1L*LO)/2.ODO) DC=((M1*(LT**2))/12.00)+(M2*(L2**2))/12.ODO+M2*((L+Y( 1 ) 1 -(L2/2.oO))**2)+MP*((L1+Y( ))**2)+INERTI+MHI*((L1+Y( ) 1 -L2)**2)+MI*(((LT/2.000)-LO)**2) C C CHECK WHETHER THE r MOTION IS ON THE UPPER OR LOWER THREAD. C PART30=( (2.0D*C1)/DM-ACC7*YDOT(8))*DSIN(KSY1)+(A4*(G* I DSIN(Y(2))+YDOT(8))-BCC*(((Y(10)**2)*((DCOS(Y(2)))**2))+ (Y(9)**2))+2.00DO I Y(10)*DCOS(Y(2))*((0.78DO*Y(13)+0.43D0*Y(14))*M2P+2.0DO*MP*( 1 Y(13)-Y(14)))+2.0DO*ROW1*Y(8)*Y(10)*DCOS(Y(2))*(0.78DO*Y(6)+ I 0.43DO*Y(7))-2.0DO*Y(9)*(M2P*( 078DO*Y(11)+0.43DO*Y(12))+ 1 2.000*MP*(Y(11)-Y(12))))*DCOS(KSY1) PART31=(-2.00*Y (8)*Y(9)*ROW1*(0.78DO*Y(4)+0.43DO*Y(5))+ I YDOT(10)*DCOS(Y(2))*(M2P*(O.78DO*Y(6)+0.43DO*Y(7))+2.OD0*MP 1 *(Y(6)-Y(7)))-(YDOT(9)-(Y(10)**2)*OSIN(Y(2))*DCOS(Y(2)))* 1 (M2P*(0.78DO*Y(4)+0.43DO*Y(5))+2.000*MP*(Y(4)-Y(5)))') 1 *DCOS(KSY1) N1=PAR 3ART30+PAT31 IF(N1.GEo0.000)GO TO 535 GO TO 600

187 C ** LOWER THREAD LOGIC FOR r MOTION. *** C 535 IF(TT1.GT.O.5DO)GO TO 542 IF(ATT1 )536.537.538 536 C1=-KS1lN*(Y(1)-CONST)-KS14N*Y(8)+IK11N*Y(15)-KS12N*Y(2) GO TO 542 537 C 1 0.000 GO TO 547 538 C1~-KSIP(Y( 1 NT)CONST)-KS14P*Y(8)+IK11P*Y(15)-KS12P*Y(2) 542 IF(TT3.GT.0.500)GO TO 571 IF(SGN(Y(8)))539,540,541 539 C1t-KS11N*(Y( 1)-CONST)-KS14N*Y(8)+K11N*Y( 15)-KS12N*Y(2) GO TO 546 541 C 1 -KS 11P(Y ( 1 )-CONST)-KS14P*Y(8)+IK11P*Y( 15)-KS12P*Y(2) GO TO 546 540 IF(TT1.LT.0.5DO)GO TO 552 ATTIR1-Y( 1) IF(ATT 1)549,550.551 549 C1=-KS1N*(Y( 1 )-CONST)-KS14N*Y(8)+IK1N*Y(15)-KSi2N*Y(2) GO TO 552 550 C1=0.000 GO TO 547 551 C=-KS11P*(Y( 1)-CONST)-KS14P*Y(8)+IK11P*Y(15)-KS12P*Y(2) 552 IF(C1)543.547.545 C C *** COMPUTATION OF THE MINIMUM *** C *** TORQUES FOR r. *** C 543 ABC 1 i.OO+MUS*(DTAN(KSY 1 ) ) ACC14O=TAN(KSY1)-MUS TMINI=-(-A4*G*DSIN(Y(2)+BCC*((Y(10)**2)*((DCOS(Y(2)))**2)+ i (Y(9)**2))-2.000*Y(10)*DCOS(Y(2))*((0.7800*Y(13)+0.43DO* 1 Y(14) )M2P+2.0DO*MP*(Y(13)-Y(14)))+2.0DO*Y(9)*(M2P*(0.78DO* I Y(11)+0.4300*Y(12))+2.ODO*MP*(Y(11)-Y(12)))-((Y(10)**2)* I DSIN(Y(2))*DCOS(Y(2))-YDOT(9))*(M2P*(0.78DO*Y(4)+0.43DO*Y(5)) I +2.000*MP*(Y(4)(5)-Y( )-YDOT(10)*DCOS(Y(2))*(M2P*(0.78D0*Y(6)+ I 0.43DO*Y(7))+2.OO*MP*(Y(6)-Y(7))))*(DM/2.ODO)*(ACC14/ABC1) IF(C1.LT.TMIN1)GO TO 546 GO TO 547 545 ABC1=1.0DO-MUS*(DTAN(KSY1)) ACC14=DTAN(KSY1)+MUS TMIN1=-(-A4*G*DSIN(Y(2))+BCC*((Y(10)**2)*((DCOS(Y(2)))**2)+ (Y(9)**2))-2.000*Y(10)*DCOS(Y(2))*((0.78DO*Y(13)+0.43DO* 1 Y(14))*M2P+2.000*MP*(Y(13)-Y(14)))+2.ODO*Y(9)*(M2P*(0.78DO* 1 Y(11)+0.43DO*Y(12))+2.000*MP*(Y(11)-Y(12)))-((Y(10)**2)* 1 DSIN(Y(2))*DCOS(Y(2))-YDOT(9))*P*M2P*(.78DO*Y(4)+.43DO*Y(5)) I +2.000*MP*(Y(4)-Y(5)))-YDOT(10)*DCOS(Y(2))*(M2P*(0.78DO*Y(6)+ 1 0.43DO*Y(7))+2.000*MP*(Y(6)-Y(7))))*(DM/2.000)*(ACC14/ABCI) IF(C1.GT.TMIN1)GO TO 546 GO TO 547 546 IF(TA1.LT.0.5DO)GO TO 31 IF(RDMAX.GE.DABS(Y(8)))GO TO 30 31 RDMAX=DABS(Y(8)) 30 TA1=TAI+1.000 ROPE R RDMAX 0.0000100 ERR1iDABS(R1-Y(1)) IF((DABS(Y(8)).GT.RDPER).OR.(ERR1.GT.ER1PER))GO TO 34 571 Y(1)=R1 C1=0.000 Y(15)=O.ODO

188 TT31. 000 GO TO 547 C 34 PART38DOTAN(KSY )+MU*SGN(Y(8)) PART39=1 o00-(MU*OTAN(KSY 1)*SGN(Y(8) ) ) A( 1 1 )aA4*(MU+DTAN(KSY ) )+ACC7*(.000-MUDTAN(KSY1 )) A( 1,2),-(M2P*(0.78DO0Y(4)+0.43D00Y(5))+2.000*MP*(Y(4)-Y(5)))* 1 PART38 A( 1,3)-DCOS(Y(2))*(M2P*(0.78DO*Y(6)+0.43DO*Y(7) )+2.0D0MP* 1 (Y(6)-Y(7)))*PART38 A( 14)-0.00 A(1.5)=O.ODO A(1.6)20.000 A(1.7)O0.0OD B(1,1)x-(-2.000*ROW1*Y(10)*DCOS(Y(2))*(0.78DO*Y(6)+0.43DO* 1 Y(7))+2.000*Y(9)*ROWI*(0.7800*Y(4)+0.4300*Y(5)))*(MU+ 1 DTAN(KSY1)) B( 1,2)-(BCC*Y(9)+2.000*(M2P*(0. 7800*Y( 1 )+0. 4300*Y( 12))+ 1 2.OOO*MP*(Y(11)-Y(12))))*PART38 B( 1I3)=-(BCC*Y( 10)*((DCOS(Y(2) ))**2)-2.0DO*DCOS(Y(2))*(( I 0.7800*Y(13)+0.4300*Y(14))*M2P+2.0O*MP*(Y(13)-Y(14)))-Y(10) 1 *DSIN(Y(2))*DCOS(Y(2))*(M2P*(0.78DO*Y(4)+0.43DO*Y(5))+2.000* 1 MP*(Y(4)-Y(5)) ) )*PART38 B( 1 e4)0.000 8(,5))=0.ODO B( 156)-O.ODO 8( 1,6)=0.000 B( 1.7)z0.000 F( )( (2.000*C1*PART39)/DM) D( )-A4*G*DSIN(Y(2))*PART38 GO TO 548 547 Y(8)=0.000 A(,2)=0.000 A( 1 3)=0.000 A( 1 4)=0.000 A( 1.5)=0.00 A(.6)=0.000 A( 1.7)0.000 B( 1 1 )=0.000 B( 1,2)=0.0DO B(1 3)=0.000 B(,o4)=O.ODO B(, o5)0.000 B(1,6)=0.000 (1.7)0.0DO F( )=0.000 D0( )=0.0DO GO TO 548 C C LOGIC FOR THE r MOTION ON THE UPPER THREAD. C 600 IF(TT1.GT.O.S5O)GO TO 601 IF(ATT )602.603,604 602 C1l-KS11P*(Y( 1)-CONST)-KS14P*Y(8)+IK11P*Y(15)-KS12P*Y(2) GO TO 601 603 C10. 000 GO TO 605 604 C1,-KS11N*(Y(l)-CONST)-KS14N*Y(8)+IK11N*Y(15)-KS12N*Y(2) 601 IF(TT3.GT.0.5DO)GO TO 606 IF(SGN(Y( 8) ))607,608. 609

189 607 CI*-KS P*(Y(1 )-CONST)-KS14P*Y(8)+IK1P*Y( 15)-KS12P*Y(2) GO TO 610 609 C1 —KS 1N*(Y( 1)-CONST)-KS14N*Y(8)+IK 11N*Y(15)-KS12N*Y(2) GO TO 610 608 IF(TT1.LT.0.5DO)GO TO 611 ATT=R1-Y( 1) IF(ATT1 )612,613,614 612 C1I-KSI P*(Y( 1)-CONST)-KS14P*Y(8)+IK1 P*Y( 15)-KS12P*Y(2) GO TO 611 613 C -O.ODO GO TO 605 614 C1~-KS11N*(Y(1)-CONST)-KS14N*Y(8)+IK11N*Y(15)-KS12N*Y(2) 611 IF(C1)615.605.617 615 ABC1 1.000-MUS(DTAN(KSY 1 ) ) ACC 14DTAN(KSY )+MUS TMIN1w-(-A4*GODSIN(Y(2))+BCC*((Y( 10)**2)*((DCOS(Y(2)))**2)+ I (Y(9)**2))-2.00O*Y(10)*DCOS(Y(2))*((0.78DO*Y( 13)+0.43D* 1 Y(14))*M2P+2.00O*MP*(Y(13)-Y(14)))+2.000*Y(9)*(M2P*(0.78D00 1 Y( 11 )+0.43D0*Y(12) )+2.000*MP*(Y( 11)-Y(12)))-((Y(10)**2)* 1 OSIN(Y(2))*DCOS(Y(2))-YDOT(9))*(M2P*(0.78DO*Y(4)+0.43DO*Y(5)) I +2.000*MP*(Y(4)-Y(5)))-YDOT( 10)*COS(Y(2))*(M2P*(0.78DO*Y(6)+ 1 0.4300*Y(7))+2.000*MP*(Y(6)-Y(7))))*(DM/2.0DO)*(ACC14/ABC1) IF(C1.LT.TMIN1)GO TO 610 GO TO 605 617 ABC1=1.0D0+MUS*(DTAN(KSY1)) ACC14=DTAN(KSY1)-MUS TMIN1=-(-A4*G*DSIN(Y(2))+BCC*((Y(10)**2)*((DCOS(Y(2)))**2)+ 1 (Y(9)**2))-2.0DO*Y(10)*DCOS(Y(2))*((0.78DO*Y(13)+0.43DO* 1 Y(14))*M2P+2.ODO*MP*(Y(13)-Y(14)))+2.0DO*Y(9)*(M2P*(0.78DO* 1 Y(11)+0.43DO*Y(12)")+2.00DOMP*(Y(11)-Y(12)))-((Y(10)**2)* 1 DSIN(Y(2))*DCOS(Y(2))-YDOT(99)*(M2P*(0.7800*Y(4)+0.4300*Y(5)) 1 +2.000*MP*(Y(4)-Y(5)))-YOOT( 10)*DCOS(Y(2))* (M2P* (0.78DO*Y (6)+ I 0.43DO*Y(7))+2.ODO*MP*(Y(6)-Y(7))))*(DM/2.ODO)*(ACC14/ABC1) IF(C1.GT.TMIN1)GO TO 610 GO TO 605 610 IF(TA1.LT.0.500)GO TO 619 IF(RDMAX.GE.DABS(Y(8)))GO TO 620 619 RDMAXxDABS(Y(8)) 620 TA 1TA+11.0DO RDPER RDMAX*0.00001DO ERR1=DABS(R-Y( 1)) IF((DABS(Y(8)).GT.RDPER).OR.(ERR1.GT.ER1PER))GO TO 621 606 Y(1)=R1 C 1=0.000 Y( 15)=0.0DO TT3=1.000 GO TO 605 C 621 PART38=DTAN(KSY1)-MU*SGN(Y(8)) PART39=1.000+(MU*DTAN(KSY1 )*SGN(Y(8))) A(1,1)=A4*(MU+DTAN(KSY1))+ACC7*(1.0DO-MU*DTAN(KSY1)) A(1,2)=-(M2P*(0.78DO*Y(4)+0.43DO*Y(5))+2.0DO*MP*(Y(4)-Y(5)))* 1 PART38 A(1,3)=DCOS(Y(2))*(M2P* (0.78DO*Y(6)+0. 43DO*Y(7))+2.0DO*MP* 1 (Y(6)-Y(7)))*PART38 A( 1.4)=0.0D0 A( 1,5)=0.000 A (1.6) 0.000 A( 1.7)=0.000 B(1 1) —(-2.000*ROW1*Y(10)*DCOS(Y(2))*(0.78DO*Y(6)+0.43DO0

100 I Y(7))+2.ODO*Y(9)*ROW1*(0.78DO*Y(4)+0.43DO*Y(5)))*(MU+ 1 DTAN(KSYI)) B(.2)z-(BCC*Y(9)+2.000*(M2P*(0.78DO*Y(11 )+0.43DO*Y( 12))+ I 2.000*MP*(Y(1i)-Y(12))))*PART38 B( o3) —(BCC*Y( 10)*((DCOS(Y(2)))*2)-2.00D0DCOS(Y(2))*(( 1 0.78DO*Y(13)+0.43DO*Y(14))*M2P+2.000*MP*(Y(13)-Y(14)))-Y(10) I *DSIN(Y(2))*DCOS(Y(2))*(M2P*(0.78DO*Y(4)+O043DO*Y(5))+2.00DO 1 MP*(Y(4)-Y(5))))*PART38 8( 1o4)0.000 B( 1,5)0.000 B( 1.6)~0.000 8( 1.7)0.000 F( 1 ), ((2.000C *PART39)/DM) ( 0)8-A4*G*DSIN(Y(2))*PART38 GO TO 548 605 Y(8)=Oo000 A( 1 1)1.000 A( I,2)=0.000 A(,3)=0.000 A( 14)=0.OD0 A( 15)=0.000 A( i.6) 0.000 A(,7)=0.00 ( 1,1)=O.ODO B(,2)*0.000O B- 1,3)=OoODO 8(. 3)=0o000 B( t,4)0o.000 8(. 5)=0o.00 B(1,6)o0.000 B(t1 7)=0.o 0 F( ) 0.000 o( )=0.000 C C CHECK WHETHER THE THETA MOTION IS OCCURING C ON THE UPPER OR LOWER THREAD. C 548 PART32=-((2.000*C2)/OM-ACC7*LO*YDOT(9))*DSIN(KSY1)+(MH1*G i -((ACC3*G*DCOS(Y(2)))/LO)-((YDOT(9)+(Y(10)**2)*DSIN(Y(2))* 1 DCOS(Y(2)) )/LO)*DC-((2 000*Y(8)*Y(9))/LO)*BCC-(ROW/LO)* ( I 0.78DO*YDOT(8)*L1*Y(4)+1.56DO*L1*Y(8)*Y( 1 )+0.4300*YDOT(8)* i L1*Y(5)+0.86DO0Y(8)*Y(12)*L1+0.78DO*Y(I)*YDOT( 11)*L+ 0.43DO*Y( 1)*L*YDOT( 12)-0.43D0*((Y(8)**2)*Y(4)+Y( 1)*YDOT(8)* i Y(4)+Y( 1)*Y(8)*Y( 1 1)) -0.68D0* ((Y(8)*2)*Y(5 )+Y( 1)*YDOT(8)* 1 Y(5)+Y(1)*Y(8)*Y(12))+0.5700*(2.000*Y( )*Y(8)*Y(11)+( Y(1)**2) I *YDOT( )+0.09 (2.ovv( 2) ( 1 2) OT( ) )2. *Y()*Y(8)*Y(12)+(Y )**2)YDOT(12)) 1 ))*DCOS(KSYI) PART33 ( - ( (2.O*MP)/LO)* (ROW4 ( YDOT(11) -YDOT (12) )-YOT(8)* I (Y(4)-Y(5)))+((2.ODO*Y( 10)*DSIN(Y (2)) *ROW)/LO) *((0.78DO 1 Y(1)*L1+0.57D0*(Y(1)**2))*Y(13)+(0.43DO*Y(1)*L1+0.09DO*( 1 Y(1)**2))*Y(14)+(0.7800*Y(8 0DO*Y() +.35DO*()*Y(8))*Y(6)+ 1 (0.43D0*L1*Y(8)-0.2500*Y(1)*Y(8))*Y(7)))*DCOS(KSY1) PART34=( ( (4.0DO*Y( 10)*DSIN(Y(2) )*MP*ROW4)/LO)*(Y( 13)-Y( 14) ) 1 +((ROWI*YDOT(10)*DSIN(Y(2)))/LO)*((0.78DO*Y(1)*L1+0.57DO* 1 (Y(1)**2))*Y(6)+(0.43DO*Y(1)*L1+0.09DO*(Y(1)**2))*Y(7)) 1 +((2.000*MP*YDOT( 10)*SIN(Y(2)))/LO)*ROW4*(Y(6)-Y(7))+( I ROWt/LO)*(((Y( 10)**2)*((OSIN(Y(2)))**2))+(Y(9)**2))* 1 ((0.78DO*Y(1)*L1+0.57DO*(Y( 1)*2))*Y(4)+(0.43DO*Y(1)*L1 1 +O.0900*(Y(1)**2))*Y(5)))*DCOS(KSY1) PART35=(((2. OO*MP)/LO)*( ( ((Y( 10)*2)*((DSIN(Y(2)) )**2))+ 1 (Y(9)**2))*(Y(4)-Y(5))*ROW4-(ROW1/LO)*(((Y(10)**2)((

191 I OCOS(Y(2)))**2))+(Y(9)*'2))*((O.78DO*Y( 1)*LL+0.57DO* I (Y( 1)*2))*Y(4)+(0.43DO*Y( 1)*Ll0.09DO*(Y( 1)**2))*Y(5)) I -((2.000*MP*ROW4)/LO)*(((Y(.10)**2)*((DCOS(Y(2)))**2))+ 1 (Y(9)**2))*(Y(4)-Y(5))+((2.0 O*Y(O10)*ROW1*DCOS(Y(2)))/LO)*( Y( )*(Y(4)*Y( 13)+Y(5)*Y( 14))+Y(8)*(0.5DO*Y(4)*Y(6)+0.5DO*Y(5) 1 *Y(7)+0.65DO*Y(5)*Y(6)-0.65DO*Y(4)*Y(7) )))*DCOS(KSY1) PART36(((8.000*MP*Y( 10)*DCOS(Y(2)))/LO)*(Y(4)*Y( 13)-Y(4)* I Y(14)-Y(5)*Y(13)+Y(5)*Y(14))-((8.0DO*MP*Y(9))/LO)*(Y(4)* 1 Y( 1)-Y(4)*Y( 2)+Y(5)*Y(12)-Y(5)*Y(11))-((2.0DO0 i Y(9)*ROWI)/LO)*(0.5DO*Y(8)*((Y(4)**2)(Y(5)**2))+Y( 1)* 1 (Y(4)*Y( 1 )+Y(5)*Y(12)))+((YDOT(10)*DCOS(Y(2))*ROW1 )/LO) 1 *Y( )*(Y(4)*Y(6)+Y(5)*Y(7)))*DCOS(KSY1) PART37t (((4.0OO*YDOT( 10)*DCOS(Y(2))*MP)/LO)*(Y(4)*Y(6)-Y(5)* I Y(6)-Y(4)*Y(7)+Y(5)*Y(7))-(M2P/LO)*(YDOT(9)-(Y(10)**2)* 1 DSIN(Y(2))*OCOS(Y(2)))*((Y(4)**2)+(Y(5)**2))-((4.0DO*MP)/LO) I *(YDOT(9)-(Y(10)**2)*DSIN(Y(2))*DCOS(Y(2)))*((Y(4)**2)+ i (Y(5)**2)-2.0DO*Y(4)*Y(5)))*DCOS(KSY1) N2=PART32+PART33+PART34+PAPART35+PART36+PART37 IF(N2.GE.O.OOO)GO TO 700 GO TO 800 700 IF(TT1.GT.0.5DO)GO TO 701 IF(ATT2)702,703,704 702 C2~-KS21P*Y(1)-KS22P*(Y(2)-CONST2)-KS25P*Y(9)+IK22P*Y(16) GO TO 701 703 C2=0.000 GO TO 705 704 C2 —KS21N*Y(1)-KS22N*(Y(2)-CONST2)-KS25N*Y(9)+IK22N*Y(16) 701 IF(TT4.GT.0.5DO)GO TO 706 IF(SGN(Y(9)))707.708,709 707 C2=-KS21P*Y(1)-KS22P*(Y(2)-CONST2)-KS25P*Y(9)+IK22P*Y(16) GO TO 710 709 C2=-KS21N*Y(1)-KS22N*(Y(2)-CONST2)-KS25N*Y(9)+IK22N*Y(16) GO TO 710 708 IF(TT1.LT.0.5DO)GO TO 711 ATT2=R2-Y(2) IF(ATT2)7 12,713,714 712 C2=-KS21P*Y(1)-KS22P*(Y(2)-CONST2)-KS25P*Y(9)+IK22P*Y(16) GO TO 711 713 C2=0.000 GO TO 705 714 C2=-KS21N*Y(1)-KS22N*(Y(2)-CONST2)-KS25N*Y(9)+IK22N*Y(16) 711 -IF(C2)715,705,717 715 ACC12=1.0DO-MUS*(DTAN(KSY1)) ACC13=DTAN(KSY1)+MUS PART60=-(MH1*G-((ACC3*G*DCOS(Y(2)))/LO)-(((Y(10)**2)* 1 DSIN(Y(2))*DCOS(Y(2)))/LO)*DC-(ROW1/LO)*(0.78DO*YDOT(8)* 1 L1*Y(4)+1.56DO*L1*Y(8)*Y(11)+0.43DO*YDOT(8) 1 *L1*Y(5)+0.8600*Y(8)*Y(12)*L1+0.78DO*Y(1)*YDOT(11)*L1+ I 0.43DO*Y(1)*L1*YDOT(12)-0.43DO*((Y(8)**2)*Y(4)+Y(1)*YDOT(8)* 1 Y(4)+Y(1)*Y(8)*Y(11))-0.68DO*((Y(8)**2)*Y(5)+Y(1)*YDOT(8)* 1 Y(5)+Y(1)*Y(8)*Y(12))+0.57DO*(2.*00 (*1)*Y(Y(8)*Y(11)+(Y(1)**2) 1 *YDOT(11))+0.09DO*(2.ODO*Y(I)*Y(8)*Y(12)+(Y(1)**2)*YDOT(12)))) PART61=-(-((2. DO*MP)/LO)*(ROW4*(YDOT(11)-YDOT(12))-YDOT(8)* 1 (Y(4)-Y(5))+((2.000*Y(10)*DSIN(Y(2))*ROW1)/LO)*((0.78DO* 1 Y(1)*L1+0.57DO*(Y(1)**2))*Y(13)+(0.4300*Y(1)*L1+0..09DO*( 1 Y(1)**2))*Y(14)+(0.78D0*Y(8)*L1+0.35DO*Y(1)*Y(8))*Y(6)+ 1 (0.43DO*L1*Y(8)-0.25DO*Y(1)*Y(8))*Y(7))) PART62-(((4.ODO*Y( 10) DSIN(Y(2)) *MP*ROW4)/LO)*(Y( 13)-Y( 14)) 1 +((ROW1*YDOT(10)*DSIN(Y(2)))/LO)*((0.78DO*Y(1)*L1+0.57DO* 1 (Y(1)**2))*Y(6)+(0.4300D*Y(1)*L+0.0900*(Y(1)**2))*Y(7))

192 1 +((2.000MP*YDOT(10)*DSIN(Y(2)))/LO)*ROW4*(Y(6)-Y(7))+( 1 ROW1/LO)*((Y(10)*2)*((DSIN(Y(2)))**2))* 1 ((0.78DO*Y(1 )L1+0.57DO*(Y(1)**2))*Y(4)+(0.43DO*Y(1)*L I +0.09DO*(Y(1)**2))*Y(5))) PART63-( ((2.000*MP)/LO)*((Y(10)*2)*((DSIN(Y(2)))**2)) 1 *(Y(4)-Y(5))ROW4-(ROW1/LO)*((Y(10)**2)*(( 1 DCOS(Y(2)) )e2)) *((0.(78DO*Y( 1)*L+057D0* I (Y(1)**2))*Y(4)+(0.43DO*Y(1)*LI+0.09DO*(Y( )**2))*Y(5)) I -((2.00O0MP*ROW4)/LO)*((Y(10)**2)*((DCOS(Y(2)))'*2)) 1 *(Y(4)-Y(5))+((200DO*Y(10)*ROW1*DCOS(Y(2)))/LO)*( I Y( )*(Y(4)*Y(13)+Y(5)*Y(14))+Y(8)*(O.5DO*Y(4)*Y(6)+0.5DO*Y(5) I *Y(7)+0.65DO*Y(5)*Y(6)-0.65DO*Y(4)*Y(7)))) PART64 —(((8.000*MP*Y(10)*DCOS(Y(2)))/LO)*(Y(4)*Y(13)-Y(4)* I Y(14)-Y(5)*Y(13)+Y(5)*Y(14)) I +((YDOT(10)*DCOS(Y(2))*ROW1)/LO)*Y(1)* 1 (Y(4)*Y(6)+Y(5)*Y(7))) PART65S-(((4.000*YDOT(10)*DCOS(Y(2))*MP)/LO)*(Y(4)*Y(6)-Y(5)* 1 Y(6)-Y(4)*Y(7)+Y(5)*Y(7))+(M2P/LO)*(Y(10)**2)* 1 DSIN(Y(2))*DCOS(Y(2))*((Y(4)**2)+(Y(5)**2))+((4.000*MP)/LO) I *(Y(10)**2)*DSIN(Y(2))*DCOS(Y(2))*((Y(4)**2)+ 1 (Y(5)**2)-2.000*Y(4)*Y(5))) TMIN2-(PART60+PART61+PART62+PART63+PART64+PART65)*(DM/2.ODO)* 1 (ACCI3/ACC12) IF(C2.LT.TMIN2)GO TO 710 GO TO 705 717 ACC12s1.OO0+MUS*(OTAN(KSY1)) ACC 13.OTAN(KSY 1 )-MUS PART60=-(MH1*G-((ACC3*G*DCOS(Y(2)))/LO)-(((Y(10)**2)* I DSIN(Y(2))*DCOS(Y(2)))/LO)*DC-(ROWI/LO)*(0.78DO*YDOT(8)* I L1*Y(4)+1.5600*L1*Y(8)*Y( 1)+0.4300*YDOT(8) 1 Li*Y(5)+0.8600*Y(8)*Y(12)*L1+0.7800*Y(1)*YDOT(11)*L1+ I o43D0*Y(I)*LI*YDOT(12)-0.43DO*((Y(8)**2)*Y(4)+Y(1)*YDOT(8)* 1 Y(4)+Y( )*Y(8)*Y( 1))- 0.68D0*((Y(8)**2)*Y(5)+Y(1)*YDOT(8)* I Y(5)+Y(1)*Y(8)*Y(12))+)0,57DO*(200O*Y(1)*Y(8)*Y(t1)+(Y(1)**2) *YDOT(11))+0.09DO*(200DO*Y(1)*Y(8)*Y(12)+(Y(1)**2)*YDOT(12)))) PART61 -(-( (2.000*MP)/LO)*(ROW4* ( YDOT( 11 )-YDOT( 12) )-YOT(8)* 1 (Y(4)-Y(5) ) )+((2.000*Y( 0)*DSIN(Y(2))*ROW1)/LO)*( (0. 78DO 1 Y(1)*L1+0.57DO*(Y(1)**2))*Y(13)+(0.43D0*Y(1)*LI+0.09DO*( 1 Y(1)**2))*Y(14)+(0.780D*Y(8)*L1+0.35D0*Y( 1)*Y(8))*Y(6)+ 1 (0.43DO*L1*Y(8)-0.25DO*Y(1)*Y(8))*Y(7))) PART62=-(((4.ODO*Y(10)*DSIN(Y(2))*MP*ROW4)/LO)*(Y( 13)-Y(14)) 1 +((ROWl*YDOT(10)*DSIN(Y(2)))/LO)*((0.78DO*Y( 1)L1+0.57DO* I (Y(1)**2))*Y(6)+(0.4300*Y( 1)*L1+0.0900*(Y( )**2))*Y(7)) 1 +((2.000*MP*YOOT(10)*DSIN(Y(2)))/LO)*ROW4*(Y(6)-Y(7))+( 1 ROW1/LO)*((Y(10)**2)*((DSIN(Y(2)))**2))* 1 ((0.78DO*Y(1)*LI+0o 57DO*(Y(1)**2))*Y(4)+(043DO*Y( 1)*L1 1 +o009DO*(Y(1)**2))*Y(5))) PART63m-(((2.ODO*MP)/LO)*((Y( 10)**2)*((DSIN(Y(2)))**2)) 1 *(Y(4)-Y(5))*ROW4-(ROW1/LO)*((Y(10)**2)*(( 1 OCOS(Y(2)))**2))*((0.78DO*Y(1)*L1+0.57DO* 1 (Y(1)**2))*Y(4)+(0.43DO*Y(1)*L1+0.09DO*(Y(1)**2))*Y(5)) 1 -((2.000*MP*ROW4)/LO)*((Y(10)**2)*((DCOS(Y(2)))**2)) 1 *(Y(4)-Y(5))+((2.0DO*Y(10)*ROW1*DCOS(Y(2)))/LO)*( I Y(1)*(Y(4)*Y(13)+Y(5)*Y(14))+Y(8)*(0.5DO*Y(4)*Y(6)+0.5DO*Y(5) 1 *Y(7)+0.65DO*Y(5)*Y(6)-0.65DO*Y(4)*Y(7)))) PART64-(((8.000*MP*Y(10)*DCOS(Y(2)))/LO)*(Y(4)~Y(13)-Y(4)* 1 Y(14)-Y(5)*Y(13)+Y(5)*Y(14)) 1 +((YDOT(10)*DCOS(Y(2))*ROW1)/LO)*Y(1)* 1 (Y(4)*Y(6)+Y(5)*Y(7))) PART65s -(((4.00*YDOT(10)*DCOS(Y(2))*MP)/LO)*(Y(4)*Y(6)-Y(5)*

103 1 Y(6)-Y(4)*Y(7)+Y(5)*Y(7))+(M2P/LO)*(Y(10)*2)* 1 OSIN(Y(2))*DCOS(Y(2))*((Y(4)**2)+(Y(5)**2) )((4.ODO*MP)/LO) 1 *(Y(10)**2)*DSIN(Y(2))*DCOS(Y(2))*((Y(4)**2)+ 1 (Y(5)**2)-2.000*Y(4)*Y(5))) TMIN2 ( PART60+PART6 +PART62+PART63+PART64+PART65)* (DM/2.OD)* 1 (ACC13/ACC12) IF(C2.GT.TMIN2)GO TO 710 GO TO 705 710 IF(TA2.LT.O.SDO)GO TO 718 IF(TOMAX.GE.DABS(Y(9)))GO TO 719 718 TDMAX0DABS(Y(9)) 719 TA2TATA2+1.000 TDPER=TDMAX *0. 00 100 ERR2=0ABS(R2-Y(2)) IF((DABS(Y(9)).GT.TDPER).OR. (ERR2.GT.ER2PER))GO TO 720 706 Y(2)=R2 C2=0.000 Y(16) 0.000 TT4=1.000 GO TO 705 720 PART40=DTAN(KSY1 )-MU*SGN(Y(9)) PART4 11.000D+MU*DTAN(KSY 1)*SGN(Y(9)) A(2,1)z((ROW1/LO)*((0.78D0*L1-0.43DO*Y(1))*Y(4)+(0.43DO*L1 I -0.68DO*Y( 1)) (5) ))-((2.000*MP)/LO)*(Y(4)-Y(5)))*PART40 A(2.2 )ACACC9LO+ACC7*LO*( 1.000D-MU*DTAN(KSY ) )+(DTAN(KSY1 )+MU) I *((OC/LO)+(M2P/LO)*((Y(4)**2)+(Y(5)**2))+((4.0DO*MP)/LO) 1 *((Y(4)-Y(5))**2)) A(2.3)=(-((ROW1*DSIN(Y(2)))/LO)*((0.78DO*Y(1)*L1+0.57D0* I (Y( )**2))*Y(6)+(0.43DO*Y(1)*L+0.09DO*(Y( 1)**2))*Y(7))-(( 1 2.000*MP*DSIN(Y(2)))/LO)*ROW4*(Y(6)-Y(7))-((ROW1*DCOS(Y(2))) 1 /LO)*Y(1)*(Y(4)*Y(6)+Y(5)*Y(7))-((4.00*MP*DCOS(Y(2)))/LO)*( 1 Y(4)*Y(6)-Y(5)*Y(6)-Y(4)*Y(7)+Y(5)*Y(7) )*PART40 A(2.4)=((ROW1/LO)*(0.78DO*Y( 1)*L1+0.570*(Y( 1)**2))+((2.ODO* 1 MP*ROW4)/LO) )*PART40 A(2.5) ( (ROW1/LO)* (0.4300*Y( 1 )*L1+0.09DO* (Y( 1)**2) )-( (2. DO 1 *MP*ROW4)/LO))*PART40 A(2 6)=0.000 A(2.7)=.0ODO B(2, 1)-(-(ROW1/LO)*((1.56DO*LI+0.71D vY(1))*Y( 11)+(0.86DO*L1-0.5DO*Y(1))*Y(12)-(0.43DO*Y(4)+ 1 0.68D*Y(5))*Y(8) ) )*PART40 B(2.2)-(-( (8.000*MP)/LO)*(Y(4)*Y( 11)-Y(4)*Y( 12)+Y(5)*Y( 12)1 Y(5)*Y( 11))-((2.0DO*ROW1)/LO)*(0.50*Y(8)*((Y(4)**2)+( 1 Y(5)**2))+Y(1)*(Y(4)*Y(11)+Y(5)*Y(12)))-((2.0DO*Y(8)*BCC)/LO) 1 )*(MU+DTAN(KSY1)) PART50=(-((Y( 10)*DSIN(Y(2))*DCOS(Y(2)))/LO)*DC+((2.ODO*ROW1* 1 oSIN(Y(2)))/LO)*((0.78DO*Y(1)*Ll+0.57DO*(Y(1)**2))*Y(13)+( 1 0.4300*Y(1)*L1+0.0900*(Y(1)**2))Y(14)+(0.78DO*Y(8)*L1+0.35DO 1 *Y(1)*Y(8))*Y(6)+(0.43DO*Y(8)*L1-0.25DO*Y(1)*Y(8))*Y(7))+(( I 4.0O0*MP*DSIN(Y(2))*ROW4)/LO)*(Y(13)-Y(14))+((ROW1*Y(10)* 1 ((DSIN(Y(2)))**2))/LO)*((0.78D0*Y( 1)*L+0.57DO*(Y( 1)**2))*Y(4) 1 +(0.4300*Y(1)*L1+0.09DO*(Y(1)**2))*Y(5))+((2.ODO*MP*Y(10)*(( DSIN(Y(2) ))**2))/LO)*ROW4*(Y(4)-Y(5)))*PART40 PART51( -((ROWI*Y( 10)*( (DCOS(Y(2)) )**2) )/LO)*( (0.78DO*L1*Y( 1) 1 +0.5700*(Y(1)**2))*Y(4)+(0.43DO*Y(1)*L1+0.09DO*(Y(1)**2))* 1 Y(5))-( (2.ODOMP*ROW4*Y( 10)*((DCOS(Y(2)))**2))/LO)*(Y(4)-Y(5)) 1 +((2.0DO*ROW1*DCOS(Y(2)))/LO)*(Y( 1)*(Y(4)*Y(13)+Y(5)*Y(14))+ 1 Y(8)*(0.5DO*Y(4)*Y(6)+. 5DO*Y(5)*Y(7)+0.65DO*Y(5)*Y(6)-0.65D0 I *Y(4)*Y(7)))+((8.000*MP*DCOS(Y(2)))/LO)*(Y(4)*Y(13)-Y(4)*Y(14) 1 -Y(5)*Y(13)+Y(5)*Y(14))+((M2P*Y(10)*DSIN(Y(2))*DCOS(Y(2)))/LO)

104 I *((Y(4)**2)+(Y(5)o2))+((4.ODO*MPeY(10)*DSIN(Y(2))*DCOS(Y(2))) I /LO)*((Y(4)-Y(5))*2) )*PART40 8(2.3)=-PART50-PART5 i B(2 4)=0.000 8(2.5)60.000 B(2,6)=0.000 B(2,7)=0.000 F(2)- (2.OOO*PART41 C2 )/DM 0(2)-(((ACC3*GODCOS(Y(2)))/LO)+MH1*G)*PART40 GO TO 721 705 Y(9) 0.000 A(2 1)=0.000 A(2,2) -.000 A(2,3)=0.000 A(2.4)=0.000 A(2.5)w0.000 A(2o6)O0.000 A(2 o7 )=0.00 8(2,1)u0.0o0 B(2,2)0O.OO B(2.3)0.0D0 B(2.4)0.000 B(2,5)=0.000 8(2,6)=0.000 B(2,7)=0.000 F(2)=0.000 o ( 2 ) =o. oo 0(2)=O0.000 GO TO 721 C C C LOGIC FOR THE THETA MOTION CARRIED ON THE UPPER THREAD. C 800 IF(TTI.GT.0.5DO)GO TO 801 IF(ATT2)802,803,804 802 C2-KS21N*Y( 1 )-KS22N*(Y(2)-CONST2)-KS25N*Y(9)+IK22N*Y(16) GO TO 801 803 C20o.000 GO TO 805 804 C2z-KS21P*Y(1)-KS22P*(Y(2)-CONST2)-KS25P*Y(9)+IK22P*Y(16) 801 IF(TT4.GT.0.5DO)GO TO 806 IF(SGN(Y(9) ))807,808,809 807 C2=-KS21N*Y(1)-KS22N*(Y(2)-CONST2)-KS25N*Y(9)+IK22N*Y(16) GO TO 810 809 C2u-KS21P*Y( 1)-KS22P*(Y(2)-CONST2)-KS25P*Y(9)+IK22P*Y(16) GO TO 810 808 IF(TT1.LT.O.5DO)GO TO 811 ATT2sR2-Y(2) IF(ATT2)812.813,814 812 C2 —KS21N*Y( 1)KS22N*(Y(2)-CONST2)-KS25N*Y(9)+IK22NY( 16) GO TO 811 813 C2=0.000 GO TO 805 814 C2=-KS21P*Y(1)-KS22P*(Y(2)-CONST2)-KS25P*Y(9)+IK22P*Y(16) 811 IF(C2)815,805,817 815 ACC 12 1.000+MUS* (DTAN(KSY 1 ) ) ACC 13DTAN(KSY 1 )-MUS PART60u-(MH1*G-((ACC3*G*DCOS(Y(2)))/LO)-(((Y(10)**2)* 1 DSIN(Y(2))*DCOS(Y(2)))/LO)*DC-(ROW1/LO)*(0.78DO*YDOT(8)* I LI1Y(4)+1.56DO*L1*Y(8)*Y(11)+0.43DO*YDOT(8) 1 *LI*Y(5)+0.86DO*Y(8)*Y(12)*LI+0.78DO*Y( 1)*YDOT( 11)*L1+

195 1 0.4300*Y(1)*L1*YDOT(12)-0.43DO*((Y(8)**2)*Y(4)+Y(1)*YDOT(8)* I Y(4)+Y( )*Y(8)*Y( ))-0.68DO*((Y(8)**2)*Y(5)+Y(1)*YDOT(8)* i Y(5)+Y(1)*Y(8)*Y(12))+0.57DO*(2.ODO*Y(1)*Y(8)*Y(11)+(Y(1)**2) 1 *YDOT( 1))+0.09DO*(2.000*Y(1)*Y()*Y(12)+(Y( )**2)*YDOT(12)))) PART61 -(-((2.000*MP)/LO)*(ROW4*(YDOT( 11)-YDOT(12))-YDOT(8)* 1 (Y(4)-Y(5)))+((2.000*Y( 10)DSIN(Y(2))*ROW1)/LO)*((O.78DO* I Y(I)*Li0.57DO*(Y(1)**2))*Y(13)+(0.43DO*Y(1)*L1+0.09DO*( I Y(1)**2))*Y(14)+(0.78DO*Y(8)*LI+0.35DO*Y(1)*Y(8))*Y(6)+ I (0.4300*L1*Y(8)-0.25DO*Y(1)*Y(8))*Y(7))) PART62-( ( (4.0DO*Y( 10)*DSIN(Y(2))*MP*ROW4)/LO)*(Y( 13)-Y( 14)) I +((ROWI*YDOT(10)*DSIN(Y(2)))/LO)*((0.7800*Y(1)*LI+0.57DO* I (Y(1)**2))*Y(6)+(0.43DO*Y(1)*L1+O.09DO*(Y(1)**2))*Y(7)) +*((2.000*MP*YDOT( 10)*DSIN(Y(2)))/LO)*ROW4*(Y(6)-Y(7))+( I ROWI/LO)*((Y(O1)**2)*((DSIN(Y(2)))**2))* I ((0.78DO*Y(1)*LI+0.57DO*(Y(1)**2))*Y(4)+(0.43DO*Y(1)*L1 +0.0900*(Y( 1)**2))*Y(5))) PART63-( ((2.0DO*MP)/LO)*((Y( 10)**2)*((DSIN(Y(2)))**2)) i *(Y(4)-Y(5))*ROW4-(ROW1/LO)*((Y(10)**2)*(( I OCOS(Y(2))))*))*((0.78DO*Y( 1)*LI+0.57DO* (Y( 1)**2))*Y(4)+(0.430*Y( 1)*L1+0.09DO*(Y(1)**2))*Y(5)) 1 -((2.000*MP*ROW4)/LO)*((Y(10)**2)*((OCOS(Y(2)))**2)) I *(Y(4)-Y(5))+((2.OO*Y( 10)*ROW1*DCOS(Y(2)))/LO)*( 1 Y(1)*(Y(4)*Y(13)+Y(5)*Y(14))+Y(8)*(0.5DO*Y(4)*Y(6)+0.5DO*Y(5) I *Y(7)+0.65DO*Y(5)*Y(6)-0.65D0*Y(4)*Y(7)))) PART64-( ((8.OO*MP*Y( 10)*DCOS(Y(2)))/LO)*(Y(4)*Y(13)-Y(4)* 1 v(14)-Y(5)*Y( 13)Y(5)*Y(14)) 1 +((YOOT(10)*DCOS(Y(2))*ROW1)/LO)*Y(1)* I (Y(4)*Y(6)Y(5)*Y(7) )) PART65s-( ((4.000*YDOT( 10)*DCOS(Y(2) )*MP)/LO)*(Y(4)*Y(6)-Y(5)* 1 Y(6)-Y(4)*Y(7)+Y(5)*Y(7))+(M2P/LO)*(Y(10)**2)* I DSIN(Y(2))*DCOS(Y(2))*( (Y(4)**2)+(Y(5)**2))+((4.ODO*MP)/LO) 1 *(Y(10)**2)*DSIN(Y(2))*DCOS(Y(2))*((Y(4)**2)+ I (Y(5)**2)-2.0D0*Y(4)*Y(5))) TMIN2(PART60+PAR T 6O+ PA R T62+PART62 T63+PAR+PART365 )*(DM/2.ODO)* I (ACC13/ACC12) IF(C2.LT.TMIN2)GO TO 810 GO TO 805 817 ACC12= 1.0-MUS*(OTAN(KSY ) ) ACC13=DTAN(KSY 1 )+MUS PART60=-(MH1*G-((ACC3*G*DCOS(Y(2) )/LO)-(((Y( 10)**2)* 1 DSIN(Y(2))*DCOS(Y(2)))/LO)*DC-(ROW1/LO)*(0.7800*YDOT(8)* I L1*Y(4)+1.5600*LI*8)*8)*Y(11)+0.43DO*YDOT(8) i *L1*Y(5)+0.86DO*Y(8)*Y(12)*LI+0.7800*Y(1)*YDOT(11)*L1+ 1 0.43DO*Y(1)*L1*YDOT(12)-0.43DO*((Y(8)**2)*Y(4)+Y(1)*YDOT(8)* 1 Y(4)+Y(1)*Y(8)*Y( 11)-0.68DO*((Y(8)**2)*Y(5)+Y(1)*YDOT(8)* 1 Y(5)+Y(1)*Y(8)*Y(12))+0.57DO*(2.000*Y(1)*Y(8)*Y(11)+(Y(1)**2) 1 *YDOT(11)))+0.0900*(2.0DO*Y(1)*Y(8)*Y(12)+(Y( )**2)*YDOT(12)))) PART61=- ( -( (2.000*MP)/LO)*(ROW4*(YDOT( 11)-YDOT( 12))-YDOT(8)* 1 (Y(4)-Y(5)))+((2.0O0*Y(10)*DSIN(Y(2))*ROW1)/LO)*((0.78D0* 1 Y(1)*L1+0.5700*(Y(1)**2))*Y(13)+(0.43DO*Y(1)*L1+0.09DO*( 1 Y(1)**2))*Y(14)+(0.78DO*Y(8)*Li+0.35DO*Y(1)*Y(8))*Y(6)+ I (0.43DO*L1*Y(8)-0.25D0*Y(1)*Y(8))*Y(7))) PART62=-(((4.0DO*Y(10)*DSIN(Y(2))*MP*ROW4)/LO)*(Y(13)-Y(14)) 1 +((ROWI*YDOT(IO)*DSIN(Y(2)))/LO)*((0.78DO*Y(1)*LI+0.57DO* 1 (Y(1)**2))*Y(6)+(0.43DO*Y(1)*L+O0.0900*(Y(1)**2))*Y(7)) 1 +((2.000*MP*YDOT( O)*DSIN(Y(2)))/LO)*ROW4*(Y(6)-Y(7))+( I ROW1/LO)*((Y( 0)**2)*((DSIN(Y(2)))**2))* 1 ((0.78DO*Y(1)*L+0.57DO*(Y( 1)**2))*Y(4)+(0.43DO*Y( 1)*L1 1 +0.09D00(Y( 1)**2))*Y(5))) PART63r-(((2.0O*MP)/LO)*((Y(10)**2)*((DSIN(Y(2))))**2))

196 I *(Y(4)-Y(5))*ROW4-(ROWI/LO)*(Y( 10)*2)*(( I DCOS(Y(2)))**2))*((0.78DO*Y(1)*L1+0.57DO* 1 (Y(1)**2))*Y(4)+(0.43DO*Y(1)*Li+0.09DO(Y(1)**2))*Y(5)) I -((2.ODO*MP*ROW4)/LO)*((Y(10)**2)*((DCOS(Y(2)))**2)) I *(Y(4)-Y(5))+((2.000*Y(10)*ROWI*DCOS(Y(2)))/LO)*( i Yv()*(Y(4)*Y(13)+Y(5)*Y(14))+Y(8)*(0.5DO*Y(4)*Y(6)+0.5DO*Y(5) I *Y(7)+o065D0*Y(5)*Y(6)-0.65DO*Y(4)*Y(7)))) PART64-(( (8.0O*MPeY( 10)*DCOS(Y(2)))/LO)*(Y(4)Y( 13)-Y(4)* I Y(14)-Y(5)*Y(13)+Y(5)*Y(14)) 1 +((YDOT(10)*OCOS(Y(2))*ROW1)/LO)*Y(1)* I (Y(4)*Y(6)+Y(5)*Y(7))) PART65 —( ( (4.000*YDOT( 1)*DCOS(Y(2))*MP)/LO)*(Y(4)*Y(6)-Y(5)* I Y(6)-Y(4)*Y(7)+Y(5)*Y(7))+(M2P/LO)*(Y(10)**2)* 1 OSIN(Y(2))*OCOS(Y(2))( (Y(4)**2)+(Y(5)**2))+((4.0DO*MP)/LO) I *(Y(O)**2)*DSIN(Y(2))*DCOS(Y(2))*((Y(4)**2)+ I (Y(5)**2)-2.000*Y(4)*Y(5))) TMIN2=(PART60+PART61+PART62+PART63+PART64+PART65)*(DM/2 ODO)* I (ACC13/ACC12) IF(C2.GT.TMIN2)GO TO 810 GO TO 805 810 IF(TA2.LT.O.5DO)GO TO 818 IF(TOMAX.GE.OABS(Y(9)))GO TO 819 818 TOMAX=DABS(Y(9)) 819 TA2TA 2+T1.ODO TDPER=TOMAX*O. 0000100 ERR2DOABS(R2-Y(2)) IF((DABS(Y(9)).GT.TDPER)oOR.(ERR2.GT.ER2PER))GO TO 820 806 Y(2)8R2 C2 0.000 Y(16)O0.000 TT4~1 o ODO GO TO 805 820 PART40DTAN(KSY )+MU*SGN( Y(9)) PART4I1.000-MU*DTAN(KSY )*SGN(Y(9)) A(2.1 )=( (ROW1/LO)*((0.78DO*L1-0.43DO*Y(1))*Y(4)+(0.43DO*L1 I -0.68DO*Y(1))*(5)) )-((2.000*MP)/LO)*(Y(4)-Y(5)))*PART40 A( 22)=ACC9*LO+ACC7*LO*( 1ODO-MU*DTAN(KSY1))+(DTAN(KSY1)+MU) i *((DC/LO)+(M2P/LO)*((Y(4)**2)+(Y(5)**2))+((40ODO*MP)/LO) I *((Y(4)-Y(5))**2)) A(2.3)-(-((ROWI*DSIN(Y(2)))/LO)*((0.78DO*Y( 1)*L1+0.57DO I (Y(1)**2))*Y(6)+(0.43DO*Y()1*L1+0.09DO*(Y(1)**2))*Y(7))-(( I 2.0O0*MP*OSIN(Y(2)))/LO)*ROW4*(Y(6)-Y(7))-((ROW1*OCOS(Y(2))) I /LO)*Y( 1)*(Y(4)*Y(6)+Y(5)*Y(7) )-( (4.O0*MP*DCOS(Y(2) ) )/LO)*( Y(4)*Y(6)-Y(5)*Y(6)-Y(4)*Y(7)+Y(5)*Y(7)) )*PART40 A( 2.4)((ROW/LO)*(0. 78DO*Y( )*L 1+0.57DO*(Y( )**2))+((2.00D* I MP*ROW4)/LO))*PART40 A( 2 5)((ROW1/LO)*(043DO*Y( 1)*L1+0.09D0*(Y(1)**2))-((2.ODO 1 *MP*ROW4)/LO))*PART40 A(2.6)0.000 A(2,7)=O,000 B(2,1)=-(-(ROW1/LO)*((1.5600*L1+0.71DO I *Y(1))*Y(11)+(0.86DO*L1-0.5DO*Y(1))*Y(12)-(0.43DO*Y(4)+ 1 0.680D*Y(5))*Y(8)))*PART40 8(2,.2 )-(-((8.000*MP)/LO)*(Y(4)*Y( 11)-Y(4)*Y( 12)+Y(5)*Y(12)1 Y(5)*Y(11))-((2.000*ROW1)/LO)*(0. 50*Y(8)*((Y(4)**2)+( 1 Y(5)**2))+Y(1)*(Y(4)*Y(11)Y( (5)*Y(12)))-((2.0DO*Y(8)*BCC)/LO) 1 )*(MU+DTAN(KSY1)) PART50=(-((Y(10)*DSIN(Y(2))*DCOS(Y(2))*)/LO)*DC+((2.ODO*ROW1* I DSIN(Y(2)))/LO)*((0.78DO*Y(1)*LI+0o57DO*(Y(1)**2))*Y(13)+( 0.43DO*Y( )*L1+0.09DO*(Y( 1)**2))*Y( 14)+(0.78DO*Y(8)*L1+0.35D0

197 1 *Y(1 )*(8))*Y(6)+(0.43DO*Y(8)*L1-0.25DO*Y( )*Y(8) )Y(7))+(( 1 4.00*MP*DSIN(Y(2))*ROW4)/LO)*(Y(13)-Y(14))+((ROW1*Y(10)* 1 ((DSIN(Y(2)))**2))/LO)*((O.78DO*Y(I)*L+0.57DO*(Y(1)**2))*Y(4) +(0.43DO*Y(1)*Ll+0.09DO*(Y(1)**2))*Y(5))+((2.000*MP*Y(10)*(( I DSIN(Y(2) ) )*2))/LO)*ROW4*(Y(4)-Y(5)) )*PART40 PART51 ( -((ROW1*Y( 10)*((DCOS(Y(2 ) )**2))/LO)*(((O.78DO*L1Y( ) +0.57DO*(Y(1)**2))*Y(4)+(0.43DO*Y(1)*L1+0.09DO*(Y(1)**2))* 1 Y(5))-((2.000*MP*ROW4*Y( 10)*((DCOS(Y(2)))**2))/LO)*(Y(4)-Y(5)) i +((2.0OO*ROW1*DCOS(Y(2) ) )/LO)*(Y( 1)*(Y(4)*Y(13)+Y(5)*Y( 14))+ Y( ) (O). 50 Y (4 ) +* Y (6)+0 5D0 Y (5) *Y(7)+0.6500 Y(5)(6) (6-0.6500 *Y(4)*Y(7)))+( (800O*MP*DCOS(Y(2 )))/LO)*(Y(4)Y( 13)-Y(4)*Y( 14) 1 -Y(5)*Y(13)+Y(5)*Y( 14))+( (M2P*Y( 10)*DSIN(Y(2) )*DCOS(Y(2)) )/LO) 1 *((Y(4)**2)+(Y(5)* *2)+( (4.000*MP*Y( 10)*DSIN(Y(2))*DCOS(Y(2))) i /LO)*((Y(4)-Y(5))**2))*PART40 B(2.3), -PART50-PART51 8(2 4)=0.000 8(2,5) 0.000 B(2.6)-0.000 6(2.7 )0.000 F(2 )(2.000*PART4 1*C2)/DM D(2 )(-((ACC3*G*DCOS(Y(2)) )/LO)+MH1*G)*PART40 GO TO 721 805 Y(9)=0.000 A-(2 1) =0.000 A(2.2)=1.000 A(2.3)0.000 A(2,4 )0.000 A( 2.5) 0.000 A(2,6)~0.000 A(2,7)0.000 B(2, ) 0.000 B(2,2)=0.000 B(2.3)z0.000 B(2.4)=0.000 B ( 2, 5) 0. 000 B(2,5)=O.ODO B(2.6)~0.000 B(2,7)=0.000 F(2) =0.000 0(2)0.000 721 C3=-KS33*(Y(3)-CONST3)-KS36*Y(10)+IK33*Y(17) A(3. 1)=ROW1*(Y( 1)*DCOS(Y(2))*(0.78DO*Y(6)+ 1 0.4300Y(7))-1.300*Y( 1)*DSIN(Y(2))*(Y(4)*Y(7)-Y(5)*Y(6))1 L1*DCOS(Y(2))*(0.7800*Y(6)+0.43DO*Y(7))1 Y(1)*DCOS(Y(2))*(0.35DO*Y(6)-0.25D0*Y(7)))+MP*2.0DO* 1 DCOS(Y(2))*(Y(6)-Y(7)) A(3,2)=ROW1*(-Y()*DCOS(Y()2))*(Y(4)*Y(6)+Y(5)*Y(7)) 1 -L1*Y(1)*DSIN(Y(2))*(0.78DO*Y(6)+0.4300*Y(7))1 (Y(1)**2)*DSIN(Y(2))*(0.57DO*Y(6)+0.09DO*Y(7)))+MP* 1 (-4000*ODCOS(Y(2))*(Y(4)*Y(6)-Y(4)*Y(7)-Y(5)*Y(6)+ 1 Y(5)*Y(7))-2.000*(L1+Y(1))*DSIN(Y(2))*(Y(6)-Y(7))) A(3.3)=IL2+MH1*(LO**2)+(INERTI+MHI*((L1+Y(1)-L2)**2)+1 ((M1*LT**2)/12.000)+((((LT/2.ODO)-LO)**2)*M1))* 1 ((DCOS(Y(2)))**2)+ROWI*ROW3*(ROW2**2)*((DCOS(Y(2)))**2)+ 1 (ROW1*(ROW3**3)*((DCOS(Y(2)))**2))/12.ODO+ROW1*(Y(1)* 1 ((DCOS(Y(2)))**2)*(Y(6)**2+Y(7)**2)+Y(1)*((DSIN(Y(2)))**2) 1 *(Y(6)**2+Y(7)**2)+Y(1)*((DSIN(Y(2)))**2)*(Y(4)**2+Y(5)**2) 1 -2.ODO*L1*DSIN(Y(2))*DCOS(Y(2))*Y(1)*(0.78DO*Y(4)+0.4300* 1 Y(5))-2.00O*DSIN(Y(2))*DCOS(Y(2))*(Y(1)**2)*(0.57DO* 1 Y(4)+0.0900*Y(5))+ROW5*((DCOS(Y(2)))**2))+MP*(4.000* 1 ((OCOS(Y(2)))**2)*(Y(6)**2+Y(7)**2-2.000*Y(6)*Y(7))+

108 1 4.000O (OSINY(2( ) )2)')**)*(Y(6)**2+Y(7)**2-2.0DO*Y(6) I Y(7)+Y(4)**2+Y(5)**2-2.0DO*Y(4)*Y(5))-4.000*DSIN(Y(2))* 1 OCOS(Y(2))*ROW4*(Y(4)-Y(5))+(ROW4**2)*((DCOS(Y(2)))**2)) A(3,4)- -ROWIt* )*D(Y )*D N(Y(2))*Y(6)-MP*4ODO*DSIN(Y(2))* 1 (Y(6)-Y(7)) A(3 5) -ROW Y ( 1 )*DSIN(Y(2))*Y(7)-MP*4.000*OSIN(Y(2 ) )*(Y(7) i -Y(6)) A(3.6)hROW1*(Y( 1 )*DSIN(Y(2))*Y(4) L *DCOS(Y(2)) *0.78DO* 1 Y( 1)-Y( 1)**2*DCOS(Y(2))*O.57DO)+MP*(4.ODO*DSIN(Y(2))*(Y(4) 1 -Y(5) )-2. 00*ROW4*DCOS(Y(2))) A(3.7)sROW1*(Y( )*DSIN(Y(2))*Y(5)-L1* I DCOS(Y(2) )*0.43DO*Y( 1 )-(Y( 1 )**2)*DCOS(Y(2)) 1 *0.0900)+MP*(4.000*DSIN(Y(2))*(Y(5)-Y(4))+2.0DO 1 *ROW4*DCOS(Y(2))) A(4i, )=ROWI*(Y( )/2.0DO)*(Y(4)-1 3DO*Y(5)) A(4.2)=ROW6*(1.56DO*L*Y( 1)+t. 14D*(Y( 1)**2)) I +MP*(2.000*ROW4) A(4,3) -ROWI*DSIN(Y(2) )*Y( 1 )*Y(6)-MP*4 000 I *OSIN(Y(2))*(Y(6)-Y(7)) A(4,4)~ROW1*Y( )+4.000*MP A(4,5)'-4.000*MP A(4.6) 0. 00 A(4 7)=0000 A(5,1)=ROWI*Y( )*(0.65DO*Y(4)+0.5DO*Y(5)) A(5,2)=ROWI*(0.43DO*L*Y( 1 )+0.090D*(Y( 1)**2)). i -MP*2 000*ROW4 A(5,3) —ROW1*DSIN(Y(2))*Y( i )*Y(7)-4.0DO*MP* i OSIN(Y(2))*(Y(7)-Y(6)) A(5.4)-4 000*MP A(5.5)=ROI*Y( 1 )+4.000*MP A(5.6)=0.000 A(5,7)=0.000 A(6 1),ROW1*Y( 1)*(0.5DO*Y(6)-0.65DO*Y(7)) A(6,2)-0.000 A(6,3)ROW*(Y( )*DSIN(Y(2) )*Y(4)-0.78DO*Y( I ) 1 DCOS(Y(2))*L1-0.57DO*(Y( )**2)*DCOS(Y(2)))+MP*(4oODO* I DSIN(Y(2))*(Y(4)-Y(5))-2.0DO*ROW4*DCOS(Y(2))) A(6.4)=O.ODO A(6,5)=0.000 A(6,6)=ROWI*Y( 1 )+4.000*MP A(6,7 ) -MP4.000 A(7o 1)=ROWI*Y( )*(0.65D0*Y(6)+0.5DO*Y(7)) A(7 2)0.000 A(7.3)=ROWI*(Y( )*DSIN(Y(2))*Y(5)-Oo43DO*L1*Y( 1 ) 1 DCOS(Y(2))-0.09DO*(Y( )**2)*DCOS(Y(2)))+MP*(4.0DO*DSIN(Y(2)) I *(Y(5)-Y(4))+2.0DO*ROW4*OCOS(Y(2))) A(774)0.ODO A(7,5)~0.000 A(7,6)a-4.O*OOMP A(7.7)=ROW*Y( 1 )+4.000*MP C C C *** COMPUTATION OF THE CORIOLIS, CENTRIPETAL, *** C *** GRAVITY AND CONTROL TERMS. * C C PART 1-ROWI*Y( 10)*(ROW2**2)* ((DCOS(Y(2)))**2) I +ROW1*ROW3*Y(1O)*ROW2*((DCOS(Y(2)))**2)-((ROW1* 1 (ROW3**2))/4.000)*Y( 0)*((DCOS(Y(2)))**2)+ROW1* I (Y(10)*((DCOS(Y(2)))**2)*(Y(6)**2

100 It Y(7)**2)+(Y(8)*DCOS(Y(2))-Y(I)*Y(9)*DSIN(Y(2))) I *(O.78DO*Y(6)+0.43DO*Y(7))+Y(1)*DCOS(Y(2))* I (0.78DO*Y(13)+0.43DO*Y(14))-Y(9)*DCOS(Y(2))*(Y(4)*Y(6)+ I Y(5)*Y(7))-DSIN(Y(2))*Y(1)*(0.50DO*(Y(11)*Y(6)+ I Y(4)*Y(13))+O.-65DO*(Y(11)*Y(7)+Y(4)*Y(14)) 1 -0.65DO*(Y(12)*Y(6)+Y(5)*Y(13))+0.5DO*(Y(12)*Y(7)+ I Y(5)*Y(14)))) PART2=ROWI*(-DSIN(Y(2))*(Y( 11)*Y(6)+Y(12)*Y(7)) I -Lt*Y(9)*DSIN(Y(2))*(0.78D0*Y(6)+0.43DO*Y(7)) I -2.000*Y(1)*Y(9)*DSIN(Y(2))*(0.57D0*Y(6)+0.09DO*Y(7)) 1 +Y(10)*((DSIN(Y(2)))**2)*(Y(6)**2+Y(7)**2)+ I (Y(8)*DSIN(Y(2))+Y( )*Y(9)*DCOS(Y(2)))* 1 (1.300*Y(6)*Y(5)-1.3D*Y(7)*Y(4))) PART3ROWi*(DSIN(Y(2))*Y( 1)*(0.5DO*(Y( 13)*Y(4)+ I Y(6)*Y(11))+0.65DO*(Y(13)*Y(5)+Y(6)*Y(12))1 0.6500*(Y(14)*Y(4)+Y(7)*Y(11))+O.5DO*(Y(14)*Y(5) I +Y(7)*Y(12)))+DSIN(Y(2))*(Y(13)*Y(4)+ 1 Y(14)*Y(5))+L*.Y(9)*DSIN(Y(2))*(0.78DO*Y(6)+ i 0.43DO*Y(7))-L1*DCOS(Y(2))*(1.56DO*Y(13)+ I 0.8600Y( 14) ) ) PART4ROW*( -(Y(8)*DCOS(Y(2))-Y( 1 )*Y(9)*DSIN(Y(2) ) )*(0.35DO* I Y(6)-0.25DO*Y(7))-DCOS(Y(2))*Y(1)*(0.35DO* I Y(13)-0.25DO*Y(14))-2.0DO*Y(1)*DCOS(Y(2))* i (0.57DO*Y( 3)+0.09DO*Y(14))+Y(10)*((DSIN(Y(2))) I **2)*(Y(4)**2+Y(5)**2)-2.ODO*L1*Y(10)*DSIN(Y(2)) 1 *OCOS(Y(2))*(0.78DO*Y(4)+0.43DO*Y(5))-4.0DO*Y( )*Y(10) 1 *DSIN(Y(2))*DCOS(Y(2))*(0.57DO*Y(4)+0.0900*Y(5))) PART5SROW1*(Y(10)*( (DCOS(Y(2)))**2)*( (L1**2)+ 2.000L1*Y()( 1)+(Y(1)**2)))+MP*(-4.0DO*Y(9)*DSIN(Y(2))* I (Y(6)-Y(7))-4.0DO*Y(10)*DCOS(Y(2))DSN(Y(2))* I (Y(4)-Y(5))+2.0O0*ROW4*Y( 10)*((DCOS(Y(2)))**2)) B(3.1 )=PRTI ARTPART2+PART3+PART4+PART5+2.ODO*Y(10) I *((DCOS(Y(2)))**2)*MH1*(L1+Y(1)-L2) PART6- 2.000 ( ((M1*(LT**2) )/12.0DO)+M1*( ( (LT/2.DO) I -LO)**2))*Y(10)*DCOS(Y(2))* 1 DSIN(Y(2))-2.0DO*ROW1*ROW3*Y( 10)*(ROW2**2)* i DSIN(Y(2))*DCOS(Y(2))-(ROW1*(ROW3**3)*Y(10)*DSIN(Y(2)) I *DCOS(Y(2)))/6.0DO+ROW1*( -2.000*( 10 )*Y( 1)*DCOS(Y(2))* i DSIN(Y(2))*(Y(6)**2+Y(7)**2)+Y(9)*Y( 1)*DSIN(Y(2))* 1 (Y(4)*Y(6)+Y(5)*Y(7.))-Y(1)*DCOS(Y(2))*(Y(11)*Y(6)+ 1 Y(4)*Y(13)+Y(12)*Y(7)+Y(5)*Y(14))-Y(1)*DCOS(Y(2))* 1 (Y( 11)*Y(6)+Y(12)*Y(7))) PART7=ROW1*(-L1'Y(1 )*Y(9)*DCOS(Y(2))* i (0.7800*Y(6)+0.43D0*Y(7))-L1*Y( 1)*DSIN(Y(2))* 1 (0.78DO*Y(13)+0.43D*Y( 14))-(Y( 1)**2)*Y(9)*OCOS(Y(2))* 1 (0.57DO*Y(6)+0.09DO*Y(7))-DSIN(Y(2))*(Y(1)**2)* I (0.57D0*Y(13)+0.0900*Y(14))+2.000*Y(10)*Y(1)* 1 DSIN(Y(2))*DCOS(Y(2))*(Y(6)**2+Y(7)**2) 1 +Y(1)*DCOS(Y(2))*(Y(13)*Y(4)+Y(14)*Y(5))) PART8=ROW1*(LI*DSIN(Y(2))*(0.78DO*Y( 1)*Y(13)+ 1 0.43D0*Y(1)*Y(14))+(Y(1)**2)*DSIN(Y(2))*(0.57DO*Y(13)+ 1 0.0900*Y(14))+2.0DO*DSIN(Y(2))*DCOS(Y(2))*Y(10)* 1 Y(1)*(Y(4)**2+Y(5)**2) 1 -2.000*L1*(Y(10)*((DCOS(Y(2) ) )**2)*Y(1) 1 -Y(10)*((DSIN(Y(2)))**2)*Y(1))*(0.7800* 1 Y(4)+0.4300*Y(5))-2.000*Y(10)*(Y(1)**2)* 1 (((DCOS(Y(2)) )**2)-((OSIN(Y(2)))**2))*(0.57DO*Y(4) 1 +0.09DO*Y(5))) PART9=ROW 1*( -2.000*Y( 10)*DSIN(Y(2))*DCOS(Y(2)) I *ROW5)+MP*(Y( 10)*DCOS(Y(2))*DSIN(Y(2))*(8.DO*

200 1 (Y(4)e2+Y(5)**2-2.0DO*Y(4)*Y(5))-2.0DO*(ROW4**2)) +4.000*Y(9)*DSIN(Y(2) )*(Y(4)*Y(6)-Y(4)*Y(7)Y(5)*Y(6)+Y(5)*Y(7))-4.ODO*DCOS(Y(2))*(2.000*Y(11)* Y(6)+Y(4)*Y(13)-2.00*Y(11)*Y(7)-Y(4)*Y(14) i 2.000Y( 12)*Y(6)-Y(5)*Y(13)+2.OO0*Y(12)*Y(7)+ Y(5)*Y(14))-2.000*Y(9)*ROW4*DCOS(Y(2))*(Y(6)-Y(7))+ 4.000*DCOS(Y(2))*(Y( 13)*Y(4)-Y(13)*Y(5)-Y(14)*Y(4)+ I Y(14)*Y(5))-(Y(10)*((DCOS(Y(2)))**2)*ROW4-Y(10)* I ((DSIN(Y(2) ) )*2)*ROW4)*(Y(4)-Y(5))4.000) 8(3,2)uPART6+PART7+PART8+PART92.000*Y( 10)* I DCOS(Y(2))*DSIN(Y(2))*(INERTI+MHI*((L1+Y(1)-L2)**2)) B(3.3)ROW1*(Y(1 )2.00D*(Y(6)*Y( 13)+Y(7)*Y( 14))+ V Y(1)*((DSIN(Y(2)))**2)*2.ODO*(Y(4)*Y( 11)+Y(5)* I Y(12))-2.000*Y( 1)*DSIN(Y(2))*DCOS(Y(2))*(0.7800* I L1*Y(11)+0.43DO*L*Y(12)+0.57DO*Y(1)*Y(11)+0.09DO* I Y( )*Y(12)))+MP*(8.000*(Y(6)*Y(13)+Y(7)*Y(14)-Y(13) *Y(7)-Y(6)*Y(14))+8.000*((DSIN(Y(2)))**2)*(Y(4)*Y(11)+ I Y(5)*Y(12)-Y(11)*Y(5)-Y(4)*Y(12))-4.000D SIN(Y(2))* I COS(Y(2))*ROW4*(Y(11))-Y(12))) 8(3,4)=O.000 B(3.5)=0.000 8(3 o6) 0.000 B(3 7) 0.00D 8(4,1)=ROW6*(Y(8)*(Y(4)-1.3DO*Y(5))+Y(9)*Y(1)* I 3.14D002.00O*Y(10)*DSIN(Y(2))*Y(6)2.00DO*Y(8)* (A13*Y(4)+A14*Y(5))+2.6DO*Y(10)*DSIN(Y(2))* 1 Y(1)*Y(7))+MP*4.ODO*Y(9) B(4.2)=-ROW1*Y(9)*Y( 1)*Y(4)-4ODO*MP*Y(9)*(Y(4) I -Y(5)) B(4.3)=ROW6*(-2 000*Y(10)*((DSIN(Y(2)))**2)*Y(1) i eY(4)+Y(10)*DSIN(Y(2))*DCOS(Y(2))*Y(1)*(1.56DO* I L11o 1400*Y(1)))+MP*(-40ODO*Y(10)*((DSIN(Y(2)))**2) i *(Y(4)-Y(5))+2.0DO*Y(10)*DSIN(Y(2))*DCOS(Y(2)) 1 *ROW4) B(4,4)=ROW6*2.0D0*Y(8) B(4,5)=-ROW6*2.6DO*Y(1)*Y(8) B(4,6)=-ROW6*Y( 10)*4.000*DSIN(Y(2))*Y(1)-80ODO I *MP*Y(10)*DSIN(Y(2)) B(4,7 )-8. OO*MP*Y( 10)*DSIN(Y(2)) B(5,1)=ROWI*(Y(8)*(0.6500*Y(4)+0.5DO*Y(5)) 1 +0.86DO*Y(9)*Y()+Y(10)*DSIN(Y(2))*Y(7)1 Y(8)*('A14*Y(4)+AI5*Y(5))-1.300D*Y(10)* i DSIN(Y(2))*Y( )*Y(6))-MP*4.0DO*Y(9) B(5.2 )=ROW1*( Y(9)*Y( 1)*Y(5))-4. DO*MP 1 *Y(9)*(Y(5)-Y(4)) B(5 3) ROW1*(-Y( 1)*Y(10)*((DSIN(Y(2)))**2) 1 *Y(5)+Y( 10)*DSIN(Y(2))*DCOS(Y(2))*Y(1)* i (0.43D0*L +0.09DO*Y (1)))+MP* (-4.0DO* 1 Y(10)*((DSIN('Y(2)))**2)*(Y(5)-Y(4))-2.0DO* 1 Y(10)*OSIN(Y(2))*DCOS(Y(2))*ROW4) B(5,4)=ROW1*1.3DO*Y(1)*Y(8) B(5.5)=ROWI*Y(8) B(5.6) MP*8.000*Y ( 10)*DSIN(Y(2)) 8(5.7)=-ROW1*200DO*Y(10)*DSIN(Y(2))*Y(1) 1 -8o000*MP*Y(10)*DSIN(Y(2)) B(6 I1)*ROWI*(Y(8)*(0 5DO*Y(6)-0.65DO*Y(7)) 1 +Y(10)*DSIN(Y(2))*Y(4)+Y(10)*DCOS(Y(2)) 1 *(-1.57DO*Y(1))+Y(10)*DSIN(Y(2))*Y(1)* I (- 300*Y(5))-Y(8)*(A13*Y(6)+A14*Y(7))) i +MP*(-4.000*Y(10)*DCOS(Y(2)))

201 B(6.2) ROW*(2.000*Y( 1)*Y( 10)*DCOS(Y(2))*Y(4)+ 1 1.5600*Y(1)*L1*DSIN(Y(2))*Y(10)+1. 1400*(Y(1)**2) 1 *Y(10)*DSIN(Y(2)))+MP*(8.0DO*Y(10)*DCOS(Y(2)) I *(Y(4)-Y(5))+ROW4*4.00*Y(10)*DSIN(Y(2))) B(6.3) -ROW1*Y( 1)*Y( 10)*Y(6)-4.DO*MP*Y( 10) 1 *(Y(6)-Y(7)) B(6.4)=2.000*ROW1*Y(10)*DSIN(Y(2))*Y( 1 )+8.0DO I *MP*Y(10)*DSIN(Y(2)) 8(6.5)-8.000*MP*Y( 10)*DSIN(Y(2)) B(6.6)tROW1*Y(8) B(6.7)-ROW1*1.3DO*Y( 1)*Y(8) B(7.1)=ROWI*(Y(8)*(0.65DO*Y(6)+0.5DO*Y(7))+Y(10) 1 *DSIN(Y(2))*Y(5)-0.86DO*Y(1)*Y(10)*OCOS(Y(2)) I -Y(8)*(A14*Y(6)+A15*Y(7))+1.3DO*Y(10)*DSIN(Y(2)) 1 *Y(1)*Y(4)) 1 +MP*4.0*Y( 10)*DCOS( Y(2 ) ) B(7.2)ROW1*(2.000*Y( 1)*Y( 10)*DCOS(Y(2))*Y(5)+ 1 0.18DO*(Y(1)*2)*Y)**2)*Y(10)*DSIN(Y(2))+0.86DO*L*Y(1) 1 *Y(10)*DSIN(Y(2)))+MP*(8.ODO*Y(10)*DCOS(Y(2))* I (Y(5)-Y(4))-4.000*ROW4*Y(10)*DSIN(Y(2))) B(7.3)-ROW1Y( 1)*Y( 10)*Y(7)-4.0DO*MP*Y(10)* 1 (Y(7)-Y(6)) B(7.4 )-MP*8.000*Y( 10)*DSIN(Y(2)) B(7.5)ROWI*2.000*Y( 10)*DSIN(Y(2))*Y( 1 )+8.ODO I *MP*Y(10)*DSIN(Y(2)) B(7,.6)ROW* 1.3DO*Y(1 )*Y(8) B(7.7)=ROWI*Y(8) F(3 )C3 F(4 )0.000 F(5) 0.000 F(6) 0.ODO F(7)=0.000 0(3 ) =0.000 D(4)=-ROW1*Y( 1)*G*DCOS(Y(2))*AL1-2.0DO*MP*G I *DCOS(Y(2))-(EI*Y(4)*ALO)/(Y(1)**3) 1 -((ROW1*CENTAC*DIST*(4.65DO*Y(4)-7.38DO*Y(5)))/Y( 1))+ 1 ROW1*CENTAC*((3.0800*L1+1.13DO*Y(1))*Y(4)-(6.96DO*L1 I +3.000*Y(1))*Y(5))-(((ROW7*CENTAC+ROW8)*(4.65DO*Y(4)7.38DO*Y(5)) )/Y( 1))+ROW9*(3.08DO*Y(4)-6.96DO*Y(5)) D(5) -ROW1 Y( 1)*G*DCOS(Y(2))*AL2+2.0DO*MP I *G*DCOS(Y(2))-(EI*Y(5)*AL11)/(Y(1)**3) I -((ROW1*CENTAC*DIST*(-7.38DO*Y(4)+32.42D0*Y(5)))/Y(1))+ t ROW1*CENTAC*(-(6.96DO*L1+3.0D0*Y(1))*Y(4)+(23.7700*L1 I +9.73DO*Y(1))*Y(5))-(((ROW7*CENTAC+ROW8)*(-7.38DO*Y(4)^ 1 32.42DO*Y(5))/))/Y()+ROW9*(-6.96DO*Y(4)+23.77DO*Y(5)) D(6)=-(AL10*EI*Y(6))/(Y(1)**3)-((ROW1*CENTAC*DIST*(4.65 *(6) -7.38DO*Y(7)))/Y(1))+ROW1CENTAC*((3.08DO*L1+1.13DO*Y()) I *Y(6)-(6o96DO*L1+3.ODO*Y(1))*Y(7))-(((ROW7*CENTAC 1 +ROW8)*(4.65DO*Y(6)-7038DO*Y(7)))/Y(1))+ROW9*( 1 3.0800D*Y(6)-6.9600Y(7)) D(7)=-(AL11*EI*Y(7))/(Y(1)**3)-((ROW1*CENTAC*DIST* (-7.38 1 Y(6)+32.42DO*Y(7)))/Y(1))+ROW1*CENTAC*(-(6.96DO*L1 1 +3.000*Y(1))*Y(6)+(23.77DO*L1+9.73DO*Y(1))*Y(7)) 1 -(((ROW7*CENTAC+ROW8)*(-7.38DO*Y(6)+32.42DO*Y(7))) 1 /Y(1))+ROW9*(-6.96DO*Y(6)+23.77DO*Y(7)) YVECT( )=Y(8) YVECT(2)tY(9) YVECT(3)Y( 10) YVECT(4)Y( 11) YVECT(5)=Y(12)

202 YVECT(6)-Y( 13) YVECT(7)Y( 14) C C C *** COMBINING THE TERMS ON THE LEFT HAND SIDE TO *** C **" ONE SINGLE VECTOR. *** C C DO 60 IN-tM PBY( IN) 0.00 DO 60 IMI,.M PBY( BY(PBY( IN)+(-BIN.IM) )*YVECT(IM) 60 CONTINUE DO 61 IY1,.M PALL( IY)=O. D0 PALL( IY)PBY( IY)+F( IY)+D(IY) 61 CONT INUE C C C *** LU DECOMPOSITION OF THE (7,7) INERTIA MATRIX. ** C C CALL DLUD(M7,A,o7,TIV) IF(IV(M).EO.O) GO TO 90 GO TO 91 90 S=0.000 GO TO 14 91 Szl.000 C C ** SOLVING FOR THE ACCELERATION TERMS. *8 C CALL DBS(Mo7,TIVPALL) C C *** THE EQUATIONS OF MOTION IN FIRST ORDER FORM. ^ C YDOT( 1 )Y(8) YDOT(2)~Y(9) YDOT(3)Y( 10) YDOT(4)Y( 11) YDOT(5)Y( 12) YDOT(6)sY( 13) YDOT(7)=Y(14) YDOT(8)-PALL( 1) YDOT(9)=PALL(2) YDOT( 10)PALL(3) YDOT( 1)=PALL(4) YDOT( 12)PALL(5) YDOT( 13 )PALL(6) YDOT( 14)~PALL(7) YDOT( 15)=R1-Y( 1) YDOT( 16)R2-Y(2) YDOT( 17)~R3-Y(3) TTTTTT1+1.000 14 RETURN END C C C C

203 C c SUBROUTINE PEDERV (N,T,Y,PD,NO) RETURN END SUBROUTINE PLTF(,IY1.Y2,Y3,Y4.Y5,Y6,Y7,Y8,Y9,Y10,Yl1, 1 Y12,Y13,Y14,Y15,Y16,Y17.01, D2.TI) C C C *** SUBROUTINE IDENTIFICATION *** C C C THIS SUBROUTINE IS WRITTEN TO MAKE TWO DIMENSIONAL CURVES. C C REAL*4 XBOX(50).YBOX(50) H1.H2.Y1(2010)Y,2(2010) REAL*4 Y3(2010).Y4(2010).Y5(2010))Y6(2010).Y7(2010).Y8(2010) REAL*4 Y9(2010),Y10(2010)YI11(2010)Y12(2010 ). Y13(2010) REAL*4 Y15(2010).Y16(2010).Y17(2010).Y14(2010) REAL*4 TI(2010),X1(100).X2(1100)X3(100),X4(100),X5(100) REAL*4 O1(2010),D2(2010),X6(100),X7(100) REAL*4 X8(100).X9(100),X10(100),X1( 100),X12(100).X13(100) REAL*4 X14(100).X15(100).ZI.Z2.Z3.Z4.Z5,Z6,Z7.Z8 ICOUNTa 1 C C DATA FOR DRAWING THE BOX THAT WILL CONTAIN FOUR PLOTS. C DATA XBOX/0.08.5,8.5.0.0.0.0/.YBOX/0.0,0.0,11.0,11.0,0.0/ DATA HI,H2/0.12, 0.1/ C C READ THE COORDINATES OF THE ORIGIN FOR THE FOUR PLOTS. C READ(8,45)Z 1, Z2, Z3, Z4, Z5,Z6,Z7,Z8 45 FORMAT(8F5.3) C C HEAD TITLES OF THE PLOTS. C READ(8,44)(X1( IL). IL=1.20) READ(8.44)(X2(IL), IL1.20) READ(8,44 ) (X3(IL), IL= 1, 20) C C SET THE LETTERING SIZE FOR THE TITLE ON A LINEAR AXIS. C CALL PLOTS CALL PAXTTL(HI) C C C SET THE LETTERING SIZE FOR THE NUMBERING ON A LINEAR SCALE. C C CALL PAXVAL(H2) C C C DRAW THE CONTOUR WHICH SURROUNDS THE FOUR FIGURES. C 489 CALL PLINE(XBOX.YBOX.5.1,0,0,0) C C READ THE TITLES OF THE PLOTS. NOTE THAT THE LATTERS ARE C UPDATED EVERY TIME FOUR PLOTS ARE DRAWN. C

204 C READ(8.44)(X4(IL).IL1,20) READ(8.44)(X5(IL), IL 1.20) READ(8o44)(X6(IL). IL1.20) READ(8.44)(X7( IL)o IL 120) READ(8.44)(X8( IL).o IL~=.20) READ(8.44)(X9( IL) IL=.20) REAO(8o44)(X 10(IL) ILw 20) REAO(8,44)(X I( IL) IL-1.20) REAO(8.44)(X12(IL),IL~1.20) IF(ICOUNT.GT.4)GO TO 371 READ(8,44)(X13(IL).IL 1,20) READ(8.44)(X14(IL). ILI.20) READ(8,44)(X15(IL). IL=1.20) 371 CONTINUE 44 FORMAT (20A4) C C WRITE UP OF THE OVERALL TITLE OF THE RUN. C CALL PSYM(1.05.10.5,0.25,XI.0.0.40.0) CALL PSYM(1.75.10.15,0.25.X2.0.0,40.0) CALL PSYM(1.05,9o8,.025,X3,0.0,40O0) C C C PLOTTING THE CURVES. C C IF(ICOUNT.GT.1)GO TO 500 CALL PLTCUR( I TI.Y1,X4,Z1.Z2) CALL PLTCUR.(ITIY2,X5.Z3,Z4) CALL PLTCUR(I TI.Y3,X6,Z5,Z6) CALL PLTCUR(ITIY4,X7,Z7.Z8) GO TO 503 C C 500 IF(ICOUNT.GT.2)GO TO 501 CALL PLTCUR(I TI, Y5X4,Z1.Z2) CALL PLTCUR(I,TI,Y6,X5,Z3,Z4) CALL PLTCUR(ITI, Y7,X6,Z5,Z6) CALL PLTCUR(I, TI Y8,X7,Z7,Z8) GO TO 503 C C 501 IF(ICOUNT.GT.3)GO TO 502 CALL PLTCUR( I,TI Y9,X4,Z1.Z2) CALL PLTCUR(I,TI.Y10,X5,Z3,Z4) CALL PLTCUR(I.TI,Y 11 oX6,Z5,Z6) CALL PLTCUR(,TI,Y12,X7,Z7,Z8) GO TO 503 C C 502 IF(ICOUNT.GT.4)GO TO 504 CALL PLTCUR( I TI, Y13,X4,Z1.Z2) CALL PLTCUR(I, TI,Y14,X5.Z3,Z4) CALL PLTCUR(I.TI,Y15,X6,Z5,Z6) CALL PLTCUR(I,TI,Y16,X7,Z7,Z8) GO TO 503 C C 504 CALL PLTCUR( I,TI,Y17,X4,Z1,Z2)

206 CALL PLTCUR(I,TI.D1.X5.Z3.Z4) CALL PLTCUR(I,TID2,X6,Z5.Z6) CALL PSYM(.75,5.275.0.15,X7,0.0.40,0) CALL PSYM(.75,5.025,0.15,X8.0.0,40,0) CALL PSYM(4.75.5.275,0.15,X9,0.0,40,0) CALL PSYM(4.75,5.025,0.15,X10,0.0.40,0) CALL PSYM(.75.0.9.0.15.X11.0.0,40,0) CALL PSYM(.75.0.65,0.15,X12, 0.0,40,0) CALL PLTEND IF(ICOUNT.EQ.5)GO TO 511 C C 503 CALL PSYM(.75.5.275,0.15.X8, 0.0.40.0) CALL PSYM(.75,5.025,0. 15, X9,0.0,400) CALL PSYM(4.75,5.275,0.15,XIO,0.0,40,0) CALL PSYM(4.75,5.025,0. 15,X 110.0,40,0) CALL PSYM(.75,0.9.0.15,X12,0.0.40,0) CALL PSYM(.75.0.65.0.15, X13.0.0,40,0) CALL PSYM(4.75,0.9,0. 15.X14,0.040,0) CALL PSYM(4.75,0.65,0. 15,5,50.0.40.0) CALL PLTEND 511 ICOUNT ICOUNT+1 C C IF(ICOUNT.LE.5)GO TO 489 RETURN END C C C THIS SUBROUTINE MAKES INDIVIDUAL PLOTTING. C C SUBROUTINE PLTCUR(I,TI,Y1,X1,Z1,Z2) C REAL*4 Z1,Z2,TI(220),Y1(220),X1(100) C C CALL PSCALE(3.0,0.5.TIMIN.TFTITI,I,1) CALL PSCALE(3.0.0.5.Y1MIN.Y1F.Y1I, 1) CALL PLTOFS(TIMIN.TIFY1MIN.Y1F, Z1,Z2) CALL PLINE(TI,Y,I,1,0,0.1) CALL PAXIS(Z1,Z2,'TIME, t [SECOND]',-17,3.0, i O.O,TIMIN.TIF,0.5) CALL PAXIS(Z,Z2,X1,30,3.0,90.0OY1MIN.YIFO.5) C RETURN END C C C THIS SUBROUTINE MAKES 2 PLOTS ON THE SAME PLOTTING SPACE. C C SUBROUTINE PLTDCU(I,TI,Y1,Y2.X1,Z1,Z2) C C REAL*4 Z1,Z2,TI(220),Y1(220),Y2(220),X1(100) C C CALL PSCALE(3.0.0.5,TIMIN,TIF,TII, 1) CALL PSCALE(3.0,0.5.Y1MIN.YIF.Y,I,1,.Y2,I.1)

206 CALL PLTOFS(TIMIN.TIF,Y1MIN.Y1F.Z1.Z2) CALL PLINE(TI.Y1.1,,O,0,1) CALL PDSHLN(T I Y2 1.1.0.05.1) CALL PAXIS(Z1,Z2.'TIME, t [SECONDJ', 17,3.0, I O.OTIMIN,TIFoO.5) CALL PAXIS(Z,Z2oX1,25.3.oO90.0.Y1MIN.YIF 0.5) C C RETURN END SUBROUTINE DIM3(I,X, YAZ) C C REAL*4 XMAX.YAMAX.ZMAXXMIN,YAMIN,;ZMIN REAL*4 X(2010),AXLGTHFACTOR,YA(2010),Z(2010) REAL*4 YESATT,OVMINOVMAX,AT INTEGER 4 N I C DATA PICI.PIC2.YES/4HPICI,4HPIC2. '3HYES/ N1I XMAX O. YAMAX =0.0 ZMAX=O O XMIN=0.0 YAMIN=0.0 ZMINOo.0 C C. SEARCH FOR THE MAXIMUM VALUE IN-'-HE X ARRAY. C DO 30 IJl,.N IF(IJ.EQ.1)GO TO 31 IF(XMAX.GE.X(IJ))GO TO 30 31 XMAX=X(Id) 30 CONTINUE C C SEARCH FOR THE MAXIMUM VALUE IN THE YA ARRAY. C DO 32 IJ=1.N IF(IJ.EQ. )GO TO 33 IF(YAMAX.GE.YA(IJ)) GO TO 32 33 YAMAX=YA(IJ) 32 CONTINUE C C SEARCH FOR THE MAXIMUM VALUE IN THE Z ARRAY. C DO 34 lJ=1,N IF(IJ.EQ. )GO TO 35 IF(ZMAX.GE.Z(IJ))GO TO 34 35 ZMAX*Z(IJ) 34 CONTINUE C C SEARCH FOR THE MINIMUM VALUES IN THE XYA,AND Z ARRAYS. C 00 36 IJ=1,N IF(IJ.EQ. )GO TO 37 IF(XMIN.LE.X(IJ))GO TO 36 37 XMIN=X(IJ) 36 CONTI NUE C DO 38 IdIoN

207 IF(IJ.EQ.1)GO TO 39 IF(YAMIN.LE.YA(IJ))GO TO 38 39 YAMIN=YA(Id) 38 CONTINUE C DO 40 IJ1,N F(IJ.EQ. 1)GO TO 41 IF(ZMIN.LE.Z(IJ))GO TO 40 41 ZMIN-Z(IJ) 40 CONTINUE C C DETERMINE THE LENGTH OF THE AXES TO BE PLOTTED IN THE THREE C DIMENSIONAL PLOTS. C IF(XMAX.GE.YAMAX)GO TO 50 IF(YAMAX.GE.ZMAX)GO TO 52 GO TO 53 50 IF(XMAX.GE.ZMAX)GO TO 51 53 OVMAX=ZMAX GO TO 54 51 OVMAX=XMAX GO TO 54 52 OVMAX=YAMAX C 54 IF(XMIN.LE.YAMIN)GO TO 60 IF(YAMIN.LE.ZMIN)GO TO 62 GO TO 63 60 IF(XMIN.LE.ZMIN) GO TO 61 63 OVMIN=ZMIN GO TO 64 61 OVMIN=XMIN GO TO 64 62 OVMIN=YAMIN 64 IF(ABS(OVMAX).GE.ABS(OVMIN)) GO TO 81 AXLGTH=ABS(OVMIN) GO TO 82 81 AXLGTrHABS(OVMAX) 82 FACTOR=0.8/AXLGTH DO 666 ILF=I,N X(ILF )=X( ILF)*FACTOR YA(ILF)=YA(ILF)*FACTOR Z(ILF )Z(ILF)*FACTOR 666 CONTINUE C C WRITE( 7,432)XMAX, YAMAX, ZMAX, XMIN,YAMIN,ZMIN 432 FORMAT(2X.'XMAX='.F15.8, 10X, 'YAMAX=',F15.8, 10X. 'ZMAX=',F15.8, 1 /,2X.'XMIN=',F15.8,10X,'YAMIN=',F15.8,10X,'ZMIN=,F15.8) WRITE(7,433)OVMAX,OVMIN 433 FORMAT(2X,'OVMAX=',F15.8,-OX,'OVMIN=',F15.8) C WRITE(7,77)FACTOR. AXLGTH 77 FORMAT(///,5X,'FACTOR=',F15.7,7X,'AXLGTH=',F15.7) C C THIS IS AN INTERACTIVE PORTION OF THE PROGRAM TO ALLOW THE C USER TO VIEW THE THREE DIMENSIONAL PLOT FROM THE DESIRED C ANGLE. C 44 WRITE(6,627) 627 FORMAT(2X,'WHAT ARE THE DESIRED ANGLES TO VIEW THE PLOT 7')

208 WRITE (6676) 676 FORMAT(2X.'THE ANGLE FOR THE X-AXIS IS: (DEGREES)') READ(5,845)AX 845 FORMAT(F 15.7) WRITE(6,901) 901 FORMAT(2X.'THE ANGLE FOR THE Y-AXIS IS: (DEGREES)') READ(5,902)AY 902 FORMAT(F15 7) WRITE(6,601) 601 FORMAT(2X.'THE ANGLE FOR THE Z-AXIS IS: (DEGREES)') REAO(5,617)AZ 617 FORMAT(F15.7) AAX=(AX*3.141592654)/180.0 AAY,(AY*3,141592654)/180.0 AAZ,(AZ*3.141592654)/180.0 C C START PLOTTING THE THREE DIMENSIONAL PLOT. C CALL IGINIT CALL IGBGNS(PICI) CALL IGBGNS(PIC2) CALL IGMA(-0.8,0.,0. ) CALL IGDA(0.8,0..O.) CALL IGMA(O, -0.8.0.) CALL IGDA(O..0 ~8.0.) CALL IGMA(0.0..-0.8) CALL IGDA(0.,0.,0.8) CALL IGMA(X(1),YA(1),Z( )) DO 45 IT=2,N CALL IGDA(( IT) YA(IT),Z(IT)) 45 CONTINUE CALL IGTXT('<RSCL>',2.0) CALL IGMA(.4,-0.05.0.) CALL IGTXT('X<E>') CALL IGMA(-0.05.-0.05,0.) CALL IGTXT( O<E>') CALL IGMA(-0.05,.4,0.) CALL IGTXT('Y<E>') CALL IGMA(-O.05,0.,.4) CALL IGTXT('Z<E>') CALL IGTRAN(PIC2.'ROTY' AAY, 'ROTX',AAX ROTZ' AAZ) CALL IGENOS(PIC2) CALL IGTXT('<RSCL>' 2.0) CALL IGMA(-0.4,-0.85,0.) CALL IGTXT('FIGURE 10: END EFFECTOR POSITION 1 <CRLF>FOR THE CASE OF R=2.0 METER. I <CRLF>THETA=0.5 RAO. AND PHI=0.5 RAD.<E>') CALL IGENDS(PICI) CALL IGDRON( 'TERMINAL ' ) C C THIS INTERACTIVE PORTION OF THE PROGRAM ALLOWS THE USER C TO MAKE HARD COPY OF THE THREE DIMENSIONAL PLOT IF DESIRED C AND TO SPECIFY ANOTHER SET OF ANGLES IF THE PLOT IS TO BE C VIEWED FROM DIFFERENT ANGLE. C WRITE ( 648) 48 FORMAT(2X 'DOES THE PLOT LOOKS GOOD?',///.2X, I 'WOULD YOU LIKE TO MAKE A HARD COPY? (YES/NO)') READ(5.49)ATT 49 FORMAT (A4)

209 IF(ATT.EQ.YES)GO TO 888 GO TO 889 888 CALL IGDRON('CALCOMP') 889 WRITE(6,324) 324 FORMAT(2X.'WOULD YOU LIKE TO TRY DIFFERENT ANGLES? (Y/N)') READ(5,325)AT 325 FORMAT(A4) IF(AT.EQ.YES) GO TO 44 RETURN END 0.5 0.5 0.5 0.4 0.0 0.0 0.0 0.0 0.0 0.0 8.0 8.0 16.0 50.0 50.0 50.0 0.0 1. 1.0 0.085 0.1 9.81 0.04286 0.5 0 00003167 2707.0 5.67.75 6.05 4.75 6005 0.75 1.5254.75 1.525 RIGID AND FLEXIBLE MOTION CASE, DESIRED POSITION: R=0.5 M THETA=O.5 RAD. AND PHI=O.5 RAD. r DISPLACEMENT (METER] THETA ROTATION (RAD] PHI ROTATION [RADI DISPLACEMENT, q (t) (M) FIGURE 1. GENERAL MOTION, r DISPLACEMENT. FIGURE 2. GENERAL MOTION, THETA DISPLACEMENT. FIGURE 3. GENERAL MOTION, PHI ROTATION. FIGURE 4. GENERAL MOTION, q (t) DISPLACEMENT. DISPLACEMENT, q (t) (M) DISPLACEMENT, q (t) (M] DISPLACEMENT, q (t) (M] VELOCITY, r (M/SEC] FIGURE 5. GENERAL MOTION, q (t) DISPLACEMENT. FIGURE 6. GENERAL MOTION, q (t) DISPLACEMENT. FIGURE 7. GENERAL MOTION, q (t) DISPLACEMENT. FIGURE 8. GENERAL MOTION, r VELOCITY. ANGULAR VELOCITY, [RAD/SEC] ANGULAR VELOCITY, [RAD/SEC] VELOC. q (t) [M/SEC] VELOCITY, q (t) (M/SEC] FIGURE 9. GENERAL MOTION, ANGULAR VELOCITY FIGURE 10. GENERAL MOTION, ANGULAR VELOCITY FIGURE 11. GENERAL MOTION, q (t) VELOCITY. FIGURE 12. GENERAL MOTION, q (t) VELOCITY. VELOCITY, q (t) (M/SEC] VELOCITY, q (t) [M/SEC] CONTROL TORQUE, T1 [N-M] CONTROL TORQUE, T2 (N-M]

210 FIGURE 13. GENERAL MOTION. q (t) VELOCITY. FIGURE 14. GENERAL MOTION. q (t) VELOCITY. FIGURE 15. GENERAL MOTION. CONTROL TORQUE FOR r. FIGURE 16. GENERAL MOTION, CONTROL TORQUE FOR CONTROL TORQUE. T3 [N-M) V DEFLECTION [M). W DEFLECTION [M]. FIGURE 17. GENERAL MOTION, CONTROL TORQUE FOR FIGURE 18. GENERAL MOTION. VERTICAL DEFLECTION V. FIGURE 19. GENERAL MOTION, HORIZONTAL DEFLECTION W.

211 APPENDIX G LISTING OF THE COMPUTER CODE FOR THE EXPERIMENTAL WORK

212 Microsoft FORTRAN77 V3.31 > 1C > 2C > 3 INTEGER*2 ENC2V,CJ1,CJ,DACVAL,DACHN,N,TERMCT,SOURCF > 4 1 ADCAVLADCHN > 5C > 6C > 7 COMMON /FRO/FREQN,DT > 8 COMMON /ENCBLUJ1,C2,ENC2V > 9 COMMON/DAC8LDACHN,DACVAL > 10 C COMMON /ADCBADCHNADCVAL > 11 COMMON XTT ERMcTSURCF > 12 COMMON STEP/K > 13C > 14 OPEN(17,FILE='DATA.TXT,STATUS='NEW) > 15C > 16 CALLINIT > 17 CALLCLK.FRQ(F5X > 18 DT=1.Q/FREQCK > 19 WRTE 50)TERMCT,SOURCF,FREQCK,DT > 20 50 FORMAT(2X,THE TERMINAL COUNT IS =',16/,2X, > 21 1 THE SOURCE FREQUENCY IS,12,2X, > 22 1 'FREQUENCY DESRED IS =',F15.8,2X, > 23 'SAMPLING PERIOD IS =,F15.8//) > 24C > 25 CALLCLOCK > 26C > 27 14 WRITE(,10) > 28 10 FORMAT(2XTERJOINT2 CHANNELNUMBERo(REALNUMBER)) > 29 READ(*,11)DACH 30 11 FORMAT(F15.8) > 31 DACHN=IFIX(DACH) > 32C >33 WRITE(,12)DACHN > 34 12 FORMAT(2X,'CHANNEL NUMBER CHOSEN FOR JOINT 2 IS,13/,2X, > 35 1 'IS THIS SELECTION CORRECT? (YN)) >36 READ(*13)AF 37 13 FORMAT(A1) > 38 IF(AF.EQ.'N)GOTO 14 39C > 40 DACVAL=0 > 41 CALLDAC > 42C > 43 CJ1=2 > 44 CJ2=3 >45C > 46 FRB,=F:<3 > 47 CALLCLSLP > 48C > 49 STOP > 50 QBC

213 Microsoft FORTRAN77 V3.31 >Name Type Offset P Class >ADCAVL INTEGER*2 >ADCHN INTEGER*2 >AF REAL 438 >CJ1 INTEGER*2 0 /ENCBL / >CJ2 INTEGER*2 2 /ENCBL / >DACH REAL 330 >DACHN INTEGER*2 0 /DACBL / >DACVAL INTEGER*2 2 /DACBL/ >DT REAL 6 /FRQ1 / >ENC2V INTEGER*2 4 /ENCBL / >FREQ REAL 0 /FRQ1 / >FRECCK REAL 16 >IFIX NTRINSIC >K INTEGER4 0 /STEP / >N INTEGER*2 4 /FRQ1 / >SOURCF INTEGER*2 2 /COUNT/ >TEFRMCT INTEGER*2 0 /COUNT/ > 51 > 52 >Name Type Size Class >CLKFRQ SLBEIROLtE >CLOCK SUBROLTME >CLSLP SUBROUTlTE >COUNT 4 COMMN >DAC SUBRJTNE >DACBL 4 COMMON >ENCBL 6 COOACN >FRQ1 10 OCVIMCN >INIT SUBROITlE >MAIN PFOGRAM >STEP 4 COMIMON >Pass One No Errors Detected > 52 Source Unes

214 Microsoft FORTRAN77 V3.31 > 1 > 2 SJUBFOUTNETLFRQ(EWX > 3 > 4 C THIS ROUTNE ISTO PUT CLOCK FREQUENCY, AND THEN > 5 C DEETETFIMCTAD SOURCF FORTHE LABTENDER COCK > 6 > 7 NTEG2TGETRMCTTSOURCF > 8 COv!MONCOUNTERCUoRCF >9 > 10 SOURCF=5 > 11 > 12 4 WRITE(,7) > 13 7 FORMAT('OEnter Frequency of Clock Interrupts (Hz)'/ > 14 ' Typical value: 1000. Maximum: 5000.' > 15 ~ ' Include a decimal point') > 16 READ (,8)FREQCK > 17 8 FORMAT(F15.8) > 18 IF(FRECK.LE 5000.) GOTO 9 > 19 WRITE(*,) 'Clock frequency must be less than 5000. Hz.' > 20 GOTD4 > 21 9 ONTNE > 22 > 23 C TESTCT REAL VALUE FOR TERMCT, NECESSARY IFTERMCT> 32767. > 24 10 TESTCT AINT((1*1 0*SOURCF)/FREQCK) > 25 > 26 C WRTE STATENTS FR VARKXS NTEMVEDIATE VALUES ARECMMENE > 27 C OU N THIS PROGRAMTO MAKETHEM INVISIBLETO THE USER. YOU MAY WSH > 28 C TO LSEE WRTESTO S HOWTHE PROGRAM WORKS, BBUT DO NOT LEAVE > 29 C THEM N YOUR FNALPROGRAM. > 30 > 31 WRITE(*,15)TESTCTSOURCF > 32 15 FORMATC TESTCT = ',F12.3,' SOURCF = ',12) > 33 > 34 C CHEKTOBESURETESTCTAND SOURCFAREWTHN THE DESIREDRAN( > 35 IF (TESTCT GT. 32767.) THEN > 36 SOURCF=SOURCF 1 > 37 GOTO 10 > 38 ELSEIF (TESTCT T. 2.) THEN > 39 WRITE(,17) FREQCK > 40 17 FORMAT(' Clock Frequency of ',F12.3,' is too high.') > 41 GCTO4 > 42 ELSEIF (SOURCF.LT. 2) THEN > 43 WRITE(*,18) FREQCK > 44 18 FORMAT(' Clock Frequency of ',F1 2.3,' Hz is too low.') > 45 GOTO4 > 46 B.SE > 47 OCNTNC E > 48 TERMCT= INT(TESTCT) > 49 ENDIF > 50 > 51 WRITE(,35) TERMCT,,SOURCF > 52 35 FORMAT(' TERMCT = ',16,' SOURCF = ',12)

216 Microsoft FORTRAN77 V3.31 > 53 C FROUT = (1*1 0*SOURCF)/REAL(TERMCT) > 54 C FREQK = FROUT/1 000. > 55 C ERROR = ((FROIT - FREQCKyFREQCK*1 00. > 56 C WRIrTE(AO) FREQCKR ERROR,FREQK > 57 C 40 FORMATC CLOCK FREQUENCY INPUT= ',F123/ > 58 C * 'COUNTER FREQUENCY OUT = ',F1 23, > 59 C * 'ERROR = ',F12.3,' %',/ > 60 C * 'FREQ. IN KHz=',F12.3) > 61 > 62 RET RN > 63 B!D >Name Type Offset P Class >AINT INTRINSIC >FREQCK REAL 0 >INT INTRINSIC >S9URF INTEGER*2 2 /COUNT / >TERMCT INTEGER*2 0 /COUNT / >TESTCT REAL 192 >Name Type Size Class >CLKFRQ SFUROUTIE >COUNT 4 COMMN >Pass One No Errors Detected > 63 Source Unes

216 Microsoft FORTRAN77 V3.31 > iC > 2 SLUBROT11NELSLP > 3C > 4 C THIS SUBROLmNE PERFORMS THE CLOSED LOOP RUN > 5 C WHICH CONSISTS OF THE RIGID BODY CONTROLLER,. > 6 C THE LATTER IS A POINT TO POINT CONTROLLER. * > 7C > 8 INTEGER*2 N,K,CJ1,CJ2,ENC2V,DACVAL,DACHN,ICL,M(3000),J, > 9 1 IFREQ,K1,K2,K3,ADCVALADCHNNF > 10C > 11 DIMENSION P(420),V(420),E(420),EI(420),VI(840),RM1(420),T(1000) > 12 DIMENSION PV(840) > 13C > 14 COMMON FRQ1/FREQN,DT > 15 COMMON /ENCBLCJ1,CJ2,ENC2V > 16 COMMON /DACBUDACHN,DACVAL > 17 COMMON /ADCBUADCHNADCVAL > 18 COMMON ISTEP/K > 19C > 20 IFREQ=F)qFREQ > 21 16 WRITE(*,10) > 22 10 FORMAT(20X,'** CLOSED LOOP RUN. *~*J,20X, > 23 1 * RIGID BODY CONTROLLER ONLY. ***,///,2X, > 24 1 ' CHANNEL NUMBER CHOSEN FOR q dot IS 0 ***',/, > 25 1 2X,"**.CHANNEL NUMBER CHOSEN FOR q IS 1 **,////,2X, > 26 1 ENTER CLOSED LOOPREERENCEVALUE DEGREES.N/2X > 27 1 TYPICAALLUE IS BEWEEN 10AND -20.0 DEGREES.) > 28 READ(*911)CL > 29 11 FORMAT(F15.8) > 30C > 31C > 32 D=1.0 > 33 ICL=IFIX(CL*31.25) > 34 AK1=-57.37/(D*31.25) > 35 AK2-8.75/(D*31.25) > 36 AK3=-51.52/(D*31.25) > 37C > 38 WRITE(*,9) > 39 9 FORMAT(2X'ENTER DURATION OFTHE RUN. (SECONDS)'V, > 40 1 2X,**** MAXIMUM 10 SECONDS. ****) > 41 READ(*,8)DN > 42 8 FORMAT(F15.8) > 43 N=IFIX(FREQ*DN) > 44 NF=N+IFREQ*5 > 45C > 46 WRITE(*,14)CL,ICL,D,N,DT,AK1 AK2,AK3 > 47 14 FORMAT(2XCLOSED LOOP REFERENCE VALUE=',F15.8,'DEGREES.'V, > 48 1 2XCLOSED LOOP REFERENCE VALUE=',17,'COUNTS'V2X, > 49 1 'SCALING FACTOR=',F10.5,3X,'N=',17,2X,'DT=',F15.8,//,30X, > 50 1 'CONTROLLER GAINS ARE:'/,2X,'K1 =',F1 5.5,3X,'K2=',F1 5.5, > 51 1 'K3=',F15.5J/,2X,'ARE THEY CORRECT? (Y/N)') > 52 READ(*,15)AT1

217 Microsoft FORTRAN77 V3.31 > 53 15 FORMAT(A1) >54 IF(AT1.NE.)GO TO 16 > 55C > 56 C INmAULZATION OFTHE POSTON VECTOR,THE VELOCITY VECTOR AND > 57 C THEERORVETOR > 58C > 59 K=1 > 60 CALL NC > 61 C > 62 ADCHN-O > 63 CALLADC > 64 VI(K)=ADCVAL > 65C > 66 ADCHN=1 > 67 CALLADC > 68 PV(K)=ADCVAL > 69C > 70 P(K=ENC2V > 71 POS=P(K) > 72 POST=P(K) > 73 ERROR=00O > 74 EI(K)=0.0 > 75 V(K)=0.0 > 76 E(K)=ENC2V-CL > 77 M(K)=0 > 78 C > 79 K=2 > 80 CALL NABLE > 81 C > 82 18 J=K > 83 CALLENC > 84C > 85 ADCHNO > 86 CALLADC > 87 VI(K)=ADCVAL > 88C > 89 ADC3HN= > 90 CALLADC > 91 PV(K)=ADCVAL > 92 C > 93 P(K)=NC2V > 94 V(K)=(P(K)-POS)*FREQ > 95 E(K)=P(K)-ICL > 96 EI(K)=E(K)*DT+ERROR > 97 POS=P(K) > 98 ERROR=EI( > 99 C > 100 C COMPLUTATKN OFTHE CXJNTROLSIGNAL > 101 C

218 Microsoft FORTRAN77 V3o3 > 102 RM=-AK1 (P(K)-POST)-AK2*V(K)-AK3*EI(K) > 103 M(K)=IFIX(RM) > 104 IF(M(K).GE.127) M(K)=127 > 105 IF(M(K).LE.-128) M(K)=-128 > 106C > 107 C SENDOUTTHEOONTROLSKNAL > 108C > 109 DACVAL=M(K) > 110 CALLDAC > 111 C > 112 C WAT FOR THE INTERRUPT SERVICE ROUTINE TO INCREMENT K > 113C > 114 17 IF(J.EQ.K)GOTO17 > 115C > 116 C CHECK WEHERTHETIMEASSKGED FOTHTS MOVEHASELAPSED. > 117C > 118 IF(KLEN)GO TO18 > 119C > 120 68 J=K > 121 C > 122 ADCHNO > 123 CALLADC > 124 VI(K)=ADCVAL > 125C > 126 ADCH=1 > 127 CALLADC > 128 PV(K)cADCVAL > 129C > 130 C SHLTOFFTHEMOTOR. > 131 C > 132 DACVAL=0 > 133 CALLDAC > 134C >135 67 IF(J.EQ.K)GOTO67 > 136C > 137 IF(KLENF)GO TO 68 > 138 C DISABLE THE NTERRUPT. > 139C > 140 CALL DISABL > 141 C > 142 C OUTPUTHE RESULTS. > 143C > 144 DO70 =1,N > 145 P(I)=P(I)/31.25 > 146 V(I)=V(1)/31.25 > 147 E(I)=E(1)/31 25 > 148 RM1(1)=M(I)*0.039055118 > 149 T(l)=(l-i )DT > 150 70 CO:NTIE > 151 C > 152 C WRITE(*,19)

219 Microsoft FORTRAN77 V3.31 > 153 C 19 FORMAT(//,4X,TlME',5X,'POSTION',4X,VELOCY,6X,'ERROR, > 154 C 15X,'CONTROL/,9X,'(DEGREES)',3X,'(DEG/SEC)',3X,'(DEGREES)', > 155 C 13X,'(VOLTS)'/) > 156C DO201=1,N > 157 C WRITE(*,21)T(l),P(I),V(I),E(I),RM1(I) > 158 C 21 FORMAT(2X,F8.5,2X,F1 0.5,2X,F10.5,2X,F1 0.5,2X,F10.5) > 159 C 2000N UE > 160C > 161 C WRITE(*,22) > 162 C 22 FORMAT(///,4X,TIME',4X,VIBRATION MEASUREMENT,/,1 7X > 163 C 1,'(MM/SECOND)',/) > 164 DO 94 1=1,NF > 165 T(I)=(I-1)*DT > 166 94 CON1TUE > 167C > 168 C DO 891=1,NF > 169 C WRITE(*,23)T(I),VI(I) > 170 C 23 FORMAT(2X,F8.5,2X,F10.5) > 171 C 89 00NTJE > 172C > 173 DO111 1=1,N > 174 WRITE(17,112)T(I),P(I),V(I),E(I),RM1 (1) > 175 1 12 FORMAT(2X,F8.5,2X,F1 0.5,2X,F10.5,2X,F1 0.5,2X,F 10.5) > 176 111 CNTNUE > 177C > 178 DO 113 11,NF > 179 WRITE(17,114)T(I),VI(I),PV(I) > 180 1 14 FORMAT(2X,F8.5,2X,F1 0.2,2X,F10.2) > 181 11300NTI JE > 182C > 183 WRITE(*,24) > 184 24 FORMAT(2X,WOULD YOU UKETO MAKEANOTHER RUN? (YN)) > 185 READ(*,25)AT2 > 186 25 FORMAT(A1) > 187 IF(AT2.EQ.Y)GO TO 16 > 188C > 189 FETURJ > 190 BD:>

220 Microsoft FORTRAN77 V3.31 >Name Type Offset P Class >ADCHN INTEGER*2 0 /ADCBL / >ADCVAL INTEGER*2 2 /ADCBL / >AK1 REAL 25584 >AK2 REAL 25588 >AK3 REAL 25592 >AT1 REAL 26012 >AT2 REAL 26182 >CJ1 INTEGER*2 0 /ENCBL / >CJ2 INTEGER*2 2 /ENCBL / >CL REAL 25514 >D REAL 25578 >DACHN INTEGER*2 0 /DACBL / >DACVAL INTEGER*2 2 /DACBL / >DN REAL 25694 >DT REAL 6 /FRQ1 / >E REAL 21776 >EI REAL 23456 >ENC2V INTEGER2 4 /ENCBL/ >EFtOR REAL 26028 >FREQ REAL 0 /FRQ1/ >1 INTEGER*4 26038 >CL INTEGER*2 25582 >IFIX NTRINSIC >IFREQ NTEGER*2 25136 >J INTEGER*2 26032 >K INTEGER*2 0 /STEP / >K1 INTEGER*2 >K2 INTEGER*2 >K3 INTEGER*2 >M INTEGER*2 15776 >N INTEGER*2 4 /FRQ1 / >NF INTEGER*2 25706 >P REAL 16 >POS REAL 26020 >POST REAL 26024 >PV REAL 12416 >RM REAL 26034 >RM1 REAL 6736 >T REAL 8416 >V REAL 1696 >VI REAL 3376 > 191 >Name Type Size Class >ADC SUBROUT\E

221 Microsoft FORTRAN77 V3.31 >ADCBL 4 CCMVtN >CLSLP SUBRO3MNE >DAC SUBROUME >DACBL 4 OCMtvON >DISABL SUBROJLTE >ENABLE SUBROJITE >ENC SUBROWT1E >ENCBL 6 COMMXN >FRQ1 10 COMOCN >STEP 2 OMCN >Pass One No Errors Detetd > 191-Source Lnes

222 The IBM Personal Computer Assembler 01-01-80 PAGE 1-1 >Experimental Work CODE > TlTLE EXPERENTAL WORKASSEMBLY CODE > PAGE,132;Set page width to 132 characters. >: This routine initializes the digital I/O port and > sets up the IRQ4 interrupt vector. > FORTRAN "CALL INT" calls this procedure. >0000 DATA SEGMENT PUBUC IDATA' > -; For local assembler program data storage. >:; Not used, but required for linking >0000 DATA ENDS >0000 SEP$ASEGMENTCOMMON$STEP > This segment is equivalent to FORTRAN's > COMMONSTEP/K. The $ and A are > required to match the segment name and ~~> -; dclass that the linker uses for the FORTRAN >; common. Eleven variables must be declared >; INTEGER2 in the FORTRAN calling routine > before the common block >0000???? K DW? >0002 STEP$A JDS > GROUP GROUP DATASEP$A >; The DATA,STEP$A segment will be linked into >; the group called DGROUP to match Microsoft >; ORTAN convention. >0000 CODE SEGMENPUBLUCXDE > ASSLUMECSCODEDSDGROUP$PSDGROUP > PUBUC INIT; Make the INIT label available > -; to other segments. >; Labtender Address, in IBM PC, for SETDDA routine. >- 033F IOCOM EQU 831; Parallel Port Control Register. >0000 INIT PROC FAR >0000 55 PUSH BP; Save calling framepointer. >0001 8B EC MOV BP,SP >0003 B2 49 MOV DL,73; Print an '1' to the screen >0005 B4 02 MOV AH,2; for debugging >0007 CD 21 INT 21H

223 The IBM Personal Computer Assembler 01-01-80 PAGE 1-2 >Experimental Work Code > INTERRUPT SERVICE ROUTINE >;Load the IRQ4 interrupt vector (address to >;go to upon interrupt 4). >0009 FA CLI Disable interrupts while we are >; modifying them to prevent any >; catastrophes. >OOOA 06 PUSH ES Save Extra Segment >00OB B8 0000 MOV AX,OOOOH >OOOE 8E CO MOV ES,AX; Point Extra Segment at the ISR address >0010 BF 0030 MOV DI,30H; Table. Offset of entry for IRQ 4 in the >0013 B8 0020 R MOVAXOFFSETISR; address table. Get offset of DDASER, >; our interrupt service routine. >0016 AB STOSW; Load offset in AX into address in Dl, >; and add 2 to Dl to prepare to store CS. >0017 8C C8 MOV AX,CS; Get Code Segment for assembly code. >0019 AB STOSW; Load code segment in AX into address >; in Dl. Now the 4-byte address of our >; DDASER begins at memorY location 30H. >; An IRQ4 Interrupt (if enabled) will >; cause the microprocessor to imme>; diately go to that address, and start >; performing our DDASER. >001A 07 POPES; Restore Extra Segment >001B FB STI; Enable interrupts >001C 8B E5 MOV SP,BP Restore framepointer >001 E 5D POP BP >001F CB RET; Return >0020 INITENDP > 'SUBTTL NTERRUPTSERVICE ROJNE >; Labtender Addresses, in IBM PC, for DDASER routine. >= 0332 INTCLR EQU 818; Timer Interrupt Clear Address. >; 8259 Programmable Interrupt Controller Addresses >; in IBM PC. >= 0020 ICR EQU 20H; Interrupt Command Register. >= 0021 IMR EQU 21H; Interrupt Mask Register. >;; Constants for DDASER routine. >= 0020 EOI EQU 20H; End of Interrupt signal for >; 8259 Interrupt Controller.

224 The IBM Personal Computer Assembler 01-01-80 PAGE 1-3 >Experimental Work Code > INTERRUPT SERVICE ROUTINE >0020 ISR PROC FAR >0020 55 PUSH BP; Save calling framepointer. >0021 8B EC MOV BP,SP >0023 50 PUSH AX; Save all the registers >0024 53 PUSH BX; we will be modifying. >0025 51 PUSH CX; This is done automatically >0026 52 PUSH DX; only during FORTRAN CALLs, >; not interrupts. >0027 FF 06000 R INC K; K K +1, count another >; time step. >002B CLKEOI: >002B BA 0332 MOV DXNCLR; Timer Interrupt Clear >002E BO 00 MOV AL,0; Data could be anything >0030 EE OUT DXAL; Write to clear the interrupt. >0031 BO 20 MOV AL,EOI; Get end~of interrupt signal >0033 BA 0020 MOV DX,ICR; Send it to the 8259's >0036 EE OUT DXAL; Interrupt Command Register >0037 5A POP DX Restore registers >0038 59 POP CX >0039 5B POP BX >003A 58 POP AX >003B 8B E5 MOV SP,BP Restore framepointer >003D 5D POP BP >003E CF IRET Interrupt return. >003F ISR ENDP >003F CODEBIDS

226 The IBM Personal Computer Assembler 01-01-80 PAGE Symbols-1 >Experimental Work Code >Segments and groups: > > N a m e Size align combine class >CODE............. 003F PARA PUBLIC CODE >DGROUP............ GRCo P > DATA.............. 0000 PARA PUBUC 'DATA' > STEP$A............ 0002 PARA WORD ' $STEP >Symbols: > N ame Type Value Attr >CLKEOI............. LNEAR 002B CCOE >EOI.o........... Number 0020 >ICR............ Number 0020 >IMR.............. Number 0021 >INIT.............. FPROC 0000 CODE Global Length=0020 >INTCLR............. Number 0332 >OCOM.............. Number 033F >ISR.............. FPROC 0020 CODE Length =001F >K................ LWORD 0000 STEP$A >Waming Severe >Errors Errors > 0

226 The IBM Personal Computer Assembler 01-01-80 PAGE 1-1 >CLOCK ROUINE TO INMIAUZE COUNERS 1 AND 2 ~> ~TITLE CLOCK - ROUlTNETO INITAUZLE COUNTERS 1 AND 2 > PAGE,132;Set page width to 132 characters >; CLOCK is a routine which initializes Counter 1 and Counter 2 >;in the Labtender. Counters can be used to count and/or >;generate various signals. The frequency sources, signal >;outputs, and maximum counts are all programmable. ~~> ~~; Counter 1 will be used as a clock, generating pulses at a >; desired frequency. Counter 2 will count the number of pulses >;that Counter 1 generates. FORTRAN "CALL CLOCK" calls this > procedure. >; CONTER1: >; TERMCT and SOURCF control the frequency generated by >; counter 1. TERMCT, the terminal count for Counter 1, >; is the number of pulses Counter 1 will count from the source >; frequency before generating a pulse. Counter 1 counts down >; from TERMCT to zero repetitively. >.; SOURCF selects the source frequency for Counter 1 from the >; latener. NOTE THAT SOURCF ISNT THE ACTUAL SOURCE >; frequency; It is used to SELECT A FREQUENCY SOURCE > C; ONTHELABTBDER >; The Labtender has a 1 MHz crystal which generates 5 possible >; source frequencies, from 1 MHz (10^6) to 100 Hz (10^2). >; To choose a source frequency, we define SOURCF as the power >; of 10 of the desired source frequency from the Labtender for >; Counter 1. We generally need frequencies below 5 kHz, and >; higher frequencies have occasionally caused problems, so we >; will limit the source frequency to 100 kHz and the clock >; frequency to 5 kz. SOURCF must then be between 2 and 5. >; TERMCT must be between 2 and 32767. A program called >; CLKFRQ.FOR is availabe to selectTERMCT and SOURCF, and >; can be copied into your program and modified as desired. >; Counter 1 will generate a pulse on pin OUT 1 every time it >; counts TERMCT pulses. The output frequency of Counter 1 >; (the 'clock' frequency) is (1*1 0**SOURCF)/TERMCT. To >; generate a 500 Hz dock, choose 500 =10000/20 so SOURCF >; = 4 and TERMCT = 20, for example. >; The OUT1 pin can be used to generate timer interrupts. >; Enabling the IRQ4 interrupt will cause the timer interrupts >; to be sent to the processor. If we don't enable IRQ4 then >; the interrupts will be ignored. >; COUNTER2: >; Counter 2 counts the pulses generated from Counter 1. The

227 The IBM Personal Computer Assembler 01 -01 -80 PAGE 1-2 >CLOCK - ROUTINE TO INlAUZE COUNTERS 1 AND 2 >; assembler routine CLKOUT(CLKTIC) can then be used to read >; the value in Counter 2 and return it to FORTRAN as CLKTIC, >; the number of clock counts. >0000 DATA SEGMENT PUBUC DATA' >; For local assembler program data storage. >; Not used, but required for linking >0000 DATA ENDS >0000 COLNT$ASEGMENTCCM4CN $COULN >;This segment is equivalent to COMMON/COUNT/ >; TERMCTSOURCFi RTRN. > The $ and A are required to match the segment name >;and class that the linker uses for the FORTRAN common. > TERMCT and SOURCF must be declared INTEGER*2 in the >;FORTRAN calling routine before the common block. >0000???? TERMCT DW? >0002???? SOURCF DW? >0004 COLXNT$AENDS > DGROUPGRUP DATACNTA >; The DATA and COUNT$A segments will be linked into >; the group called DGROUP, to match Microsoft >; FORTRAN convention. >0000 CODE SE(3ENT PUBUC ODE > ASSLuECSCODEDS:DGROUPSSDGRUP > PUBUC CLOCK; Make the CLOCK label available >; to other segments. >; Labtender Addresses, in IBM PC, for the programmable timer. >= 0338 CLKDATA EQU 824; Data Port for 9513 Timer. >= 0339 CLKCOM EQU 825; Command Port for 9513 timer. >0000 CLOCK POCFAR >0000 55 PUSH BP; Save calling framepointer. >0001 8B EC MOV BP,SP >0003 B2 43 MOV DL,67; Print a 'C' to the screen >0005 B4 02 MOV AH,2; for debugging >0007 CD21 INT 21H; First reset all counters to zero. >0009 BA 0339 MOV DX,CLKCOM; Master Reset command.

228 The IBM Personal Computer Assembler 01 -01 -80 PAGE 1-3 >CLOCK - ROUTLNE TO INmALIZE COUNTERS 1 AND 2 >OOOC BO FF MOV AL,255 >OOOE EE OUT DXAL >;Set up the Master Mode Register. >OOOF BA0339 MOV DX,CLKCOM; Set Data Pointer to Master >0012 B0 17 MOV AL,23; mode regster >0014 EE OUT DXAL >0015 BA0338 MOV DX,CLKDATA; Write data to Master Mode >0018 BO 00 MOV AL,0; Register, lo-byte: >001A EE OUT DXAL; Use default values >001 B BO 80 MOV AL,128; Write data to Master Mode >; Register, hi byte: >001D EE OUTDXAL Use BCD division when dividing > frequencies. Ex: 1 MHz >; crystal/1 000 (base 10)=1000Hz > Set up Counter 1. >001E BA 0339 MOV DXCCOM; Disarm Counter 1, so we can >0021 BO C1 MOV AL,193; write to its control port >0023 EE OUT DX,AL >0024 BO 01 MOV AL1; Set Data Pointer to Counter 1's >0026 EE OUTDX,AL; Mode Register. >0027 BA 0338 MOV DX,CLKDATA; Write Data to Counter 1 Mode Register >002A BO 21 MOV AL,33; lo-byte, Count repetitively, reload; from load register, >002C EE OUT DX,AL; Count down, Terminal Count pulse higho > Manipulate SOURCF to create command to select source > frequency. By coincidence, (17 - SOURCF) will be correct > c;ommand. >002D 8B OE 0002 R MOV/CXSURC; PutSOURCFin CX. >0031 B80011 MOV AX,17; AX = 17 >0034 2B C1 SUB AX,CX A <- AX - CX. Now AL has command. >0036 EE OUT DX,AL; Write Data to Counter 1 Mode Register >; hi-byte: Count on rising >; edge, source frequency = 10**SOURCF > >; Put TERMCT in Counter 1's Load Register. TERMCT is the value >; from which Counter 1 will start counting down. >0037 BA 0339 MOC/DX,CLCOM; Set Data Pointer to Counter 1's Load >003A BO 09 MOV AL,9; Register. >003C EE OUT DXAL

220 The IBM Personal Computer Assembler 01-01-80 PAGE 1-4 >CLOCK - ROLJINE TO INALIZE COUNTERS 1 AND 2 >003D BA0338 MOV DXCLKDATA; Write data to Counter 1's Load Register. >0040 Al 0000 R MOVAX,TERMCT; Put TERMCT in AX. >0043 EE OUT DX,AL; Send lo-byte to Load Register. >0044 8A C4 MOV ALAH; Put hi-byte of TERMCT into AL >0046 EE OUT DXAL; Send hi-byte to Load Register. >; Set up Counter 2. >0047 BA 0339 MOV DXCLKCOM Disarm Counter 2, so we can >004A BO C2 MOV AL,194 write to its control port. >004C EE OUT DXAL >004D BO 02 MOV AL2; Set Data Pointer to Counter 2's >004F EE OUTDX,AL; Mode Register. >0050 BA 0338 MOV DX,CLKDATA;Write Data to Counter 2 Mode >0053 B0 2A MOV AL,42;Register, lo-byte: Count repetitively, >;reload from load register, >0055 EE OUTDXAL;Count up, square wave output. >0056 BO 00 MOV AL,0; Write Data to Counter 2 Mode; Register, hi-byte: >0058 EE OUT DX,AL; Count on rising edge, source >; -frequency = Counter 1. Each time >; Counter 1 =0 we get a pulse. >; Put zero in Counter 2's Load Register. >0059 BA 0339 MOV DXCLKCOM; Set Data Pointer to Counter >005C BO OA MOV AL,0; 's Load Register. >005E EE OUT DXAL >... >005F B8 0000 MOV AX,0; Put zero in AX. >0062 EE OUT DX,AL; Send lo-byte to Load Register. >0063 8A C4 MOV AL,AH; Put hi-byte of AX into AL. >0065 EE OUT DXAL; Send hi-byte to Load Register. >;Turn on Counters 1 & 2. >0066 BO 63 MOV AL,99; Command to Load and Arm >0068 EE OUT DX,AL; Counters 1 & 2 >0069 8B E5 MOV SP,BP; Restore framepointer >006B 5D POP BP >006C CB RET Return >006D GmClCKDP >006D CXOEBqS[ B\D

230 The IBM Personal Computer Assembler 01-01-80 PAGE Symbols-1 >CLOCK - ROUlNE TO INTAUZE COUNTERS 1 AND 2 >Segments and groups: > N am e Size align combine class >CODE...e.......... 006D PARA PUBLIC CODE' >DGROUP............. GRC P > DATA.............. 0000 PARA PUBLIC 'DATA' > COUNT$A............ 0004 PARA WORD $COUNT' >Symbols: > N a m e Type Value Attr >CLKCOM............. Number 0339 >CLKDATA............ Number 0338 >CLOCK..............o FPROC 0000 CODE Global Length=006D >SOURCF........... LVWORD 0002 COUNT$A >TERMCT............. LWORD 0000 COUNT$A >Waming Severe >Errors Errors > 0

231 The IBM Personal Computer Assembler 01-01-80 PAGE 1-1 >DISABL - ROUTINETO DISABLE ITERRUPTS > TITLE DISABL - ROUTNETO DISABLE INTERRUPTS > PAGE,132;Set page width to 132 characters. >; FORTRAN "CALL DISABL" calls this procedure. >0000 DATA SEGMENT PUBUC "DATA' >; For local assembler program data storage. >; Not used, but required for linking >0000 DATA ENDS > DGROUPGROUP DATA; The DATA segment will be >; linked into the group called >; DGROUP to match Microsoft >; FORTRAN convention. >0000 CODE SEGMBT PUBIC CODE > ASSUMECSXCODEDSDGROUPSS:GROUP > PUBUC DISABL; Make the DISABL label avai>; lable to other segments. >; Labtender Addresses, in IBM PC, for initialization routine. >= 0332 INTCLR EQU 818; Timer Interrupt Clear Address. >= 033D IOPORTB EQU 829; Parallel Port B Data. >; 8259 Programmable Interrupt Controller Addresses in IBM PC. >= 0021 IMR EQU 021H; Interrupt Mask Register. ~~> ~; Constants: >= 0000 TRAIL EQU OH >0000 DISABL PROC FAR >0000 55 PUSH BP; Save calling framepointer >0001 8B EC MOV BP,SP >0003 BA 0021 MOVDX,IMR >0006 EC IN AL,DX; Get contents of IMR. >0007 OC 10 OR AL,0001 0000B; Mask to block bit 4.1 =disable. >0009 EE OUT DX,AL; Reset the IMR. >OOOA BA0332 MOV DX,INTCLR; Clear timer interrupt, >OOOD BO 00 MOV AL,0; just in case its not already. >OOOF FA CLI; Clear interrupt flag >0010 EE OUT DX,AL; Write to clear the interrupt >0011 FB STI; Reset the interrupt flag >0012 BA 033D MOV DX,IOPORTB; To allow the stepper motor to be

232 The IBM Personal Computer Assembler 01 -01-80 PAGE 1-2 >DISABL - ROUTINETO DISABLE INTERRUPTS >; manually reset, we must leave the >0015 BO 00 MOV AL,TRAIL; output to it high, so we don't >0017 EE OUT DX,AL; complement (NOT) the low trail here. >0018 B2 44 MOV DL,68;Print a 'D to the screen >001A B4 02 MOV AH,2; for debugging >001C CD 21 INT21H >001E 8B E5 MOV SP,BP; Restore framepointer >0020 5D POP BP >0021 CB RET Return >0022 DISABLENDP >0022 CODE3DS > >! ~D >Segments and groups: > N am e Size align combine class >CODE........ O.... 0022 PARA PUBIUC OXDE >DGROUP............. GROC P > DATA.............. 000 PARA PUBLIC DATA>Symbols: > N ame Type Value Attr >DISABL............. FPRO 0000 CODE Global Length=0022 >IMR............... Number 0021 >INTCLR............. Number 0332 >IOPORTB............ Number 033D >TRAIL............. Number 0000 >Waming Severe >Errors Errors > 0

233 The IBM Personal Computer Assembler 01-01-80 PAGE 1-1 >ENABLE - ROU TNETO ENABLE NTE=RRUPTS > > TTLE ENABLE NETO ENABLE mNETONTERRUPTS > -PAGE,132;Set page width to 132 characters. > -; FORTRAN "CALL ENABLE" calls this procedure. > >0000 DATA SEGMENT PUBLC 'DATA' >; For local assembler program data storage. >; Not used, but required for linking >0000 DATA ENDS > DGROUOUPGO DATA; The DATA segment will be > ~; linked into the group called >; DGROUP to match Microsoft ~~> ~' ~; FORTRAN convention. >0000 CODE SEGMN PUBUC CODE > ASSUVMECSmODEDSDGROuPSDGROUP > PUBUC ENABLE; Make the ENABLE label avai>; lable to other segments. >; Labtender Address, in IBM PC. >= 0332 INTCLR EQU 818; Timer Interrupt Clear address. >;8259 Programmable Interrupt Controller Addresses in IBM PC. >= 0021 IMR EQU 021H; Interrupt Mask Register. >0000 ENABLEPROCFAR >0000 55 PUSH BP; Save calling framepointer. >0001 8B EC MOV BP,SP >0003 BA 0332 MOV DX,INTCLR; Clear timer interrupt, >0006 BO 00 MOV AL,0; just in case its not already. >0008 EE OUT DXAL; Write to clear the interrupt >0009 B245 MOV DL69; Print an 'E' to the screen. >00OB B4 02 MOV AH,2;for debugging >000D CD 21 INT 21H >OOOF BA 0021 MOVDXIMR >0012 EC IN AL,DX; Get contents of IMR. >0013 24 EF AND AL,11101111B; Mask to include bit 4. >0015 FA CLI; Clear interrupt flag >0016 EE OUT DX,AL; Reset the IMR. >0017 FB STI; Reset the interrupt flag

234 The IBM Personal Computer Assembler 01-01-80 PAGE 1-2 >ENABLE - REC)mNETO ENABLE INTERRUPTS >0018 8B E5 MOV SP,BP; Restore framepointer >001A 5D POP BP >001B CB RET; Retum >001C BEABLEEDP >001C OODEBIED EBD >Segments and groups: > N am e Size align combine class >CODE............ 001 C PARA PUBLIC CODE >DGROUP..........oo GROUP > DATA......... o 0000 PARA PUBLIC 'DATA' >Symbols: > Nam e Type Value Attr >ENABLE............. FPRO 0000 CODE Global Length=001C >IMR............. Number 0021 >INTCLR.......... Number 0332 >Waming Severe >Errors Errors > 0

236 The IBM Personal Computer Assembler 01 -01 -80 PAGE 1-1 >ENC- ENOODER EADNG ROUTNE > TTTLE ENC- ENCODER READNG ROUTNE >; This routine reads the contents of the counters >; of the second joints and sends their value in the >; variable 'enc2V' to the fortran code. > PAGE,132;Set page width to 132 characters. >0000 DATA SEGMENT PUBUC 'DATA' >; For local assembler program data storage >; not used here, but required for linking purposes. >0000 DATA ENDS >0000 ENC8i$ASEGMBETCOMMON$aCBL' >; This segment is equivalent to COMMON/ >; ENCBLKICJ1,CJ2,ENC2V >; in FORTRAN. The $ sign and A are required to >; match the segment name and class that the linker >; uses for the FORTRAN common statement ENC2V, >; CJ1 and CJ2 must be dedared INTEGER*2 in the >; FORTRAN calling routine before the common block. >0000???? CJ1 DW? >0002???? CJ2 DW? >0004???? ENC2V DW? >0006 BENCL$A ENDS > DGROUPGRUP DATAENCBL$ >; The data and ENCBLK$A segments will be >; linked in the group called DGROUP, to match > ICR; OSOFT FORTRAN cxnvenion. >0000 CODESEGMENTP BUUCODE > -ASSU JE CSXCODESDGROUPSS~GROUP,~> PUBIC LP ~> ~ PUBUC ENC >; make ENC label available to other segments > SUBTTLREADiGOFTHE ECOOER; — >= 0330 SLOTNO EQU816 >= 033F ENC EQU SLOTNO015 >= 033D OUTPORT EQU SLOTNO+13 >0000 ENPROCFAR >0000 55 PUSH BP; save calling framepointer >0001 8B EC MOV BP,SP; on the stack. >0003 50 PUSH AX; save AX register. >0004 53 PUSH BX; save BX register.

236 The IBM Personal Computer Assembler 01-01-80 PAGE 1-2 >EEN- BEODER EADNGF OUTE > READING OF THE ENCODER >0005 52 PUSH DX save DX register. >0006 BA 033F LP: MOV DX,ENCM;setting the i/o ports by writing >0009 BO 90 MOV AL,090H;a value to AL according to the >000B EE OUT DXAL labtender specifications and >;sending the latter to the control > port through the DX register. >; This will determine which port > is to be used as input port and > which to be used as output port >000C BO 08 MOV AL,08H strobing the counters by sending >OOOE BA 033D MOV DX,OUTPORT a high signal to bit 4 through the >0011 EE OUT DX,AL; output port. Physically, this will >; lock all the counters, mount their > c;ontents on the latches and wait >0012 8B 1 E 0002 R MOV BX,CJ2;get the address of the counter to >0016 8A C3 MOV AL,BL; be read. Mask off high byte since >0018 EE OUTDX,AL; only low byte is used. Send it >0019 4A DEC DX through the DX register. Move to >001 A EC IN AL,DX the input port. Read the counter. >001 B 24 OF AND AL,OFH Since only 12 bits are assigned to > joints 2. Therefore, the 4 most > significant bits of the upper byte > of joint 2 word must be masked. >001 D 8A EO MOV AH,AL; Save the counter reading in AH. >001 F 42 INC DX; Move back to the output port. >0020 8B 1 E 0000 R MOV BX,CJ1; Read the address of the low byte > in joint 2 counter word. >0024 8A C3 MOV AL,BL; Mask off high byte since only >; low byte is used. >0026 EE OUT DX,AL; Send it >0027 4A DEC DX; Move to the input port. >0028 EC IN AL,DX; Read the content of the counter in AL. >0029 2D 0412 SUB AX,0412H; Set up the origin of the theta motion. >002C A3 0004 R MOV ENC2VAX; Transfer the data to FORTRAN. >002F 58 POP AX restore registers >0030 5B POP BX >0031 5A POP DX >0032 8B E5 MOV SP,BP;restore framepointer >0034 5D POPBP >0035 CB RET; return >0036 BECBDP >0036 COODE3SEN > I BD

237 The IBM Personal Computer Assembler 01-01-80 PAGE Symbols-1 >ENC- ENOODER EADiNG ROUINE >Segments and groLps: > N ame Size align combine class >CODE............ 0036 PARA PUBLC CODE >DGROUP............. G > DATA.............. 0000 PARA PUBUC 'DATA' > ENCBL$A............. 0006 PARA WORD $ENCBL' > >Symbols: > N ame Type Value Attr >CJ1............. LWORD 0000 ECBL$A >CJ2........... LWORD 0002 ENCBL$A >ENC............... FPROC 0000 CODE Global Length >ENC2V............. LWORD 0004 ENCBL$A >ENCM.............. Number 033F >LP.......... LNEAR 0006 CODE Gbbal >OUTPORT..... Number 033D >SLOTNO............. Number 0330 >Waming Severe >Errors Errors > 0

238 The IBM Personal Computer Assembler 01-01-80 PAGE 1-1 > ~~> ~; ANALOGTO DKTrAL ONVERSION FRINE >0000 DATA SEGMENT PUBUC 'DATA' >; For local assembler program data storage >; Not used here, but required for linking >0000 DATA ENDS >0000 ADCBL$A SEGMENT COMMON $ADCBU >; This segment is equivalent to COMMON/ADCBL >; /ADCHN,ADCVAL >; in FORTRAN. The $ and A are required to match the segment >; name and dass that the linker uses for the FORTRAN common. >; ADCHN and ADCVAL must be declared INTEGER*2 in the >; FORTRAN calling routine before the common block. >0000???? ADCHN DW? >0002???? ADCVAL DW? >0004 ADCBL$AENDS > DGROUPGROUPDATAADCBl$A >; The DATA and ADCBL$A segments will be >; linked into the group called DGROUP, >; to match Microsoft FORTRAN convention. >0000 CODE SEGMENT PUB3LCODE > ASSUME CS.CODE,DS:DGROUP,SS:DGROUP > PUBULC ADC; Make ADC label available >; to other segments. > SUBTTL A/D CONVERSION > -------------- --------------------------- >; ADCHN is passed to ADC from the calling >; program, and ADCVAL >; is returned through the COMMON/ADCBLADCHN, >; ADCVAL bobd >; FORTRAN "CALL ADC" calls this A/D procedure > 0330 LABTR EQU816 AddressofLatenderin >; IBM-PC memory >= 0331 ADDATA EQU 817; A/D Converter data register

239 The IBM Personal Computer Assembler 01 -01 -80 PAGE 1-2 > A/D CONVERSION > >0000 ADCPROC FAR >0000 55 PUSH BP; Save calling framepointer >0001 8BEC MOV BP,SP; on the stack. >0003 BA 0330 MOV DX,LABTDR; Indirect addressing using >; DX will have to >; be used when using IN >; and OUT commands. >0006 Al 0000 R MOV AXADCHN; Get channel number >0009 B4 00 MOV AH,0; Mask off high byte, we >; only use AL >;Now create the proper command byte for the Labtender's A/D >;routine,based on the channel number. Use the AL register. >OOOB DO EO SHL AL1; Shift ADCHN left 1 bit >.; (from bits 0-2 to 1-3) >OOOD 24 3F AND AL,3FH; Mask off bits 6 and 7 >OOOF 0C 01 OR AL,1; Put a 1 in bit 0 to start conversion >0011 EE OUT DX,AL; Send command to ADC command port > Now create a byte which will match the value in the > Labtender's status port when the A/D conversion is complete. >0012 24 FE AND AL,OFEH; Mask off error bit (bit 0), >; leave bits 1-6 >0014 OC80 OR AL,80H; the same so we're still >; looking at the same >; channel, and put a 1 in bit 7 >; (status bit -a 1 indicates that con>; version is complete). >0016 8A EO MOV AH,AL; Save the reference for >; 'conversion complete' in AH. >0018 CYCLE: >0018 EC IN AL,DX; Read status port. >0019 3A C4 CMP AL,AH; Conversion complete? >001B 75 FB JNE CYCLE; Wait until it is. >001D BA0331 MOV DX,ADDATA; Point to data port. >0020 EC IN AL,DX; Read data. >0021 B4 00 MOV AH,0; Clear hi-byte of AX >0023 2D 0080 SUB AX,128; Convert from unsigned; TOSIG[ED

240 The IBM Personal Computer Assembler 01-01-80 PAGE 1-3 > A/D CONVERSION >; 0 to 255 becomes -128 to 127 >0026 A3 0002 R MOV ADCVALAX Return A/D value to > calling program. >0029 8B E5 MOV SP,BP;Restore framepointer >002B 5D POP BP >002C CB RET; Reurn >002D ADCENDP >002D CODE[NDS

241 The IBM Personal Computer Assembler 01-01-80 PAGE Symbols-1 >Segments and groups: > N am e Size align combine class >CODE.............. 002D PARA PUBUC CODE >DGROUP............ GP > DATA.............. 0000 PARA PUBLUC 'DATA' > ADCBL$A............. 0004 PARA WORD '$ADCBL' >Symbols: > N ame Type Value Attr >ADC............... FPROC 0000 CODE Global Length=002D >ADCHN.............. LORD 0000 ADCBL$A >ADCVAL............ LWORD 0002 ADCBL$A >ADDATA............. Number 0331 >CYCLE.............. LNEAR 0018 CODE >LABTDR............ Number 0330 >Waming Severe >Errors Errors > 0

242 The IBM Personal Computer Assembler 01-01-80 PAGE 1-1 >DIGTALTO ANALOG CONVERSKONROLTNE T>TLE DK3ITALTO ANALOG CONVERSION ROLUTNE ~~~> PAGE,132;Set page width to 132 characters. >; This routine sends the digital to analog conversion value >; (DACVAL) for a speafied channel (DACHN). >; FORTRAN "CALL DAC" calls this D/A procedure. >000 DATA SEGMENT PUBUC DATA' >; For local assembler program data storage >; Not used here, but required for linking >0000 DATA ENDS >0000 DACBL$A SEGMENT COMMON $DAC BL >; This segment is equivalent to COMMON/DACBL/DACHN, >; DACVAL in FORTRAN. The $ and A are required to match >; the segment name and class that the linker uses for the >; FORTRAN common.DACHN and DACVAL must be delared >; INTEGER2 in the FORTRAN calling routine before the >; common bock. >0000???? DACHN DW? >0002???? DACVAL DW? >0004 DCBLENDS > DGROUP GROUP DATA,DACBL$A >;The DATA and DACBL$A segments will be >;linked into the group called DGROUP, > V match Microsoft FORTRAN convention. >0000 CODE SEGMBN PUBLCCODE > ASSLME CSCODE3S:DGROUPSSDGROUP > PUBUC DAC;Make DAC label available to other segments. >= 0334 DACNTL EQU 820;D/A control port location in memory >= 0335 DADATA EQU 821;D/A converter data register >0000 DACPROCFAR >0000 55 PUSH BP;Save calling framepointer >0001 8B EC MOV BP,SP;on the stack. >0003 BA 0335 MOV DX,DADATA;Indirect addressing using DX will have to >;be used when using IN and OUT commands. >0006 8B 1 E 0002 R MOV BX,DACVAL;Get D/A value >000A B7 00 MOV ' BH,0;Mask off hi byte

243 The IBM Personal Computer Assembler 01 -01 -80 PAGE 1-2 >DKGTALTO ANOG CONVERIN ROUiTNE >OOOC 81 C3 0080 ADD BX,128,Convert from signed to unsigned >;-128 to 127 becomes 0 to 255 >0010 A1 0000 R MOV AX,DACHN;Get channel number >0013 B4 00 MOV AH,0;Mask off high byte, we only use AL ~~> ~; Now create the proper command byte for the Labtender's D/A >; routine based on the channel number. Use the AL register >0015 24 OF AND AL,OFH;Mask of bits 4-7 >0017 OC 08 OR AL,8;Set bit 3 hi >0019 8A EO MOV AHAL;Save D/A command byte in AH >001B 8A C3 MOV ALBL;Get DACVAL (only lower byte) >001D EE OUT DX,AL;Load value into D/A data register >001E BA0334 MOV DX,DACNTL;Point to D/A control word >0021 8A C4 MOV AL,AH;Get D/A command byte >0023 EE OUT DX,AL;Start conversion >0024 8B E5 MOV SP,BP;Restore framepointer >0026 5D POP BP >0027 CB RET;Return >0028 ACB EDP >0028 OCDEB3D > BM)~~8\

244 The IBM Personal Computer Assembler 01-01-80 PAGE Symbols-1 >DIGKALTOANALOG ONVERSION ROUTINE >Segments and groups: > N a m e Size align combine class >CODE.......... 0028 PARA PUBLIC CODE >DGROUP............. G P > DATA.............. 0000 PARA PUBUC 'DATA' > DACBL$A............ 0004 PARA WORD $DACBL' >Symbols: > N a m e Type Value Attr >DAC.............. FPROC 0000 CODE Global Length= 0028 >DACHN.............. LWORD 0000 DACBL$A >DACNTL............. Number 0334 >DACVAL............. LWORD 0002 DACBL$A >DADATA............. Number 0335 >Warning Severe >Errors Errors > 0

BIBLIOGRAPHY 246

246 BIBLIOGRAPHY [1] Thompson, B.S., and Sung, C.K. "A Variational Formulation for the Dynamic Viscoelastic Finite Element Analysis of Robotic Manipulators Constructed from Composite Materials," Presented at the ASME Design and Production Engineering Conference, Dearborn, MI, September 1983. [2] Asada, Ho, Kanade, T., and Takeyama, I., "Control of a Direct Drive Arm," in Robotics Research and Advanced Applications, Book, W.J. (ed.), ASME Booklet, November 1982, pp. 63-72 3]1 Kuntze, H.-B., and Jacubusch, A., "On the Closed-Loop Control of An Elastic Industrial Robot," American Control Conference Proceedings, June 1984, pp. 1217-1223. [41 Zalucky, A., and Hardt, D.E., "Active Control of Robot Structure Deflections," in Robotics Research and Advanced Applications, Book, W.J. (ed.), ASME Booklet, November 1982, pp. 83-100. [51 Paul, R.P., Robot Manipulators-Mathematics, Programming and Control, MIT Press, Cambridge, 1981. [6] Vukobratovic, M., and Potkonjak, V., Dynamics of Manipulation Robots, Springer-Verlag, New York, 1982. [71 Brady, M., et al, Robot Motion: Planning and Control, MIT Press, Cambridge, 1982. [8] Turney, J.L., Mudge, T.N., and Lee, C.S.G., "Equivalence of Two Formulations for Robot Arm Dynamics," Report No. RSD-TR-3-82, Center for Robotics and Integrated Manufacturing, College of Engineering, University of Michigan, Ann Arbor, MI, December 1980. [91 Turney, J.L., Mudge, T.N., and Lee, C.S.G., "Connection Between Formulations of Robot Arm Dynamics With Applications to Simulation and Control," Report No. RSD-TR-482, Center for Robotics and Integrated Manufacturing, College of Engineering, University of Michigan, Ann Arbor, MI, November 1981.

247 [10] Winfrey, R.C., "Elastic Link Mechanism Dynamics," Journal o Engineering for Industry, Transactions of the ASME, B 93 (1971), pp. 268-272. [11] Sunada, W.H., and Dubowsky, S., "On the Dynamic Analysis and Behavior of Industrial Robotic Manipulators with Elastic Members," ASME Paper No. 82-Det-45. [12] Shabana, A., and Wehage, R.A. "Variable Degree of Freedom Component Mode Analysis of Inertia Variant Flexible Mechanical Systems," ASME Paper No 82-Det-93. [13] Turcic, D.A., and Midha, A., "Generalized Equations of Motion For The Dynamic Analysis of Elastic Mechanism Systems," Journal of Dynamic Systems, Measurement and Control, Transactions of the ASME, Vol. 106, December 1984, pp. 255-260. [14] Turcic, D.A., and Midha, A., "Dynamic Analysis of Elastic Mechanism Systems. Part I: Applications," Journal of Dynamic Systems, Measurement, and Control, Transactions of the ASME, Vol. 106, December 1984, pp. 249-254. [15] Turcic, D.A., Midha, A., and Bosnik, J.R., "Dynamic Analysis of Elastic Mechanism Systems. Part II: Experimental Results," Journal of Dynamic Systems, Measurement and Control, Transactions of the ASME, Vol. 106, December 1984, pp. 255-260. [16] Wielenga, T.J., "Simplifications in the Simulation of Mechanisms Containing Flexible Members," Ph.D. Thesis, University of Michigan, March 1984. [17] Book, W.J., Maizza-Neto, 0., and Whitney, D.E., "Feedback Control of Two Beams, Two Joint Systems with Distributed Flexibility," Journal of Dynamic Systems, Measurement and Control, Transactions of the ASMIE, Vol. 97, No. 4, December 1975, pp. 424-431. [18] Chalhoub, N.G., and Ulsoy, A.G. "Dynamic Simulation of a Flexible Robot Arm and Controller," Report No. RSD-TR-13-83, Center for Robotics and Integrated Manufacturing, College of Engineering, University of Michigan, Ann Arbor, MI, September 1983. [19] Book, W.J., "Recursive Lagrangian Dynamics of Flexible Manipulator Arms Via Transformation Matrices," Report No. CMU-RI-TR-83-23, The Robotics Institute, Carnegie-Mellon University, Pittsburgh, PA, 1983.

248 [20] Dubowsky, S., and Desforges, D.T., "The Application of Model-Reference Adaptive Control to Robotic Manipulators," Journal of Dynamic Systems, Measurement and Control, Transactions of the ASME, Vol. 101, No. 3, September 1979, pp. 193-200. [21] Horowitz, R., and Tomizuka, M., "An Adaptive Control Scheme for Mechanical Manipulators-Compensation of Nonlinearity and Decoupling Control," ASME Paper No. 80- WA/DSC-6. [22] Landau, Y.D. Adaptive Control Systems, Marcel Dekker, 1979. [23] Balestrino, A., De Maria, G., and Sciavicco, L., "An Adaptive Model Following Control for Robotic Manipulators," Journal of Dynamic Systems, Measurement, and Control, Transactions of the ASME, Vol. 105, September 1983, pp. 143-151. [24] Lee, C.S.G., and Lee, B.H., "Resolved Motion Adaptive Control for Mechanical Manipulators," Journal of Dynamic Systems, Measurement, and Control, Transactions of the ASME, Vol. 106, June 1984, pp. 134-142. [25] Donalson, D.D., and Leondes, C.T., "A model Referenced Parameter Tracking Technique for Adaptive Control Systems: Part I - The Principles of Adaptation and Part II - Stability Analysis by the Second Method of Lyapunov," Transactions of the IEEE on Applications and Industry, Vol. 82, No. 68, September 1963, pp. 241-262. [261 Gilbert, E., and Ha, IoJ. "An Approach to Nonlinear Feedback Control With Applications to Robotics," IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-14, No. 6, November/December 1984, pp. 879-884. [27] Sangveraphunsiri, V., and Book, W.J., "An Approach to the Minimum Time Control of a Simple Flexible Arm," in Robotics Research and Advanced Applications, Book, W.J. (ed.), ASME Booklet, November 1982, pp. 219-232. [281 Kahn, M.E., and Roth, B., "The Near-Minimum Time Control of Open-Loop Articulated Kinematic Chains," Journal of Dynamic Systems, Measurement and Control, Transactions of the ASME, September 1971. [29] Wang, S.S., and Sharma, A., "Dynamic Control Models of a Robot for High Speed and High Precision Assembly with Tolerance Constraints," Proceedings of the 1984 American Control Conference, Vol. 2, San Diego, California, 1984, pp. 872-877.

249 [30] Balas, M., "Feedback Control of Flexible Systems," IEEE Transactions on Automatic Control, Vol AC-23, No. 4, August 1978. [31] Takahashi, Y., Rabins, M., and Auslander, D., Control and Dynamic Systems, Addison-Wesley, November 1972, pp. 431-452. [32] Meckl, P.H., and Seering, W.P., "Active Damping in a Three-Axis Robotic Manipulator," Presented at the Ninth ASME Vibration Conference, Dearborn, MI, September 1983. [33] Meckl, P.H., and Seering, W.P., "Minimizing Residual Vibration for Pointto-Point Motion," ASME Paper No. 85-DET-17. [341 Usoro, P.B., Nadira, R., and Mahil, S.S., "Control of Lightweight Flexible Manipulators: A Feasibility Study," Proceedings of the 1984 American Control Conference, Vol. 2, San Diego, California, 1984, pp. 1209-1216. [35] Fukuda, T., and Kuribayashi, Y., "Flexibility Control of Elastic Robotic Arms and its Application to Contouring Control," IEEE Computer Society, International Conference on Robotics, Atlanta, March 1984. [36]1 Cannon, R.H., and Schmitz, E., "Initial Experiments on the End-Point Control of a Flexible One Link Robot," The International Journal of Robotics Research, November 1983. [37] Book, WoJ., and Majett, M., "Controller Design for Flexible, Distributed Parameter Mechanical Arms Via Combined State Space and Frequency Domain Techniques," in Robotics Research and Advanced Applications, Book, W.J. (ed.), ASME Booklet, November 1982, pp. 101-120. [38] Cannon, R.H., et al, First Annual Report of the Center for Automation and Manufacturing Science, Stanford University, November 1983. [39] Singh, S.N., and Schy, A.A., "Decomposition and State Variable Feedback Control of Elastic Robotic Systems," Proceedings of the 1985 American Control Conference, Vol. 2, Boston, MA, 1985, pp. 375-380. [40] Song, J.O., and Haug, E.J., "Dynamic Analysis of Planar Flexible Mechanisms," Computer Methods in Applied Mechanics and Engineering, 24 (1980), pp. 359-381.

260 [411 Meirovitch, L., Elements of Vibration Analysis, McGraw Hill, New York 1975. [42] Greenwood, D.T., Principles of Dynamics, Prentice Hall, New Jersey, 1965. [43] Young, D., "Vibration of Rectangular Plates by the Ritz Method," Journal of Applied Mechanics, Vol. 72, December 1950, pp. 448-453. [44] Young, D., and Felgar, R.P., "Tables of Characteristic Functions Representing Normal Modes of Vibration of a Beam," The University of Texas Publication 4918, July 11, 1949. [45] Meirovitch, L., Analytical Methods in Vibrations, McGraw Hill, New York, 1967, pp. 440-445. [461 Desai, C., and Abel, J., Introduction to the Finite Element Method, A Numerical Method for Engineering Analysis, Van Nostrand Reinhold, 1972. [47] Shigley, J.E., Mechanical Engineering Design, McGraw Hill, New York, 1977. [48] Marui, E., and Kato, So, "Forced Vibration of a Base-Excited Single-Degreeof-Freedom System with Coulomb Friction," Journal of Dynamic Systems, Measurement, and Control, Transactions of the ASME, Vol. 106, December 1984. [49] D'Azzo, J., and Houpis, C.H., Linear Control System Analysis and Design, McGraw Hill, New York, 1981. [50] Gear, C.W., "Ordinary Differential Equation System Solver," University of Michigan Computing Center Report, December 1974. [511 Owens, D.H., Multivariable and Optimal Systems, Academic Press, 1981. [52] Chen, C.T., Introduction to Linear Systems Theory, Holt, Rinehart and Winston, 1970. [53] Kailath, T., Linear Systems, Prentice-Hall, 1980. [54] Elbert, T.F., Estimation and Control of Systems, Van Nostrand Reinhold, 1984.

261 [55] Wonham, W.M., "On Pole Assignment in Multi-Input Controllable Linear Systems," IEEE Transactions on Automatic Control, Vol. AC-12, No. 6, December 1967, pp. 660-665. [56] Moore, B.C., "On the Flexibility Offered by State Feedback in Multivariable Systems Beyond Closed Loop Eigenvalue Assignment," IEEE Transactions on Automatic Control, Vol. AC-21, October 1976, pp. 689-692. [571 Chang, M.J., "Eigenstructure Assignment by State Feedback," Proceedings of the 1984 American Control Conference, Vol.l, 1984, pp. 372-377. [58] Rangan, K.V., "Position and Velocity Measurement by Optical Shaft Encoders," Robotics Institute, Carnegie-Mellon University, Pittsburgh, Pennsylvania, June 1982. [59] Desilva, C.W., "Motion Sensors in Industrial Robots," Mechanical Engineering, June 1985, pp. 40-51. [60] Gervater, W.B., "Basic Relations for Control of Flexible Vehicles," A.I.A.A. Journal, Vol. 8, No. 4, April 1970, pp. 666-672. [61] Holman, J.P., Experimental Methods for Engineers, McGraw-Hill, Third Edition, 1978. [621 Hayt, W.Ho, and Kemmerly, J.E., Engineering Circuit Analysis, McGrawHill, Third Edition, 1978. [63] Franklin, G.F., and Powell, J.D., Digital Control of Dynamic Systems, Addison-Wesley, June 1981. [641 Astrom, K.J., and Wittenmark,., Computer Controlled Systems: Theory and Design, Prentice Hall, 1984.

UNIVt:Hl IT ur MII"lItIuN III 901111111111111111111115 02651 8525III 3 9015 02651 8525