”WillllHllfil”WWII“IIIIHIIJHHIIIHIIIlllllilll 3 1293 10699 7376 ‘5" "“""‘? -~.' www.cra- I .. \M' a 3 .9‘ f. A ‘ ._, an > - _.. '“""’.. .u v15» v~.'»\nr.-~ I‘ ’ —..:*~. t ‘-'- - v- Ir." This is to certify that the dissertation entitled A MODIFIED LUENBERGER OBSERVER FOR THE INVERTED PENDULUM presented by Steve C. Southward has been accepted towards fulfillment of the requirements for Master of Science Mechanical Engineering degree in Date W MS U is an Affirmative Action/Equal Opportunity Institution 0-12771 RETURNING MATERIALS: lVlESI_l Place in book drop to LIBRARIES remove this checkout from .1—u-zjl-IL. your record. FINES will , be charged if book is returned after the date stamped below. A IODIFIED LUWBHGH (BSBVE FOR UK INVITED PENDULUI By Steve C. Sonthvard A.IBESIS Snbnitted to liehigan State University in partial fulfillnent of the requirement for the degree of NASTEB OF SCIENCE Department of Mechanical Engineering 1985 ABSTRACT A DDIFIED LUBE-Gm QSBVI FOR THE INVKTED PWDULUM By Steve C. Soathward The inverted pendulum. aside from being a fascinating eclassical control problem. has primary applications in the aerospace industry. This inherently unstable system has geometric as well as discontinuous coulomb nonlinearities. Presented here is an attenpt to apply linear control and observation theories. with modifications that include nonlinear coulomb forces. to the real system. The linear plant is shown to ad-it reductions which allow full state observation through cart position only. Computer simulation and actual system demonstration verify that stability is highly sensitive to the accuracy of the nonlinear danping characterization. DEDICATION To my grandmother. Laura Hoffman ii ACINOILEDGEIENT I would like to express my most sincere appreciation and gratitude to Dr. Clark Radcliffe for providing me with the benefit of his vast experience in real time control. His knowledge and friendship have made this research an enjoyable part of my academic experience. I would also like to extend many thanks to Dr. Suhada Jayasuriya. Dr. Steve Shaw. and Yossi Chait for their expertise and aid throughout my nasters study. Special thanks to Jeff flopwood for his friendship and his invaluable electrical engineering assistance. Finally. I would like to thank my parents Charles and Mildred. and my brothers David and Bob. for their constant love and support during my academic career. iii TABLE OF CONTENTS LIST OF FIGURES NOMENCLATURE INTRODUCTION PLANT IODEL LINEAR STATE FEEDBACK CONTROL LUENBERCER OBSERVER DESIGN EXPERIMENTAL RESULTS CONCLUSIONS REFERENCES APPENDICES A. Nonlinear model derivation Linearixation of system model LSF control gain algorithm Observer gain algorithm Root locus source code (LOCOS) Control gain source code (CALC) Observsbility calculations DIFFEO simulation function subroutines Parameter study Control routine source code Reduction of system order Transformation of coordinates iv Page vi 11 16 20 22 24 32 35 47 56 73 88 115 117 Figure Figure Figure Figure Figure Figure Figure Figure Figure Figure Figure Figure Figure Figure Figure Figure _ Figure 1. 2. A1. 11. 12. 13. I4. 15. 16. 17. LIST OF FIGURES The inverted pendulum system . Bond graph for the hybrid linearised plant model Inverted pendulum root locus Nonlinear system with LSF control law Nonlinear plant with modified Luenberger observer Nonlinear system with convergence conditions not met The experimental inverted pendulum setup Actual system output under LSF control Pendulum free-body diagram DC servo motor bond graph model Determination of Gyrator Constant Nonlinear damping parameters of DC motor Experimental Hysteresis Loops Damping terms from the energy equation Calibration of linear cart potentiometer sensor Calibration of Hall Effect transducer Power amplifier schematic diagram Page 10 14 16 18 25 27 74 76 80 81 83 84 85 I” l> fi @- IU I: In an l'fl IO 5. NOIENCLATURE (5:5) linearised system plant matrix (4x4) reduced order plant matrix (5x1) input vector (4x1) reduced order input vector equivalent angular viscous damping coeff. (pend.) equivalent linear viscous damping coeff. (cart) equivalent viscous danping coeff. (motor) (512) coulomb damping matrix (432) reduced order coulomb damping matrix (2x5) output matrix (2x4) reduced order output matrix equivalent pendulum length (Srl) coeff. matrix for LSF gain solution force applied to the cart coulomb damping force in cart motion coulonb dumping force in pend. motion (4x2) matrix of observer gains gravitational constant (5x5) coeff. matrix for LSF gain solution DC servo motor anmsture current equivalent DC servo motor armature inertia (1x5) matrix of LSF control gains vi ‘11 hi be we IQ D!) 54 Ed "’3 (3 b4) 54 (1x4) reduced order matrix of LSF control gains torque constant for DC servo motor electrical constant for DC servo motor DC servo motor armature inductance equivalent cart mass equivalent pendulum mass transfornation const. from armature to cart motion net nonconservative force associated with qj generalised coordinate for each degree of freedom equivalent DC servo notor armature resistance (4x4) coeff. matrix for CBS gain solution (4:1) coeff. matrix for CBS gain solution scalar kinetic energy function (1x1) input matrix scalar potential energy function actual voltage applied to the DC servo motor cart velocity (5x1) state vector: [x.v.9.m.i]r (4x1) reduced order state vector: [x.v.0.m]T (4x1) observed state vector (2x1) velocity state vector: [v.mJ'r cart position _(2x1) output state vector: [3.91T (2x1) observed output state vector pendulum position pendulum angular velocity coefficient of characteristic polynomial vii INTRODUCTION The inverted pendulum is one of the more fascinating classical control problems. The pendulum pivot is fired to a cart. allowing the pendulum to freely rotate with respect to the cart. 'hen the pendulum pivot is above its center of gravity. the system is at a stable equilibrium position. Inverting the pendulum. such that the center of gravity is above the axis of rotation. places the system in an unstable state. However. a control force can be applied to the cart to keep the pendulum in this inverted position. Stabilisation of the inverted pendulum involves determining the proper control force. In its simplest form. this classic problem has proven to be an excellent example problem for introductory stability and control study [10.14]. In its most complex form. this basic problem has many interesting and important practical applications. Probably the most notable application is in the aerospace industry. A similar control system is employed for attitude control of a space booster on take off [14]. Immediately prior to take off. the space booster is balanced in an upright position by the launch platform. It is free to fall over. just as an inverted pendulum will. unless some control force is applied at the base. Other practical applications that involve balancing mechanical systems in an unstable vertical position include missile guidance systems [3] and nobile robotics [7]. A laboratory inverted pendulum system was studied to test possible control schemes. The 15 inch pendulum. hinged to the cart. allowed only a planar motion. The 4 inch long cart was constrained. via two rails. to a linear motion of about three feet. The cart and pendulum positions provide two degrees of freedom. A flexible 'toothed drive belt is 2 connected to each end of the cart. through pulleys at each end of the rails. forming a continuous leap. One pulley shaft is driven by a 12 volt DC servo motor through a gear and chain connection. Control forces can be applied to the cart by applying a voltage to the DC motor. F —— M /'b2’fx ‘<\ ‘\.U\\ ‘\.U\\i‘\ “\ ‘\.“\l‘\ }——p x,v Figure 1: The inverted pendulum system The complete system. made up of the cart. pendulum. and motor. is modelled here by a fifth order model. The cart/pendulum subsystem. shown in Figure 1. has four coupled first order nonlinear equations of motion with a force input. Primary uonlinearities in these equations are the transcendental functions of the pendulum angle terms. The real system also exhibits coulomb damping primarily associated with the cart 3 motion. and this discontinuous noulinearity is included in the model. The DC servo motor is modelled by a second order linear system. Due to the assumed rigid coupling between the motor and the cart. the motor armature velocity is dependent on the cart velocity. There are only five independent states of motion. thus the system is fifth order. A A linear model is required to develop. control and observation algorithms. Using small angle approximations. and neglecting coulomb effects. the system model can be linearised about the unstable vertical position. It is this linear model which is studied in classical inverted pendulum control problems. Using the experimental system parameters in the linear model. the open loop poles are: (4801.7. -7.42. 53.7. 0.0. 5.4} This linear system has an unstable pole from the pendulum. and a rigid body pole from the cart. The large stable pole is from the DC servo motor. indicating that the motor dynnmics are much faster than the cart/pendulum system dynemics. The nonlinear system can be controlled with Linear State Feedback. designed to stabilise the linearised system. Theoretical simulations. as well as actual experimental demonstration. verify this result. A linear state feedback law produces a control voltage. in this case. which is a sum of proportional contributions from each of the five system states. Linear control theory provides an algorithm for detenmining the control gains to place the closed loop poles in desired locations. 4 Implementation of such a control law requires state estimation. or observation. The real experimental system readily provides two of the five required states. Direct measurements are made for the cart position (x) and the pendulum position (0). A simple reduction allows the DC motor state to be eliminated from the control law. Linear Luenberger observer theory can then be used to design an observer which will provide the remaining stater for the LSF control law. This linear observer must be modified for implementation on the real system. where the coulomb forces will act as disturbance inputs. PLANT MODEL The nonlinear equations of motion for the cart and pendulum were obtained through application of Lagranges equation. which is based on the interaction between kinetic and potential energy in the system. and the nonconservative forces present [12]. These equations contain geometric and discontinuous uonlinearities (Appendix A): L1-->lTfrT-‘NJZF-45(Dl‘--7T7FtD-J[F—‘*(:.4 Li kEJct I n l Lv L d -(mgd) I I 3 I .I. L J' l M m Figure 2: Bond graph for the hybrid linearised plant model This hybrid linear plant has the matrix representation: INC “ I As Is + fit 0 + Q: 38n(l') (6) IN I 9.. s. (7) where the linear matrices are defined in Appendix B. along with the complete linearixation procedure. LINEAR STATE FEEDBACK CONTROL Some control law is needed to stabilise the nonlinear system. A Linear State Feedback (LSF) law of the form: g'v‘gng (3) has been designed for the linearised system. and its effectiveness for implementation on the real nonlinear system investigated. The LSF control law must at least push the single unstable open loop pole. and the rigid body pole of the linear system. into the left half plane. In order to implement the relatively simple LSF control law. the four position and velocitiy states of the cart/pendulum. ,and the armature current in the DC motor must either be measured or calculated. Fivi feedback gains must be determined. one for each of the system states. Only a sero valued control gain would eliminate the need to observe a particular state. A root locus study was performed to investigate the possibility of eliminating one of the states by seroing a gain. thus allowing partial state feedback. The LOCOS program (Appendix E) was developed whereby the LSF gains could be interactively tuned to visually see the effect on the linear system poles. A set of partial state feedback gains were found which placed all poles in the left half plane. Figure 3 shows the root locus as the gains were taken from sero values one by one to~ their neainal values plains tho poles at (41.23.41;sz 0n the first branch of the root locus. the pendulum position gain (K3) was varied from 0 to 195.34. The pendulum velocity gain ([4) was then varied from 0 to 36.77. For the third branch. the cart position gain ([1) was varied from 0 to 19.1. 8 The final branch was obtained by varying the cart velocity gain (K2) from O to 19 .14. >01 mm was our in non: Locus PLOT I » E 'I I L A *A. AA— A A W m— 4 ‘ AAA ‘ L A—-—- a m A A . AAAAAAAA L41 1 \ i 0.000 «((1.3) < 195.34: t 0.000 «(1.4) < 3.77! [ First Branch Second Branch ll >IM WT LOCUS PLOT mOT LUCUS PLOT : . .--’. ........ ...-.AII-.-A-.AA --:¢I...........ILU‘ la a I MP t 0.0” (“1.1) < 19.0% 0.000 (11) .01). I I> (12) I04 ) I ID M) IN) where A.and B are the system matrices given above. z is the reduced state vector. and 1 is the system output vector. The matrix of unknown observer gains 9 is given by: 11 12 61 65 g - 62 G6 (13) 63 G7 64 GB. The solution procedure for placement of the observer poles follows along the same lines as for the LSF control gains except that the observer is a multiple input system. The Luenberger observer has both the actual cart and pendulum positions as inputs. The Luenberger observer design procedure involves solving for the observer gains in 5 such that the poles of [A-ED] are the desired observer poles. which should be to the left of the desired closed loop system poles [10]. Inltiple input systems require solving an underdetermined system of equations. Pole placement. in this particular problem. will only provide four equations. with eight unknown observer gains to solve for. An extra design constraint must be imposed on the system in order to obtain a unique solution. Typical imposed design constraints are norm minimisation of the gain matrix Q [11]. and eigenvector placement [13]. The extra design constraint chosen here is the elimination of the pendulum position sensor. Obaervability tests (Appendix 0) indicate that the system is observable with only the cart position as an input. It is not observable with only the pendulum position input. This is intuitively correct since the dynamics of the pendulum are not a function of the global location of the cart. whereas the cart dynamics are dependent on the pendulum position. Knowing the cart dynamics. the pendulum states may be estimated. Following the same design procedure for pole placement. utilising the design constraint: 13 GS = 66 3 G7 B GB 8 O (14) the solution for the remaining observer gains for a given system and desired set of observer poles. reduces to solving a linear system of equations given by: geoldg) . 1' (15) where the elements of S and I are functions of the desired observer poles and the reduced order system parameters (Appendix D). At this point. linear Luenberger observer theory has been used to design an observer whose states will converge to the linear plant states. The coulomb damping which exists in the actual system 'is not included in this observer. Some observation theories. such as Kalman filtering [10]. allow coulomb forces as nonlinear disturbances. but do not readily allow the elimination of one position sensor. A modified Luenberger observer can be designed which includes coulomb forces as disturbance inputs. Using the design constraint (14). the observer (11) has a single input. which is the cart position x. The modified observer will require two additional inputs from the actual system. and is given by: IN) IN) 3-4 +§y+§(z->+g..n(r> (16) The presence of coulomb damping terms in the modified observer will have no effect on the convergence of the observed states to the linear system states. since the coulomb damping terms are also present in the linear model. The two actual states in 3'. (sgn(v).sgu(u)). are also not readily available. Using the signs of the observed velocities. l4 (sgn(c).sgn(c)). this modified observer can be stabilised under certain strict conditions. This modified Luenberger observer was tested on the linear and nonlinear system models in a DIFFEO simulation (Appendix B). The hybrid linear plant. as well as the nonlinear plant were stabilised with this observer and linear state feedback. “3854.74 2H (DMD co TIlE (SEC) Figure 5: Nonlinear plant with modified Luenberger observer System stability is achieved under strict conditions. determined through the DIFFEQ simulations. First. all parameters of the observer must be very accurate in describing the system under observation. Convergence of the observer states. with coulomb damping present. is very sensitive to inaccuracies in the observer. model. The second 15 condition for convergence is that all initial conditions for the observer must be very close to the actual initial system states. (0354.74 50 - X I N 0 e 5 -50 V 3 -100 L T s .150 l l I l l 0.0 0.2 0.4 0.5 0.9 (.0 we (sat) Figure 6: Nonlinear system with convergence conditions not met The nonlinear plant is unstable with the modified Luenberger observer and the LSF control law. if any of these convergence conditions are not met. Figure 6 shows the nonlinear system response for a nonsero initial tracking error. EXPERIMENTAL RBULTS An existing laboratory inverted pendulum setup was used to test the theoretical control algorithms. The primary test stand included the cart and pendulum subsystem connected through a drive belt/gear train to the DC servo motor (Figure 7). A DEC LSI-11/23+ digital computer performed all real time data acquisition and control processing. Other hardware included two power supplies and a power amplifier to interface between the computer and the DC servo. Figure 7: The experimental inverted pendulum setup Control software was designed to run under all standard DEC single-user RT-ll operating systems [5]. he program is menu driven to facilitate the tasks required for testing and demonstration. Complete source listings and documentation can be found in Appendix J’. 16 17 There are four major tasks available to the user. The user must first define the origin of the coordinate system referenced by the computer. This task is important since the control algorithms are designed to drive the system to the coordinate system origin. where all states are sero. Another option allows the status of these user-defined sero positions to be checked. There are many gains and parameters associated with the various control algorithms implemented in the program. Initially. these gains all have default values. The gains option allows the user to check or change these values. Iith this feature. it is easy to see the effect of changing any gains. The default LSF control gains place the closed-loop poles at [-3.-4.-5.-6]. and the default observer gains place the observer poles at {-8.-8.-9.-9]. be final option allows the user to run any of the three control algorithms available. All three algorithms have a discrete nature. in the sense that they are implemented on a digital computer. In the general algorithm. voltages from the two position sensors are read and digitised. Using these digitised states. the remaining states are computed. Once all the states are known. a control voltage is computed through the LSF law. The control voltage is then applied to the motor. This process repeats at a user specified rate. or sampling frequency. ‘Of the observation schemes available. the easiest to implement uses finite difference derivative approximations to calculate the velocity states. Knowing the present and previous positions. velocities can be computed with respect to the sampling period. Increasing the sampling rate increases the accuracy of the simple derivative approximations. A maximu sampling rate of 190 Us. is possible due to the small number of 18 floating point operations performed in this algorithm. One advantage of this method is that control gains can be designed in the continuous domain. rather than in a discrete domain. The finite difference approximation with the LSF control law stabilizes the actual nonlinear system. as shown in Figure 8. Experimental Inverted Pendulum HLEzTESLDAT 10.0 -— 6 (deg) X (in.) TIME (seconds) Figure 8: Actual system output under LSF control The remaining two observation schemes use the modified Luenberger observer to calculate the unknown states. The first scheme uses the continuous observer system. which is a set of four first order differential equations. These equations are integrated by an Euler approximation in real time. and the algorithm is able to run at a maximum 175 Rs. The second continuous design. It also runs control algorithms stabilise the From nonlinear system model initial conditions which are order to track the real system. actual implementation. has been shown to stabilise the Luenberger observer scheme. this algorithm has stabilised the system. the user may switch over the Luenberger observer algorithm at the touch of a key. use the final states this first algorithm is turned on. calculated 19 observer is a discretisation of the at a maximum 175 Rs. Neither of these system. simulations. the observer must have very close to actual initial states in This problem is easily solved in the The first observation scheme (finite difference) real system. Upon choosing either Once to Both'observers finite difference from the approximations for the initial conditions. CONCLUSIONS Through computer simulation results. the nonlinear system model can be stabilised with linear control and the modified Luenberger observer. under certain strict conditions. All parameters must be well known. and the initial tracking error of the observer must be near sero. Stability of the simulated model is demonstrated under these conditions. Computer simulations also indicate that system stability is more sensitive to inaccuracies in the viscous and coulub damping terms. The results of a parameter study on cart damping (Appendix 1) indicate an inadequacy of the asst-ed damping mechanisms present. Data plotted from the hysteresis loOp experiments would have been linear if the assumed linear combination of viscous and coulub damping forces were correct. Based on the scatter of data points. the actual damping present in the cart motion is more complex than originally assumed. The nonlinear model. used in the computer simulations. and as a basis for the control and observer designs. is not an exact representation of the actual laboratory system. Since system stability. under linear observation and control. is sensitive to actual parameter values. stability of the real system will not be attainable utilising the modified observer design and LSF. The real system response under linear observation is similar to the response of a simulated system whose observer has erroneous parameters. Though the'actual system response cannot be directly compared to the 20 21 nonlinear model response. the similar outputs indicate that the problem lies in the nonlinear model representation of the actual system. A.more accurate representation of the damping mechanisms in the cart motion are needed for the nonlinear model. Then. based on this model. a new observation scheme can be developed. This investigation. theoretical and experimental. has raised a very important and interesting observation. Linear multiple input systems. where pole placement is desired. define an underdetermined system of equations. This is true for linear state feedback control and for linear Luenberger observer designs. There are an infinity of valid control gain solutions to place the poles at desired locations. If these linear control or observation theories are to be applied to a nonlinear system. some solutions may be better than others. Given several sets of gains which all place the same set of closed loop poles. some may provide stability for the nonlinear system. and some may not. The finite difference approximation. to compute the velocity states. has proven to be the most effective means of stabilising the inverted pendulum with linear state feedback control. This combined observation and control scheme appears to be insensitive to the nonlinear coulmb damping disturbances.‘ Though the effects of the coulomb forces can be seen in the actual system response. the pendulum remains in a stable limit cycle. REFERHICES 10. 11. REFERUGS Bechith. T.. Buck. N.. and Marangoni. R.. W. Third Ed.. Addison-Iesley. 1982. Case Center for Cuputer-Aided Design. W. College of Engineering. Michigan State University. 1984. Chobotov. V.. "Dynamic Stability of a Pendulous Missile Suspension Syn-I." WW. Vol. 29. pp. 276-282. June. 1962. Davison. 8.. "A Method for Simplifying Linear Dynamic Systus." W. Vol. AC-11. pp. 93-101. 1966. Eckhouse. R.. and Morris. L.. W. Second Ed.. Prentice-Ball. 1979. Blootro-Crnft Corporation. W M. Fifth Ed.. August. 1980. Renami. R.. Ieimer. F.. and Koosekanani. 8.. "Sue Aspects of the Inverted Pendulum Prcblem for Modeling of Locomotion Systems." WW. Vol- AC-18. pp. 658-661. Dec. 1973. humour. 1).. and Rounbors. 8-. WWW Junior. McCraw-Bill. 1983. Kokotovic. P. . O' Mal ley. R. . and Sannuti. P. . "Singul ar Perturbations and Order Reduction in Control neory - An Overview." Augustin. v61. 12. pp. 123-132. 1976. Kwakernaak. R.. and Sivan. R.. n m em . John Iiley and Sons. 1972. Lee. 0.. and Jordan. 0.. ”Pole Placement with Feedback Gain Coast: tints. " WW. pp- 130-190. Dec. ~197s. . . 22 23 12. Mierovitch. L.. W929. The lac-111m Company. 1967. 13. Moore. B.C.. "On the Flexibility Offered by State Feedback in Multivariable Systems Beyond Closed Leap Eigenvalue Assignment." W. (M. AC-21. 06:. 1976. pp- 689-692. 14. Ogata. K.. W. Prentice-Ball. 1970. APPEI DI CES APPENDIX A The complete inverted pendulum system consists of two primary coupled subsystems which are easily identified. The cart and pendulum make up the first subsystem. It is nonlinear and completely mechanical in nature. The second subsystem is a DC servo motor. and will be modelled here as a linear system. The servo motor provides a coupling between electrical and mechanical energy. Due to the -intrinsic differences between these two subsystems. the equations of motion for each can be derived separately. and then coupled together in the proper fashion to produce a complete set of system equations. Lagranges method is invoked to determine the system equations for the cart/pendulum system. This method is well suited for the gecmetric and discontinuous uonlinearities present in the system [12]. Lagranges equation including non-conservative forces is given by: (1 3T 31' 0|] — -—-.—- — — + — . Qj (J 8 1,2) (A1) Placing the datum line at the axis of the pendulum. and noting that the potential energy of the cart is invariant. the potential energy function describing the cart/pendulim subsystem is: U - (m g d) cos(0) (A2) 24 25 The kinetic energy of this subsystem is due to the velocity magnitudes of the pendulum and cart masses. T - - M v’ + - n v (A3) i _| x T: (damping torque) / Fr.(r~eac’r ion force) Figure Al: Pendulum free-body diagram. Using the coordinate definitions in the pendulum free-body diagrem of Figure A1. the magnitude of the pendulum velocity can be expressed as: v: = (v+i.>' + (y'.>‘ (m 26 Using simple trigoncmetric relations between the desired state variable (0). and the position variables (x..y-). the velocity can be written: V - v3 + 2dvucoa(0) + (d (n0)a (A5) 2 m Substituting (A5) into the original expression (A3). and rearranging terms yields the final expression for the kinetic energy of the system: 1 l T - - (l‘+ n) v’ + - m (d n)’ + m d v u cos(0) (A6) 2 2 The non-conservative forces acting on this subsystem are the external applied driving force on the cart. and the damping.forces. Lagranges method calls for formulation of the net non-conservative forces associated with each of the degrees of freedom. For this particular model. these net forces are given by: Q: - F - b,v - fx sgn(v) (A7) 09 II -b1 00 - f9 sgn(n) ' (A3) Evaluation of the derivatives of the potential and kinetic energy functions. and substitution of the results into Lagranges equation gives the two nonlinear system equations: (M111); + mecos(O) - mdu’sin(0) + b,v + “sgn(v) - F (ID) nd;c0s(O) + .03 - mgdsiu(0) + bgfl + fesgn(e) - 0 (A10) These two nonlinear equations represent the dynamics of the cart/pendultm subsystem. The force input to this system is provided by the DC servo motor. 27 Since a linear model for the DC servo motor will be used. a bond graph is the most efficient method to derive the defining equations for this subsystem. R:R R=b3 I I SE V Ail—AoY—Ai-AITF-E-Ai I; 1.5,)... I/ n Figure A2: DC servo motor bond graph model Note that since a force output (F) is desired from the motor. causality indicates that the model will only be first order. This is due to the assumed rigid coupling between the cart and the motor armature. The bond graph should therefore provide a force relation. and a single differential equation of motion for the motor. The force relation is given by: F - (k. n) 1 - (n’ I) 31' - (n'b.) v (111) and the equation of motion for the motor is given by: L (di/dt) - V - i R - (kg n) v (A12) 28 The force relation (All). can be substituted directly into) (A9). the first nonlinear system equation. Cubining this result with (A10) and (A12) provides three nonlinear system equations describing the complete pendulu system dynamics. (0+.+n‘1); + (na)3.o.(o) — (Id)e'sin(0) - (ktn)i - (b,+n’b.)v - fxsgn(v) (A13) (md).x.oos(0) + (Id')3 - (mgd)sin(0) - - bu) - fesgn(e) (A14) L (di/dt) - v - 1 0 - (k3 n) v (A15) Another important nonlinear characteristic of the real system is the saturation of the applied motor voltage. The DC servo power supply has a finite limit. which can be expressed by: I v I < 15.3 volts (A16) There are only three assuptions or approximations made in this model. he first is the use of a linear model to describe the DC servo motor. The second assumption is the rigid coupling between the motor armature and the cart. The third approximation is the characterisation of the nonconservative damping forces. These areas alone govern the accuracy of the model to describe the pendulum system dynamics. APPENDIX B A linear set of equations are required to develOp either a Linear State Fbedback control law. or a Luenberger Observer. The nonlinear system equations must be linearised about some desired operating point in the state space. This is most efficiently accomplished through small angle approximations. Linearisation of this type will remove all geometric uonlinearities. Only the discontinuous couloub uonlinearities will remain. These coulomb uonlinearities will be carried through in the linear model. creating a hybrid linear plant with a nonlinear part. Upon application of the small angle approximations to the system equations (A13). (A14). and (A15). the following set of equations result: (nu-+1.31); + («)3 + (b,+n'b.)v + fxsgn(v) -= (1...“ (31) (so); + (nd')3 - (now + O10 - fesgnhe) - o . (32) L (di/dt) - v - iR - (ran). (33) Solving equations (BI) and (B2) simultaneously for I and 0. combined with the definitions: i-v (114) (B5) produces a set of- linear equations which are suitable for matrix notation. The state space representation. including nonlinear coulomb 29 30 damping terms is given by: .30 ' As Is * Es H + 9s 983(1') (36) X ' II. X. (B7) Since some of the individual terms of the above matrices are lengthy expressions. a simpler notation has been adopted. The defining matrix equations are given by: Fx 0 1 0 O O x 0 0 0 d v 0 -A1 -A2 A3 A4 v 0 -C1 C2 sgn(v) —e-ooo1oe+ov+ooun(u) dt 00 0 A5 A6 -A7 -B. 00 0 C3 -C4 _i o -o o 0 -A1 1 91. o 0 xi - 1 o o o o x o o o 1 o o v ‘ e U 1 The elements of the system matrix A are defined to be: A1 - (n‘b, + b,)/(u + .91) (3) A2 - (mg)/(M + n'J) (9) A3 - (11./6)“): + .31) (1110) M - (ktn)/(M + 6‘1) (811) A5 - (Al/d) (312) A6 - (g/d)(M + m + n’J)/(u + n’J) (313) A7 - (b./(-d’))(l + m + n’J)/(a + n’J) (n14) - (Inga/emu + 11’!) (B15) )9 - (inn/d) (316) A10 - (R/L) (B17) 31 The input matrix B has only one non-sero element. and is defined to be: P1 '- (1.0/L) (BIS) The coulab damping matrixg has four non-sero elements. which are defined to be: c1 - (ta/(10+ {1) (319) 62 - (rein/(0+ n’J) (320) c3 -_ Clld ' (331) C4 - “alumna + + n’J)/(a + 6’1) (322) This particular notation was chosen because of the significant umber of sero elements in the matrices. All of the newly defined parueters (matrix elements) are functions of the original set of system parameters. All elements have a positive value. characteristic of a parameter. The signs of individual terms in the system equations r-ain in the matrix definitions. APPENDIX C The probl- of controlling the linear system with state feedback translates to one of choosing the five control gains 5, such that the poles of [A-fl], are in the left half plane. where A and B are the fifth order linear system matrices derived in Appendix B. Equation (C1) is the characteristic polynomial whose roots are the five desired closed loop poles: 8’ + t..‘ + {.6’ + t..' + tn + g. - 0 (c1) Th. coefficients {i are functions of the desired closed loop poles. and represent constant values for a known set of poles. he actual system characteristic polynomial is obtained by taking t1" determinant 0‘ I'l'IA’Elng. 'DIe coefficients of this fifth order polynomial are functions of the eluents of the A“ 3,. and 5. matrices. Equating these coefficients to those of the desired closed loOp polynaial produces five equations in the five unhiown control gains of I... These relations are given by: 32 33 I. ' P1KI(A2!B:-A4A6) (C2) 6. - P1(K1(A4A7-A3A8) + amass-MM) + K3(A4A5-A1A8) + K5(A2A5-A1A6) + AlO(A2A5-A1A6) + 10 (Ans-MM) (c3) {3 . P1(K1(A4) + K2(A4A7-A3!8) + K4(A4A5-A1!B) + K5(A1A7-A3A5-A6)) 4' A10(A1A7-A3A5-A6) + 3(A4A7-A318) - K3(A8) + (A2A5-A1A6) (C4) 1:. - p1(r2(A4) - nus) + KS(A1+A7)) + A1(A7+A10) + A7A10 - A3A5 - A6 + MD (CS) 6. - P1(K5) + (A1+A7+A10) (C6) Th. LSF 0011““ 841” I. can be obtained by rearranging equations (C2) through (C6). and solving the equivalent linear matrix equation: 55? - F , (M) where the 3 matrix is given by: 0 0 O O 1 0 (A4) 0 ('3) (A1+A7) g - Pl (A4) ('A4A7-A33) PB) (MAS-A13) (A1A7-A3A5-A6) (MA'I-ASE) (A2ASr-A4A6) (MAS-MAB) 0 (A2A5-A1A6) (A2.B.-A4A6) 0 O 0 0 and the 13 matrix is given by: t. - (A1+A7+A10) I. - (A1(A7+A10)+A7A10-A3A5-A6+MD) E ' {3 - (A10(A1A7-A3A5-A6)+D (MA7-A3IB)+(A2A5-A1A6)) (g, - (A10(A2A5-A1A6)+D (A218rMA6)) I. ' 0 Given numerical values for the desired set of closed loop poles. and the system parameters. (C7) can be solved for the stabilising control gains 5. in the linear state feedback law. To eliminate the need for measuraent of the armature current state. the fifth control gain K5 must be be identically sero. For this 34 to be possible. all five of the linear equations in (C7) must still be satisfied. Equation (C6) is the only one containing the single control gain K5. Setting this current gain to sero. the remaining equation is: 94 = (A1 + A7 + A10) (CS) By choosing the desired set of closed loop poles such that (C8) is 34t13fiid- C. will take on a value that will force the current gain to be sero. The algorithm above is implemented in the program CALC (Appendix F). In that program. only two sets of desired closed loop poles are considered: "1 - I-“ 0-630-03 s-age-ug} (0) I]; . {-agtjpg s-Cgtjpgs'ag} (C10) For each of these sets. the respective values of {g are: [:1]; - a! + a; + a. + a. + a, (C11) [6.], - 2(o, + a.) + a. (C12) The user must specify four desired closed loop poles from one of the above sets. no fifth pole is then computed such that ((3) will be satisfied. By choosing the computed fifth pole. the current gain is forced to be identically sero. APPENDIX D A linear Luenberger Observer can be designed for the linear part of the reduced fourth order model derived in Appendix K. The complete fourth order model is given by: O 0 O x O x d v 0 -A11 -A2 A3 v A13 -C1 C2 sgn(v) - 0 - 0 0 O 1 O + 0 V + 0 0 sgn(e) dt 0 0 A12 A6 -A7 0 -A14 C3 -C4 Neglecting the coulomb damping terms. a linear Luenberger observer for full state observation will have the form: A A A .1 - A.l + 5 Q + 9 (x - 1) (31) A A x- 21 mm where A and Q are the system matrices given above. and the matrix of unknown observer gains 9 is given by: (D3) m I 9383 8388 The observed states can be made to converge to the actual linear system states by choosing the poles of [Argn] to be to the left of the closed loop system poles. The observer gain solution procedure is similar to the LSF control gain solution found in Appendix C. 35 36 First the characteristic polynomial of [A-@] is obtained. 'Dle coefficients of this equation can be written in terms of the system par-eters. i.e. the elements of A. D. and the observer gains 9. For a desired set of observer poles. the characteristic polynuial will have the form: s‘ + {'3’ + {33' + {13 + g. - 0 (D4) Equating the coefficients of the two characteristic polynomials: t. - (A11+A7) + (01+G7) (35) t. - (A11+A7)(61+G7)+(G1G7+A11A7)-(A12A3+6466)+((B+Gz-A6) (1)6) t. - (Gl+A11)(A707+(B.-A6)+01A11(A7+G7)-A12A3(01+G7)+G4(A3-65H Al2(A2+C6)-@ (A2+G6+G5 (A11+A7) )+62 (A7+G7) (D7) t. - (O1A11-I-02)(A267+0:-A6)+(A1261-O4)(A2+G6-A307)- 03 (A3 ((3rA6-O5A12)+A7(A2+G6+A1105) )-05 (O4A11+G2A12) (D8) These four equations must be solved for the eight observer gains. Since the system of equations is underdetermined. four of the gains are arbitrary [10]. From observability calculations (Appendix C). it was determined that the linear system is observable with only the cart position as input. Since they are arbitrary. the four gains associated with the pendulu position may be set to sero: GS=G6=G7=(B.-0 ([9) Applying (ID) to equations (D5) through (IE). produces a linear set of equations in the unknown gains of g. having the form: _8_ col;(§) . I (D10) 37 where the 8 matrix is given by: 1 o o o s, - (A11+A7) 1 o o (1)11) (AllA7-A3Al2-A6) (A7) (-A2) (A3) (A2A12-A11A6) (-A6) (A3A6-A2A7) (-A2) and the T'matrix is given by: t. - (A11+A1) .1.‘ - t. - (A11A7-A3A12-A6) (312) t. - (A2A12-A11A6) to It is interesting to note that. given a real set of system parameters. there is a unique solution for the observer gains of (D10). If the cart position measurement had been eliminated instead of the pendulum position. a new system of four equations would have been produced which has no unique solution. This result is as predicted by the observability tests. The algorithm for determining the observer gains ‘9 reduces to solving the linear system of equations (D10). This is implemented in the program CALC (Appendix F). All that is required on input are the system parameter values. and the desired observer pole locations. The only desired set of observer poles considered here is: n - Pa..—a..-a..-a.} (n13) 38 For this particular set of poles. the coefficients of the characteristi c polynomial are: I. - u, + o, + u, + a. (D14) §. - ago, + (o; + a.)(u. + d.) + o,o. (D15) {1 - o,o,(u, + o.) + u,u.(o, + o,) (D16) §. - a. (2,3, a. (D17) Substituting numeric values for these coefficients and the system parameters into the matrices of (D10). a solution for the observer gains can be obtained with a linear algebra subroutine. APPENDIX E A root locus program was developed as an interactive tool to study pole placement for linear state space form systems with linear state feedback implemented as the control scheme. The root locus study performed in this research was based on a very simple algorithm given the pre-existeuce of several canned software packages. The user must input the A (system). B (input). and K (control gain) matrices defining the linear system. Varying a single element of one of the three matrices generates a root locus as the eigenvalues of the new system matrix [A-BK] change. The FORTRAN 77 source code below was developed on the PRIME 750 computer system for use on the TEKTRONIK graphics terminals. at the MSU Case Center for Computer Aided Design. The Case Center Mathematical Library Subroutine package provides subroutines for the matrix manipulations [2]. The eigenvalues are computed with the EIGRF subroutine from the IMSL Eigensystem Analysis package [2]. After calculation of the eigenvalues. the root locus is plotted on the terminal screen via TCS graphic routines from the Thktronix PLOTlO package. 0000000 0 O GOO GOO GOG 4O PROGRAM LOCUS This is the main program to compute and plot the root locus for a matrix system with linear state feedback. Declare all variables REAL A09).B(9.9).K(9.9).G(9.9).VK(9) INTEGER N.M COMPLEI,EGV09) Format Statements 100 Forum/Jon. moor-Locus EVALUATOR roe MATRICES' . 4- /./.1ox.'nxm.1] - Alum] ‘ XINJI + BINJII a 001.11'. + I./.ISX.'UUI.1] - mom ' xtN.1]') 11o mauuhhsxnmm m3 311011431014 N: as) 120 FoaIAr(/.sx.'mm 1113 3100110310): M: at) 130 mauuloxnnrmsmu 1031‘ 33 LESS mm 1o.') Input the dimensions IRITE(1.100) 10 IRITE(1.110) READ(1.‘.ERR-10) N IF (N .08. 10) TEEN IRITE(1.130) GOTO 10 ENDIF 20 IRITE(1.120) READ(1.‘.ERR-20) M 1F (M .68. 10) TEEN IRITE(1.130) GOTO 10 ENDIF -Go on to the main program CALL INPUT(N.M.A.B.K.O.IK.mV) OOGOGOO GOO 41 SUBRwTINE INPUT(N.M.A.B.K.G.IK.MV) This subroutine accepts the input matrices A. B. and K. and also the matrix element to vary between the specified limits. Declare all variables RENL A(N.N).B(N.M).K(M.N).LL.UL.UL.G(N.N).VK(N). + KMIN.KMAK.TMIN.TMA1 INTEGER N.M.1.J.IP.JP 001111.111 EOV(N) CHARACTER MAT‘1.COMMNDN9 COMMON [BLOCK] KMIN.XMAK.TMIN.TMAX ---Format Statements 7oo roaIA1'(/.I.sx.'n0m 1113 1111.10] MATRIX. . J) 710 maur(/.sx.'u0m 1113 3110.11] “111111. . J) 720 301111A'r(/.sx.'n(m 1113 1111.101 unn. . J) 730 mauruoxuuunn. .11.') - '.S) 740 11001111'(1ox.'3('.11.'.'.n.') - at) 750 mauuuoxuu'. .'.'.11.') - at) 760 3011111'r(/./.sx.'o~11.1 01111 11.31111107' or 1113 ms- MATRICES' . + /.1OK.'A. 3. AND 1. MA! 33 named) 77o rouA1'(/.I.sx.'(noos1z 0103 or 11111 “1111333 (A.B.K): at) 730 roaaAr(/.sx.'rou 11111 cnoosa 1113 31.313111 '.A1."(1..t)') soo 301111A'r(/.1ox.'n0m 1113 110w 303111011 1: at) 310 roanr(/.1ox.'mm n13 001.3101 903111011 J: at) 1120 FORMAT(/./.5K.'EITER 1113 11110110111 VALUE or '.A1.'('.11.i) 330 BonuA'r(/.5x.'u~1m 1113 1111111111 VALUE or '.A1.'('.11.S) s40 30111111('.'.11.') : mi) 860 maIA1(/.I.sx.'mm 11111 murmur: mi) 370 1101111111(/.1ox.'(1) or (N) at) XMIN - -10.0 XMAX I 10.0 YMIN - -7.0 TMAI ' (XMAX-KMIN)‘7S0.0/1024.0 + YMIN -1nput the A matrix IRITE(1.700) IRITE(1.‘)' ' DO 30‘ I - 1.N DO 20 J I 1.N 10 IRITE(1.730) 1.1 READ(1.'.ERR-10) A(I.J) 2O CONTTNUE . lRITE(1.‘)' ' OGO nan GOO 42 30 NNTINUE ----Input the 3 matrix 'RITB(1.710) 'RI‘I'B(1.‘)' ' no 60 I I 1.N ll) 50 J I 1.! 40 'RITBUJW) LI READ(1.‘.HRI40) B(I.J) 50 MTINUB 'RITE(1.‘)' ' 60 CONTINUE Input the K matrix 62 "Influ'lzo, "ITB(1.‘)' ' m 80 I I 1.- m 70 J I 1.N 63 "ITE(1.750) 1.1 lEMHlu‘omI‘S) [(1.1) 70 “TIN“! 'IITB(1..)' ' 80 “TRIBE Choose the parameter to vary as II118(1.7601 9o n1111<1.170) am(1.'(A1)'.m-901 11x1 11? ((ur.un.'x').mn.(1u11.1~111.'11').mn.(1111.118.'A'n 601090 IRITB(1.780) MT 110 IRITB(1.800) READ(1.‘.FRRI110) 11’ IF (IP .L'I'. l) 6010 110 IF (“'1' .m. 'I') 111111 IF (IP .613. I) 6010 110 E88 IF (IF .G‘l‘. N) 6010 110 DIDIF 120 IRITB(1.810) READ(1.‘.ERRI120) JP IF (JP .L’l'. l) 6010 120 IF (HAT .110. '3') 1111311 IF (JP .GT. I) 0010 120 E83 IF (JP .G‘I‘. N) 6010 120 84an C ----6et tho limits on tho variablc parameter C 000 130 "1180320) IAT.IP 140 150 --—-Calculate and plot the root locus 160 170 IRI1'B(1.840) JP READ“ .‘. BRI130) LL IRITBU .830) HAT. IP 'RI1'B(1.8.40) JP READ“... 111111-140) 0].. IF (UL .L1'. LL) 6010 140 IRITB(1.860) RBAD(1.‘.ERRI150) DL I]. I ABSUJL) CALL QUNGUI. I. A. B. K. 6. 'K. EV. NAT. 11’. JP. LL. UL.DL) IRITB(1..)' 'RITB(1.‘)' IRITB(1.870) READ“. ' (D ) ' .ERRIlGO) COIAND N 100 'MT 10 VAR! A ND PARAETER. IF (commiu) .N8. 6010 85 HDIF “11'3“.” ' "118“,.) ' 'RI‘I'B(1.870) READ“. ' (D ) ' .ERRIl'IO) COIAND N 100 'AN1 10 INPUT A NH R IA'IRIX. IF (COIAND(l:l) .NB. 6010 62 ELSE IRITB(1.‘) ' IRI‘I'B(1.‘) ' OK. . . “118(1)? HDIF RETURN 'N') 111111 'N') mm 43 000006 000 000 44 SUBRW'I’INE (RUNCEUV. I. A.B. K. 6. IE. WV. HAT. IP. JP. LL. UL. 1]..) his is the main number crunching routine to calculate and plot the root locus. Doclare all variables REAL A(N.N).B(N.I).E(I.N).LL.UL.II..V0.VA. + XIIN.XIAX.YIIN.IIAX.6(N.N) .‘K(N) INNER N.I.IP.JP WHILE! FBV(N) .20 .9) GARACI'ER IAT'IJIAE‘P .mmnq 0011103 [BLOCK] XIIN.XIAX.YIIN.1IAX Format Statements soo FORRAT(I./.51."OULD 103 L113 10 NA]! 133 q_3113...'), 510 3033A1(/.sx.'33133 133 NE! FILENAIE: '.i) 520 301111A'r(/./.sx.'vom.n 103 L113 111 3331' 133 unwound) 530 3033A1(/.1ox.'(!) 011 (N) at) 540 301111A1‘(3x.33.2.9(2.x.:.'('.38.z.'.'.33.2.')')) 550 FORIIAT(/.SX.A1.'('.11.'.'.11.')'.101. + 'EIGBWALDES 0F TEE SISTER (REJI). . .'./) ---Calculate and plot the root locus 10 'RITE(1.500) IRITE(1.530) READU. ' (D ) ' .ERRIIO) COIAND 13 (003A33(1:1) .33. '1') 1333 20 13113(1.5101 33An(1.'(11)'.333-201 NAIE ELSE NAIB - 'q_Locus' 33313 * IF (NAT .311. 'K') 1331 V0 I [(IPJP) ELSE IF (IA‘I' .311. 'A') 13311 V0 I A(IP.JP) ESE V0 I B(IP.JP) BIDIF 0PM(5.FILEI'LOCUS.DAT') IRITE(5.‘)' ' 'RITE(5.‘)' ROOT LOCOS DATA NR IA'IRIX SYSTEI' IRI'I'E(5.550) IA1'.IP.JP : C C 25 30 35 45 1.113 I 0 COIAND I 'xxxxxxxxx' Do the graphics kind of stuff CALL INITIHSO) CALL 0PEN1'K(NAB.IER) CALL D'INDO(XIIN.XIAX.YIIN.YIAI) CALL AXIS(l.0.l.0.8) I!) 40 VA I LL.0L.DL IF (NAT .311. 'K') 13311 K(IP.JP) I VA ESE IF (HAT .311. 'A'113m A(IP.JP) I VA ESE E(IP.JP) I VA MDIF CALL “116.8. I.N.I.N) no 30 I I 1.N D0 25 J I 1.N 6”.” I AM.“ + 6(I.J) MTINUE CONTINUE CALL EIGRF(6.N.N.IJ(B.IV.Z.N.'E.IEE) 'RI‘I’E(5.540) VA. (EGV(I).II1.N) I!) 35 I I 1.N X I EEAL(EGV(I)) 1 I AIIA6(E6V(I)) IF ((1. 61'.XIIN) .MD. (X.L1'.XIAX) .AND. (Y.61'.1IIN).MD. (Y.L1‘.YIAX)) 13311 CALL IDVEA(X.Y) ’ CALL IDVRE(-3.-4) CALL GARTK(COIAND(I:I).0.6) RIDIF MTINUE 40 CONTINUE EEEEEEEEE I)VABS(90.730) GARTK( 'E001‘ LOCOS E01" .1 .2) IDVEA(0.0.YIAE) mVREUS #18) GIRTK( ' I" .1.0) UVEA(XIIAX.0.0) ”VERA-23 {'23) GAR1'K( 'EE' .1.0) IDVABS(25.20). ' 'RITE(COIAND. ' (F9 .3) ') LL 46 CALL GARTK(COIAND.1.0) CALL CEARTKU <'.l.0) CALL IDVRE(20.0) CALL GAR1'K(IAT.1.0) CALL CEARTK('('.I.0) IRITE(COIAND. '(Il)') IP CALL (BAR1'K(COIAND(1:1).1.0) CALL GAR1I('.' .l.0) 'RITE(COIAND. '(Il)') JP CALL CEARTK(COIAND(1:1).1.0) CALL CEARTK(') <'.1.0) IRITE(CONAND. ' (19 .3) ') UL CALL GARTK(COIAND.1.0) CALL DVABS(0.700) CALL (LOSTKUER) CALL INDDE CALL EOE (LOSEU) IF (IAT .m. 'K') 1331 K(IP.JP) I V0 ESE IF (RAT .311. 'A') TEN A(IP.JP) I V0 ESE B(IP.JP) I V0 31an Reset the window 50 IRITE(1.520) IRITE(1.530) READ(1. ' (D ) ' .ERISO) COIAND IF (COIAND(1:1) .m. '1') “EN 60 IRITE(1.I)‘ ' 'RITE(1.‘)' XIIN I SHIN IRITE(1.‘)' XMAS I HM IRITE(1.‘)' “IN I '.YIIN IRITE(1.‘)' TIA! I MINA! VRITE(1.‘)' ' 'RITE(1.‘)' BITER XIIN. HA1. AND YIIN. . . IRITE(l.‘)' ' READ(1.‘.ERI60) XIIN.XIAX.YIIN TIA! I (XIAX-XllN)‘780.0/1024.0 + THIN 6010 10 84an RETURN APPENDIX.F Tho Linear State Feedback control gains and the Luonberger Observer gains are all functionally dependent on the linoar system model parameters. The solution algorithms for both sets aro similar in form. For this reason. the single program CALC was written as an interactivo program to facilitate the computation of either set of gains. After supplying the basic system parameters. tho user may compute tho.control gains for either the controller. or tho observer. placing the poles of oach at user specified locations. Both tho Linear State Feedback controller and Luonberger Obsorvor aro formulations of linear theory. Tho solution for tho gains with tho indicated polo placement configuration reduces to that of solving a linoar system of equations. as dorivod in Appendicos C and D. The LINEQ subroutino. supplied by the Iatholatical Subroutine Library of the Case Center [2]. is used to perform these calculations. Below is an annotated sample interactive output showing tho control gains computed for the given input paramoters. and desirod closod loop pole locations for both the LSF controller. and the Luonborgor Obsorvor. The gain solutions hero aro tho dofault gains in tho implemented control routine. For clarity. underlined comments havo boon added to tho output. Following this output is the source code for CALC. 47 3111311 up: 0.0236 a 311m NC: 0-0385 Was). 311311 D: 1.2396 mm mm 31: 1.133—4 1: 311m 82: 0-025 WILL! mm 83: 2468-4 WW ENTER 1 = 5-28-5 WW mm L: 6-4078-3 MW 131m B: 1787 MW 1311311 KB: 0.0813 WM 311311 N: 32.0 Wad/.11.). mm F1: 0-0698 WW mm FT: 0.002 WWII). Al - 2.63326 A2 - 3.21600 A3 - 9.9351403-04 A4 - 20.9115 A5 - 2.16462 A6 - 32.6315 A1 - 3.9115103—03 A3. - 16.3695 19 - 406.056 1110- 2304.14 A11- 5.11011 1112- 4.60690 A13- 1 .1633» A14- 0 .9 33160 31 - 156.019 c1 - 0.160130 c2 - 1.1535333-02 ca - 0.613130 C4 . 6.9331523-02 (L)INEIR STATE FEENACX DESIGN - 03 - (0)38ERVER DESIGN DES IGN: L NEAT SET OF SYSTEM ROOTS II) 100 'ANT. . . l .) (-RR1+-J ‘R Il .-R82+-J‘RI2 .-RR3} 2.) {-RR1.-RR2.-RR3.-EEA.-RE5} 2 mm 3R1: 3 31m RR2: 4 BITER RR3: 5 3191311 R84: 6 FOR [SIG . . . RR5 I 2789.43 BITE RR5: 27$ .43 TEE GARACTERISTIC MEATION IS: 89‘5 : 1.0 S"4 : 2807.43 8‘” : 50328.7 SII2 : 332284. SIIl : 954345. S‘IO : 1.00419 5E+06 TEE K IATRII 0F GAINS IS: -11.85394 -16.17418 -175.72162 43.0349 -0.00175 IER I 0 (L) INEAR STATE FE-ACK DESIGN (0)38EV: DESIGN DESIGN: 0 MTER 1E8 QSERVE POLES . . . mm RR1: 8 mm RR2: 8 BITER RR3: 9 311311 REA: 9 TEE G MATRIX 0F GAINS IS: 28.285 303.973 -425.307 -2331.515 IER- 0 (5660606 0 50 PROGBAI CALC This program calculates tho LSF control gains for tho the 5th order pendulum and motor model. given tho system parameters as input. Declare all variables REM. IP.IC.D.G.B1.BZ.B3.J.L.R.EE.N.FX.FT REM. A1.A2.A3.A4.A5.A6.A7.!8.D .A10.A11.A12.A13.A14.Pl REM. RR1.RIl.RR2.R12.RR3.RR4.RR5 REM. xo.x1.n.13.14.01.m.13.01 REAL E(5.5).E(5).F(5).IORE(6.6) REM. S(4.4).GA(4).T(4).'ERK(5.5) GARACTER (DIANDQ ---INPDT TEE SYSTEI PARABTES 10 FORNAT(/.A12.3) IRITE(1.I)' ' IRITE(l.lO)' 31133 113: ' 333111..) 113 13113(1.10)' 31m 110: ' 333111..) 110 “11311.101' 31133 n: ' 3331(1)) 11 0 - 32.114 1311311.10)' 311133 31: ' 33331.9) 31 n113(1.10)' 31133 32: ' 33111111..) 32 1311311.1o>' 31133 33: ' READ(1.‘) 33 nmunor 31133 J: ' READ(1.‘) J “11311.101' 31133 L: ' 333111..) L "11311.101' 31m 333111..) 3 “11311.101' 31133 333111..) 33 13113(1.10)' 31133 READ(1.‘) :1 “11311.101' 31133 33mm.) 31 13113(1.1o1' 31133 33111111..) FT 3???? C C C C C 51 -—---CALGILATE TEE NA'IRII PARAETERS A1 I (N‘N‘E3+B2)/(NC*N‘N‘J) A2 I (NP‘G)/(NC+N‘N‘J) A3 I B1l(D‘(lC+N‘N‘J)) A4 I lE‘N/(l.356‘(NCi-N‘N'J)) A5 I (NIN'B3+B2)/(D‘ (NCO'N‘N‘JH A6 I (G/D)‘(NC+NP|'N'N‘J)/(NC+N‘N‘J) A7 I (Ell (D‘D‘NP) )‘ (NCtNPi-N‘N‘JHWGFN‘N‘J) M. I IE‘N/(l.356‘D‘(NCtN‘N‘J)) D I lE‘N/L A10 I R/L P1 I 1.0/L All I A1 + M‘U IA10 A12 I A5 + 39‘ IA10 A13 I M‘Pl/AIO A14 I BIPIIAIO C1 I FX/ (NGN‘N‘J) C2 I (FT/D)/(NC+N‘N‘J)) C3 I C1/D C4 I (FT/ (NP‘D‘DP (NCtNPtN‘N‘J)/(NC+N‘N‘J) IRITE(1.‘)' ' IRITE(1.‘)' A1 '.A1.’ A2 I '.A2 'RITE(1.‘)' A3 '.A3.’ A4 I '.M 'RITE(1.‘)' A5 '.A5.’ A6 I '.A6 IRITE(1.‘)' A '.A7.‘ B.I '.AS 'RITE(1.‘)' D '.D.’ A10. '.A10 'RITE(1.‘)' AllI '.A11.’ A12- '.A12 IRITE(1.‘)' A13I '.A13.’ A14I '.A14 IRITE(1.‘)' Pl I '.Pl IRITE(1.‘)' ' IRITE(1.‘)' Cl I '.C1.' C2 I '.C2 'RITE(1.‘)' C3 I '.C3.’ C4 I '.C4 'RITE(1.‘)' ' Q ----CE008E BET'EEI (ESEVER (I LSF DESIGN 50 IRITE(1.‘)' ' IRITE(1.‘)' (L)INEM STATE FEEDBACK DESIGN' 'RITE(1.‘)' - (I -' IRITE(1.‘)' (0)38EVE DESIGN' 'RITE(1.‘)' ' IRITE(1.10) ' DESIGN: ' READ(1.'(D)') WNAND IF (CONMID(1:1).N.'L') 1331 6010 100 ESE IF (CONAND(1:1).EQ.'0') 1331 6010 590 ESE IF (CONAND(1:1).m.'G') TEEN CALL EXIT ESE 6010 50. 52 HDIF C C ---GET TEE TYPE OF GMACTERISTIC R0013 C 100 IRITE(1.‘)' ' 'R1TE(1.‘)' ' IRITE(1.‘)' NEAT SET OF SISTEN R0013 IN) YOU WANT. . .' 'RITE(1.‘)' ' 'RITE(1.‘)' 1.) (IRR1+-J‘R11.-RR2+-J‘R12.-RR3)' 'RITE(1.‘)' ' 'RITE(1.‘)' 2.) (-RR1.-RR2.-RR3.-RRA.-RR5}' IRITE(1.‘)' ' READ(1.‘.ERRI100) ISET IF ((ISET.NE.1).RJD.(ISET.NE.2)) 6010 100 C 6010 (200.300) ISET C C ---SET 1: TIO mNPLEX PAIRS RID (NE REAL ROOT C 200 'RITE(1.‘)' ' IRITE(1.10)' BITE RRI: ' READ(1.‘) RRl IRITE(1.10)' BITER R11: ' READ(1.‘) R11 NR1TE(1.10)' BITER RR2: ' READ(1.‘) RR2 'RITE(1.10)' 311m R12: ' READ(1.‘) R12 IRITE(1.‘)' ' RR3 I (A1+A7+A10) '- (2.0‘(RR1+RR2)) IRITE(1.‘)' NR R5I0 . . . RR3 I '.RR3 'RITE(1.10)' EITER RR3: ' READ(1.‘) RR3 C C ----CAL(11LATE 13E (0EFFIC1E‘I13 OF CE C 01 I RRI‘RRI + R11'R11 02 I RR2‘RR2 + R12‘R12 10 I RR3 ‘ 01 ‘ 02 C 0.3 I 2‘(RR1‘02 4- RR2‘Ql) 11 I RR3 ‘ 0.3 + 01 ' 02 C 04 I 4‘RR1‘RR2 + 01 + 02 12 I RR3‘Q4 4’ Q3 C X3 I 2‘RR3‘ (RR1+RR2) + 04 I4 I RR3 + 2‘(RR1 4- RR2) c . 6010 400 C C ----SET 2: ALL REM. R0013 C 300 IRITE(1.‘)' ' 53 IRIT'E(1.10)' DITER RRl: ' READ(1.‘) RRl lRITE(l.10)' ENTER RR2: ' READ(1.‘) RR2 IRITE(1.10)' BITE RR3: ' READ(1.‘) RR3 IRITE(1.10)' BITER RR4: ' READ(1.‘) RR4 'RITE(1.I)' ' IRS I (A1+A7+A10) - (381+3324333+RR4) IRITE(1.I)' FOR [5-0 . . . RR5 I '.RR5 lRlTE(1.10)' BITER RR5: ' READ(l.‘) RR5 C C “CALCULATE TEE (DEFEICIENTS 0F CE C XO I RRl‘RR2‘RR3‘RR4‘RR5 X1 RR5‘ (RR3 IRR4‘ (RR1+RR2 )+RR1IRR2' (RR3+RR4) ) X1 X1 + RR1‘RR2'RR3‘RR4 X2 RR5‘ (RR1+RR2 ) " (RR3+RR4 )+RR3 ‘RR4‘ (RR1+RR2+RR5) X2 X2 + RRl'RRZ‘ (RR3+RR4+RR5) X3 RR5. (RR1+RR2+RR3+RR4 )+RR3‘RR4+RRI'RR2 X3 X3 + (RR1+RR2)‘(RR3+RR4) x4 331+332+333+334+335 C C -—-'R1TE (NIT TEE (BARACTERISTIC MUATION C 400 'R1TE(1.‘)' ' 'RITE(1.‘)' ' 'RITE(1.‘)' TEE GARACTERISTIC EQUATION IS: ' IRITE(1.‘)' ' IRITE(1.‘)' S”5 : 1.0' IRITE(1.‘)' 8‘94 : '.X4 'RITE(1.‘)' S”3 : '.X3 'R1TE(1.‘)' S”2 : '.X2 'RITE(1.‘)' S“1 : '.X1 'RITE(1.‘)' S‘IO : '.XO D0 550 I I 1.5 D0 500 R! I 1.5 3(I.Kx) I 0.0 500 001113103 550 CONTINUE C C ---CONPDTE TEE 815.5] NA‘lRIX C P1 Pl‘A4 -P1‘A8. P1‘(A1+A7) 3(2.2) . Pl‘ (MINI-.13 IAs) 3(2.4) P1. (Mus-Alt”) E(l.5) E(2.2) E(2.4) R(2.5) R(3.1) E(3.2) E(3.3) B(3.4) (500 GOO GOO GOO 000 54 E(3.5) I P1‘(A1‘A7-A3‘A5-A6) E(4.l) I E(3.2) E(4.2) I P1‘(A2‘A8IA4‘A6) E(4.3) I E(3.4) E(4.5) I Pl‘(A2‘A5-A1‘A6) E(5.1) I E(4.2) ”II-CONPDTE TEE F15] NAan EU) I X4 .. (A1+A7+A10) F(2) I X3 - (Al‘(A7+A10)+A7'A10-A3‘A5-A6+A4‘D) F(3) I X2 - (A10. (A1‘A7-A3‘A5-A6)+fl‘(A4‘A7-A3‘A8) + +(A2'A5-A1‘A6H F(4) X1 (A10‘ (A2'A5-A1‘A6)+0 ‘ (A2.“ :-A4‘A6)) F(5) X0 ---CONPO1E TEE GAIN NATRIX X15] CALL LINEQ(X.F.E.I(RX.5.6.1ER) 'RITE(1.‘)' ' 'RITE(1.‘)' TEE X NATRIX OF GAINS IS:' 20 FORNAT(/.2X.5(F10.5.2X)) IRITE(1.20) (X(1).1I1.5) 'RITE(1.‘)' ' IRITE(1.‘)' IER I '.IER 601050 m-CALCIJLATE ‘IEE GSEVE GAINS 590 'RITE(1.')' ' “11311.91' 31133 133 «131mm 1mm . . .' 1311311.~)' ' 131131130) ' 31133 331: ' 333111..) 3111 13113(1.10) ' 31133 332: ' READ(1.‘) 3112 13113<1.10) ' 31133 333: E 333111..) 333 n113u.10) ' 31133 334: ' 333111..) 334 W-CALCUAm 133 0033310131113 03 c3 X0 I RR1‘RR2‘RR3 'RR4 X1 I RRI‘RRZ‘ (RR3+RR4) + RR3‘RR4‘ (RR1+RR2) X2 I RR1‘RR2 + RR3‘RR4 + (RR1+RR2)‘(RR3+RR4) X3IRR1+RR2+RR3+RR4 “CALCULATE 111E S[4.4] NA’IRIX D0650 II 1.4 55 DO 600 [X I 1.4 S(1.XX) I 0.0 600 CONTINUE 650 CONTINUE C S(1.l) I 1.0 8(2.1) - All + A7 S(2.2) I 1.0 S(3.1) I A11‘A7IA3‘A12IA6 S(3.2) I A7 8(333) . “A2 S(3.4) I A3 S(4.1) I A2’A12IA11‘A6 S(4.2) I IA6 S(4.3) I A3‘A6IA2‘A7 S(4.4) I IA2 C C IIIIICALGILATE TEE T14] NATRIX C T(1) I X3 I S(2.1) T(2) I 12 - S(3.1) T(3) I 11 - S(4.l) 1(4) I 10 ' C C IIIIICONPUTE TEE GAIN NAIRIX 614] C CALL LINM(GA.T.S.'EX.4.5.1ER) C 'RITE(1.‘)' ' 'RITE(1.‘)' TEE 6 NA'IRIX 0F GAINS IS:' 30 FORNAT(/.2X.4(F11.3.2X)) 'R1TE(1.30) (GA(1).1I1.4) IRITE(1.‘)' ' 'RITE(1.‘)' IER I '.IER 6010 50 APENDIX G The observability of the linear fourth order systen nodel approximation derived in Appendix I. can be determined by exmnining the rank of the observability metrix 0 The transpose of tho systen nutrix ‘A is given by: 0 0 0 0 _A_T - 1 -A11 0 A12 , (01) 0 -A2 0 A5 0 A3 1 -A6 The transpose of the output matrix D is given by: 31 0 3T - o 0 (32) o 32 o o The nonxoro elements of the output matrix have been left as variables for analytical generalization. By constructing the observability natrix from those two matrices [14]. the observability can be determined us a function of the two variables D1. and D2. Fran-tho definition of the observability natrix. 0 can be written D1 0 0 0 0 0 0 0 Q I O 0 01 0 IA11D1 A12D2 (A11’+A3A12)D1 I(A12(A11+A6))D2 0 D2 0 0 IA2D1 A5D2 (A2A11+A5A3)D1 I(A2A12+A5A6)D2 0 0 0 D2 A3Dl IA6D2 I(A3(A11+A6)+A2)D1 (A3A12+A6‘+A5)D2 56 57 The rank of this metrix indicates the observability 01 the systel. It is easy to see that the rank is dependent on the variables D1. and D2. There are three cases of interest to investigate in this system. The first case will assume both the cart and pendulun positions will be measured. or equivalently: D1 I D2 I 1 . (63) For this case. the rank of the observability matrix is: rankIQ] I 4 (64) Ihen both position states are measured. the observability matrix has full rank. therefore the system is completely observoblo. In the second case. only the cert position will be measured and the pendulum position will be neglected: D1 I 1 (65) 32 I 0 (G6) Substituting these volues into the observability natrix. the rank is: ranle] I 4 (G7) The observability Intrix has full rank and thus is completely observable when only the cart position is evailable. This indicates that an observer may be designed to ostinato the full state vector given only the cart position as an input. 58 The final case allows the pondulum position as an input. and the cart position is neglected. or equivalently: 01 I’ 0 (68) D2 I: 1 (09) For this case. the rank of the observability matrix is: rank[Q] I 3 (610) Since the observability matrix is of rank 3. there is one unobservable state. This systel is not completely observable when only the pendulum position is available for input. APPENDIX R All computer sinulations of the system response were performed on the R118 750 computer system in the 180 Case Center for CAD. utilizing the DIFFEQ software package [2]. This package integrates a given set of coupled first order differential equations and provides a graphical output compatible with Toktronix terminals. The first order equations are coded into a function subroutine supplied by the user. There were six models studied in the simulations for stability analysis. and therefore six function subroutines. Those subroutines hold both fifth order and the equivalent fourth order models for the nonlinear as well as linearised systens. under linear state feedback control. Tho of the subroutines contain the equivalent fourth order nodels. linear and nonlinear. of the system as well as the nodified observer equations. The subroutine names and their relevant functions RIO: LIN4.74 . . . linear 4th order approximation w/direct LSF LIN5.74 . . . linear 5th order model w/direct LSF NON4.74 . . . nonlinear 4th order approximation w/direct LSF NON5.74 . . . nonlinear 5th order nodel w/diroct LSF LOBS4.74 . . . linear 4th order approx. w/nodifiod observer NOBS4.74 . . . nonlinear 4th order approx. wlnodified observer 60 CCCCCCC SOBRwTINB DESQIPI'ION CCCCC c C SOBRGJTINE LIN4.74 C . C TRIS SDBRwTINE mNTAINS 133 NORTH GDER LINEARIZED C INVBTED PEIDOLOI ”DEL. l/LSF THEMED. C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CVCCCCCCCC VARIABLE IDENTIFICATION CCCCCCCC C C C X 133 INDEPENDBIT VARIABLE. C C I VECI'OR (DNTAINING S(lOTIONS. C C DER! VECTOR mNTAININO USER DEFINED DERIVATIVES. C C C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C (SCCCCCCC ENTRY AND STORAGE BLOCK BLOCK 0000 C C SDBRwTINE FCT(X.T.DERT) C REM.“ DER! (20).K1.K2.K3.K4.R5.0. + Y (20) e 4- PA (20) C Cw—COIIDN FOR FCT PARAETERS C comm/Formula C C—IDEFINE PARABTBS OF A. B. AND K MATRICES C DATA A1.A2.A3/2.683.8.276.9 9363-4! DATA A4.A5.A6/20.91.2.16.32.63/ DATA A7.IB..D l3.92E-3.16.87.406.0/ DATA A10.Bl/2804.7.156.08l DATA A11.A12.A13.A14/5.711.4.607.1.164.0.939/ DATA C1.C2.C3.C4l0.544.1.33E-2.0.439 .5.268—2/ C [1 - PA(1) K2 I PA(2) K3 I PA(3) K4 I PA(4) C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CPCCCCCCCC PROCESS BLOCK ' BLOCK 0200 C C C-I-CALGJLATE CONTRG. INPUT 1: O I (x1¢1(1) + 32-!(2) + [391(3) + 34‘!(4)) IF (A33(U).0‘r.15.0) 0 - m‘r(ABS(0)/0)‘15.0 1(5) I u c CIIISET DERIVATIVE 61 C DER1(1) I 1(2) DER1(2) I I A11‘1(2) I A2‘1(3) 4’ A3‘1(4) + A13‘l] DER1(3) I 1(4) DER1(4) I A12‘1(2) + A6I1(3) I A7‘1(4) I A14I!) IF (1(2) .61. 1.8-5) TED) DER1(2) I DER1(2) I Cl‘1(2)/AES(1(2)) DER1(4) I DER1(4) + C3‘1(2)/ADS(1(2)) MDIF IF (1(4) .61'. 1.EI5) 1333 DER1(2) I DER1(2) + C2‘1(4)/ARS(1(4)) DER1(4) I DER1(4) I C4‘1(4)/ABS(1(4)) ENDIF C 1(6) I 1(1)‘12.0 1(7) I 1(3)‘180.0/3.l4l$26 C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C RETURN ND 62 CCCCCCC SDBRWTINE DESG IPI' ION CCCCC C C C SDBRINJTINE L1N5 .74 C C C C 1318 SDBROUTINE (DNTAINS THE NORTH QDER LINEARIZED C C INVETED FWDULUN ”DE (”DEED 10 133 SECOND ORDER C C LINEHIZEJ UTOR ”DEL. TEE (BOEING RODUGS A C C FIFTH ORDER SYSTEN ”DEL OVEALL. C C C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CV CCCCCCCC VIE IABLE IDEITIFI CATION CCCCCCCC C C C X TEE INDEPENDENT VMIABLE. C C 1 VECTOR (113131111110 SECTIONS. C C DER1 VEC‘IOR (1131111111110 USE DEFINED DER N Am. C C C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CSCCCCCCC ““1 AND S'IORAGE BLOCK BLOCK 0000 C C SDBRWTINE FCT(X. 1. DER1) C REAL‘4 DER1 (20).!1.X2.XS.X4.X5.U. + Y (20) s 4- PA (20) C CIIICOW FOR FCT PARAIBTERS C (DNIDN/ 31310011! PA C CIIIDEFINE PARABTERS OF A. B. MD I NAIR ICES C DATA A1 .A2 .A3/2 .683 .8.276 .9 .9 36E-4/ DATA A7.” 30 I3 .9 2EI3 .16 .87 .406 .0/ DATA A10 .31/2804 .7 .156 .08/ DATA A11.A12.A13 .A14/5 .711 .4 .607 .1 .164 .0 .9 39 / DATA C1 . C2 . C3 . C4/0.544 .1 .33EI2 .0 .439 .5 .26E-2/ C X1 I PA(I) X2 I PA(2) K3 I PA( 3) [4 I PA(4) I5 I PA(5) C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CPCCCCCCCC HOCESS BLOCK BLOCK 0200 C C CIIICALGJLATE (DN'IRE INPUT C D I (K1‘1(1) + 12.10) + KS‘1(3) + R4‘1(4) + K5‘1(5)) 1F (ABS(D).GT.15.0) D I 1NT(ABS(D)/D)‘15.0 63 1(6) I 0 C (II-ISET DERIVATIVE C DER1(1) I 1(2) DER1(2) I '- A1‘1(2) - A2‘1(3) + A3I1(4) + M‘1(5) DBRY(3) I 1(4) DER1(4) I A5‘1(2) + A6‘1(3) - A7‘1(4) - flI1(5) DB1”) I - D'1(2) .. A1091“) + Bl‘D IF (1(2) .GT. 1.8-5) mm DER1(2) I DER1(2) - 01‘1(2)IABS(1(2)) DER1(4) I DER1(4) + C3‘1(2)/AB8(1(2)) MDIF IF (1(4) .GT. 1.8-5) mm DER1(2) I DER1(2) + CZI1(4)/ABS(1(4)) DER1(4) I DBR1(4) - C4‘1(4)/ABS(1(4)) DIDIF C 1(7) I 1(1)‘12.0 1(8) I 1(3)‘180.0/3.141$26 C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C RETURN BID 64 CCCCCCC SDBRW TIN E DESCR II’T ION CCCCC C C C SDBRWTINE NW4 . 74 C C C C IBIS SDBRW'TINE amps TEE NCNLINEAR S1S'1EN MDATIONS C C 'I'IE TEE FULL STATE-FEEDBACK counmLm INEMTED. C C C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CV CCCCCCCC VARIABLE IDENTIFICATION CCCCCCCC C C C K TEE INDEMDENT VARIABLE. C C 1 VECTOR (DNTAIN ING S(LU'TIONS. C C DER1 VECTOR munmmo USER DEFINED DERIVATIVES. C C C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CSCWCCCC ““1 AND STORAGE BLOCK BLOCK 0000 C C SDBRW'TINE KT“. 1. DER1) C REAL‘4 DER1 (20).K1.K2.K3.K4.K.N.J. + 1 (20).NC.NP.B1.B2.B3.L.R. 4» PA (20) C CI-ICONIDN FOR FCT PARAETBS C mNUN/FCTmI/PA C C DEFINE PARAETERS OF EQNS. IND K um: C DATA NP. NC/0.0236 .0 .0385/ DATA D. G/ 1 .23 6 .32 .174/ DATA BI 082 033/]. 0138.4 .0 .025 .2 .163.‘/ DATA K. N/0.0813 .32 .0/ DATA C1 . C2 . C3 . “/0 .544 .1 .33E-2 .0 .439 .5 .26E—2/ C 4 K1 I PA(I) K2 I PA(2) K3 I PA (3) K4 I PA (4) c . CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CPCCCCCCCC HOGSS BLOCK BLOCK 0200 C C C-I-CALCDLATE (DN'IRG. INPUT C C U I (K1‘1 (1) + K2‘1 (2) + K3‘1(3) + K4‘1 (4)) IF (ABS(D).G'T.15 .0) U I INT(ABS(U) /D)‘15 .0 1 (5) I D C c-.. SET DER IV ATIV E C 65 DER1(1) I 1(2) TOP I (K I N/(1.356 I R)) I (U - K I N I 1(2)) TOP I TOP + IP I D I 1(4) I 1(4) I SIN(1(3)) TOP I TOP - NP I'G I SIN(1(3)) I COS(1(3)) TOP I TOP'+ (BI/D) I 1(4) I COS(1(3)) TOP I TOP - (N I N I B3 + B2) I 1(2) IF (1(4) .08. 1.E-5) TEEN A TOP I TOP + C2I(NC+NINIJ)ICOS(1(3))I1(4)/ABS(1(4)) ENDIF ‘ IF (1(2) .08. 1.8-5) TEEN TOP I TOP - C1I(N0+NINIJ)I1(2)IABS(1(2)) ENDIF DER1(2) I TOP/(NC + NPI(SIN(1(3))II2) + NINIJ) DER1(3) I 1(4) TOP I GISIN(1(3)) '- DER1(2)ICOS(1(3)) - 1(4)IB1/ (EPID) IF (1(4) .GE. 1.8-5) TEEN TOP I TOP - C4IDI((IC+NINIJ)I(E0+RP*NINIJ))I1(4)/ABS(1(4)) ENDIF DER1(4) I TOP/D 1(6) I 1(1)I12.0 1(7) I 1(3)I180.0/3.1415926 CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C RETURN END 66 ccccccc SUDEUUTINE DEsaIIPTIoN ccccc c c c SUDEUUTmE NON5.74 c c c c TEIs SUEEUUTINE Duns TEE NmLINEAI srsm EQUATIONS c c Irm TEE FULL STATE—EE-lAcx UUN'mun IIPLEIENTED. c c DARPING Is INCLUDED IN nus STATE mDEL. TEE mm c c Is UDUELED To nus 313m noun. 130. c c c ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc c c cvcccccccc VARIABLE IDENTIEI CATIoN cccccccc c c c 1 IEE INDEPENDENT VARIABLE. c c 1 VEcron mNTAmINU sa.UTIoNs. c c DER1 VECTOR mNTAININa nan DEFINED DERIVATIVE. c c c ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc c . c csccccccc Em! AND SIDEAUE ELocx ELocx 0000 c c sUDnoUTINE ECT(I.T.DEEI) c REALM DEE! (20).:I.I2.13.I4.T.N.J. + 1 (20).uc.IP.EI.Dz.Es.L.n. + PA (20) c c—UUIIDN FOR FCT PAIAIBTERS c mwN/ECTUoI/PA c c DEFINE anms 0E EQNS. IND x IAnn c DATA IP. uc10.0236.o.0385/ DATA D.GII.2396.32_.174/ DATA DI.Ez.33/1.13E-4.o.ozs.z.16E-4/ DATA I.N/0.0813.32.0/ DATA J.L.nls.2E-s.6.4o7E-3.I7.9 7/ DATA c1.cz.c3.c4/o.544.1.33E-2.o.439 .s.26E-2/ c :1 - PA(l) :2 - PA(2) x3 - DA(3) x4 - PA“) :5 - PA(S) c ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc c c crcccccccc PROCESS DLocx BLOCK 0on c c c--CALCULATE UUNIna. INPUT c C III (K1I1(1) + K2I1(2) + K3I1(3) + K4I1(4) + K5I1(5)) IF (ABS(D).GT.15.0) U I INT(ABS(U)/D)I15.0 C 67 1(6) I U CIIISET’DERIVATTVES C C DER1(1) I 1(2) TOP I KINI1(5)/1.356 + EPIDI1(4)I1(4)ISIN(1(3)) TOP I TOP I IP I G I SIN(1(3)) I COS(1(3)) TOP I TOP'+ (Bl/D) I 1(4) I COS(1(3)) TOP I TOP I (N I N I B3 + B2) I 1(2) IF (1(4) .08. 1.EI5) TEEN TOP’I TOP + C2I(IC*NINIJ)ICOS(1(3))I1(4)IABS(1(4)) ENDIF IF (1(2) .GE. 1.EI5) TEEN TOP I TOP I C1I(IO+NINIJ)I1(2)/ABS(1(2)) ENDIF DER1(2) I TOP/(EC + IPI(SIN(1(3))II2) 4' NINIJ) DER1(3) I 1(4) TOP I GISIN(1(3)) I DER1(2)ICOS(1(3)) I 1(4)IB1I(IPID) IF (1(4) .08. 1.EI5) TERI TOP I TOP I C4IDI((IC+NINIJ)/(N0+IP*NINII))I1(4)/ABS(1(4)) ENDIF DER1(4) I TOP/D DER1(5) I (U I RI1(5) I KINI1(2))/L 1(7) I 1(1)I12.0 1(8) I 1(3)I180.0/3.1415926 CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C RETURN END 68 CCCCCCC SUBRW TIN E DESQ IPT ION CCCCC C C C SUBRGJTINE [£384 .74 C C C C TEIS SUBRCIJTINE (ONTAINS TEE FOURTH ORDER LINEARIZED C C INVBTED PENDULUN ”DEL. AND TEE QSERVB ”DE. C C C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CV CCCCCCCC VB IABLE IDENTIFICATION CCCCCCCC C C C K TEE INDEPENDENT VARIABLE. C C 1 VECTOR CONTAINING SILUTIONS. C C DER1 VECTOR CONTAIN INO USER DEFINED DERIVATIVB. C C C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CSCCCCCCC MR1 AND STORAGE BLOCK BLOCK 0000 C C SUBRWTINE FCT(K.1.DER1) C REALI4 DER1 (20).K1.K2.K3.K4.K5.U. + T (20). + PA (20) C CIIICOIDN FOR FCT PARABETRS C NMN/ FCTCDN/ PA C CIIIDEFINE PARABTERS OF A. B. AND K MATRICES C DATA A1 .A2 .A3/2.683 .8 .2769 .9 368-4! DATA A4 .A5 .A6/20 .9 1 .2 .16 .32 .63/ DATA A10.Bl/2804.7 .156 .08/ DATA A11 .A12.A13 .A14/5 .711.4 .607 .1 .164.0 .9 39/ DATA C1 .02 . C3 .C4l0.544 .1 .33E-2 .0 .439 .5 .26E-2/ C K1 I PA(I) K2 I PA(2) K3 I PA(3) K4 I PA(4) C 01 I PA(5) 62 I PA(6) G3 I PM?) 04 I PA (8) C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CPCCCCCCCC PROCESS BLOCK BLOCK 0200 C C CI-ICALCULATE (DNTRG. INPUT C fl U I (K1I1(5) + K2I1(6) + K3I1(7) + K4I1(8)) IF (ABS(U).GT.15.0) U I 1NT(ABS(U)/U)I15.0 1(9) I U ’ C CIIISET DERIVATIVES C DER1(1) I 1(2) DER1(2) I I A11I1(2) I A2I1(3) + A3I1(4) + A13IU DER1(3) I 1(4) DER1(4) I A12I1(2) + A6I1(3) I A7I1(4) I A14IU C IF (1(2) .OT. 1.EI5) TEE) DER1(2) I DER1(2) I ClI1(2)/ABS(1(2)) DER1(4) I DER1(4) + C3I1(2)/ABS(1(2)) MDIF IF (1(4) .GT. 1.EI5) mm DER1(2) I DER1(2) 4‘ C2I1(4)/ABS(1(4)) DER1(4) I DER1(4) I CAI1(4)/ABS(1(4)) ENDIF C 1(10) I 1(1)I12.0 1(11) I 1(3)I180.0/3.141$26 C cmmsmvm S1STEN C BR I 1(1) I 1(5) C DER1(5) I 1(6) + 01 I ERR DER1(6) I I A11I1(6) .. A2I1(7) + A3I1(8) + A13IU + G2 I ERR DER1(7) I 1(8) + 03 I ERR DER1(8) I A12I1(6) + A6I1(7) I A7I1(8) I A14IU + 04 I ERR C IF (1(6) .OT. 1.EI5) TEm DER1(6) I DER1(6) I C1I1(6)/ABS(1(6)) DER1(8) I DER1(8) + C3I1(6)IABS(1(6)) MDIF IF (1(8) .GT. 1.EI5) TEN DER1(6) I DERY(6) + C2I1(8)/ABS(1(8)) DER1(8) I DER1(8) I C4I1(8)/ABS(1(8)) BIDIF ' C 1(12) I 1(5)I12.0 1(13) I 1(7)I180.0/3.141$26 C A . CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C RETURN BID ' 70 CCCCCCC SUBRWTINE DESG IP'T ION CCCCC C C C SUBRwTINE NmS4 . 74 C C C C TEIS SUBRWTINE ECLDS TEE NOILINEAR S1STEN EUATIONS C C AND TEE LINEAR (BSEVE UDEL. C C C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CV CCCCCCCC VARIABLE IDETIFI CATIM CCCCCCCC C C C K TEE INDEPENDENT VARIABLE. C C 1 VECTOR MTAIN ING SILUTIONS. C C DER1 VECTOR (DNTAIN INC USE DEFINED DERIVATIVE. C C C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CSCCCCCCC ENTR1 AND STORAGE BLOCK BLOCK 0000 C C SUBRWTINE FCT(X. 1. DER1) C REALI4 DER1 (20).K1.K2.K3.K4.K.N.J. + 1 (20).IC.IP.B1.B2.B3.L.R.P1. + PA (20).A2.A3.A6.A7.A11.A12.A13.A14 C CIIICOEEN FOR FCT PARAETES C (DWI FCTNE/PA C C DEFINE PARAETES 0F ENS. AND K NATRIK C DATA IP.ICI0.0236.0.0385/ DATA D.0/1.2396.32.174/ DATA B1 .B2.B3/1.13EI4 .0.025.2.16EI4/ DATA K.N/0.0813.32.0/ DATA J.L.RI5 .2EI5 .6 .407E-3 .17 .9 7/ DATA A1.A2.A3/2.683.8.276.9 .936EI4/ DATA A4.A5.A6I20.91.2.16.32.63/ DATA A7.H:.D‘/3.92E-3.16.87.406.0/ DATA A10.P1/2804.7.156.08/ DATA A11.A12.A13.A14/5.711.4.607.1.164.0.939/ DATA C1.C2.C3.C4/0.544.1.33EI2.0.439 .5.26EI2I K1 I PA(I) K2 I PA(2) K3 I PA(3) K4 I PA(4) 01 02 G3 G4 PA(S) PA(6) PA(7) PA(8) 71 CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C C CPCCCCCCCC PROCESS BLOCK BLOCK 0200 C C CIIICALCULATE CONTROL INPUT' C C IF (PA(20) .BD. 0.0) TEEN U I (K1I1(1) + K2I1(2) + K3I1(3) + K4I1(4)) ELSE U I (K1I1(5) + K2I1(6) + K3I1(7) + K4I1(8)) ENDIF IF (PA(19) .E. 1.0) TEE IF (ABS(U).GT.15.0) U I INTTABS(U)/U)I15.0 ENDIF C 1(9) I U C CIIISET’DERIVATTVES C DER1(1) I 1(2) TOP I (K I N/(1.356 I R)) I (U I K I N I 1(2)) TOP I TOP + IP I D I 1(4) I 1(4) I SIN(1(3)) TOP’I TOP I IP I G I SIN(1(3)) I COS(1(3)) TOP I TOP + (Bl/D) I 1(4) I 008(1(3)) TOP I TOP I (N I N I B3 + B2) I 1(2)‘ IF (1(4) .GE. 1.EI5) TEEN TOP I TOP + C2I(IC+NINIJ)ICOS(1(3))I1(4)/ABS(1(4)) ENDIF IF (1(2) .GE. 1.EI5) TEEN TOP I TOP I C1I(ICHNINIJ)I1(2)/ABS(1(2)) ENDIF DER1(2) I TOP/(EC + IPI(SIN(1(3))II2) + NINIJ) DER1(3) I 1(4) TOP I GISIN(1(3)) I DER1(2)ICOS(1(3)) I 1(4)IBll(IPID) IF (1(4) .GE. 1.EI5) TEEN TOP I TOP I C4IDI((NC+NINIJ)/(IC+IP+NINIJ))I1(4)/ABS(1(4)) ENDIF DER1(4) I TOP/D 1(10) I 1(1)I12.0 1(11) I 1(3)I180.0/3.1415926 C A CIIIQ SERVER S1STEI C ERR I 1(1) I 1(5) C DER!(5) DER1(6) DER1(7) DER1(8) 1(6) + 61 I ERR - A1101“) - A2'!(7) + A3I1(8) + A13IU + 62 0 Eu T(8) + 63 I ERR A12I1(6) + A6I1(7) - A7-Y(8) - A14IU + 64 0 EEE IF (PA(11) .B0. 1.0) TEEN IF (1(6) .GT. 1.EI5) TEEN 72 DER1(6) I DER1(6) I C1I1(6)/ABS(1(6)) DER1(8) I DER1(8) + C3I1(6)/ABS(1(6)) ENDIF IF (1(8) .GT. 1.EI5) TEE DER1(6) I DER1(6) + C2I1(8)/ABS(1(8)) DER1(8) I DER1(8) I C4I1(8)/ABS(1(8)) ENDIF EDIF C 1(12) I 1(5)I12.0 1(13) I 1(7)I180.0/3.141926 C CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC C RETURN ED APPENDIX I As with any real control system. an accurate parameter study must he completed in order to deter-ine the various control gains which will be i-ple-ented. In this particular system. several sub-systems could be identified and the parameters determined separately for each. 99 §egvo lotgr The drive uotor used in the control system was a 12 volt D.C. Specialty servo uotor. There are six parameters required to define the linear motor model given in Appendix A. Of these. the most difficult parameter to determine experimentally is the armature inertia. therefore the manufacturer specification has been used. I. - 0.005 oinnIs’ - 2.6E+s ft-lh-s' 0n the electrical side of the nodel. the armature inductance is in series with the armature resistance. The inductance of a series BL circuit can he ueasured directly with an i-pedance bridge. The RP 4260IA Universal Bridge vas used to determine this parameter value as: L I 6.4lE-3 Henry The gyrator constant couples the mechanical and electrical sides of the motor. This parameter can easily be deter-ined experimentally using one of the constitutive relations for a gyrator: 73 74 Vb ’- kEm (11) By applying a known and constant arnature speed (”n)- . bgck gut (vb) 1. generated. For a series of fixed input speeds. a set of voltages can be read. 'hen this data is plotted. the slope of the line is the gyrator constant (kg). 20- :03 10- '5 > .. v E 0- Q) .2 o 1 C3 13 > -10- -2o . ' . r 1 , j --200 -1'oo (T 100 200 Armature speed (rod/sec) Figure 11: Deternination of Gyrator Constant For this DC servo. the linear constitutive relation defined in (11) correctly nodels the physics of the notor as the plotted data of Figure 11 indicates. The slape of the line. i.e. the electrical gyrator constant. is thus: 75 kg I 0.0813 VIs/rad In standard bond graph use. only one constant is required for a gyrator. Due to the particular choice of units here. another gyrator constant nust be defined such that power will be conserved through the gyrator. A torque constant which accounts for this choice of units. is related to the electrical constant by: k, - kE/1.356 (ft-Ib/amp) (12) The two damping parameters. electrical resistance and nechanical viscous danping. are not easily neasured. Looking at the two equations of notion for the linear notor. a sinple procedure to experiientally deter-ine these paraneters can be obtained [6]. The two equations are very similar in form: one representing the electrical side. and the other representing the nechanical side. di dt “- T'-J-—Ib-u.+kti (I4) dt Applying a voltage to the actor under no-load conditions. the armature rotational speed and current will reach a steady state. Thus both derivatives in (13) and (I4). and the torque input will be zero. leaving two sinple linear equations. The systel states q. gnd 1 c'n be neasured for a range of input voltages. For each of the test points. V1100! for 3 ltd 5. can then be computed from equations (13) and (I4). 76 The linear notor nodel breaks down in representing the actual notor with the given test data. Instead of constant danping values. the parameters are actually functions of "iv .3 shown in Figugo 12. 241 . 181 IOI 81 4 Armature rcsnslonce (ohms) i" Viscous damping coo". (3:10») 5' Q U‘ v f‘va vvvv vvvvrrTvv v'v—rrv YVVV'VrVY‘fiYVVI -u0T lfim -&I 0 so 60"'60 :uw'Ylh£"-ua 5 50 mo mo Armature speed (rod/sec) Armature speed (rcd/ sec) Figure 12: Nonlinear danping paraneters of DC notor Tb use the linear nodel. constant values for the dmnping parnneters are required. A simple arithnetic nean was chosen. which effectively provides a linearixation about some naninal u , The computed V‘lugs .3. given by: R 17.970 b. .. 10.81E—s u—nm 0.21.22.19.12 The notor armature thaft is connected through a gear train to the belt drive attached to the cart. It is assumed that there are no dynamics between the armature notion and the cart notion. The cart notion is rigidly connected to the arnature notion. Therefore a sinple linear transfomner between arnature position and cart position is 77 needed. This parameter is neasured to be: n I 32.0 rad/ft The gear train also has an inertia and nechanical damping associated with it. Due to the rigid coupling assumption. this inertia and dnnping can be lulped into the notor para-stars. The equivalent inertia of the notor armature. and the equivalent dmnping are: I I 5.2EI5 ftI1bIs' b. I 2.16EI4 ftIlbIs .This set of equivalent paraneters will be used for the notor paraneters thus eliminating the direct inclusion of the gear train. ngflglnl The pendulum has three para-eters associated with it. they are the equivalent mass. equivalent length. and eqivalent viscous dnnping. Since the nodel used here assunes a nassless rod. vith no loss of generality. the equivalent nass is just the total mass of the pendulul. The equivalent length is the distance from the rotational axis to the pendulun center of gravity. These values are deter-ined to be: u I 0.76 lbm. I 0.0236 slugs d I 14.87 in. I 1.24 feet b. = 1.13EI4 ft-lbIs £321 The cart also has three paraneters associated with it. The first. and easiest to determine. is the cart nass. Once again. due to the rigid connection between the cart and armature via the gear train. the 78 connecting drive belt will be lumped into the cart nass. The equivalent cart mass is thus neasured to be: I I 1.32 lbm I 0.041 slugs The nost difficult cart parnneters to determine were the equivalent danping coefficients. A linear ccnbination of viscous and coulomb damping effects have been assuned in the nedel formulation. These parameter values are determined experinentally utilising a conservation of energy principle [12]. Thking the first nonlinear system equation (A13). from Appendix A. and fixing the pendulum such that: 0 - é - 0 - 0 (Is) the second nonlinear system equation (A14). can be elininatsd. and (A13) can then be written as: I'E + b'i + f,... “m 0: ‘ AVAvAv—I = b—-0 >— 5.am + f) D 5; 3.9m = 0.4 7pF 2~szas - 2N6285 e... 1' 3.91m - ‘W, Ikfl 1} 9V :i 1m / i9 m * c 3.9m 56011: :5600 rvvv mluAA 'I"— f N; 3,; Figure 17: Power amplifier schematic diagram (5 00600606 0 GOO 000 86 PRWRAI AREA 1318 mock»: READS IN A DATA FILE 0F (LI) COORDINATE PAIRS 'RICE DEFINE A 1‘0 DIIENSIONAL SURFACE. AFTER ALL THE NINTS ARE READ IN. THE BEA mNTAINED BY THE (DRDINATE EWNDAR! 'ILL BE (DINTED. DEmARE AL VAR IABLES REAL X(100).Y(100) INTEGER BOR.I.N.J GARACTB FILNAI‘32 IIII-OET TIIE FILMAIE (mummc THE DATA 10 “ITE(1.'(I.SX.A.$)') 'DATA mmm: ' nm(1.'(A)'.m-10) FILNAI OPEN ( 5 . FILE-FILNAN. ERR-10 . STATUS- ' mD ' ) -----READ IN ALL THE DATA 20 30 40 1-1 “OR-0 nm 1(1),!(1').J I-I+l 001020 EROR-BOR-i-l 001020 (1088(5) IF (EROR .GT. 0) mm IRITE(1.‘)' ' IRITEU...) "HERE IBE'.EROR.' READ ERRORS IN '.FILNAI MDIF N 8 I - l SDI-I 0 IIII-CALCULATE THE AREA no 50 I I 1.N IF (I .80. 1) THE] ~80! I SDI + (X(N)IX(2))‘Y(1) ELSE IF (I .m. N) THE! SUN I SDI + (X(N-l)-X(1))‘Y(N) ELSE 87 SUN I SDI + (X(II1)IX(I+1))‘Y(I) EDIE 50 MTINDE SUI I ”NEON/2.0) IIIIIIRITE TEE RBOLTS TO THE TERNINAL 'RITE(1.'(I.SX.A.F14.6)')'HEA I '.SDN IRITE(1.‘)' ' APPENDIX.J The following control program was written in FORTRAN IV for use on a single user DEC minicomputer under the RTIll operating system. Though this pregram is implemented on the DEC LSI-ll/23+. it will run on lost DEC computers under RTIll. Standard DEC vector addresses for the various system devices have been used [5]. and due to the modular structure of the code. minor changes can easily be nade for nonstandard vector addresses. PROGRAM PEND This is the Iain progran which accesses all the control subroutines for the inverted pendulum. [STEVE C. SODTRIARD. B.S.I.E] III-Overall program Logistics MD [Iain program: PEND.FOR] IFOSN [Subroutine: IPOSNJOR] SAMPLE [Subroutine: SAIPLE.IAC] CGAINS [Subroutine: CGAINS.FOR] CONTRL [Subroutine: CONTRL.FOR] DISQT [Subroutine: DISQTJDR] RDESF [Interrupt Service: RDMSFJKJR] PAUSE [Subroutine: CLOCK. FOR] SETIC! [Interrupt Service: CLOCLFOE] Fornat Note: name [typez storage filename] OOOOOOOOGOOOOGOOOOOOOOOOO REEL 0(24).F(24) O LOGICAL‘I IDATA 88. GOO GOO GOO DIMENSION IDLIST(21) EXTERNAL SETICI (”MN IADNI/ IISR. Im. IM. ICG. ITCK CONIDN IADllzl IPSR. IP18. IBPR. IADVEC. IADERR CONIDN IADIIS/ ICSR. IADR. IDAR. IADDUF comm IIISC2I F.G.IR(2.IRPZ (ONION ITIB/ I. IEND ---Initialise the variables DATA IIIZJRFZIOJI ---These control gains place the poles at {-3.-4.-5.-6 F(l) I 11.854 [[1 I F(2) I 16.174 [[2 LSF controller F(3) I 175.722 [R3 gains F(4) I 33.0895 [14 I ----These gains place the observer poles at {-8.-8.-9 .-9} F(5) I 28.285 [61 I F(6) I 303.973 [62 Observer F(7) I 425.307 [GS gains F(8) I -2331.515 [64 I F(9) I 1.0I(l.0 + 3.0) [position F(10)I 1.0 - F(9) [filter F(11)I 5.7107 [A11 F(12)' 8.276 [A2 F(13)I9.9357E-4 [A3 I F(l4)I 1.163$ [A13 System F(15)I 4.6069 [A12 parameters F(16)I 32.631 [A6 I F(17)I 3.91757E-3 [A7 F(18)I 0.93876 [A14 F(19)I 1.0/(1.0 + 10.0) [derivative F(20)I 1.0 - F(3) [filter F(21)I 0.76078 [Cl I F(22)I 1.75854E—2 [CZ Coulomb damping F(23)I 0.61373 [C3 coefficients F(24)I 6.933758-2 [C4 I Set up address registers IFS! I "102 [processor status word IISR I "177560 [keyboard status register man I IISR + 2 [keyboard data register GOO 000 90 IPSR I IESR + 4 [printer status register 1m I IKSR + 6 [printer data register ICSR I "170420 [real tine clock status reg. IBFR I ICSR + 2 [clock buffer preset reg. ICKV I "440 [clock vector address IDAR I "170440 [DIA base output addr.: ch.0 1A0! I "170400 [AID control status register IADBUF I IADR + 2 [AID buffer register IADVEC I "400 [AID done interrupt vector IADERR I IADVEC + 4 [AID error interrupt vector IBM I '7 [bell tone IZERO I "4000 [zero value for DIA converter Protect System fro. crash IIIJSTU) I IISR [keyboard status IDLISTU) I IPEEIUKSR) 1m.IST(3) I IPSR [printer status IDLISTM) I IEEKUPSR) IDLISTU) I IDAR [sero DIA channel 0 IDLIST(6) I 12.0 IDLIST(7) I IDAR + 2 [zero DIA channel 1 IDLIST(8) I IZBO IDLIST(9) I IDAR + 4 [zero DIA channel 2 IDLISTUO) I IZBO IDLIST(11) I IDAR + 6 [zero DIA channel 3 IDLIST(12) I IZERO IDLIST(13) I IAN [AID status IDLISTU‘) I O IDLISTUS) I IADVEC [AID done interrupt vector MISTUG) I IPEEKUADVEC) IDLIST(17) I ICSR [clock status IDLIST(18) I IFEEK(ICSR) IRISTUD) I IPSI [processor status IDLIST(20) I IPEEK(IFSI) IDLIST(21) I 0 [end of list narker CALL DEVICE (IDLIST) For-at Statements 500 FORlAT(I.I.I.22X. 'INVERTED PENDULUN Gonna. RWTINES' . + /.221. ' 510 FORIAT(/.I.22X. 'Enter the PENDDLUI AID channel: ') '.3) 520 FORMAT(/.22X.'Enter the cm up channel: 33) 530 FORNAT(/.I.221.'Enter the DIA output channel: '.i) 540 FORIAT(I2) 550 FORIATUJ .233. '- ++++++ IAIN “(Inn-H /.I.18X.'(I)nitia1ise the zero reference positions'. I./.18X.'(G)ain settings for LSF control B Observer'. I.I.lSX.'(S)tatus of Zero positions'. I.I.181.'(R)un the active controller'. I.I.18!.'(Q)uit the pregram'. I.I.I.22X.'Choose one of the above. . . '.8) 060 a GOOD 9]. 560 FORIAT(I.I.221. 'STATIJS of Zero References. . .' . 4' I .221. ' ') 570 FORNAT(I.22X. 'CART sero reference: '.16) 580 FORIIAT(I.22X. 'PBID sero reference: '.I6) 590 FORIAT(/.I.22X.'Eriting Control Routine. . .'.5(I)) 600 FORIAT(I.I.181. 'Zero reference positions NOT initialised') Calibrate the PAUSE subroutine IEND I 10000 [set initial value IPR I 7 , [highest priority interrupt 10 I I IN'BET(ICRV.IR.1.SETICR) [attach to RTC vector IF (I .m. O) 0010 20 IRITE(7.‘)'INTSET error -- R'I'C vector. cons I '.I 0010 10 20 IRATE I "111 [set up clock to generate ICOON'I‘ I -16667 [an interrupt after 1 tick CALL IFORE(IBPR.ICODNT) CALL IFORE(ICSR.IRATE) [start the clock 00 30 I I 1.10000 [do nothing loop while- 30 CONTINUE [waiting for one tick ITCR I IND + 1 IRITE(7.‘)'ITICR I '.ITCI -Set zero (0) volts on the DIA output D0 40 I I 0.6.2 [ CALL IPOKE(IDAR+I.IZBO) [sero out all 4 channels 40 CONTINUE [ -Get the cart and pendulun AID channels IRITE(7.500) [initial header 50 IRITE(7.510) [get PEND channel first READ(5.540.BRI50) 1“! IF ((IPm.0T.15).m.(IPG.LT.0)) GOTO 50 60 IRITE(7.520) [get CART channel second READ(5.540.ERRI60) 1cm IF ((ICG.GT.15).M.(IC(3.LT.O)) GOTO 60 IF (1cm.m.lpm> GOTO 60 70 IRITE(7.530) [get DIA output channel IF ((IDACE.LT.0).m.(IDACE.GT.3)) GOTO 70 IDAR-IDAR+ IDACR‘2 Print out the options nenu GOO GOO 000 000 non 060 000 92 80 IRITE(7.550) [print the nenu options CALL PADSE(50.ITCK) [wait for printing to finish Disable the keyboard I I IPEER(IRSR) .AND. "177477 [clear keyboard interrupt CALL IPORE(IRSR.I) ---Iait for keyboard input 90 CALL IFOREUPIIJBELL) [ring the bell 100 I I IPEER(IRSR) .AND. "200 [test bit 7 of IRSR IF (I .80. 0) GOTO 100 IDA’I‘A I IPEENIRII) [a key has been pressed Check for a valid character IF ((IDATA.NE. 'I' ) .AND. (IDATA.NE. '0' ) .AND. (IDATA.NE. ' S' ) .AND. + (IDATA.NE.'R').AND.(IDATA.NE.'Q')) GOTO 90 CALL IFOIE(IPDR.IDATA) [print the proper key ---Enable the keyboard I I IFEER(IRSR) .OR. "100 [set bit 6 of IKSR CALL IFORE(IRSR.I) IRITE(7.‘)' ' ---Go to the preper place IF (IDATA .80. ‘1') GOTO 110 IF (IDATA .ED. '6') GOTO 120 IF (IDATA .80. 'S') GOTO 130 IF (IDATA .30. 'R') GOTO 140 IF (IDATA .80. '0') GOTO 150 -—I--INITIALIZE the zero reference positions 110 CALL IPOSN GOTO 80 [return to menu ----0AIN settings for LSF control law 120 CALL COAINS GOTO 80 [return to nenu ---—STATOS of Zero positions 130 IRITE(7.560) [initial header 93 IRITE(7.570) IRCZ IRITE(7.580) IRPL GOTO 80 [return to nenu ---RDN the active control routine 140 IF ((IRCZ.NE.O).MD.(IRPZ.NE.O)) 0010 145 IRITE(7.600) [sero references 00'“) 80 [not initialized 145 CALL CON'IRL GOTO 80 [return to nenu III-QUIT the program 150 IRITE(7.590) [exit nessage CALL PADSE(60.ITCR) CALL EXIT OOGO 0 GOO GOO OGO SDBRWTINE IPOSN 94 This subroutine allows the user to define the initial reference positions of the cart and pendulum. REAL C(24) .F(24) LM ICAL‘I IDATA mm IADRII IRSR.Im.IPG.ICG.ITCI NNIDN IIISCZI F.G. 13¢.an -Fornat Statements lOO FORIAT( '+ ' .261. I4 .121. I4) 110 FORlAT(I.I./.201. 'INITIALIZE the Reference Positions' . U I301. ' +++++++ I—I-Iain Routine IRITEUJIO) CALL PADSEUOOJTCR) I-II-Turn off the keyboard I I IEERUISR) .AND. "177477 CALL IPORE(IISR. I) 5 ICDUN I O IPDONIO ----S tart Sampl ing 10 IF (ICDON .80. 1) GOTO 20 CALL SAMPLEUCG. ICVAL. IFLAG) IF (IFLAG.NE.O) GOTO 40 IRCZ I ICVN. 20 IF (IPDDN .m. l) GOTO 30 CALL SAMEUPG. IPVAL. IFLAG) IF (IFLAO.NE.O) GOTO 40 IRPZ I IPVAL 3O 'RITE(7.100) IRCZ. IRPZ CALL PAUSE”. ITCR) /./.I.15I.'Iove the cart and pendulum to their respective'. /.I.151.'reference positions to define the ZDOESJ. I.I.I.151.'Press "P" to define PENDOLDI reference . . .'. /.I.151.'Press "C" to define the CART reference . . .'. /./.151.'Press NO" to RBTART . . .'. I.l././.27X. 'CART' .101. 'PENDOLUI' .I.I) [print the instructions [wait for (RT [clear keyboard interrupt [reset the flags [get CART AID sample [get PEND. AID sample [wait for 7 ticks (566 95 -------Check for keyboard input I I IPEERHRSR) .AND. "200 [test bit 7 of IKSR IF (I .m. O) 0010 10 IDATA I IPEERURIR) [a key has been pressed IF (IDATA .30. 'C') ICDON I 1 IF (IDATA .m. 'P') IPDDN I 1 IF (IDATA .m. '0') OOTO 5 IF ((ICDON.M.1).AND.(IPDON.m.1)) GOTO 40 GOTO 10 ----Turn on the keyboard 40 I I IPEERUKSR) .oa. "100 CALL IPOKEUISR. I) RETURN .e .e .e .e .e 'e .e .e .e De .e .e SAIFLE: .TTDLE SAMPLE 96 This is a IAQO-ll. mRTRAN compatible subroutine called with CALL SAIFLE(ICRAN.IVAL.IFLO) I I IFLG I - his subroutine gets a sanple from the specified AID channel ICRAN. and the sanpled data is passed back in IVAL. The flag IFLG is returned to the main program with the status: 0 I N0 ERRORS DETECTED 1 I CHANNEL NOISE OUT OF RANGE (0-15) 2 I AID SAIFLING ERROR .GLOBLiSAIELE LCI. .IIOOO+LC CSRI17O4OO DRRICSR+2 :nis makes SARPLE a global symbol :Find current assembly location :Start assenbling at location 1000 3Define AID status register address :Define AID buffer register address ; R5 points to the first address of a table of arguments J(R5) is the first data (I # of arguments passed) I2(R5) is the first argument passed START: IAIT: ERRl: gag..- E [m [sum .1“ 1m 12(35).mp 31c #177760.ICRAN (u no cap mamas sun mu sun 1mm Iov Immuiocss ms “Jules: arr noanss um um nov [#nssduns) arr sioooooJrcsn sun n32 uov FLO.J6(R5) RTS pc mos mums mamas BIS £1,314; at sun ms 02.114; as nous LOCAL VARIABLES .wosn o .wosn o .wosn o sPut first arguent in IGAN :Put first argument in TRIP also :Clear bits 4 to 15 sCIear the FLO :Check for ICEIN ) 15 :Branch to ERRI if ICRM > 15 :Swap high byte for low byte :Set AID Control Status Register :Start the AID conversion .'Test bit 7 for AID done :Iait if not set sPut sampled data into argument table :Test bit 15 for AID sanpling error :Branch to ERR2 if bit 15 is set :Put flag back into argument table :Return from sub. to calling progra- :Set bit 0 of FLG :Go back to start sSet bit 1 of FLO :Go back to DONE OOOOOOOOOOOOOOOOOOOOG00000060 O 000 97 SDBRODTTNB CGAJNS This subroutine allows the user to input the control gains for the linear state feedback law. the luenberger observer. or the first order filter parameters. -Definition of parameters F() F(l) I LSF proportional cart gain F(Z) I LSF derivative cart gain F(3) I LSF proportional pendulum gain F(4) I LSF derivative pendulum gain F(5) I observer gain #1 F(G) I observer gain #2 F(7) I observer gain #3 F(S) I observer gain #4 F0) I filter parameter (alfa) F(10)I filter parameter (l-alfa) F(II)‘ . I . observer system parameters . I F(18)I F(19)I deriv. filter parameter (alfa) F(20)I deriv. filter parameter (l-alfa) F(21)I Cl I F(22)I C2 ‘Coulomb damping F(23)I C3 coefficients F(24)I 04 I REAL G(24).F(24).8N mmu IIISCZI F. G. IRCZ. IRPZ ----Format Statements 100 FORIAT(I.I.I.ZOX.'INPDT'the System and Control Gains'. + /.2ox.'* A ') 110 FORIAT(I.I.203.'1.) Input LSF Controller gains.'. ' I.201.'2.) Input Luenberger Observer gains.'. l.20!.'3.) Input Observer System Parameters.'. I.201.‘4.) Input Filter Parameters.'. I.203.'5.) Check Status of all gains.'. I.20!.'6.) EXIT to main routine. .') 120 FORlAT(I./.251.'Your Selection: '.3) 130 FORIAT(11) 14o FORlAT(/.27I.'K('.Il.') [anon] - '.i) 150 rowan/.271. '-o('.n.') [Observer] - '.3) 160 FORMAT(/.3OX.A3.' = '.3) + + + + + 170 FORIAT(I.18X.'Enter #SAIP.I'.A4.'. FILTER TIRE CONST.: 180 FORMAT(I.251.'LSF Controller Gains:'.I. + 4(I.303.'!('.11.') I '.F10.4)) 190 FORIATI/.251.'0bserver Gains:'.I. ' +' 4(/,3OX.'G(',II.') I '.F10.4)) '.t) GOO (500 066 GOO O 0606 200 FORlAT(/.251.'0bserver Systma Parmesters:'./. + 12(/.3OX.A3.' I '.F10.4)) 210 FORIAT(/.25!.A4.'. Filter Parameters:'./. + I.30X."8amp./TC I '.FS.1./. + I.33X.' alfa I '.F10.4. + l.333.'1-alfa I '.FIO.4) ---lain routine IRITE(7.100) 10 IRITE(7.110) [display menu Options 20 'RITE(7.120) READ(5.130.ERRI20) IOP ‘ IF ((IOP.LT.I).OR.(IOP.GT.6)) GOTO 20 ----Goto the proper place GOTD(30.40.50.70.80.90) IOP Input new LSF controller gains 30 'RITE(7.140) l.'VoltIFt ' lproportional cart gain READ(5.‘.ERRI30) F(1) 32 IRITE(7.140) 2.'Volt-S/Ft ' lderivative cart gain lw(5s.e m-32) 3(2) 34 IRITE(7.140) 3.’Volt/Rad ' lproportional pend. gain READ(5.‘.ERRI34) F(3) 36 IRITE(7.140) 4.'Volt-SIRad' lderivative pend. gain RBAD(5.‘.ERR-36) F(4) 00T0 10 lreturn to cgains menu ---Input new Luenberger Observer gains 40 IRITE(7.150) l lget observer gain #1 READ(5.‘.ERRI40) F(5) 42 IRITE(7.150) 2 lget observer gain #2 RBAD(5.‘.ERRI42) F(6) 44 IRITE(7.150) 3 lget observer gain #3 READ(5.‘.ERRI44) F(7) 46 IRITE(7.150) 4 tget observer gain #4 REAIHS... “3.46) F(s) .0010 10 lreturn to cgains menu ---Input new observer system parameters 50 GOTD 65 GOO 99 50 'RITE(7.160) 'All' READ(5.‘.ERRI50) F(Il) 52 'RITE(7.I60) 'A2 ' READ(5.‘.ERRI52) F(IZ) 54 IRITE(7.160) 'A3 ' READ(5.‘.ERRI54) F(13) 56 'RITE(7.160) 'A13' Rm(5s.sM'5‘) F(l‘) 58 IRITE(7.160) 'A12' READ(5.‘.ERRI58) F(IS) 60 IRITE(7.I60) 'A6 ' READ(5.‘.ERRI60) F(16) 62 'RITE(7.160) 'A7 ' READ(5.‘.ERRI62) F(I?) 64 IRITE(7.160) 'A14' READ(5.‘.ERRI64) F(IS) 65 IRITE(7.160) 'C1 ' READ(5.‘.ERRI65) F(21) 66 'EITB(7.160) '02 ' 67 IRITE(7.I60) 'C3 ' READ(5.‘.ERRI67) F(23) 68 IRITE(7.160) '04 ' READ(5.‘.ERRI68) F(24) GOTO 10 ----Input the filter parameters 70 IRITE(7.170) 'POSN' READ(5.‘.RRI70) F(9) F(9) I 1.0‘I (1.0 + 17(9)) F(IO) I 1.0 - PG?) 72 'RITE(7.170) 'DERI' READ(5.’.BRI72) F09) F119) I 1.0 I (1.0 + F(19)) F(ZO) I 1.0 -' F(3) GOTD 10 lget A11 lget A2 lget A3 lget A13 lget A12 lget A6 lget A7 m: A14 lget C1 lget C2 lget C3 [get 04 lreturn to cgains menu lget position filter first lget derivative filter next [return to cgains menu GO 100 Check the status of all gains IRITE(7.200) 'All'.F(11).'A2 '.F(12).'A3 '.F(13).'A13'. + ‘ F(l4).'A12'.F(15).'A6 '.F(16).'A7 '.F(17).'A14'.F(18). + 'C1 '.F(21).'02 '.F(22),'C3".F(23).'C4 '.F(24) SN I (1.0 I F(9)) I 1.0 IRITE(7.210) 'POSN'.8N.F(9).F(10) 8N I (1.0 /F(19)) I 1.0 IRITE(7.210) 'DERI'.8N.F(I9).F(20) GOT!) 10 lreturn to cgains menu IIIIIEXIT to the main program 90 RETURN END (365(56563 101 SUBRWTINE (DNTRL This is the control subroutine which sets up the clocked interrupt service control routine. and starts the control action. REAL C(24) .F(24) .A(4.4) .D(4) .C(4.4) .D(4) REAL XN.VN.TN.IN.XO.T0.VO.IO.m.V(LT LOGICAL‘l IDATA.T11(8).T12(8) EXTBNAL 8031.81? [to use as subroutine arg. mm IADNII IISR. um. I“. ICG. ITCR comm: IADNZI IPSR.IPN.IER.IADVEC.IADERR COW IADII3/ ICSR.IAN.IDAR.IADBUF COIIDN IIISCI/ 1cm . ICEZ .PER. IFAST COIIDN IIISCZI 17.6. IRCZJRPZ [DUNN IIISC3I C.D.XO.VO.T0.'O.NN cannon ISTATE/ X.T.XN.VN.TN.IN.V(I.T.ERR Format Sta tements 100 FORIAT(/o/.I.22X.'RUN the Clocked Control Routiue'. + I.221.' ') 11o FORIAT(I.I.181.'Enter the smmc mmumcx (Rm): '.3) 120 FORIATUJJSL'ERROR. . .[Sampling frequency is too high.]') 130 mnuI(/.I.1sx.'moe. . .[Sampling frequency is too low.]') 140 FORIIAT‘(/.I.1SX.'BRG. . .[Sampling rate TOO RIGR for system.]') 150 FORIIATUJJSL'Actual sampling freq. I '.E11.4.' Es.') 160 FORIAT(/.18X.'Actul mp1. ”glad - 5811.4” SCCe'e/el) 170 FORIIAT'(I.I.1SX.'Press ANY IE! to STOP active control. . .') mo FORMAT(I.I.1BX.'Are you ready to begin control [LN]. . . '.3) 190 FORIIAT(I.I.201.'1.) Derivative State Approximation'. + I.2OX.'2.) Continuous Luenberger Observer'. + I.201.'3.) Discrete Luenberger Observer') zoo menu/Janna“ Selection: '.i) 210 FORIATUI) 220 FORIAT('+'.ZOX.'Turn OFF the Line Time Clock. . .') ' 230 FORIAT‘(I./.1SX.'BROR. . .[Pendulum out of controllable range]') 240 maufl/Jasxun system near the origin [1.N]. . . '.i) IBEL-I "7 IPR I 7 [highest priority interrupt IFAST I 0 [reset too-high flag FIAT I 200.0 [set max. sampling frequency “LT I 0 [initial voltage as I 0 [initial error [IN I 1 C! I I1225.2 [sensor calibration on r or I I1241.3 [sensor calibration on theta CV I (4(35.I20.)I1.58 [voltage output calibration 102 C ---Convert real gains to controller gains C 0(1) I F(l) I CV I ex [[1 I 0(2) I F(2) I CV I C! [R2 LSF controller 0(3) I F(3) I CV I CT [R3 gains 0(4) I F(4) I CV I CT [[4 I C 0(5) I F(5) [01 I 0(6) I F(6) [02 Observer 0(7) I F(7) I CT I C! [03 gains 0(8) I F(S) I CT I C! [04 I C 0(9) IF(9) [position 0(10) I F(lO) [filter C 0(11) I I F(ll) [A11 0(12) I I F(l2) I C! I CT [A2 0(13) I F(13) I C! I CT [A3 I 0(14) I F(14) I C! I CV [A13 Observer system 0(15) I F(15) I CT I C! [A12 para-stars 0(16) I F(16) [A6 I 0(17) I I F(17) [A7 0(18) I I F(lS) I CT I CV [A14 C 0(19) I F(19) [derivative 0(20) I F(20) [filter C . 0(21) I F(21) I C! [ 0(22) I I F(22) I C! [coulomb damping 0(23) I I F(23) I CT [coefficients 0(24) I F(24) I CT [ C C Set up discrete observer matrix C A(1.1) I I F(5) A(l.2) I 1.0 A(1.3) I 0.0 A(1.4) I 0.0 A(2.1) I F(14) I F(l) I F(6) A(2.2) I F(14) I F(2) I F(ll) A(2.3) I (F(14) I F(3) I F(12)) I C! I CT A(2.4) I (F(l4) I F(4) + F(13)) I C! I CT A(3.1) I I F(7) I CT I C! A(3.2) I 0.0 A(3.3) I 0.0 A(3.4) I 1.0 A(4.1) I I (F(lS) I F(l) + F(8)) I CT I CX A(4.2) I I (F(lS) I F(2) I F(15)) I CT I C! A(4.3) I I F(lS) I F(3) + F(l6) A(4.4) I I F(lS) I F(4) I F(lT) C 3(1) I F(5) 3(2) I 8(6) 8(3) I F(7) I CT I C!- 000 103 8(4) I F(S) I CT I CX III-ISet up clocked ISR IRITE(7.100) [intial header 'RITE(7.190) 5 IRITE(7.200) [choose state estimation READ(5.210.ERRI5) IOP IF ((IOP.NE.1).AND.(IOP.NE.2).AND.(IOP.NE.3)) GOTO 5 00TO(10.15.20) IO? 10 I I INTSETIIADVEC.IFR.1.RBNLSF) [attach to AID done vector IF (I .m. 0) COT!) 25 IRITE(7.I)'INTSET’error II AID Vector. CODE I '.I GOTO 10 15 I I INTSET(IADVEC.IFR.2.RDNLSF) [attach to AID done vector IF (I .80. O) GOTO 25 'RITE(7.I)'INT‘SET‘ error II AID Vector. CODE I '.I GOTO 15 20 I I INTSET(IADVEC.IFR.3.RUNLSF) [attach to AID done vector IF (I .m. 0) GOT!) 25 IRITE(7.I)'INTSET error II AID Vector. CODE I '.I GOTO 20 25 1 - mmmumm.1m.4.mmsm [attach to AID error vector IF (I .m. 0) GOT!) 30 IRITE(7.I)'INTSET error II AID Error Vector. CODE I '.I GOTO 25 -----Input the sampling frequency 30 IRITE(7.110) [get the sampling frequency READ(5.‘.mI30) mm IF ((FRED.LE.FIAX).AND.(FRED.GTLO.)) GOTD 35 lRITE(7.120) [sampling frequency too high GOTO 30 ICalculate the best base clock rate 35 IR I 1 [start at highest clock rate 40 TICK I (10.0II(7IIR))IFRED IF (TICK .LT. 32767.) GOTD 45 [integer out of range IR I IR + 1 [next lower base frequency 000 0 GOO GOO (306656560 600 000 104 IF (IR .LE. 7) GOT!) 40 IRITE(7.130) [sampling frequency too low GOT!) 30 I—IICalculate the ticks for IBPR 45 ITICR I IFIX(-l.0I(TIC!+0.5)) [nearest integer Calculate the actual sampling frequency and period an I (10.0II(7IIR))IFL0AT(IITICR) PER I 1.0/Rm IRITE(7.I50) mm IRITE(7.160) PER Calculate the discrete observer matrix system IF (IOP .NE. 3) GOT!) 50 CALL DISGT(4.1.A.B.C.D.PER.10) IRITE(7.I)' ' IX) 48 I I 1.4 IRITE(7.‘) (C(I.J).JI1.4).D(I) 48 CONTINUE 'RITE(7.I)' ' Set up the clock status and AID status registers 50 IRATEI (IE IS) +3 ICRI I "40140 + ICG I (2II8) [set up initial sample ICE2 I "40001 + IPG I (2II8) [set up second sample III-Ilake sure the line time clock is turned off 55 CALL TIE(TII) [get the first time CALL FAUSE(60.ITCK) [wait for 1 second CALL TIIE(T'I2) [get the second time IF ((1'11(s).m.m(8)).mn.(111(7).m.m(7))) GOT!) 60 CALL IPOKE(IPII.IBH.L) IRITE(7.220) [message to user GOT!) 55 -Ready to begin active control 60 'RITE(7.170) [user instructions IRIT'E(7.180) [stop message CALL PADSE(40.ITCR) [wait for printer. C 000 (500qu COO 65 105 Disable keyboard and wait for input [1.N] I I IFEEKUISR) .MD. "177677 [reset bit 6 of IISR CALL INKEUISLI) I I IFEEK(IRSR) .MD. "200 [test bit 7 of IKSR IF (I .m. 0) 0010 65 IDATA I IPEEHIKII) [a key has been pressed IF ((IDATA.EOJ!').(I.(IDATLNJN'H CALL IFOIE(IPIR.IDATA) IF (IDATA .m. '1') GOTO 70 IF (IDATA .m. 'N') GOTO 85 CALL INIEUPM. BEL) 0010 65 ---Disabl e the printer 70 72 + I I IFEEIUPSR) .010. ”177677 [reset bit 6 of IPSR CALL IPOKE(IPSR.I) 'RIT'E(7.I)' ' 'RITE(7.I)' ' Set up initial conditions for the system CALL SAIFLEUPGJPVALJFLAG) [get pend. position CALL SAIPLEUCCRJCVALJFLAB) [get cart position 1 I (IR(2 I ICVAL) [initial T I (IRFZ .. IPVAL) [positions IN I 1 TN I T VN I 0 [initial IN I O [velocities Check for pendulum too far from stable origin IF ((ABS(XICX).LE.O.5).AND. [six inches from origin (ADS(T'ICT).LE.0.175)) 0010 74 [ten degrees from origin IDATA I IPEEHIIDR) [reset the keyboard status I I IEEXUPSR) .on. "100 [set bit 6 of IPSR CALL IFORE(IPSR.I) IRIT'E(7.I)' ' IRITE(7.240) 0010 65 Start clock and ISR 106 74 CALL IPOIE(IBPR.ITICE) CALL IFOKE(ICSR.IRAT'E) [start clock ticking CALL INKE(IAH.IGI) [start AID conversion C IF (101’ .m. 1) 0010 80 C C I'ait for system to stabilize/keyboard input C 75 IF (IFAST .NE. 0) 0010 85 I I IPEEKURSR) .AND. "200 [test bit 7 of IKSR IF (I .m. 0) 0010 75 C IDATA I IFEEUIIN) [a key has been pressed [IN I IOP [system is no. stabilized C IF (IDATA .NE. '0') com 85 [check for a go on observer C C III-I'ait for keyboard input or too-fast error C 80 IF (IFAST .NE. 0) 0010 85 C 01000 FORIIA1‘('+' .4(2X.EIO.3)) D IRITE(7.1000) IN.VN.TN.IN I) CALL PADSE(10.ITCR) C I I IFEERUISR) .AND. "200 [test bit 7 of IISR IF (I .m. 0) 0010 80 C C I-IIIEnsble the keyboard and printer C 85 I I IPEEUIRSR) .OR. "100 [set bit 6 of IISR CALL IFOKE(IKSR.I) C I I IFEEK(IPSR) .OR. "100 [set bit 6 of IPSR CALL IFOKEUPSRJ) C C Check for too-fast error C IF (IFAST .m. 0) 001095 IF (IFAST .m. 2) 001090 C IRITE(7.I)' ' IRITE(7.140) [print an error nessage 001095 C 90 IRITE(7.I)' ' IRITE(7.230) [print an error message C C III-ITurn off clock and AID converter C 95 CALL IFOIE(IAN.O) CALL IFOKE(ICSR.0) 'RIT‘E(7.I)' ' 107 C -----Zero output voltage C IZERO I "4000 CALL IFOEE(IDAR.IZRO) C RETURN C OOOOOGGOGO GOO GOO 108 SUERWTINE DISQ'HN. N. A.E. AD. 30. 1'. IAXIT) This subroutine computes the discretized matrices AD. and DD from the continuous syst- matrices. DX(t) I A I X(t) + B I U(t) X(k+I) I AD I X(k) + ED I U(k) Declare all variables DIINSION A(N. N) .AD(N.N) .D(N.I) .BD(N. I) . + A1(IO.10).A2(10.10).C(10.10) REAL A.D.AD.BD.A1.A2.C.KF.T INTEGER N.I.IAXIT.I.J.R.L III-IInitialixe the matrices [F I 1.0 DO 20 I I 1.N DO 10 J I 1.N IF (I .NE. J) 0010 5 A1(I.J) I 1.0 AD(I.J) I 1.0 C(I.J) I T 0010 10 5 A1(I.J) I 0.0 AD(I.J) I 0.0 C(I.J) I 0.0 ENDIF 10 CONTINUE 20 CONTINUE --IICompute the matrix exponential D0 80 R I 1.IAXIT DO 50 I I 1.N D0 40 J I 1.N A2(I.J) I 0.0 DO 30 L I 1.N A2(I.J) I A2(I.J) + A1(I.L) I A(L.J) 30 CONTINUE 40 CONTINUE 50 CONTINUE IF I IF I I DO 70 I I 1.N D0 60 J I 1.N AD(I.J).I AD(I.J) + A2(I.J)I(TIIK)IKF C(I.J) I C(I.J) + A2(I.J)I(TII(I+1))I(RFI(I+1)) IQ A1(I.J) I A2(I.J) 6O CONTINUE 7O CONTINUE 80 CMTINUE C no 110 I I 1.N I!) 100 J' I 1. ED(I.J) I 0. 0090 L I 1 N ED(I.J) I ED(I.J) + C(I.L) I E(L.J) 90 CONTINUE 100 MTINUE 110 C(NTINUE C I 0 RETURN C BID OOOOOOOGOOOOOOOOGO0000006000000 006600 110 SUERGJ TIN E RUESF ( ID) This is an interrupt service routine to get clocked samples of the cart and pendulum positions. file system states are calculated in the observer equations. or by derivative ap- proximations. The linear state feedback control voltage is calculated and applied to the motor. IIIIIDefinition of parameters 0() 0(1) ' proportionsl cart gain 0(2) I derivative cart gain 0(3) I proportional pendulum gain 0(4) I derivative pendulum gain 0(5) I observer gain #1 0(6) I observer gain #2 0(7) I observer gain #3 0(8) I observer gain #4 0(9) I filter perameter (alfa) 0(1O)I filter parameter (l-alfa) 0(11)‘ . I . observer system parameters . I 0(18)I 0(19)I deriv. filter parameter (alfa) 0(2O)I deriv. filter parameter (l-alfa) 0(21)I I 0(22)I coulub damping 0(23)I coefficients 0(24)I I REAL 0(24) .F(24) .VG.T.PER.HR.X. T REAL DX.DV.DT.D'.XN.VN.TN.'N.TIAX.VIAX REAL XO.VO.10.IO.C(4.4).D(4) INTER IA(2).IB(2).IVS.I'S.IV.II EQUIVALMCE (VN.IA). (IA(I) .IV) . ('N.IB) . (13(1) .N) (”RUN IADII3I ICSR.IAN.IDAR.IADEUF CONIDN INISCII mu . 102 . PER. IFAST (OMEN INISCZI F.0.IR(2.IRPZ WHEN IRISCSI C.D.XO.V0.10.I0.NN (ONION ISTATEI X.T.IN.VN.1N.'N.V(LT.BR DATA TIAX. VNAXI541 .6 .2047 .5/ ISelect entry: ID I 1 . AID Sample Done [Deri. approx.) ID I 2 . . . AID Sample Done (Cont. Obs.] ID I 3 . . . AID Sample Done [Disc. Obs.) ID I 4 . . . AID Sample Error 00T0 (10.5.5.50) ID IRITE(7.I)'RUN.SF entry error. ID I '.ID GOO 0 00000 ('5 COO 00000 006 GO 111 CALL EXIT 5 0010 (10.20.30) NN <<<<< DERIVATIVE STATE APPROHNATIONS )>>>) Get a new 1 position for the cart 10 X I 0(9)I(IR(2 I IFEEHIADBUFH + 0(10)IXN CALL IPORE(IAIR.ICR2) [start next sample Calculate a net velocity VN I 0(19)I(X I XN)IFER + 0(2O)IVN Get a new theta value for pendulum T I 0(9)I(IRFZ I IPEER(IAIBUF)) + 0(10)ITN IF (AES(T) .01‘. TIA!) 0010 50 [pend. out of 25 deg. range CALL IFORE(IADR.I(31) [reset AID status register I-IIICalculste a new angular velocity 'N I 0(19)I(T I 0()/PER + 0(2O)IIN IIIIIShift neu variables into the old va.riables 3888 E! EEEE H” ”II-Use these approximated states for LSF control law 0010 40 (<<<< CONTINUOUS LUENIEBGB (BSERVER >>>>> Get a new 1 position for the cart 20 X I (IRCZ I IFEERUADEUFH 20 X I 0(9)I(IR(2 I IPEER(IADEUF)) 4' 0(10)IX CALL IFORE(IAM.ICE2) [start next sample ERR I X I IN Get the sign of the twoivelocities non" 000 066 O OGOGO GOO 112 IVS I 1 + 2 I ( (IV .AND. "100000) .80. "100000) IVS I 1 + 2 I ( (II .AND. "100000) .N. "100000) ICalculste the observer derivatives or I VN + 0(5)IERR [IV I 0(11)IVN + 0(12)ITN + 0(13)IIN + 0(14)IV(I.1'+ 0(6)IERR + + 0(21)I1V8 + 0(22)IIIS - n'r - IN + 0(7)Im DI I 0(15)IVN + 0(16)ITN + 0(17)IIN + 0(18)IV(LT + 0(8)IERR + + 0(23)IIVS + 0(24)II'S III—Approximate states from derivatives (Euler) m-m+nxom VNIVN+DVIPER TNITN‘i-DTIPER INIIN+NIPER Get a new beta value for pendulun T I (IRPZ I IPEER(IADBUF)) IF (ARS(T) .0T. TIA!) 0010 50 [pend. out of 25 deg. range CALL IFORE(IAII.ICEI) [reset AID status register III-IUse these approximated states for LSF control law 0010 40 (((<< DISQE‘E LUMBEOR (BSERVE >>>>> IGet a nev 1 position for the cart 30 X I 0(9)I(IR(2 I IPEEIUADEUFH I 0(10)IX CALL IFORE(IAm.ICR2) [start next sample III-ICalculste the observed states IN I C(1.1)IXO + C(1.2)IVO + C(1.3)ITO + C(1.4)I'O + D(1)IX VN I C(2.1)IXO + C(2.2)IV0 + C(2.3)ITO + C(2.4)I'0 + D(2)IX TN I C(3.1)IXO + C(3.2)IVO + C(3.3)IT0 + C(3.4)I'0 4' D(3)IX IN I C(4.1)IXO + C(4.2)IV0 4’ C(4.3)IT0 + C(4.4)I'O + D(4)IX III-IShift new variables into the old variables XOIXN VOIVN TOITN 'OI'N 0000060 0 COO 113 III-I0et a nev Theta velue for pendulum T I (IRPZ I IPEER(IADBUF)) IF (ABS(T) .01. TIA!) 0010 50 [pend. out of 25 deg. range CALL IPORE(IAN.ICEI) [reset AID status register Use these approximated states for LSF control law <(<<( LINER STATE FEEDACK )>)>> Calculate the LSF control law 40 VET I 0(1)IXN + 0(2)IVN + 0(3)ITN + 0(4)IIN Check for voltage out of range IF (WIT .LT. IVIAX) V0.1 I IVIAX IF (VET .0T. VIA!) “ET I VIAX “(IT I IFIX(V(I.T + 2048.0) CALL IFORE(IDAR.IV(I..T) [apply the voltage RETURN IIIIIAID Sanpl e error 50 CALL IPOKE(IAm.O) [turn off AID CALL IFORE(ICSR.O) [turn off RTC IFAST I 1 [set the too-fast flag IF (ARS(T) .0T. T'IAX) IFAST I 2 [pendulum out of range RETURN 34D “COCO ('3 66000 O 114 SUBRWTINB PAUSE(NT‘ICIS . ITCI) This subroutine is approximately calibrated to pause in ITC! intervals. as specified on input. There are 60 ticks in one second. no 20 I I 1.NTICE8 D0 10 J I 1.1TCR [one tick per outer 10 CONTINUE [do loop step on (I) 20 CONTINUE RETURN mD SUERWTINE SETICKUD) This is an interrupt service routine that is called uhen the real tine progralnable clock.has generated an interrupt after one tick (1/60 sec.). mum ITIIE/ I. IND IF (ID .m. 1) com 10 [check for entry error 'RIT'E(7.‘)'SETICR entry error. ID I '.ID CALL EXIT 10 IEND I I I I 10000 [reset do loop counter RETURN APPENIDIX K A fifth order linear matrix representation of the inverted pendulum system equations was developed in Appendix B. This linear system model can equivalently be written as: x 0 1 0 0 x 0 0 0 d v 0 -A1 -A2 A3 v M -Cl CZ sgn(v) - 0 I 0 0 0 1 0 + 0 i + 0 0 sgn(u) (Kl) dt 0 0 A5 A6 -A7 0 -A8. C3 -C4 (dildt) I {-01 v + [-A10] i + [P1] V ([2) The fifth order system has been partitioned into a fourth order system ([1) coupled to a first order system ([2). The fourth order model represents the cart/pendulum subsystem. and the first order equation represents the DC servo motor dynamics. It is well known that the motor dynamics are much faster than the rest of the system. An eigenvalue study based on.the parameters in Appendix I. has shown the motor eigenvalue to be three orders of magnitude greater than the pendulu system ei genvalues. Because of the fast eigenvalue of the motor, the armature current will approach a steady state value much faster than any other state [4.9]. The derivative of the current state may therefore be assumed sero. leaving a constraint relation: [P1] V I [D] v + [A10] i (K3) 115 116 The fifth order systcm has now been reduced to a fourth order model plus a constraint. Substitution of this constraint equation into the fourth order model ([1). yields a new fourth order model. This neu model is given by: x. 0 l 0 0 x 0 0 0 d v 0 -A11 -A2 A3 v A13 -C1 C2 sgn(v) -— 0 I 0 0 0 1 8 + 0 V + 0 0 sgn(m) dt 0 0 A12 A6 -A7 0 -A14 C3 -C4 where the new matrix elements are defined to be: All I A1 + A4 0 / A10 (R4) A12 I A5 + M. .0 / A10 ([5) A13 I A4 Pl I A10 ([6) A14 I A8.Pl / A10 ([7) This new system model is an approximation based on fast motor dynamics. The fifth order model can be replaced by this fourth order model which indirectly includes information about the motor. The new model has a voltage input just as the real systcm does. yet the motor state. (armature current). is not directly included in the model. Because of this. the approximate model facilitates easier design of a Luenber ger observer . APPENDIX L Ihen implementing a real digital controller. the measured quantities are not the actual physical .states of the system. The measured states are discrete. or quantized integers. which are proportional to the actual states at the time of sampling. This linear transformation can be expressed by: z-ty. '(m where: j I the real measured state (physical coordinates) I; I the discrete measured state (integer numbers) The complete matrix transformation (L1). is given by: x C3 0 o o Ix v . 0 C? o o Iv (L2) ' 0 O 0 C" o 19 m o o 0 ca‘ 1.. where the two constants are the sensitivities of the measurement sensors. and are defined in Appendix I. . The Linear State Feedback control law can now be transformed to operate on the discrete coordinates. This will allow a computer algorithm to directly use the discrete coordinates. Using equation (L1). this transformation is given by: 117 118 V= tz-=xf§ (u) s‘-Izy (M) For implementation. the actual voltage applied to the DC servo motor must also be discretixed by: VOLT' I V CV (L5) where CV is also defined in Appendix I. The Luenberger observer has been designed to estimate the actual system states. Since the LSF law has been transformed. in (L3). to utilise the available discrete coordinates. the observer must also be transformed to provide the LSF control law uith estimated discrete states. The original observer equations are given by: A A 2Q ‘ I) + C san') (L6) he) a b» on) + I: ‘< 4. [65 Using the transformation (L1). on both the actual and the observed states. (L6) can be written as: 0 e 0' e e 0 e ‘5 I} I A _I_§+§ VG.T+ col1(§)(L§-_§)+§ sgn(I') (L7) The matrices in (L7) have absorbed the transformation of coordinates. and are given by: 119 A‘ - N..“ AL] (18) g‘ - c? [y‘ a] (19) g‘ - IL." 6 g _1_.J (L10) 9. 8 - [L—I' 9] (L11) The leading negative sign in (L11) comes from the transformation: sgn(l) I sgn(L. L3) (L12) For this particular laboratory setup. the sensitivity parameters associated with the transformation matrix L are all negative. Therefore equation (L12) can be written: sgn(L Q) I - sgn(n) (L13) The transformed observer system (L7). is now suitable for implementation on the computer. The observer has the discretixed voltage input WIT. and calculates the discretixed output states. These states are then used directly by the transformed LSF control law to calculate the new discretixed voltage.