MECHANICAL DESIGN AND INTERNET-BASED CONTROL OF THE GYROBOT ADRIAN JENKYN LEE. B.S., Northwestern University, 1998 B.S., Northwestern University, 1998

Transcription

1 MECHANICAL DESIGN AND INTERNET-BASED CONTROL OF THE GYROBOT BY ADRIAN JENKYN LEE B.S., Northwestern University, 998 B.S., Northwestern University, 998 THESIS Submitted in artial fulfillment of the reuirements for the degree of Master of Science in Mechanical Engineering in the Graduate College of the University of Illinois at Urbana-Chamaign, Urbana, Illinois

2 MECHANICAL DESIGN AND INTERNET-BASED CONTROL OF THE GYROBOT Adrian Jenkyn Lee, M.S. Deartment of Mechanical and Industrial Engineering University of Illinois at Urbana-Chamaign, Mark W. Song, Advisor In this thesis we consider the control of the Gyroscoic Pendulum, an underactuated endulum, consisting of a single link with an inertial, motor-driven flywheel mounted at the free end. We first discuss the mechanical design of the Gyrobot, including the motor selection and link stiffness reuirements, lus the controller interface from a PC. We next derive the motion euations, and determine emirically and theoretically the system arameters by techniues utilizing conservation of energy and free resonse oscillation freuency. Both Non-collocated and Collocated Partial Feedback Linearization swing u controllers were investigated and designed to yield linearized closed-loo dynamics. Balancing controllers including classical PD and PID, state feedback, and Feedback Linearization algorithms are develoed to stabilize the Gyrobot in the unstable vertical euilibrium. Root locus and ole lacement techniues are used for obtaining the desired closed-loo resonses of the PD and state feedback controllers resectively, while energy minimization techniues are used to develo the Feedback Linearization controller. Hysteresis switches are develoed to combine the swing u and balancing controllers. With the conclusion of the design of the Gyrobot and obtaining reliable and reeatable controlling algorithms, an internet-based controller was develoed using Java, which allows the remote oeration of the Gyrobot through the world wide web. As a result, internet-based laboratories can be conducted by students studying controls related material through their home PC s. iii

3 ACKNOWLEDGMENTS I would first like to thank Professor Mark W. Song for his idea of adding the Gyrobot to his arsenal of underactuated systems. His extensive study of nonlinear controllers on the Acrobot heled focus the swing u control issues of this new system to a secific disciline. I owe overwhelming gratitude to Dan Block, who day after day has listened to and romtly assisted my uestions, and for teaching me a riceless amount of ractical control alications. Thanks to Dan s solid develoment of motion control hardware and software, the internet control develoment rocess was far more realizable. Thanks definitely goes to Henry Hahn, who dedicated many of his hours in develoing the Java to Visual Basic communication code, the core of our internet control. Many thanks goes to Greg Cler for machining three sli-ringed versions of the Gyrobot, for his suerior uality of workmanshi on each assembly, and for romtly altering the arts for any small modifications. Finally, I would like to thank my family for suorting my decisions throughout undergraduate and graduate studies, and for roviding me with the oortunity to develo a assion for engineering. iv

4 TABLE OF CONTENTS Page LIST OF TABLES.vii LIST OF FIGURES..viii. INTRODUCTION MECHANICAL DESIGN AND CONTROLLER INTERFACE SYSTEM MODEL SYSTEM IDENTIFICATION. 4. CAD Solid Model.. 4. Energy Euation Method. 4.3 Free Resonse Analysis Comarison of Results Motor Parameters Natural Freuency 8 5. SWING UP CONTROL Non-collocated Partial Feedback Linearization Collocated Partial Feedback Linearization. 5.3 Sinusoidal Excitation BALANCING CONTROL Classical PD Control Classical PID Control State Feedback Feedback Linearization COMBINING THE CONTROLLERS IMPLEMENTING THE CONTROLLERS Wincon RTX...4 v

5 8. RTSS and Visual Basic SIMULATION RESULTS EXPERIMENTAL RESULTS 5. INTERNET-BASED CONTROL APPROACH.57. TCP/IP Visual Basic to Java Alet Communication Interface Otions 58.3 Data Transmission..6. CONCLUSION 65 APPENDIX A APPENDIX B APPENDIX C SIMULATION FILES 66 WINCON SIMULINK FILES 74 MECHANICAL DRAWINGS...8 APPENDIX D RTSS C CONTROLLER CODE APPENDIX E APPENDIX F VISUAL BASIC SERVER INTERFACE CODE. JAVA APPLET CODE.8 BIBLIOGRAPHY.6 vi

6 LIST OF TABLES Page Table 4.4. System Parameter Comarison 6 Table 4.5. Pittman Motor Parameters... 7 vii

7 LIST OF FIGURES Page Figure. Gyrobot In Rest Position Figure. Gyrobot At Stable Euilibrium Figure.3 Gyrobot At Unstable Euilibrium Figure.4 Gyrobot Web Cam View... 6 Figure 3. Definition Of Coordinate System.. 7 Figure 5. Phase Portrait Of The Zero Dynamics. 4 Figure 5.3 Figure 6.. Sinusoidal Swing U Control Effort 6 Block Diagram Of Classical PD Controller.8 Figure 6.. Root Locus Of sq (s)/τ(s). 9 Figure 6..3 Reduced Block Diagram Of PD Controller. 9 Figure 6..4 Root Locus Of PD Control On New Plant, H(s) 3 Figure 6..5 Final Root Locus Design. 3 Figure 6.. Root Locus Of PID Control On Plant. 3 Figure 6.. Bode Plot Of PID Control On Disc Velocity Transfer Function. 3 Figure 6..3 Bode Plot Of Second Order DC Filter. 33 Figure 9. Non-collocated Swing U Simulation Resonse 43 Figure 9. Collocated Swing U Simulation Resonse 44 Figure 9.3 Sinusoidal Swing U Simulation Resonse 44 Figure 9.4 Linearized PD Balancing Simulation.. 45 Figure 9.5 Linearized PID Balancing Simulation. 46 Figure 9.6 State Feedback Balancing Simulation. 46 Figure 9.7 Feedback Linearization Balancing Simulation 47 Figure 9.8 Non-collocated Swing U With PD Balancing Simulation Resonse Figure 9.9 Collocated Swing U With PD Balancing Simulation Resonse viii

8 Figure 9. Sinusoidal Swing U With PD Balancing Simulation Resonse 49 Figure. Non-collocated Swing U Exerimental Resonse.5 Figure. Collocated Swing U Exerimental Resonse 5 Figure.3 Sinusoidal Swing U Exerimental Resonse 5 Figure.4 Linearized PD Balancing Exerimental Resonse.. 5 Figure.5 Figure.6 Figure.7 Linearized PID Balancing Exerimental Resonse.53 State Feedback Balancing Exerimental Resonse.54 Feedback Linearization Balancing Exerimental Resonse 54 Figure.8 Non-collocated Swing U With PD Balancing Exerimental Resonse. 55 Figure.9 Collocated Swing U With PD Balancing Exerimental Resonse. 56 Figure. Sinusoidal Swing U With PD Balancing Exerimental Resonse. 56 ix

9 . INTRODUCTION To further the field of underactuated lanar robotic research at the University of Illinois, an inertial flywheel-driven endulum named the Gyrobot has been designed to accomany the reviously develoed Pendubot and Acrobot. The Gyrobot is a single link endulum with an inertial, motor-driven flywheel located at the free end of the link. The motor controls the acceleration of the flywheel, which assively transfers energy in the flywheel to energy in the link. A change in velocity (acceleration) of the flywheel causes a change in link angle, which through careful control, enables the endulum to swing u from the downward vertical stable euilibrium to the inverted unstable euilibrium. Since the motion euations of the Gyrobot are much simler than both the Pendubot and Acrobot, containing only one nonlinear term, feedback linearization techniues can be easily used to balance the Gyrobot. Alternatively, the classical aroach of linearizing the motion euations about the euilibrium and utilizing root locus techniues can be imlemented to lace the oles of the closed-loo system to any desired location. State feedback can be easily alied, with analysis of controllability and observability of the disc angle. With the less comlex motion euations and nonlinearity, this system lends itself as a very good laboratory develoment for controls-related courses, and in articular, the develoment of an internet-based laboratory. The thesis first tours through the design of the Gyrobot, from its mechanical design to the controller interface. Then, we ste through the derivations of the motion euations, using Lagrangian dynamics. Parameters are next determined through on-line schemes of conservation of energy, and free resonses due to nonzero initial conditions. Non-collocated and collocated artial feedback linearization algorithms are derived to roduce linear closed-loo dynamics, which are used to create the energy for swinging u the endulum close to the inverted osition. From here, the controller would switch to a balancing control. Classical methods of PD and PID control, state feedback, and feedback linearization techniues are derived and comared for stabilizing the Gyrobot. Ste resonses generated by an imulse disturbance torue can be measured for characteristics such as overshoot, daming, and time resonses.

10 The next sections of the thesis demonstrate the results of the simulation trials and comare them to exerimental resonses. Adatation of the Gyrobot to the internet will then be considered, after a final robust design has been constructed. A full-wire sli ring hels the Gyrobot exeriment become more reeatable, by reventing the tangling of wires through remote oeration. The Gyrobot is linked to a PC server using a Visual Basic interface, which in turn runs RTSS software. The Visual Basic interface incororates a communication rotocol, which enables it to talk to the Java alet running on a remote internet browser through TCP/IP. This allows the control of the Gyrobot to become accessible by students at their home PC s to conduct exeriments in varying the gains or even the tye of controller, while viewing a real-time animation and scoe lot of the resonse, lus watching the system through a webcam. Included in the Aendices are the simulink simulation and Wincon models, the mechanical CAD art drawings, the RTSS controller C coding, the Visual Basic interface code, and the Java alet code.

11 . MECHANICAL DESIGN AND CONTROLLER INTERFACE An AutoCAD drawing of the Gyrobot is shown in Figure., deicted in its stable configuration. The base and atch anel was designed to be consistent with the Pendubot, a two-link underactuated endulum, for comatible use in the control system laboratories at the University of Illinois. The base accommodates the modularly designed encoders and shaft suorts resently used in these control labs. A 3/8 inch diameter steel shaft rotates the shoulder of the link on the rotational axis of the inertial frame encoder, and runs through two needle roller bearings. A secially designed aluminum couler solidly fixes the shaft and sli ring to the link. The distance between the two bearings was chosen to reduce the lay in directions other than the desired rotation. A rubber couler forms an easy connection between the shaft and the inertial frame encoder, while reducing the transfer of vibration and noise to the encoder. The base is mounted on a table such that the link hangs over the edge. Caution must be taken to roerly secure the table to hel avoid undesired swinging dynamics of the table as much as ossible. Although not with any theoretical intention, the length of the link was chosen to be 8 inches from the shoulder to the motor, for an aroriate length in our lab systems. The length, however, is roortional to the reviously studied endulums, the Pendubot and the Acrobot, and was short enough to cause the first vibrational mode to occur at a high enough freuency. The link was machined out of ¼ inch stock aluminum for rigidity. A second link was also Figure.: Gyrobot In Rest Position 3

12 made twice as long to allow the ossibility of counterbalancing ercentages of the motor/flywheel weight. This longer link accommodates cylindrical brass weights at one inch sacing, and is the link used throughout this thesis. A Pittman motor/encoder combination was mounted on the free end of the link for the drive torue. A 3.5 inch diameter brass flywheel slides on the motor shaft, which rovides the additional inertia for storing the energy from the motor acceleration. Several diameter flywheels were machined for analyzing the effects of inertia arameters. The motor was chosen with the reuirements of low weight, high torue, and a maximum of 4 V drive voltage for using the desired servo amlifier. A 4 VDC Pittman 9X37 brush tye motor was chosen for meeting these reuirements, lus the otion of a mounted otical encoder as one unit. Simulations verified that a motor with these weight and torue caabilities was sufficient to swing u the Gyrobot ast the vertical in 4 to 5 oscillations. A sli ring was added after reliminary designs to eliminate the ossibility of tangling wire from continuous rotations. The sli ring is caable of conducting u to 5 ams through each of its 8 rings. Since four contacts are needed for the motor encoder, the remaining four rings were couled in airs to allow a maximum of ams er lead to the Pittman motor. The sli ring is mounted rigidly on the shaft ost, while also fastened to the aluminum link couler, to allow shaft rotation. To hel reduce the friction in the sli ring radial bearings, low-seed, low-friction grease was alied. The inertial frame encoder is a uadrature otical encoder roviding 5 counts/rev, while the motor encoder is a uadrature otical encoder roviding counts/rev. A Pentium 3MHz PC euied with a DAC card and an encoder card rovides the interface between the system and a rogrammable microrocessor. The DAC card sends the control signal to an Advanced Motion Control PWM servo amlifier to convert the voltage DAC signal to a current signal. The amlifier was reset to current mode with a gain of.5 A/V, and using the torue gain given in the motor secifications, the voltage control effort was converted to a torue control effort. 4

13 Matlab 5.3 Simulink Wincon Real Time Worksho ackage was used to construct controller block diagrams, which are comiled and run by Windows NT. The interface allows the viewing and data recording of the link and disc angles and velocities, and the control effort for analysis and imlementation debugging. A 5 ms samling eriod was used to reduce rocessor time, and reduce the noise in angular velocity calculations. Figures. through.4 show ictures of the actual lant at rest and in action, as it balances at the vertical. Figure.4 shows a icture orthogonal to the rotational lane for ossibilities of vision control and internet camera angle. Figure.: Gyrobot At Stable Euilibrium 5

14 Figure.3: Gyrobot At Unstable Euilibrium Figure.4: Gyrobot Web Cam View 6

15 7 3. SYSTEM MODEL The euations of motion for the gyroscoic endulum can be found using Lagrangian dynamics. Writing the kinetic energy yields (3.) (3.) (3.3) While the otential energy consists of (3.4) (3.5) Now using LaGrange s euation, ( ) ( ) ( ),,,,,, I ML I T I ML I L m T I I Mv mv T P disc Q link P disc C link P disc C link P C & & & & & & & & & & & cos MgL mgl V Mgh mgh V P C Figure 3.: Definition Of Coordinate System

16 d dt T T & i i V i Q nc (3.6) The following two motion euations arise d dt d dt {( I ML ) & I ( & & ) link, Q disc, P { I disc P ( & &, ) τ m ( I ML I ) I disc, P m mgl MgL sin link, Q disc, P I disc, P mgl MgL sin disc, P && I && τ && && (3.7) (3.8) Where τ m is the torue alied by the motor, is the link angle, and is the disk angle. m mass of link M combined mass of motor, disc, and disc couler L distance from center of flywheel to shoulder I link,q combined inertia of link, couler, and shaft around oint Q I disc,p combined inertia of disc and motor armature around oint P g gravitational acceleration The above individual arameters are divided into three arameter euations. I link,q ML (3.9) I disc,p (3.) 3 ½ mgl MgL (3.) Exressing the euations of motion into a matrix euation yields the following: && ( 3 )sin && τ m (3.) Since the disc angle does not alter the state of the system, eliminating the disc angle,, as a state variable does not hinder the solution and control to the above euation. Hence, only the disc angular velocity will be found. Therefore, using the state variables 8

17 9 (3.3) the reresentation as state euations leads to (3.4) The inverse of the mass matrix is comuted as follows: (3.5) Using the inverse of the mass matrix, the state euations become (3.6) Where u τ m. 3 x x x & & m x x x x x τ sin 3 3 & & & ) / (/ / / / ) det( ) ( det inv A A adj A adj u x x x x x x / / / ) sin / ( ) sin / ( & & &

18 4. SYSTEM IDENTIFICATION 4. CAD Solid Model The masses and inertia of machined arts were calculated through AutoCAD solid model drawings. The mass of motor was determined from weighing, the inertia of motor was aroximated as a oint mass at distance L from rotational oint, and the motor armature inertia was taken from the sulied data sheet secifications. Mass of link: m.745 kg Mass of motor/flywheel: M.9 kg Center of flywheel to ivot: L.3 m Gravity: g 9.8 m/s I link,q e-3 kg m ML 4.487e- kg m I disc,p 8.374e-4 kg m ½ mgl. N m MgL.493 N m.4685 kg m.8374 kg m N m Taking into account the amlifier gain, K am, and the torue constant of the motor, K T, the arameters can be converted to units of voltage for the DAC inut voltage signal. Am Current Gain: Motor torue constant: K am.5 A/V K T.437 N m/a θ.8846 V s θ.536 V s θ V

19 4. Energy Euation Method One on-line identification method to determine the three system arameters uses a least suares algorithm. This articular method worked well for identifying the arameters for the Pendubot, a two-link underactuated endulum, and considering the gyroscoic endulum s less comlex nonlinear euations and fewer arameters, this method was hyothesized to work just as well. Starting with writing the kinetic energy as (4..) (4..) The otential energy is written as (4..3) Since the kinetic and otential energies are linear in terms of the arameters,, 3, a simle way to write these two energy euations is in the following form: (4..4) (4..5) For the gyroscoic endulum, DK and DV are (4..6) (4..7) (4..8) (4..9) (4..) (4..) ( ) [ ] ( ) K K D K T & & & & & & & & & & 3 cos V i i i i i i i i i i i i DV V V DK K K 3 DK DK DK & & & & & 3 cos DV DV DV

20 The energy theorem states that the change in energy euals the work of the alied forces. The alied force in this endulum is the motor torue acting over the disc angle, so the work is found by integrating the torue times angular velocity over the alied interval of time. & [ τ ] dt ( K( t ) V ( t )) ( K( t ) V ( t )) L( t ) L( t ) t m t & (4..) Where L(t) is the total energy at time t. Friction is not incororated in the calculation under the assumtion that the friction arameters will be much less than the motion euation arameters. The endulum system was designed with low friction bearings, and relatively low friction encoders in order to decrease the effects of daming in the swing u oscillation, and making the balancing control non-trivial. Defining the difference in total energy over the time interval as DL T [(DL (t )-DL (t )) (DL (t )-DL (t )) (DL 3 (t ) DL 3 (t ))] (4..3) DL T L(t ) L(t ) (4..4) Euating the change in total energy, DL T, to the work alied gives t t & τ m dt DL T (4..5) And therefore, the arameters [ 3 ] T can be found by least suares. The Matlab m-file energy.m was used to imlement this least suares algorithm. The endulum was driven by a eriodic wave tuned to the exerimental modal freuency of.8 Hz to give sufficient excitation resonse. The m-file first takes the data accumulated from an exerimental trial, and defines, d, d, tau, and t as the link angle, link angular velocity, disc angular velocity, inut torue, and time resectively. Since the control effort, tau, is in terms of a voltage signal from the DAC outut, the arameters would have units of volts and volts s. The m-file solves the least suares roblem in the following way:

21 dl.5*d.^; dl.5*d.^ d*d.5*d.^; dl3 cos(); taud tau.*d; for i : (length(dl)-) DL(i,) dl(i) dl(); DL(i,) dl(i) dl(); DL(i,) dl(i) dl(); It(i,) traz(t(:i),taud(:i)); end P DL\It With the 3 volt eak-to-eak.8 Hz suare wave inut, the following arameter values were obtained: θ.6 V s θ.34 V s θ V This leads to the arameters in SI units of.65 kg m.5475 kg m N m With the 3 volt eak-to-eak.8 Hz suare wave inut, the following arameter values were obtained: θ.93 V s θ.37 V s θ V 3

22 This leads to the arameters in SI units of.598 kg m.769 kg m N m Using either a sine or triangle wave for the control effort yielded oorer results than the suare wave, since the voltage level is below the motor stall level for too long a time eriod. Therefore, only a suare wave was used in this method to find the arameter values. 4.3 Free Resonse Analysis If the motor armature is fixed so that the flywheel is not allowed to rotate, then the euation of free resonse becomes ( ) && ( ) 3 sin && 3 sin (4.3.) (4.3.) The encoder reads the angular osition, (t), only, and therefore, the angular acceleration needs to be aroximated. However, aroximation of acceleration from osition measurements lead to a very noisy waveform. To hel alleviate this, the angular velocity waveform will first be generated and then assed through a third order filter. This angular velocity waveform will then be used to generate the angular acceleration waveform in a similar fashion, also utilizing the same third order filter. We do this by first aroximating the angular velocity, θ & () t θ () t θ ( t ) Τ (4.3.3) Then filter over three consecutive values θ& () t θ& () t θ& ( t ) θ& ( t ) 3 (4.3.4) Next, aroximate angular acceleration, 4

23 && θ () t θ& () t θ& ( t ) Τ (4.3.5) Finally, filter over three consecutive values && θ () t && θ () t && θ ( t ) && θ ( t ) 3 (4.3.6) Since the angular acceleration euals zero eriodically, finding the / 3 ratio around these times becomes difficult due to large noise errors. By looking at a lot of / 3 versus time, and by erasing the noise eaks, a relatively constant line can be seen to arrive at the arameter ratio. P avg / 3.35 s Parameter 3 are easily comuted based on the weighed masses and dimensions, so the inertia of the link/couler/shaft in is the remaining constant left to be identified. I link,q.944 kg m.793 kg m If friction is included, then the euation of motion is in the following form: ( ) && b& ( ) 3 sin && 3 3 b& sin (4.3.7) (4.3.8) From trial and error in fitting a free resonse simulation to exerimental data, the linear friction coefficient was aroximated to be B.6 N m s Following the same rocedure as without friction, the inertia of the link/couler/shaft was found to be, I link,q.3398 kg m.8374 kg m 5

24 4.4 Comarison of Results Table 4.4.: System Parameter Comarison Parameter AutoCAD Energy Euation Method: Energy Euation Method: Free Resonse solid modeling.8hz Suare Wave.8Hz Suare Wave Analysis (kg m ) (kg m ) n/a 3 (N m) n/a The arameters found by the AutoCAD calculations are used in the simulation and exerimental Simulink block diagrams, since these arameters do not exhibit error from the friction within the sli ring assembly. From the comarison above, using the.8hz suare wave in the energy euation method roduced results with the closest match to those calculated. 4.5 Motor Parameters Since the motor torue is directly roortional to the flywheel acceleration, the torue sulied by the motor is limited by the maximum seed it can obtain. Therefore, in simulation models, a motor block subsystem can be made to simulate this uer limit in torue availability. In addition, the block can demonstrate the time delays exhibited by the electrical and mechanical roerties of the internal armature windings. From the data sheet sulied by Pittman motors, the following roerties can be acuired: 6

25 Table 4.5.: Pittman Motor Parameters Friction Torue T F.8 oz in e-3 N m Armature Inertia J M. oz in s 8.474e-6 N m s Electrical Time Constant τ E.6 ms Mechanical Time Constant τ M 8.88 ms No-Load Seed S O 533 rm rad/s Torue Constant K T 6. oz in/a.437 N m/a Reference Voltage E 4. V Exerimental verification of the torue constant was consistent for a.8 volt constant inut signal. A small torue meter was attached to the motor shaft, which measured the stall torue of the motor. At.8 volts inut and.5 A/V amlifier gain, am was delivered to the motor. A value of 6. oz in was exected and confirmed by the dial gauge. The frictional torue was also seen through this measurement, since the meter could be moved aroximately.8 oz in in either direction before the dial reset to 6. oz in. From angular velocity measurements, however, the maximum seed of the motor with the flywheel mounted was 345 rad/s. This maximum velocity must be used in the simulation block since the above seed secification is for no load. Both the electrical and mechanical time constants were used in the simulation block, simly because the addition of both worked the best in matching to exerimental time resonse trials. Therefore, a total of 9.94 ms of time resonse delay was used. When the motor shaft is accelerating in the direction of the disc velocity, the torue is limited by the seed of the shaft. This seed to torue ratio is aroximately linear, as given in the data secification sheets. When the motor shaft is decelerating in the direction of the disc velocity, the 7

26 8 torue is not limited by the velocity anymore, and can achieve the torue roortional to the deceleration magnitude. See the simulink subsystem Gyrobot Motor for imlementation of these concets. 4.6 Natural Freuency Using a digital signal analyzer (DSA), a sine swee from.5 Hz to Hz was sent to the endulum. The resultant magnitude lot contained one eak resonance at.8 Hz. This modal freuency can be verified theoretically by finding the eigenvalues of the A matrix from the set of linearized state euations. If the state euations are linearized about π, the vertically down osition, then the sine term, sin( ), is aroximated by. (4.6.) (4.6.) (4.6.3) (4.6.4) From the eigenvalues, there exists modal freuencies at λ, rad/s, or λ,.56 Hz. The natural freuency at.56 Hz is urely oscillatory, since no friction is accounted for in the motion euations. In addition, the endulum would swing sinusoidally if excited at that freuency. A test for vibrational freuencies was erformed by sending a sinusoid of 3 volts eak to eak, with tunable freuency from a function generator. An accelerometer was fixed on the side of the motor, measuring tangential acceleration. Peaks in acceleration occurred at freuencies 88.6 Hz, 35 Hz, and 83 Hz. ( ) /, / / / det det j I m ± λ λ λ λ λ λ τ && &&

27 5. SWING UP CONTROL To start the derivations of the swing u algorithms, the euations of motion (3.7, 3.8) are reeated for convenience. ( ) && && φ && && τ Where φ - 3 sin( ). 5. Non-collocated Partial Feedback Linearization To linearize the unactuated joint resonse, let the outer loo control v be eual to the link angular acceleration. : φ ( ) && & v v (5..) Solving for the disc acceleration and substituting into the second motion euation roduces an euation for the feedback torue. && (( & φ ) ) && && φ τ (( && φ ) ) τ (5..) (5..3) (5..4) To obtain a linear control law, the outer loo control v is chosen as a PD control for an arbitrary reference trajectory d (t). v && & d d ( & ) k ( ) d kd (5..5) Selecting the state variables as follows: z z & η η & d & d (5..6) 9

28 and setting the reference trajectories to zero, the vertical resting osition, gives the following state reresentation of the motion euations and outer loo control: (5..7) The control effort is defined as (5..8) and by substituting into the motion euations (5..9) will yield the following closed loo euations: (5..) Finally, by subtracting the two euations within the above matrix euation, the linear second order differential euation results for the link joint dynamics. (5..) The motion of the disc must be also analyzed by defining the zero dynamics of the system. The zero dynamics are comuted by secifying that will identically track d, the reference trajectory. Since we are trying to swing u to the vertical osition, d d d, then we can write euations (5..7) as (5..) v z k z k z z z d φ η η η & & & & ( ) 3 sin k k v d & φ τ τ φ && && k k d & & && && φ φ ( ) k k d & && ( ) η,η z w Az z & &

29 Since the desired trajectories are d d d, then z z, as defined in (5..7). This surface z in state sace defines an invariant manifold for the system. For ositive values of k and k d, the A matrix is Hurwitz, and the invariant manifold is globally attractive. The dynamics on this manifold are given defined when z, and are called the zero dynamics. ( ) & η w,η (5..3) By substituting d d d into the euations of motion (3.7), we arrive at the following exression for the zero dynamics of the system: & (5..4) As it turns out, the zero dynamics of the non-collocated artial feedback linearization for this system does become linear. If the link tracks the desired vertical setoint exactly, then according to the zero dynamics, the disc will sin at some constant velocity. This is true since the system will balance exactly at the vertical only if no acceleration of the flywheel occurs. The gains k and k d not only determine the resonse of the link angle, as in euation (5..), but also determine the final constant velocity of the disc when the link reaches the vertical. Therefore, careful tuning of these gains is necessary to achieve a swing u controller that will aroach the vertical with a low flywheel velocity for the transition to balancing controllers. 5. Collocated Partial Feedback Linearization To linearize the actuated joint resonse, let the outer loo control v be eual to the disc angular acceleration. : ( ) && v & φ v (5..) Solving for the disc acceleration and substituting into the second motion euation roduces an euation for the feedback torue.

30 (5..) (5..3) (5..4) To obtain a linear control law, the outer loo control v is chosen as a PD control for an arbitrary reference trajectory d (t). (5..5) Selecting the state variables as follows: (5..6) and setting the reference trajectories to zero, since the disc angle is arbitrary and we want the disc to sin to zero velocity, gives the following state reresentation of the motion euations and outer loo control: (5..7) The control effort is defined as (5..8) and by substituting into the motion euations ( ) ) ( φ & & && ( ) ( ) k k v d d d d & & && z z d d & & & η η v z k z k z z z d φ η η η & & & & ( ) ( ) 3 sin k k v d & φ τ ( ) ( ) τ φ τ φ && && &&

31 3 (5..9) yields the following closed loo euations: (5..) Finally, by subtracting ( )/ times the second euation from the first euation from the above matrix euation, the linear second order differential euation results. (5..) The motion of the link must be also analyzed by defining the zero dynamics of the system. The zero dynamics are comuted by secifying that will identically track d, the reference trajectory. Since the flywheel angle is arbitrary, and we desire the velocity to be constant, we can choose the trajectory to be d d. We can then write euations (5..7) as (5..) Since the desired trajectories are d d, then z z, as defined in (5..7). This surface z in state sace defines an invariant manifold for the system. For ositive values of k and k d, the A matrix is Hurwitz, and the invariant manifold is globally attractive. The dynamics on this manifold are given defined when z, and are called the zero dynamics. (5..3) By substituting d d into the euations of motion (3.7), we arrive at the following exression for the zero dynamics of the system: (5..4) τ φ && && k k d & & && && φ φ ( ) k k d & && ( ) η,η z w Az z & & ( ) η,η w & ( ) sin 3 & &

32 The zero dynamics of the collocated artial feedback linearization for this system becomes nonlinear. If the flywheel tracks the desired zero velocity setoint exactly, then according to the zero dynamics, the link will exhibit dynamics of a simle endulum. The system in euation (5..4) has two euilibrium oints, at (,) T, which is a saddle, and (π,) T, which is a center. A tyical hase ortrait of this system is shown in Figure n n Figure 5.: Phase Portrait Of The Zero Dynamics The gains k and k d will determine which trajectory in the hase ortrait the link will converge to. Therefore, careful tuning of these gains is necessary to achieve a swing u controller that will aroach the vertical with a small link velocity for a smooth transition into balancing controllers. 5.3 Sinusoidal Excitation One roblem that exists with both the non-collocated and collocated swing u controllers is that when the endulum swings aroximately 3 degrees above the rest osition, the voltage control effort exceeds volts. Since this voltage is the saturation of our DAC card, most of the controller is 4

33 wasted through banging between ± V, as the endulum oscillates. Also, when the endulum rises above the horizontal, less torue is needed to achieve a higher swinging angle. Since the control effort is at a maximum through this region, the Gyrobot finds itself aroaching the inverted osition too uickly to catch for balancing. While methods to limit the non-collocated and collocated controller oeration zones were alied, a new swing u controller was desired, which would not exhibit this behavior. As a result, the sinusoidal swing u controller was develoed. The design rocedure for this tye of swing u controller relied urely on grahical analysis. We desired a voltage control effort that would not exceed ± V at any angle, would have maximum effort near the rest osition, and would have a minimum effort through the inverted osition. In addition, if the controller exhibited a daming behavior as it aroached the vertical, then the Gyrobot would slow down just before catching. From the fact that the otential energy of the endulum varies sinusoidally as the link angle changes, we choose to counter this torue with a sinusoidal control effort. If we choose τ(π)vmax, and τ(), then the control effort becomes θ τ ( θ) K sin To add the daming, a fourth harmonic is subtracted, (5.3.) θ τ ( θ) K sin K sin( θ ) (5.3.) Figure 5.3. shows each waveform searately, and the resultant addition of both, using [K K ][4 ]. Finally, to mirror the ositive and negative angles, a modulated link angle between and π was utilized, and a sign function caused the mirroring about the inverted osition. 5

34 6 5 Final control effort 4 control effort (volts) 3 daming theta (radians) Figure 5.3: Sinusoidal Swing U Control Effort 6

35 7 6. BALANCING CONTROL 6. Classical PD Control The simlest balancing control for the inverted endulum is the classical aroach of tuning roortional and derivative gains from root locus lots. The first ste in finding the aroriate PD controller is to linearize the euations of motion about the unstable euilibrium oint. First, we linearized the sine term about θ (6..) Then, by substituting into the motion euations, we obtain the set of linearized balancing euations, (6..) To erform root locus techniues, we reuire the transfer functions of link angle and disc velocity to inut torue. Taking the Lalace transform of the motion euations gives (6..3) Simultaneously solving these two euations roduces the following two transfer functions: (6..4) (6..5) The transfer function of disc angular velocity to inut torue is found by multilying euation (6..5) by s. (6..6) ( ) ( ) ) ( ) ( ) ( ) ( ) ( ) ( 3 s s Q s s Q s s Q s s Q s Τ 3 3 sin m τ 3 ) ( && && && && ( ) ) ( ) ( ) ( ) ( ) ( s s s s s Q s s s Q Τ Τ ( ) 3 3 ) ( ) ( ) ( s s s s s sq Τ

36 The oles and zeros of this second transfer function are located at oles s ± 3 ± 6.637, s zeros s ± 3 ± Notice that the oles lie just slightly outside the zeros, so that a negative gain would ush the unstable ole farther into the right half lane. Also, a ositive gain would only send the ole closer to the right half lane (RHP) zero. In order to force the lant oles into the stable region from PD gains alone, a single derivative feedback gain on the disc angular velocity will be used to de-coule the ole-zero air exhibited by the lant (Figure 6..). By choosing a very small derivative gain, the ole located at the origin will be moved slightly into the RHP, allowing a second PD feedback controller to lace of all the three closed-loo oles in the strict left half lane (LHP). The full block diagram for this classical PD controller is resented in Figure 6... R(s) e(s) Σ K K d s Σ sq (s) T(s) sq (s) Q (s) sq (s) Q (s) K d Figure 6..: Block Diagram Of Classical PD Controller 8

37 negative gains K Imag Axis Real Axis ositive gains K Imag Axis Real Axis Figure 6..: Root Locus Of sq (s)/τ(s) After alying the gain on the disc velocity, the new root locus contains only three oles and a zero at the origin. Performing block diagram reduction methods to the inner loo, as shown in Figure 6..3 can derive the new transfer function containing the disc velocity feedback gain. R(s) PD Controller new TF e(s) Σ K K d s H(s) Q (s) Figure 6..3: Reduced Block Diagram Of PD Controller The resulting transfer function, H(s), can be treat like a lant in the classical design of finding roortional and derivative gains. new TF : H ( s) s 3 K s ( ) s 3s Kd 3 d (6..7) 9

38 A simle PD controller can maniulate these ole locations to force all three oles into the stable region (Figure 6..4). k k ( s ) d s K z k d (6..8) 5 Root Locus of PD control on new transfer function Zero laced at s -7.5 Imag Axis Zero -5 laced at s - 5 Real Axis Imag Axis Zero laced at s - 5 Real Axis Imag Axis Real Axis Figure 6..4: Root Locus Of PD Control On New Plant, H(s) A PD zero is laced in the LHP, just outside the LHP ole, to cause a desired root locus ath of accetable closed-loo time resonses. Then the gain is chosen such that the closed-loo resonse is stable with a high daming ratio (Figure 6..5). The combination of a PD controller on the link angle and velocity, lus the derivative controller on the disc velocity creates a balancing controller caable of stabilizing the link angle at, and causing the disc velocity to aroach zero. Thus, the simle PD controller succeeds in controlling two states with one inut control effort. The gains chosen for the Gyrobot lant to obtain good resonse characteristics were [ k k k ] [ ] d d 3

39 4 Root Locus Design 3 Imag Axes Real Axis Figure 6..5: Final Root Locus Design The region of attraction that this tye of controller was caable of ulling in the endulum to the vertical was about ±5 with this set of gains. 6. Classical PID Control With PD control, we found that we could stabilize both the link angle and the flywheel velocity to zero. By adding integral control and by taking advantage of the motor armature friction, it can be shown that we can stabilize both the link angle and flywheel velocity by reading only the link angle encoder. The PID controller can be written as s s k d k k d k s k i d (6..) 3

40 Using the gains [k k d k i ] [.8.3], the root locus in Figure 6.. can be obtained for the link angle transfer function, euation (6..4). 5 5 Closed loo oles: i i -.67e-6 Root Locus Design Imag Axes Real Axis Figure 6..: Root Locus Of PID Control On Plant As noted, all three closed loo oles are stable, but one lies extremely close to the origin. Looking at the bode lot of the PID control on the disc velocity transfer function, euation (6..6), we see that Bode Diagrams 4 From: U() 3 Phase (deg); Magnitude (db) To: Y() Freuency (rad/sec) Figure 6..: Bode Plot Of PID Control On Disc Velocity Transfer Function 3

41 the magnitude is large at low freuencies, and rolls off close to zero at higher freuencies. Since the magnitude is low at freuencies above 5 rad/s, the control effort will be smaller than the static coulomb friction of the motor armature, due to the massive inertia of the flywheel. What remains is to reduce the low freuency gains, or rimarily, the DC gain of the motor. Preceding the PID controller with a second order, low freuency filter cuts off the DC gain in the following way, as shown by the filter s bode lot in Figure The second order filter euation used was s ( s.)( s.) (6..) Bode Diagrams From: U() Phase (deg); Magnitude (db) To: Y() Freuency (rad/sec) Figure 6..3: Bode Plot Of Second Order DC Filter This tye of balancing controller relies on the resence of friction in the system, such that when the link angle is slightly off euilibrium, the control effort voltage will be lower than the effort to overcome the static coulomb friction. The filter laces a heavy reduction on the DC gain, which eliminates the velocity windu of the flywheel, and allows the Gyrobot to balance at euilibrium through only larger link angle changes. 33

42 State Feedback The linearization of the euations of motion about, yielded the following: (6.3.) (6.3.) Or, in matrix form, (6.3.3) (6.3.4) Defining the state variables as the following: (6.3.5) Then the state reresentation of the linearized euations becomes (6.3.6) (6.3.7) (6.3.8) From the system identification chater, we recall the following set of system arameters:.4685 kg m.8374 kg m N m Therefore, the A and B matrices numerically become ( ) m τ 3 && && && && τ τ Γ Μ m && && && 3 3 x x x & & u x x u x x x x x x Β Α Μ Γ Μ & & & & && ) / ( / / / τ

43 A B (6.3.9) (6.3.) This laces the eigenvalues at s, ± Checking for controllability, we find that C [ B AB A ] B Thus we can see that the linearized system is controllable, since C is of full rank. (6.3.) Using the feedback u -Kx, gains can be calculated using Matlab s lace function to locate the osition of closed loo oles. Closed-loo ole locations need to be close to the origin in order to kee the derivative gain (link velocity) low. Poles [-4 -i --i] K [ ] As a result of lacing the oles close to the origin, the ste resonse to a disturbance will be slow, which narrows the region of attraction for the endulum s balancing caability. Defining our outut as the link angle, y x, we can check for the ossibility of observing the disc velocity: C O CA CA (6.3.) Since O is not of full rank, we can conclude that the disc velocity state is not observable. This fact can also be viewed a second way. Since the flywheel can sin at any arbitrary seed, and still kee 35

44 36 the link angle at zero, the outut of our euation, y x, is not a function of the velocity of the disc, but rather a function of the change in velocity. Therefore, x 3 is not an observable state. 6.4 Feedback Linearization To begin the feedback linearization rocedure, we start by defining an outut euation (6.4.) Then, by taking successive derivatives of y, (6.4.) (6.4.3) (6.4.4) (6.4.5) (6.4.6) (6.4.7) We obtain a fourth order outut euation. By designing the control effort, τ, to be (6.4.8) Then y (4) u. If we define the states (6.4.9) The state euations become (6.4.) ( ) y ( ) ( ) ( ) τ β α τ τ ) ( ), ( cos sin cos sin cos sin sin cos cos sin (4) (4) (4) 3 3 y y y y y y & & & & && & &&& && && && & & & [ ] ), ( ) ( u & α β τ y x y x y x y x &&& && & 4 3 u x x x x x x x & & & &

45 37 (6.4.) We can lace the closed loo oles of this matrix euation arbitrarily by state feedback u -Kx. Poles were chosen to be located at P [-i -i -8 -], and the resulting K gains found by using the matlab lace command were: K [ ] While these gains are very large, non are gains of strictly the angle derivatives, and do not cause large amlification in noise. This fact was verified in imlementation, where the control effort exhibited a relatively smooth signal. u x x &

46 7. COMBINING THE CONTROLLERS After designing the swing u control and the balancing control, some sort of switch was needed to toggle between the two. The obvious solution is to choose a threshold angle for which the two controllers would oerate. By observing the region of angles that each tye of balancing controller was able to attract the endulum to the vertical, this maximum deviation angle could be set as the threshold angle. The swing u controller would oerate until the link swung u to this threshold angle, uon which the balancing controller would take over. Conversely, if the endulum fell too far from the vertical, the swing u controller would restart when the threshold angle was exceeded. If the threshold angle were selected too far from the vertical, the balancing controller would not have the available torue to ull the link to the zero osition. If the threshold angle were chosen too close to the vertical, then a disturbance laced during balancing would switch the swing u controller back on, and cause the endulum to swing with excessive seed away from the vertical. This scenario caused the endulum to gather velocity across the vertical, which was too fast for the balancing controller to bring to rest. The solution to these roblems was to take the benefit of choosing a threshold close to the vertical for switching from the swing u to the balancing controllers, and combine the benefit of using a larger threshold angle for switching from the balancing to the swing u controller. Using this hysteresis tye switch allows the balancing controller to only start when it has the torue caable of balancing the link, and only restart the swing u controller when the endulum had decidedly fallen from the euilibrium. The next roblem to arise from this switching algorithm was that when the link angle rose above the horizontal, lesser amounts of torue are needed to swing the endulum u higher. The noncollocated and collocated algorithms demanded higher voltage than the DAC could suly during most of the swing u, so both controllers bounced between ± volts until the link reached aroximately 3 degrees from the vertical. When the endulum swung ast the horizontal, this maximum of volts sulied to the motor caused much larger amlitudes in successive swing 38

47 oscillations, resulting in the endulum gaining far too much velocity for the balancing controller to sto when the threshold angle was exceeded. The endulum would then swing ast the vertical, and gain more energy due to the swing u controller in the same rotational direction. Therefore, the endulum would gain seed with each assing of the vertical. As exlained earlier, this behavior is the reason for develoing the sinusoidal swing u controller. As a solution however, if the swing u controller was turned off when the link angle assed ±9, then swing u controller would still be able to rovide enough energy to swing the link ast the vertical (taking a few more oscillations), and aroached the vertical with much less velocity. The balancing controller was able to slow the motion and balance the endulum with small overshoot. The larger the region that the balancing controller can oerate is thus more useful in catching the endulum, hence the Feedback Linearization controller would work better than the simler PD and state feedback controllers. In summary, a swing u to balancing threshold angle was chosen close to the vertical, while a balancing to swing u threshold angle was chosen farther away from the euilibrium. In addition, the non-collocated and collocated swing u controller efforts were set to zero above the horizontal to reduce swing velocity. 39

48 8. IMPLEMENTING THE CONTROLLERS 8. Wincon RTX Simulink s Wincon real time worksho was used to imlement the controlling algorithms for the Gyrobot. To avoid confusion in reading the block diagrams, subsystems were built for the swing u and balancing controllers, the lant motion euations, and for reading and interreting the encoders. Since both the swing u and balancing controllers alied gains to the link angle, the link angle was modulated between ±π for balancing, and between and π for swing u in case the endulum cycled ast vertical. In addition, the disc angle was also identically modulated to avoid a large gain as a result of high seed sinning of the disc during the collocated swing u. The modulation on the disc angle enabled the endulum to be brought to the stable euilibrium and recommencing the swing u without reinitializing the encoder ositions. Since the simulations are reeatable, and the gains can be easily tuned so that the link does not travel 36, it is only necessary to modulate the angles during exerimental imlementation. The two encoders on the Gyrobot are only able to measure osition, so the angular velocity must be aroximated. A filter was used to aroximate the velocity for freuencies less than rad/s. This was achieved by multilying by the transfer function s H ( s) s (8..) The bandwidth of rad/s could be adjusted to yield the smoothest results. For this system, the chosen bandwidth was high enough for the velocities encountered. To imlement the hysteresis in the combination of swing u and balancing controllers, a relay block was used, which outut to activate the balancing controller, and to activate the swing u controller. The relay switches off at π/, and switches on at π/, for the balancing to swing u threshold, and swing u to balancing threshold resectively. The relay was receded with the absolute value of the modulated link angle, to account for aroach from either direction. 4

49 A switch block was used to switch between the swing u and balancing controllers. The switch block threshold was set to, in between the ± oututs of the relay. Finally, a saturation block immediately before the DAC block limits the outut voltage torue calculation between ±9.5 volts to avoid harming the DAC. 8. RTSS and Visual Basic In order to control the Gyrobot over the internet, this thesis rogressed the work by Hahn using the Java to Visual Basic communication rotocols. To imlement the swing u and balancing controllers through a Visual Basic interface, the Matlab Simulink block diagrams needed to be converted to RTSS Visual C code. This RTSS real-time code allowed the controllers to oerate on the designated system, while listening for inut changes through Visual Basic from the user. Aendix D contains only the core of the RTSS code, gyrobot.c, in which all the controller code and timing is alied. All include files are left out, since they remained unaltered by the coding changes from this thesis. Aendix E contains the Visual Basic code, including both the grahical interface and module files, of which the latter is necessary to understand the communication to RTSS. As the user clicks on buttons to either start, sto, change gains, etc. on the Visual Basic (VB) interface, VB sends the command to the mecha.dll file, which waits for the next iteration of the RTSS timing eriod. At the beginning of the RTSS timing loo, the code checks for any changes, imlements these changes, then continues on to read in the encoder values. From these new osition values, the code aroximates the velocity by taking the Tustin aroximation of the velocity filter resented in euation (8..). H ( s) ( x ( k) x ( k ) ) s Tustin 9.5 s x ( k).948x ( k ) (8..) x ( k).948x ( k ) 9.5 ( x ( k) x ( k ) ) (8..) 4

50 Since the code now has all four states, x, x, x 3, and x 4, the balancing or swing u controller efforts can be calculated. The tye of controller imlemented deends on the integer stored in either g_swingu_flag or g_balance_flag, and the resent link encoder value. After calculating the control effort, the control voltage is sent out the DAC via the volt_out(,g_u) command. Finally, the resent four states are stored into old state variables for use in the following iteration s velocity filter, and the timing cycle begins again. Using the RTSS method hels make real-time changing of gains, alying disturbance ulses, and changing controller tyes easier and faster through the oerations of the user. For these reasons, and the built in TCP/IP functions in Visual Basic, this method was referred for our internet-based control exeriments. 4

51 9. SIMULATION RESULTS The simulink block diagrams used to simulate the various controllers can be found in Aendix A. Before combining the swing u controllers with the balancing controllers, it was necessary to individually examine their behaviors. First, we look at the non-collocated swing u controller time resonse, shown in Figure 9.. The swing u time takes about 8 seconds, or 8 oscillations to achieve the vertical osition. As the link angle moves further from the hanging osition, the eriod increases, as seen by the sreading distance between eaks in the disc velocity and control effort lots. Notice, that the motor achieves full seed during each oscillation, caused by the large torue demanded by this controller. PD gains of [k k d ] [.4.] were used in the linear dynamics. Thetadot (rad/s) Thetadot Theta (degrees) (rad/s) 4 Noncollocated Swing U Controller Link Angle 4 Link Velocity 6 8-4Disc Velocity Control Effort 6 8 Torue (V) time (sec) Figure 9.: Non-collocated Swing U Simulation Resonse 43

52 Thetadot (rad/s) Thetadot Theta (degrees) (rad/s) Torue (V) 4 Collocated Swing U Controller Link Angle 4 Link Velocity 6 8-4Disc Velocity 6 8-4Control Effort time (sec) Figure 9.: Collocated Swing U Simulation Resonse Thetadot (rad/s) Thetadot Theta (degrees) (rad/s) Torue (V) 4 Sinusoidal Swing U Controller Link Angle 4 Link Velocity 6 8-4Disc Velocity Control Effort time (sec) Figure 9.3: Sinusoidal Swing U Simulation Resonse 44

53 Examining the collocated swing u resonse in Figure 9. shows similarities in the number of oscillations to vertical, and saturating the disc velocity. Since the gains in front of the sinθ term and PD gains on the osition error are so small, a multilier of at least must be used to amlify the torue signal for the lant to overcome the effects of gravity and friction. This gain was incororated in the torue constant block. PD gains of [k k d ] [.4.] were used in the linear dynamics. The sinusoidal swing u controller resonse to the gains [k k ] [4 ] is shown in Figure 9.3. As seen, the number of swings is less, since the control effort does not bounce between maximum values, and therefore, the controller guides the swing energy in a smoother manner. Next, we look at the balancing controllers. Figure 9.4 shows the resonse of the balancing endulum to a. N m,.5 sec duration ulse, alied at t and t 5 seconds. The PD controller has an overshoot aroximately eual to the initial dislacement, but has high daming with the chosen gains of [k k d k d ] [ ]. Imortant to notice is the controller s ability to stabilize the link back to the vertical, while bringing the disc velocity to rest. Thetadot (rad/s) Theta (degrees) Thetadot (rad/s) Torue (V).5 Classical PD Balancing Controller Link Angle Link Velocity Disc Velocity 6 8 4Control Effort time (sec) Figure 9.4: Classical PD Balancing Simulation 45

54 The PID balancing controller in Figure 9.5 takes a longer time to reduce the flywheel seed, since the motor encoder value is not used in its effort calculation. The state feedback algorithm roduces the same controller as the classical PD algorithm if the feedback gains are selected to lace the closed-loo oles at the same locations as in the root locus design. Thetadot (rad/s) Theta (degrees) Thetadot (rad/s) Torue (V).5 Classical PID Balancing Controller Link Angle Link Velocity Disc Velocity Control Effort time (sec) Figure 9.5: Classical PID Balancing Simulation Thetadot (rad/s) Theta (degrees) Thetadot (rad/s) Torue (V).5 State Feedback Balancing Controller Link Angle Link Velocity Disc Velocity 6 8 4Control Effort time (sec) Figure 9.6: State Feedback Balancing Simulation 46

55 The remaining balancing controller is the Feedback Linearization resonse, as shown in Figure 9.7. Subject to the same disturbance amlitude and duration, this balancing controller reacts stronger around the euilibrium, without causing the control effort to become noisy due to amlification of derivative aroximations. Thetadot Thetadot (rad/s) Theta (degrees) (rad/s) Torue (V).5 Feedback Linearization Balancing Controller Link Angle Link Velocity Disc Velocity 6 8-4Control Effort time (sec) Figure 9.7: Feedback Linearization Balancing Simulation Finally, we can link the two controllers together to swing u and catch the Gyrobot at the unstable euilibrium. Figures show the simulations of the non-collocated, collocated, and sinusoidal swing u controllers with PD balancing resectively. Each controller used the same gains as chosen in the individual tests above. The motor simulation block which limits the maximum seed of the motor was used on the swing u controllers, but not on the PD controller. Notice that both cases reuired the disc to sin at a seed larger than the measured maximum of 35 rad/s, but as we will see in the exerimental results, the disc can sin faster than this when the link is in motion. 47

56 Thetadot (rad/s) Thetadot Theta (degrees) (rad/s) 4 Noncollocated Swing U Controller Link Angle 5 Link Velocity 5-5 Disc Velocity Control Effort 5 Torue (V) time (sec) Figure 9.8: Non-collocated Swing U With PD Balancing Simulation Resonse Thetadot (rad/s) Thetadot Theta (degrees) (rad/s) 4 Collocated Swing U Controller Link Angle 5 Link Velocity 5-5 Disc Velocity Control Effort 5 Torue (V) time (sec) Figure 9.9: Collocated Swing U With PD Balancing Simulation Resonse 48

57 Thetadot (rad/s) Thetadot Theta (degrees) (rad/s) Torue (V) 4 Sinusoidal Swing U Controller Link Angle 5 Link Velocity 5-5 Disc Velocity 5-5 Control Effort time (sec) Figure 9.: Sinusoidal Swing U With PD Balancing Simulation Resonse 49

58 . EXPERIMENTAL RESULTS Figure. shows the resonse of the non-collocated swing u controller. The simulations redicted reaching the vertical osition in 4 oscillations, but due to the sli ring friction, the Gyrobot was unable to swing u with the large inertial brass flywheel. A lower inertial flywheel was used for the exerimental trials of the non-collocated swing u controllers, which allowed the uick acceleration necessary to overcome the static friction zone of the sli ring. The new arameters for this low-inertia flywheel setu were:.4685 kg m.866 kg m N m The control effort was set to switch off when the link surassed π/4, to reduce the aroach seed. This causes a counter acceleration of the disc, which alies a slight counter torue on the link, causing this slower swinging effect. The control effort is nonzero at the hanging osition, so the endulum is able to start the swing u without any external interference. The linear dynamic gains used were [k k d ] [.4.]. Thetadot (rad/s) Thetadot Theta (degrees) (rad/s) Torue (V) 4 Noncollocated Swing U Controller Link Angle 4 Link Velocity 6 8-4Disc Velocity Control Effort time (sec) Figure.: Non-collocated Swing U Exerimental Resonse 5

59 Thetadot (rad/s) Thetadot Theta (degrees) (rad/s) Torue (V) 4 Collocated Swing U Controller Link Angle 4 Link Velocity 6 8-4Disc Velocity Control Effort time (sec) Figure.: Collocated Swing U Exerimental Resonse Figure. shows the resonse of the collocated swing u controller. Though the simulations redicted the non-collocated and collocated controllers to swing to the vertical in 4 or 5 oscillations, again the sli ring friction was too large for these tyes of controllers to work with a large flywheel inertia. Also, due to the small gains discussed in the simulations, a choice of the torue gain amlifying factor of was needed to swing u the endulum. The collocated controller also caused the endulum to ass the vertical in the oosite direction to the non-collocated controller and simulations. This occurrence demonstrated the necessity to modulate the link angle, to revent false gains on the osition error. The linear dynamic gains used for the collocated controller were [k k d ] [.4.]. Since the sinusoidal swing u controller does not feed back any arameter gains, and follows a strict voltage to link angle curve, the Gyrobot was able to swing u to the vertical using the original high-inertia brass flywheel used in the arameter identification schemes earlier. Figure.3 shows the sinusoidal swing u resonse of the Gyrobot. Notice the controlled level of inut voltage to the system. 5

60 Thetadot (rad/s) Thetadot Theta (degrees) (rad/s) 4 Sinusoidal Swing U Controller Link Angle 4 Link Velocity 6 8-4Disc Velocity 6 8-4Control Effort Torue (V) time (sec) Figure.3: Sinusoidal Swing U Exerimental Resonse Figure.4 demonstrates the effectiveness of the PD balancing controller. A substantial disturbance (aggressive ta with a finger) ushed the link - away from the vertical. The PD gains of [k k d k d ] [ ] were stable and fast enough to re-stabilize the endulum and zero the disc velocity in about 5 seconds. Thetadot (rad/s) Thetadot (rad/s) Theta (degrees) Torue (V) 5 Classical PD Balancing Controller Link Angle Link Velocity Disc Velocity Control Effort time (sec) Figure.4: Linearized PD Balancing Exerimental Resonse 5

61 Figure.5 illustrates how the PID controller can balance the endulum by measuring only the link angle. The disc velocity cannot be sent to zero, but it does stabilize at a constant velocity after a disturbance inut. Thetadot (rad/s) Thetadot (rad/s) Theta (degrees) Classical PID Balancing Controller Link Angle - -4 Link Velocity Disc Velocity Control Effort time (sec) Torue (V) Figure.5: Linearized PID Balancing Exerimental Resonse As in the simulation discussion, the state feedback balancing controller is essentially the classical PD controller if the closed-loo oles are laced identically with state feedback. Figure.6 shows that the state feedback method of balancing the linearized system does reject a disturbance imulse torue well, and is an easily imlemented controller for classroom exeriments. The feedback linearization balancing algorithm roduced the strongest ull within the euilibrium region. This controller was caable of recovering from larger disturbance torues than the other three balancing controllers, and did not succumb to the static friction zone roblems as much. See Figure.7 for the exerimental resonse due to this feedback linearization balancing controller. 53

62 Thetadot (rad/s) Thetadot (rad/s) Theta (degrees) Torue (V) - State Feedback Balancing Controller Link Angle Link Velocity Disc Velocity Control Effort time (sec) Figure.6: State Feedback Balancing Exerimental Resonse Thetadot (rad/s) Thetadot (rad/s) Theta (degrees) Torue (V) Feedback Linearization Balancing Controller Link Angle - Link Velocity Disc Velocity Control Effort time (sec) Figure.7: Feedback Linearization Balancing Exerimental Resonse 54

63 Connecting the controllers brought on some difficulties. The swing u controllers tended to swing the endulum towards the vertical with more seed than the PD controller could counter. By adjusting the relay threshold angles, and by increasing the roortional gain, a swing u and catch combination could be accomlished. The PD balancing and swing u controller gains used were the same as reviously tested. Figures.8 and.9 show the catch resonse of the non-collocated and collocated swing u controllers with the PD balancing controller resectively. Note again, that these two swing u controllers used the lower inertia flywheel to for swing u. This, however, hindered the balancing ability because the flywheel had to accelerate uickly to overcome the static friction zone of the sli ring. The sinusoidal swing u with PD balancing algorithm was imlemented with the larger inertial flywheel, and whose resonse is given in Figure.. This swing u controller also had to be tuned in the threshold values to roduce a consistent catch behavior. Overall, the sinusoidal swing u controller was the most robust with flywheel choices and resonded most consistently for catching at the vertical. Any of the four balancing controllers could be couled with the sinusoidal swing u controller to stabilize the Gyrobot in the web-based exeriments. Thetadot (rad/s) Thetadot Theta (degrees) (rad/s) 4 Noncollocated Swing U with PD Balancing Link Angle 4 Link Velocity Disc Velocity Control 6 Effort 8 Torue (V) time (sec) Figure.8: Non-collocated Swing U With PD Balancing Exerimental Resonse 55

64 Thetadot (rad/s) Thetadot Theta (degrees) (rad/s) 4 Collocated Swing U with PD Balancing Link Angle 4 Link Velocity Disc Velocity Control 6 Effort 8 5 Torue (V) time (sec) Figure.9: Collocated Swing U With PD Balancing Exerimental Resonse Thetadot (rad/s) Thetadot Theta (degrees) (rad/s) Torue (V) 4 Sinusoidal Swing U with PD Balancing Link Angle 5 Link Velocity5 5-5 Disc Velocity Control Effort time (sec) Figure.: Sinusoidal Swing U With PD Balancing Exerimental Resonse 56

65 . INTERNET-BASED CONTROL APPROACH. TCP/IP Visual Basic to Java Alet Communication Visual Basic contains a tool called Winsock, which allows the VB rogram to communicate using either a TCP or UDP rotocol, and exchange data in both directions. With the addition of this tool block into the form, the Gyrobot VB rogram can communicate over the internet to the web server, and make the exeriment available to anyone browsing the web. The Gyrobot VB code must first be loaded at the controller server, which constantly listens over ort 5 for a new connection. As the first remote user resses the Connect button on the Java interface alet, the Winsock accets the IP address and closes the socket to listening for additional users. This allows only one oerator, the controlling client, to run the exeriment, while other viewers must wait until the system is available. The observing clients can meanwhile watch the animation and webcam resonse of the controlling client s exeriment, without the ability to alter any of the controlling functions. As soon as the controlling client exits the alet, VB detects the absence of a connection, and reoens the Winsock for listening. At the Java end, uon loading the exeriment age, the control alet loads, and waits for the user to ress the Connect button. With a connection reuest, ExerimentSocket.java looks on ort 5 over a socket named aletsocket for a handshake with the VB software. If there is no user running the exeriment, then comm_status, the communication status, is set to true, and the user is allowed to run the exeriment. If a user is already controlling the exeriment, then since VB is no longer listening over the socket, Java catches this error with an excetion, and sets comm_status to false. After establishing a connection, the TCP socket on VB can send the encoder and control effort data to Java for animation and lotting, while Java can send the user commands back to VB for communication to RTSS. The TCP/IP socket lends itself well for internet control, since data can be exchanged eriodically, and the data series is stacked when exeriment data transmission rates are faster than the internet communication rate. Therefore, if the internet connection is slow, such as 57

66 through an older modem, then after stoing the controller, the animation and lotter will continue until the stack is emtied. One drawback of this delay can be inability to save the balancing Gyrobot by changing the controller gains uickly. However, since this system only erforms in a small environment, any loss of control due to oor gains will not harm either the lant or surrounding laboratory euiment. The controlling user would instead have to wait for the animation to catch u, and then recommence control through swing u.. Interface Otions In order to resent the controlling client with error-free oeration, a well-designed grahical user interface is essential on the web age. Java is an excellent language for develoing such an oerator anel, since it is indeendent of the oerating system that the web browser is running on. Thus, the internet control exeriment can be erformed on a PC, a Macintosh, or UNIX machine with no variations of interface code. Aendix F.. shows a screen shot of the Gyrobot Java grahical user interface. The window is subdivided into four uarters for easy location of functions. The uer left uadrant is the exeriment communication and control anel. Here, the user can connect to the host server, start and sto the controller oeration on the Gyrobot, aly a disturbance ulse, and set the new controller gains. Buttons unused at a articular time are disabled to revent confusion for the user and, more imortantly, to hel revent communication error. Below these control buttons, a section is artitioned for helful instructions and communication confirmations. The uer right uadrant contains a anel of text boxes where the user can tye in the swing u and balancing controller gains. A selection list of each tye of controller is also available, so that the user can not only udate the gains, but also change the tye of controller on the fly. If a gain is needed for the controller, the text box is enabled with a white background, while if not needed, the box is disabled with a gray background. Finally, a number error checking algorithm in Java determines if all the gains entered are a number of tye float, and informs the user with a warning 58

67 window if one was incorrectly entered. If such an error occurred, the gains would not be sent until all necessary gains were correct numbers. The lower left uadrant contains an animation of the Gyrobot. The left animation is of the link angle, mounted icturesuely on a tableto. Since the disc is hard to animate on the end of the link, a tachometer gauge is drawn to the right of the table. As the disc increases seed, the needle of the tachometer rotates roortionally, while the numerical velocity is dislayed in the digital text box. The animation is udated at the rate of the Transmission Samling Period, as will be described shortly. Through GyroCanvas.java, located in Aendix F, the entire icture is redrawn every samling eriod, to give the imression of animation. Visual Basic sends the encoder angles and velocities to Java, which then calculates the end ositions of the link and needle for udating the icture. Even though the entire icture needs reainting, only the link, disc, needle, and tachometer text needs udating every samle eriod. These few items allow a more fluid animation, and allow the animation to track real-time with less transmission delay. The final lower right uadrant holds the Gyro-Scoe, an oscilloscoe screen for lotting various data series. The scoe oerates in roll mode, scrolling several seconds worth of data from right to left. The Java code Plotter.java takes the data, such as the link angle for examle, that is transmitted from Visual Basic every Transmission Samling Period, and deosits the new data oint on the right of the scoe. As new data oints arrive, the old series of oints shifts left to accommodate the new oint, causing this roll effect. Since the scoe udates at the same rate as the animation, if the connection haens to be slow, and the animation is delayed, at least the user will be able to coincide the Gyro-Scoe resonse with the Java animation. On the Scoe menu bar, Otions can be selected, which allows more interactions with the scoe, with otions such as zooming in and out, and dislaying different data series. Selecting Samling from the Otions menu dislays a window allowing the alteration of two samling eriods. A to scroll bar allows the change of the Controller Samling Period, which is the samling eriod used by RTSS for the discrete time control of the Gyrobot. A default value of 5ms is 59

68 initially hard-coded, but the user can change this time between and 5ms. The bottom scroll bar allows the change of the Transmission Samling Period, which is the samling eriod used to send the encoder ositions and velocities and control effort from VB to Java. This samling eriod allows the adjustment of the fluidity of the animation, deending on the internet connection seed. A default value of ms initially resides, which can be changed between 5 and 5ms. Notice that if a Controller Samling Period is selected higher than the Transmission Samling Period, Visual Basic would be sending the same data value to Java for successive eriods until a new data value arrives from RTSS. The final feature of the user interface is the hel files. Under the Control Euations menu item, any of the swing u and balancing controllers can be selected to dislay text covering the various euations. Here, an exlanation of necessary controller gains, as well as the controller euation is accessible..3 Data Transmission Encoder values and control effort is sent from VB to Java through float values. Since Java obtains the binary reresentation of the float value in reverse byte order, the VB rogram first reverses the bit order, then sends the byte array through the server socket in the following way (data() is the link angle in radians): tsingle.f CSng((data() * 8) / i) LSet tbytes tsingle reverse_float_array() tbytes.bf(3) reverse_float_array() tbytes.bf() reverse_float_array() tbytes.bf() reverse_float_array(3) tbytes.bf() On Error GoTo errorhandler server_socket.senddata reverse_float_array 6

69 Java receives this data array in GetDataThread.java, and reads in the 4 byte float number through the inutstream.readfloat() function values[](double)( inutstream.readfloat() ); Different data series can be sent successively and read into the values[] array in the GetDataThread.java code. Since data transmission from Java to VB not only may contain a float or integer value, but it also must contain an instruction for what the number will be used. This Java code uses a method of receding the series of numbers by two characters to communicate the command and numbers in one ste. A character is exactly one byte in length, so coding within VB can look at the first two bytes for the command, and read a redetermined length of bytes for each articular command. For examle, look at the following code used by Java to send a new set of gains and controller tyes. ublic void sendgains(float[] gains, int[] controller){ set_gains[](byte) S ; set_gains[](byte) E ; // Swing U Controller switch (controller[]) { case : set_gains[](byte) A ; // Sinusoidal case : break; set_gains[](byte) B ; // Noncollocated case : break; set_gains[](byte) C ; // Collocated break; default: set_gains[](byte) A ; 6

70 // Balancing Controller switch (controller[]) { case : set_gains[3](byte) A ; // Classical PD case : break; set_gains[3](byte) B ; // Classical PID case : break; set_gains[3](byte) C ; // State Feedback break; case 3: set_gains[3](byte) D ;//FeedbackLinearization break; default: set_gains[3](byte) A ; for(j;j<8;j){ set_gains[4j*8](byte)( (Float.floatToIntBits(gains[j])) & xf); set_gains[5j*8](byte)((float.floattointbits(gains[j])>>4) & xf); set_gains[6j*8](byte)((float.floattointbits(gains[j])>>8) & xf); set_gains[7j*8](byte)((float.floattointbits(gains[j])>>)& xf); set_gains[8j*8](byte)((float.floattointbits(gains[j])>>6)& xf); set_gains[9j*8](byte)((float.floattointbits(gains[j])>>)& xf); set_gains[j*8](byte)((float.floattointbits(gains[j])>>4)&xf); set_gains[j*8](byte)((float.floattointbits(gains[j])>>8)&xf); 6

71 Each tye of swing u and balancing controller is given a letter, which is assed immediately following the SE command. Thus, VB reads SE and knows to look for two more characters for the controller tyes, and then for eight sets of 4 byte float numbers. Since the first four items sent are characters, and therefore bytes, the float values of the gains must also be sent as bytes. Java sends a byte as a signed integer, so the eighth bit is a sign and not reresentative of a number. To roerly send the controller gain, the 4 bytes are sent in 8 byte ackets, with the first four bits eual to zero. This is accomlished by bit shifting right (>>), while ANDing (&) with HEX xf. VB must then remove these zeros to re-coule the roer float value. Even though eight gains lus four characters amounts to a large number of bytes sent in this fashion, we are only udating gains, and the time taken to aly this information to the system is not critical. Finally, transmission of the numbers is accomlished through the oututstream.write() followed by a oututstream.flush() command. try{ oututstream.write(set_gains); oututstream.flush(); catch(excetion e){ release false; One imortant item to be aware of is that with the transmission of bytes from Java to VB, every other byte starting with the first byte contains useful information. This means that the alternate bytes do not contain any information at all. Therefore, VB must read in only the even indexes before decihering the command and float numbers. cmd Chr(vtDataArray()) Chr(vtDataArray()) Select Case cmd Case "SE" 63

72 swingu_controller Chr(vtDataArray(4)) balance_controller Chr(vtDataArray(6)) For j To 3 tbytes.bf()vtdataarray(6*j8)6*vtdataarray(6*j) tbytes.bf()vtdataarray(6*j)6*vtdataarray(6*j4) tbytes.bf()vtdataarray(6*j6)6*vtdataarray(6*j8) tbytes.bf(3)vtdataarray(6*j)6*vtdataarray(6*j) LSet tsingle tbytes swingk(j) tsingle.f Next j For j 4 To 7 tbytes.bf()vtdataarray(6*j8) 6*vtDataArray(6*j) tbytes.bf()vtdataarray(6*j)6*vtdataarray(6*j4) tbytes.bf()vtdataarray(6*j6)6*vtdataarray(6*j8) tbytes.bf(3)vtdataarray(6*j)6*vtdataarray(6*j) LSet tsingle tbytes balk(j - 4) tsingle.f Next j The zeros in each transmitted byte are removed by multilying 6, or x times the second byte, and adding this to the first byte, resulting in one of the 4 float bytes. Finally, the new gains are stored in the VB text boxes, and sent to RTSS through the activation of the Set Gains button. txtswingk.text swingk() txtbalk.text balk() Me.cmdSetGains_Click 64

73 . CONCLUSION This thesis discussed the design and control of a new underactuated gyroscoic endulum, the Gyrobot. The mechanical design was discussed and the system arameters were identified by on-line schemes using conservation of energy and free resonse, and verified by mathematical formulae. Non-collocated and collocated artial feedback linearization swing u controlling algorithms were develoed which linearized the closed-loo dynamics, for designing resonse characteristics. After linearizing the motion euations about the unstable euilibrium, PD and state feedback controllers heled balance the Gyrobot in the inverted state. To exand the region of balancing caabilities, a Feedback Linearization algorithm was develoed, as with a PID controller, which relied on reading only one osition encoder. Exerimental trials were comared to simulation, and the concets of friction, unknown initial conditions, and reeatability were discussed. In comletion of the Gyrobot design, consistent sets of controller gains and a low maintenance designed lant were develoed for imlementation in an internet interface controller for web-based class laboratory exeriments. The simlicity of the Gyrobot s motion euations, and its convenient hysical characteristics allow this system to be a great introduction to both controlling through and coding Java alets. Although limits can be seen from the rate of data transfer over the internet, over the next few years, advancements in comuter and internet seeds will tie in closely to more freuent use of internet-based controlled activities. 65

74 Aendix A APPENDIX A SIMULATION FILES Page A..: A..: A..: A..: A.3.: A.3.: Non-collocated Swing U with PD Balancing Control Simulation Model.67 Subsystem Non-collocated Swing U.67 Collocated Swing U with PD Balancing Control Simulation Model 68 Subsystem Collocated Swing U.68 Sinusoidal Swing U with PD Balancing Control Simulation Model 69 Subsystem Sinusoidal Swing U.69 A.4.: Classical PD Balancing Controller Simulation Subsystem 7 A.4.: A.4.3: A.4.4: A.5: A.6.: A.6.: Classical PID Balancing Controller Simulation Subsystem with DC Filter...7 State Feedback Balancing Controller Simulation Subsystem.7 Feedback Linearization Balancing Controller Simulation Subsystem 7 Subsystem Plant...7 Subsystem Gyrobot Motor...73 Subsystem Time delayed resonse

75 Aendix A Noncollocated Swingu with PD balancing control Simulation model theta Switch theta thetadot torue thetadot Noncollocated Swingu Control Effort inut torue theta thetadot Theta thetaddot theta Relay on off - theta Switch threshold at theta thetadot disturbance torue thetaddot 3 thetadot thetadot torue Plant thetadot 4 tau PD control A..: Non-collocated Swing U with PD Balancing Control Simulation Model theta *i Constant thetadot mod Math Function si n Trigonometric Function.4 k. kd (3) (3) () /c torue constant u Abs Zero Switch Saturation 3 thetadot V_in (V) Torue (N*m) Disc Velocity Gyrobot Motor torue A.. Subsystem Non-collocated Swing U 67

76 Aendix A Noncollocated Swingu with PD balancing control Simulation model theta theta control effort (N*m) Switch Control Effort theta thetadot Collocated Swingu inut torue theta thetadot Theta thetaddot theta Relay theta on off - theta Switch threshold at thetadot disturbance torue thetaddot 3 thetadot thetadot torue Plant thetadot 4 tau PD control A..: Collocated Swing U with PD Balancing Control Simulation Model theta theta.4 k si n Trigonometric Function -K*(3)/() -K*()/() gain control effort (N*m) 3 thetadot. kd A..: Subsystem Collocated Swing U 68

77 Aendix A Sinusoidal Swingu with PD balancing control Simulation model Switch theta control effort (N*m) Control Effort dot Sinusoidal Swingu theta inut torue thetadot Theta thetaddot theta Relay theta on off - theta Switch threshold at thetadot disturbance torue thetaddot 3 thetadot thetadot torue Plant 4 thetadot tau PD control A.3.: Sinusoidal Swing U with PD Balancing Control Simulation Model.5 Gain si n Trigonometric Function -4 Gain5 - tau sin - Gain Gain4 si n Trigonometric Function Gain3 Product Trigonometric Function Sign c - dot Switch torue constant torue inversion control effort (N*m) A.3.: Subsystem Sinusoidal Swing U 69

78 Aendix A theta K thetadot.47 Kd control effort (N*m) 3 thetadot.6 K A.4.: Classical PD Balancing Controller Simulation Subsystem s s.s. Velocity Filter4 K 5s s5 Velocity Filter.8 Kd tau (N*m) control effort s Integrator. Ki A.4.: Classical PID Balancing Controller Simulation Subsystem with DC Filter 7

79 Aendix A x K() K x K() K control effort (N*m) 3 x3 K(3) K3 A.4.3: State Feedback Balancing Controller Simulation Subsystem si n 3 Trigonometric Function 3 cos -K- Product Trigonometric Function 3/ - dot Product yddot 6 - Product3 control effort (N*m) -K- K3 PP ydot dot P K -K y 64 3 K 3 P3 cos Product K4 Trigonometric Function A.4.4: Feedback Linearization Balancing Controller Simulation Subsystem 7

80 Aendix A Simulink Model for Simulating Pendulum Euations of Motion "Plant" inut torue * theta ddot Sum /() /() Thetadot s Thetadot() Theta s Theta() Thetaddot Demux -K- Thetaddot Thetadot Mux Demux () * theta ddot si n 3 sin(theta_) (3) * sin(theta_) Sum /() /(P) s Thetadot() s Theta() Theta Mux theta 4 theta thetadot 5 thetadot disturbance torue 3 thetaddot 6 thetaddot A.5: Subsystem Plant 7

81 Aendix A V_in (V) V_in (V) Current (A) Time delayed resonse.437 Torue Constant (N*m/A) Product 35 Vmax (rad/s) Product Disc Velocity u Abs Sum Sign3 Product Torue (N*m) Sign Switch Torue A.6.: Subsystem Gyrobot Motor V_in (V).5 k_am alied voltage Sum /(.888.6) s * i s Integrator Current (A) /(electrical time constant) A.6.: Subsystem Time delayed resonse 73

82 Aendix B APPENDIX B WINCON SIMULINK FILES Page B..: B..: B..: B..: B.3.: B.3.: B.4.: B.4.: B.4.3: B.4.4: B.5: B.6: Non-collocated Swing U with PD Balancing Control Wincon Model.75 Subsystem Non-collocated Swing U.75 Collocated Swing U with PD Balancing Control Wincon Model.76 Subsystem Collocated Swing U.76 Sinusoidal Swing U with PD Balancing Control Wincon Model.77 Subsystem Sinusoidal Swing U.77 Classical PD Balancing Controller Wincon Subsystem..78 Classical PID Balancing Controller Wincon Subsystem with DC Filter 78 State Feedback Balancing Controller Wincon Subsystem..79 Feedback Linearization Balancing Controller Wincon Subsystem.79 Encoder Angle and Angular Velocity Subsystem...8 Parameter Identification Block Diagram.8 74

83 Aendix B theta threshold at i/ theta ( to i) Theta thetadot control ef f ort (V) theta (-i to i) Noncollocated Swingu thetadot Zero Switch threshold at theta theta (-i to i) thetadot Encoders Thetadot u Abs theta thetadot control ef f ort (v ) Relay on off - Switch Saturation Control Systems Lab RTI-85 DAC Analog Outut thetadot PD balancing control Control Effort Thetadot B..: Non-collocated Swing U with PD Balancing Control Wincon Model theta thetadot.4 k. kd si n 3 Trigonometric (3) Function /c /c () torue inversion control effort (v) B.. Subsystem Non-collocated Swing U 75

84 Aendix B theta theta ( to i) Theta theta thetadot control ef f ort (V) threshold at i/ theta (-i to i) Collocated Swingu thetadot u Zero Switch threshold at theta theta (-i to i) thetadot Encoders Abs Relay on off - Switch Saturation Control Systems Lab RTI-85 DAC Analog Outut theta thetadot control ef f ort (v ) Thetadot thetadot PD balancing control tau Thetadot B..: Collocated Swing U with PD Balancing Control Wincon Model si n -K- theta Trigonometric Function *(3)/().4 -K- gain control effort (N*m) theta k *()/() 3. thetadot kd B..: Subsystem Collocated Swing U 76

85 Aendix B dot control ef f ort (V) theta ( to i) Theta Sinusoidal Swingu threshold at i/ theta (-i to i) thetadot Zero Switch threshold at theta theta (-i to i) thetadot Encoders Thetadot theta u Abs Relay on off - Switch Saturation Control Systems Lab RTI-85 DAC Analog Outut thetadot control ef f ort (v ) thetadot Thetadot PD balancing control Control Effort B.3.: Sinusoidal Swing U with PD Balancing Control Wincon Model.5 Gain si n Trigonometric Function -4 Gain5 - tau sin - Gain Gain4 si n Trigonometric Function Gain3 Product Trigonometric Function Sign dot Switch c torue constant - torue inversion control effort (N*m) B.3.: Subsystem Sinusoidal Swing U 77

86 Aendix B theta K thetadot.47 Kd control effort (N*m) 3 thetadot.6 K B.4.: Classical PD Balancing Controller Wincon Subsystem s s.s. Velocity Filter4 K 5s s5 Velocity Filter.8 Kd tau (N*m) control effort s Integrator. Ki B.4.: Classical PID Balancing Controller Wincon Subsystem with DC Filter 78

87 Aendix B x K() K x K() K control effort (N*m) 3 x3 K(3) K3 B.4.3: State Feedback Balancing Controller Wincon Subsystem si n 3 Trigonometric Function 3 cos -K- Product Trigonometric Function 3/ - dot Product yddot 6 - Product3 control effort (N*m) -K- K3 PP ydot dot P K -K y 64 3 K 3 P3 cos Product K4 Trigonometric Function B.4.4: Feedback Linearization Balancing Controller Wincon Subsystem 79

88 Aendix B Subsystem "Encoders" for Reading and Differentiating Joint Angles Control Systems Lab RTI-85 Timer *i Constant mod Math Function theta ( to i) Time-Base theta Control Systems Lab US Digital ENC Encoder Inut (*i)/5 Convert Theta from counts to radians i *i Constant mod Math Function theta (-i to i) s s Velocity Filter theta dot 3 thetadot offset at rest theta 4 Control Systems Lab US Digital ENC Encoder Inut (*i)/ Convert Theta from counts to radians *i Constant mod Math Function i theta 5 theta (-i to i) offset at rest s s Velocity Filter theta dot 6 thetadot B.5: Encoder Angle and Angular Velocity Subsystem 8

89 Aendix B Parameter ID Simulink Wincon model used for arameter identification Control Systems Lab US Digital ENC Encoder Inut (*i)/5 Convert Theta from counts to radians i Constant z- aram(bdroot, Discrete differentiator d Signal Generator Control Systems Lab RTI-85 DAC Analog Outut tau Control Systems Lab US Digital ENC Encoder Inut (*i)/ Convert Theta from counts to radians z- aram(bdroot, Discrete differentiator d Control Systems Lab RTI-85 Timer Time-Base B.6: Parameter Identification Block Diagram 8

90 Aendix C APPENDIX C MECHANICAL DRAWINGS Page C.: C..: C..: C..3: C..4: C.3: C.4: C.5.: C.5.: C.6.: C.6.: C.6.3: C.7: C.8: C.9: C.: C.: C.: Gyrobot, Solid Model, Rest Position...83 Base Plate, Patch Panel Location Dimensions 84 Base Plate, Slot Dimensions 84 Base Plate, Block Location Holes...85 Base Plate, Patch Panel and Shield Location Holes 85 Patch Panel..86 Am Shield..86 Encoder Suort Base.87 Encoder Suort Post..87 Shaft Suort Base..88 Shaft Suort Post, Encoder End 88 Shaft Suort Post, Sli Ring End..89 Shaft.89 Couler 9 Flywheel..9 Link.9 Sli Ring Mount Plate.9 Parts List..9 8

91 Aendix C C.: Gyrobot, Solid Model, Rest Position 83

92 Aendix C C..: Base Plate, Patch Panel Location Dimensions C..: Base Plate, Slot Dimensions 84

93 Aendix C C..3: Base Plate, Block Location Holes C..4: Base Plate, Patch Panel and Shield Location Holes 85

94 Aendix C C.3: Patch Panel C.4: Am Shield 86

95 Aendix C C.5.: Encoder Suort Base C.5.: Encoder Suort Post 87

96 Aendix C C.6.: Shaft Suort Base C.6.: Shaft Suort Post, Encoder End 88

97 Aendix C C.6.3: Shaft Suort Post, Sli Ring End C.7: Shaft 89

98 Aendix C C.8: Couler C.9: Flywheel 9

99 Aendix C C.: Link C.: Sli Ring Mount Plate 9

100 Aendix C Part Descrition Part # Quantity Manufacturer DC ower source PSX3W4 Advanced Motion Controls Amlifier A8E Advanced Motion Controls Encoder Dynamics Research Cororation 4 VDC brush motor 937S Pittman Sli ring Tye 988 Fabricast, Inc Roller thrust bearing BR5- W. M. Berg, Inc Needle roller bearing NRB-65 W. M. Berg, Inc Set screw collar CS-54 W. M. Berg, Inc C.: Parts List 9

101 Aendix D APPENDIX D RTSS C CONTROLLER CODE Page D.: gyrobot.c

102 Aendix D /********************************************************************************* ** FILE: gyrobot.c (Using RTSS Timers to imlement control of the Gyrobot) ** ** ** ** MODIFIED: January 3, ** ** Adrian Lee ** *********************************************************************************/ #include <windows.h> #include <stdio.h> #include <math.h> #include <rtai.h> #include "cslrtx.h" #include "rtsscom.h" #include "rtidrv.h" #include "encodrbd.h" #define PI F #define K_torue F #define.43f #define.866f #define F // torue gain // system arameters // globals // A number of globals are used inorder to communicate back and // forth between the main background task and the foreground timer // tasks. Care must be taken on when these variables are udated. // Kee in mind that the foreground timer tasks interrut the back // ground task. //"g_" for global variables volatile float g_xk.f; volatile float g_xk.f; volatile float g_x3k.f; volatile float g_x4k.f; volatile float g_xold.f; volatile float g_xold.f; volatile float g_x3old.f; volatile float g_x4old.f; volatile float g_modxk_.f; volatile float g_modxk_.f; volatile float g_modx3k_.f; volatile float g_modx3k_.f; volatile float g_xerr.f; volatile float g_x4err.f; volatile float g_xint.f; volatile float g_u.f; volatile float g_tau.f; volatile float g_tauold.f; volatile float g_tau_dcfiltered.f; volatile float g_tauold_dcfiltered.f; volatile float g_direction.f; volatile float g_kswing.f; volatile float g_kswing.f; volatile float g_kswing3.f; volatile float g_kswing4.f; volatile float g_kbalance.f; volatile float g_kbalance.f; volatile float g_kbalance3.f; volatile float g_kbalance4.f; volatile float g_swingu.f; volatile float g_balance.f; volatile int g_swingu_flag ; volatile int g_balance_flag ; volatile float g_newkswing; volatile float g_newkswing; volatile float g_newkswing3; volatile float g_newkswing4; volatile float g_newkbalance; // Link osition // Link velocity // Disc osition // Disc velocity // revious link osition // revious link velocity // revious disc osition // revious disc velocity // modulated link osition (-i to i) // modulated link osition ( to i) // modulated disc osition (-i to i) // modulated disc osition ( to i) // link osition error // disc velocity error // integral sum // DAC outut voltage // control effort // left or right halfcircle rotation // swingu gains // balancing gains // controller selection sent from VB in float array // controller selection flags // new swingu gains (sent from VB) // new balancing gains (sent from VB) 94

103 Aendix D volatile float g_newkbalance; volatile float g_newkbalance3; volatile float g_newkbalance4; volatile float g_samleeriod.5f; volatile float g_senddata[]; volatile int g_dontudateflag ; volatile int g_newgains ; volatile float g_amlitude.f; volatile float g_duration.f; volatile int g_pulse ; volatile float g_timer.f; volatile long int g_time; volatile int halt_flag ; volatile int first_run ; // samling rate, Ts // data array // background flag // new gains available flag // Disturbance ulse amlitude // Disturbance ulse duration // aly ulse flag // ulse duration timer // time index // declaration of timer handler (Actual Function at end of file) // think ISR when you read TIMER HANDLER int RTFCNDCL TimerHandler(PVOID unused); // **************************************************************** // MAIN // **************************************************************** VOID main(int argc, char **argv) { LARGE_INTEGER time; // LARGE_INTEGER in a 64bit integer LONG command; int uit_status ; // uit flag int timer_status ; // timer on flag HANDLE htimer; float tmdata[]; // temorary float buffer char tmbuff[max_buff_len]; // temorary char buffer // // Setu and start the eriodic timer. // if (!(htimer RtCreateTimer( NULL, // Security - NULL is none, // Stack size - is use default TimerHandler, // Timer handler NULL, // NULL context (argument to handler) RT_PRIORITY_MAX, // Priority CLOCK_FASTEST))) // Always use fastest available clock { rintf("error: Could not create the RTX timer.\n"); ExitProcess( ); // // Initialize Communication with Win3 DLL // if (InitRTSS_Communication()! ) { rintf("error: Could not setu communication with DLL\n"); ExitProcess(); // init_rti must be called before any other RTI-85 (rtidrv3) functions // It enables ort I/O for the I/O sace used by the RTI-85 init_rti(); // Initialize flags uit_status ; timer_status ; // Loo until told to uit while (uit_status ) { // Continue to wait for a command from DLL // Also every milliseconds check to see if Timer Handler // Has activated an event. Presently this code does not use // events but it can be easily added. You may want to talk to // TA on how to add events calls. rtsscomm.c has all the needed // functions to call events and you will have to modify your // VB rogram to handle events. 95

104 Aendix D command WaitforCommandandProcessEvents(); switch( command ) { case x4: // Command Start // Check that Start command had not already been sent if (timer_status ) { if(first_run ) { // Initialize US Digital Encoder board init_766(); // Initialize variables g_xold PI; g_xold.f; g_x3old.f; g_x4old.f; g_u.f; g_tau.f; g_tauold.f; g_tau_dcfiltered.f; g_tauold_dcfiltered.f; g_modxk_.f; g_modxk_.f; g_modx3k_.f; g_modx3k_.f; g_xerr PI; g_x4err.f; g_xint.f; first_run ; // Set time index to zero g_time ; // Zero savedata buffer init_savedata(); // Start timer with given samle Period // The eriod is loaded in units of ns // Note that the minumun eriod is.5 milliseconds for a // RTSS timer. time.quadpart (LONGLONG) (g_samleeriod*..5); if (!RtSetTimerRelative( htimer, &time, &time)) { rintf("error: Could not set and start the RTAPI timer.\n"); ExitProcess( ); // Flag that Start Command Active timer_status ; halt_flag ; break; case x4: // Command Quit uit_status ; break; case x4: // Command Udate Both Controller Gains GetSentData(tmdata,tmbuff); g_newkswing tmdata[]; g_newkswing tmdata[]; g_newkswing3 tmdata[]; g_newkswing4 tmdata[3]; g_newkbalance tmdata[4]; g_newkbalance tmdata[5]; g_newkbalance3 tmdata[6]; g_newkbalance4 tmdata[7]; g_swingu tmdata[8]; g_balance tmdata[9]; // the reset of the data values not used in this command // tmdata[]; // tmdata[]; // tmdata[]; // tmdata[3]; // tmdata[4]; // tmdata[5]; // tmdata[6]; 96

105 Aendix D // tmdata[7]; // tmdata[8]; // tmdata[9]; g_swingu_flag ; if(g_swingu >.5F) g_swingu_flag ; if(g_swingu >.5F) g_swingu_flag 3; g_balance_flag ; if(g_balance >.5F) g_balance_flag ; if(g_balance >.5F) g_balance_flag 3; if(g_balance > 3.5F) g_balance_flag 4; // Set Default to Periodic // Swingu (g_swingu.) // Change to Non-collocated // Swingu (g_swingu.) // Change to Collocated // Swingu (g_swingu 3.) // Set Default to Classical PD // (g_balance.) // Change to Classical PID // (g_balance.) // Change to State Feedback // (g_balance 3.) // Change to Feedback // Linearization (g_balance3.) g_newgains ; //Tell the Timer Handler that there are new //gains to use next time the timer handler is called break; case x43: // Command Halt if (timer_status ) { // Only Halt if Start command occurred first halt_flag ; // halt flag //RtCancelTimer( htimer, NULL); // Halt timer volt_out(,.f); volt_out(,.f); timer_status ; // zero DACs // Flag timer off break; case x44: // Command Udate Samle Period //GetSentData(tmdata,tmbuff); //g_samleeriod tmdata[] /.F; // The rest of the data values are not used with this command break; case x45: // Send stored data u to Win3 DLL and then to VB rogram // Flag telling the timer handler not to udate the g_senddata // array until the entire array has been loaded into tmdata g_dontudateflag ; tmdata[] g_senddata[]; tmdata[] g_senddata[]; tmdata[] g_senddata[]; tmdata[3] g_senddata[3]; tmdata[4] g_senddata[4]; tmdata[5] g_senddata[5]; tmdata[6] g_senddata[6]; tmdata[7] g_senddata[7]; tmdata[8] g_senddata[8]; tmdata[9] g_senddata[9]; tmdata[] g_senddata[]; tmdata[] g_senddata[]; tmdata[] g_senddata[]; tmdata[3] g_senddata[3]; tmdata[4] g_senddata[4]; tmdata[5] g_senddata[5]; tmdata[6] g_senddata[6]; tmdata[7] g_senddata[7]; tmdata[8] g_senddata[8]; tmdata[9] g_senddata[9]; // flag timer handler that it is again ok to udate // g_senddata g_dontudateflag ; // Send data to Server DLL SetSendData(tmdata); break; case x46: // Save Data Command // Get Filename to save data to GetSentData(tmdata,tmbuff); 97

106 Aendix D // Only erform the save to file if we are in Halt mode if (timer_status ) { // Save data saved with the savedata function to file tmbuff if (savedatafile(tmbuff)! ) { rintf("error saving data"); break; case x47: // Inut Disturbance Pulse GetSentData(tmdata,tmbuff); g_amlitude tmdata[] * K_torue /.F; // (convert from N*mm to Volts) g_duration tmdata[] /.F; // (convert from ms to seconds) g_pulse ; g_timer.f; break; default: break; // // Sto and delete the timer. // RtDeleteTimer( htimer); volt_out(,.f); //zero dacs volt_out(,.f); // Close communication objects DeleteRTSS_Communication(); ExitProcess( ); // **************************************************************** // TIMER HANDLER // **************************************************************** int RTFCNDCL TimerHandler(PVOID unused) { // New version of ISR... // Udate Swingu and Balancing controller gains with new values // when background task flags that there are new gains if (g_newgains ) { g_kswing g_newkswing; g_kswing g_newkswing; g_kswing3 g_newkswing3; g_kswing4 g_newkswing4; g_kbalance g_newkbalance; g_kbalance g_newkbalance; g_kbalance3 g_newkbalance3; g_kbalance4 g_newkbalance4; // Flag Newgains back to NO g_newgains ; // // Read encoder ositions // read_encoders((float *)&g_xk,(float *)&g_x3k); g_x3k -g_x3k; // // Modulate angles // // ma link angle to (-PI,PI) range g_modxk_ g_xk-(.f*pi)*((float) floor(g_xk/(.f*pi))) - PI; // move link angle zero osition to downward vertical g_xk PI; // ma link angle to (,PI) range g_modxk_ g_xk-(.f*pi)*((float)floor(g_xk/(.f*pi))); // ma disc angle to (-PI,PI) range 98

107 Aendix D g_x3k PI; // adjust angle for modulation g_modx3k_ g_x3k-(.f*pi)*((float)floor(g_x3k/(.f*pi))) - PI; // ma disc angle to (,PI) range g_x3k - PI; // correct angle from revious modulation g_modx3k_ g_x3k-(.f*pi)*((float)floor(g_x3k/(.f*pi))); // // Calculate velocity using Tustin aroximation // { Matlab command: sysd cd(tf([ ],[ ]),.5, tustin ) // g_xk.948f*g_xold 9.5F*(g_xk - g_xold); g_x4k.948f*g_x4old 9.5F*(g_x3k - g_x3old); // ####################### // BALANCING CONTROLLERS // ####################### if(fabs(g_modxk_)<.45f) { // // Classical PD (modulated -i to i) // if(g_balance_flag ) { g_tau g_kbalance*g_modxk_ g_kbalance*g_xk g_kbalance4*g_x4k; g_u -K_torue*g_tau; // // Classical PID (modulated -i to i) // if(g_balance_flag ) { g_xint g_xint.5f*(g_xk g_xold); if(g_xint >.F) { g_xint.f; if(g_xint < -.F) { g_xint.f; g_tau g_kbalance*g_xk g_kbalance*g_xk g_kbalance3*g_xint; g_tau_dcfiltered.9998f*g_tauold_dcfiltered.9995f*(g_tau - g_tauold); // // State Feedback (modulated -i to i) // if(g_balance_flag 3) { g_tau g_kbalance*g_modxk_ g_kbalance*g_xk g_kbalance4*g_x4k; g_u -K_torue*g_tau; // // Feedback Linearization (modulated -i to i) // if(g_balance_flag 4) { g_tau -( g_kbalance*(*g_x3k()*g_modxk_) g_kbalance*(*g_x4k()*g_xk) ); g_tau -( g_kbalance3*3*( (float)sin(g_modxk_) ) g_kbalance4*3*g_xk*( (float)cos(g_modxk_) ) ); g_tau -( 3*( (float)sin(g_modxk_) )*( 3/*( (float)cos(g_modxk_) ) - (g_xk*g_xk) ) ); g_tau g_tau / (-3/*( (float)cos(g_modxk_) ) ); g_u -K_torue*g_tau; // ###################### // SWING UP CONTROLLERS // ###################### else { 99

108 Aendix D // // Sinusoidal (modulated to *i) // if(g_swingu_flag ) { if(sin(g_xk) > ) g_direction.f; else g_direction -.F; g_tau g_kswing*(((float)sin(g_modxk_/.f))-(.5f*g_direction*((float)sin(.f*g_modxk_)))); if(g_xk > ) g_u g_tau; // clockwise rotation else g_u -g_tau; // counterclockwise rotation // // Non-collocated (modulated to *i) // if(g_swingu_flag ) { g_tau 3*( (float)sin(g_modxk_) ) *(g_kswing*g_modxk_ g_kswing*g_xk); g_u -K_torue*g_tau; // // Collocated (modulated to *i) // if(g_swingu_flag 3) { g_tau ( *3/() )*( (float)sin(g_modxk_) ) ( */() )*(g_kswing*g_x3k g_kswing*g_x4k); g_u -K_torue*g_tau; // // Error calculation // g_xerr.f - g_modxk_; g_x4err.f - g_x4k; // // Disturbance Pulse // if (g_pulse ) { if(g_timer < g_duration){ g_u g_amlitude; g_timer g_samleeriod; else { g_pulse ; // // Emergency check // if(g_u > 9.5F){ g_u.f; if(g_u < -9.5F){ g_u.f; // // Check if controller has been halted // if(halt_flag ) { g_u.f; // // send control outut to DAC // volt_out(,g_u);

109 Aendix D // Save data to g_senddata array. // First check to see if Background task is resently using the // g_senddata array. If it is not, g_dontudateflag, // then go ahead and save new data to the array if (g_dontudateflag ) { g_senddata[] g_xk; g_senddata[] g_xk; g_senddata[] g_x3k; g_senddata[3] g_x4k; g_senddata[4] g_u; g_senddata[5] g_xerr; g_senddata[6] g_x4err; g_senddata[7] ((float) g_samleeriod*g_time); g_senddata[8].f; g_senddata[9].f; g_senddata[].f; g_senddata[].f; g_senddata[].f; g_senddata[3].f; g_senddata[4].f; g_senddata[5].f; g_senddata[6].f; g_senddata[7].f; g_senddata[8].f; g_senddata[9].f; // Savedata saves 8 floating oint values to a buffer that it // manages. Deending on the first arameter this buffer can be // circular (circ ) or once through (circ ). The Buffer size // is determined by the define NUM_POINTS in rtsscom.h // Each call to savedata increments the buffer (unless POINT_INCREMENT greater than (See // rtsscom.c)) // The function savedatafile can be used to write this buffer // to a assed filename. See Save Data Command savedata(,((float) g_samleeriod*g_time),g_xk,g_xk,g_x3k,g_x4k,g_tau,g_u,.f); // kee track of ast states g_xold g_xk; g_xold g_xk; g_x3old g_x3k; g_x4old g_x4k; g_tauold g_tau; g_tauold_dcfiltered g_tau_dcfiltered; // udate time index g_time g_time ; return ;

110 Aendix E APPENDIX E VISUAL BASIC SERVER INTERFACE CODE Page E.: E.: E.3: E.4: E.5: Visual Basic 6 Grahical Interface 3 Visual Basic Form gyrofront.frm Code Visual Basic Module mydatatyes.bas Code..4 Visual Basic Module myints.bas Code...4 Visual Basic Module rtsscomm.bas Code.5

111 Aendix E E.: Visual Basic 6 Grahical Interface 3

112 Aendix E ########################################### # # # Visual Basic Form "gyrofront.frm" Code # # # # Author: Adrian Lee, Henry Hahn # # Date: 3/3/ # # # ########################################### Otion Exlicit Dim sservermsg As String Private Sub cmdbrowsefile_click() Dim errcode As Integer Dim tmstrg As String Set Hex value for ShowOen Flags See Online hel for more info cdlggetfile.flags &H884 Show All Files cdlggetfile.filter "(*.*) *.*" Start ShowOen Common Dialog cdlggetfile.showoen Get filename chosen txtfilename.text cdlggetfile.filename End Sub Private Sub cmdclearlist_click() lststatus.clear End Sub Private Sub cmdinit_click() Dim errcode As Integer Dim tmstrg As String cdlggetfile.flags &H884 cdlggetfile.filter "(*.rtss) *.rtss" cdlggetfile.showoen If cdlggetfile.filename <> "" Then tmstrg "rtssrun " cdlggetfile.filename errcode InitRTSSCommunication(gHW, tmstrg) If errcode <> Then MsgBox "Error Init of RTSS, errcode " & errcode Else cmdstart.enabled True cmdsetgains.enabled True cmdsamle.enabled True otsinusoidal.enabled True otcollocated.enabled True otnoncollocated.enabled True otpd.enabled True otpid.enabled True otstate.enabled True otlinear.enabled True otadatation.enabled True txtswingk.enabled True txtswingk.enabled True txtswingk3.enabled True txtswingk4.enabled True txtbalk.enabled True txtbalk.enabled True txtbalk3.enabled True txtbalk4.enabled True scrtimer.enabled True scrsamle.enabled True scramlitude.enabled True scrduration.enabled True cmdinit.cation "Running\n" cdlggetfile.filetitle cmdinit.enabled False 4

113 Aendix E Set flag gcommstarted End If End If End Sub Private Sub cmdquit_click() Quit Unload Me End Sub Public Sub cmdsamle_click() Dim errcode As Integer Dim data() As Single Dim dummybuff As String dummybuff "dummydata" errcode data() scrsamle.value Send Pulse errcode SendCmdToRTSS_RecvDataFromVB(&H44, data(), dummybuff) If errcode <> Then MsgBox "Error sending Samling Rate, errcode " & errcode End If End Sub Private Sub cmdsavefile_click() Dim errcode As Integer Dim savedatafilename As String Dim dummydata() As Single If txtfilename.text <> "" Then savedatafilename txtfilename.text Send Savedatatofile command x46 with File name txtfilename errcode SendCmdToRTSS_RecvDataFromVB(&H46, dummydata(), savedatafilename) If errcode <> Then MsgBox "Error sending Save File Command, errcode " & errcode End If End If cmdsavefile.enabled False End Sub Public Sub cmdpulse_click() Dim errcode As Integer Dim data() As Single Dim dummybuff As String dummybuff "dummydata" errcode data() scramlitude.value data() scrduration.value Send Pulse errcode SendCmdToRTSS_RecvDataFromVB(&H47, data(), dummybuff) If errcode <> Then MsgBox "Error sending Disturbance Pulse, errcode " & errcode End If End Sub Public Sub cmdsetgains_click() Dim errcode As Integer Dim data() As Single Dim dummybuff As String Dim swingu As Single Dim balance As Single dummybuff "dummydata" errcode 5

114 Aendix E Error checking (Gains can only be Single (float) values) If IsNumeric(txtSwingK.Text) False Then errcode MsgBox "Error in your Swing U K value" End If If IsNumeric(txtSwingK.Text) False Then errcode MsgBox "Error in your Swing U K value" End If If IsNumeric(txtSwingK3.Text) False Then errcode MsgBox "Error in your Swing U K3 value" End If If IsNumeric(txtSwingK4.Text) False Then errcode MsgBox "Error in your Swing U K4 value" End If If IsNumeric(txtBalK.Text) False Then errcode MsgBox "Error in your Balancing K value" End If If IsNumeric(txtBalK.Text) False Then errcode MsgBox "Error in your Balancing K value" End If If IsNumeric(txtBalK3.Text) False Then errcode MsgBox "Error in your Balancing K3 value" End If If IsNumeric(txtBalK4.Text) False Then errcode MsgBox "Error in your Balancing K4 value" End If Check which Swing U Controller is selected If otsinusoidal.value True Then swingu End If If otnoncollocated.value True Then swingu End If If otcollocated.value True Then swingu 3 End If If otpd.value True Then balance End If If otpid.value True Then balance End If If otstate.value True Then balance 3 End If If otlinear.value True Then balance 4 End If If otadatation.value True Then balance 5 End If If errcode Then data() txtswingk.text data() txtswingk.text data() txtswingk3.text data(3) txtswingk4.text data(4) txtbalk.text data(5) txtbalk.text data(6) txtbalk3.text data(7) txtbalk4.text data(8) swingu data(9) balance Send Gains 6

115 Aendix E errcode SendCmdToRTSS_RecvDataFromVB(&H4, data(), dummybuff) If errcode <> Then MsgBox "Error sending Controller Gains, errcode " & errcode End If End If End Sub Public Sub cmdstart_click() Dim errcode As Integer Send Start Command errcode SendCmdToRTSS_Simle(&H4) If errcode <> Then MsgBox "Error sending command START, errcode " & errcode End If If errcode Then Activate/Deactivate aroriate buttons timudatedata.enabled True cmdstart.enabled False cmdsto.enabled True cmdsavefile.enabled False cmdpulse.enabled True End If End Sub Public Sub cmdsto_click() Dim errcode As Integer Send Halt command errcode SendCmdToRTSS_Simle(&H43) If errcode <> Then MsgBox "Error sending command HALT, errcode " & errcode End If Activate/Deactivate aroriate buttons timudatedata.enabled False cmdstart.enabled True cmdsto.enabled False cmdsavefile.enabled True cmdpulse.enabled False End Sub Private Sub Form_Load() Global flag indicating if RTSS rocess start or not gcommstarted Get VB rogram s window handle ghw Me.hWnd Start catching windows messages (events) Add this next line when doing events Hook lbltimer.cation scrtimer.value If server_socket.state <> sckclosed Then server_socket.close End If server_socket.listen lststatus.additem ("Listening to ort " CStr(server_socket.LocalPort) "...") End Sub Private Sub Form_Unload(Cancel As Integer) Dim errcode As Integer If gcommstarted Then Send Quit errcode SendCmdToRTSS_Simle(&H4) If errcode <> Then MsgBox "Error sending command QUIT, errcode " & errcode End If Close RTSS rocess errcode DeleteRTSSCommunication() If errcode <> Then 7

116 Aendix E MsgBox "Error Delete of RTSS, errcode " & errcode End If End If If server_socket.state <> sckclosed Then server_socket.close End If End Sub Private Sub scrsamle_change() lblsamle.cation scrsamle.value End Sub Private Sub scrtimer_change() lbltimer.cation scrtimer.value timudatedata.interval scrtimer.value End Sub Private Sub scramlitude_change() lblamlitude.cation scramlitude.value End Sub Private Sub scrduration_change() lblduration.cation scrduration.value End Sub Private Sub server_socket_connectionreuest(byval reuestid As Long) sservermsg "Connection reuest from " & server_socket.remotehostip lststatus.additem (sservermsg) If server_socket.state <> sckclosed Then server_socket.close End If server_socket.accet reuestid lststatus.additem ("Connection acceted.") If (Len(txtStartMsg.Text) > And chkstart.value True) Then server_socket.senddata (txtstartmsg.text "X") Else server_socket.senddata ("Hello " server_socket.remotehostip ".X") End If End Sub Public Sub server_socket_close() sservermsg "Connection closed: " & server_socket.remotehostip lststatus.additem (sservermsg) server_socket.close server_socket.listen me.cmdsto End Sub Private Sub server_socket_dataarrival(byval bytestotal As Long) Dim vtdata As String Dim vtdataarray() As Byte Dim cmd As String Dim swingu_controller As String Dim balance_controller As String Dim j As Integer Dim swingk(3) As Single Dim balk(3) As Single Dim tsingle As MySingle 8

117 Aendix E Dim tbytes As MyBytes Dim tint As MyIntegers Dim tintbyte As MyIntBytes Dim Control As Integer Dim transmit As Integer Dim amlitude As Integer Dim duration As Integer server_socket.getdata vtdata, vbbyte vtdataarray vtdata cmd Chr(vtDataArray()) Chr(vtDataArray()) Select Case cmd Case "ST" lststatus.additem ("Starting controller...") Me.cmdStart_Click Case "HA" lststatus.additem ("Stoing controller...") Me.cmdSto_Click Case "SE" lststatus.additem ("Setting Gains...") swingu_controller Chr(vtDataArray(4)) balance_controller Chr(vtDataArray(6)) Select Case swingu_controller Case "A" otsinusoidal.value True Case "B" otnoncollocated.value True Case "C" otcollocated.value True Case Else End Select Select Case balance_controller Case "A" otpd.value True Case "B" otpid.value True Case "C" otstate.value True Case "D" otlinear.value True Case "E" otadatation.value True Case Else End Select For j To 3 tbytes.bf() * vtdataarray(6 * j 8) 6 * vtdataarray(6 * j ) tbytes.bf() * vtdataarray(6 * j ) 6 * vtdataarray(6 * j 4) tbytes.bf() * vtdataarray(6 * j 6) 6 * vtdataarray(6 * j 8) tbytes.bf(3) * vtdataarray(6 * j ) 6 * vtdataarray(6 * j ) LSet tsingle tbytes swingk(j) tsingle.f Next j For j 4 To 7 tbytes.bf() * vtdataarray(6 * j 8) 6 * vtdataarray(6 * j ) tbytes.bf() * vtdataarray(6 * j ) 6 * vtdataarray(6 * j 4) tbytes.bf() * vtdataarray(6 * j 6) 6 * vtdataarray(6 * j 8) tbytes.bf(3) * vtdataarray(6 * j ) 6 * vtdataarray(6 * j ) LSet tsingle tbytes 9

118 Aendix E balk(j - 4) tsingle.f Next j lststatus.additem (CStr(swingK()) " " CStr(swingK()) " " CStr(swingK()) " " CStr(swingK(3))) lststatus.additem (CStr(balK()) " " CStr(balK()) " " CStr(balK()) " " CStr(balK(3))) txtswingk.text swingk() txtswingk.text swingk() txtswingk3.text swingk() txtswingk4.text swingk(3) txtbalk.text balk() txtbalk.text balk() txtbalk3.text balk() txtbalk4.text balk(3) Me.cmdSetGains_Click Case "SA" lststatus.additem ("Saving Run Data...") Case "SP" Control CInt(vtDataArray(4) 8 * vtdataarray(6)) scrsamle.value Control lblsamle.cation Control Me.cmdSamle_Click transmit CInt(vtDataArray(8) 8 * vtdataarray()) scrtimer.value transmit lbltimer.cation transmit timudatedata.interval transmit Case "PU" tintbyte.bf() vtdataarray(4) 6 * vtdataarray(6) tintbyte.bf() vtdataarray(8) 6 * vtdataarray() LSet tint tintbyte amlitude tint.i lststatus.additem (CStr(amlitude)) tintbyte.bf() vtdataarray() 6 * vtdataarray(4) tintbyte.bf() vtdataarray(6) 6 * vtdataarray(8) LSet tint tintbyte duration tint.i lststatus.additem (CStr(duration)) scramlitude.value amlitude scrduration.value duration Me.cmdPulse_Click Case Else lststatus.additem ("Data uninterretable") tbytes.bf() vtdataarray() tbytes.bf() vtdataarray() tbytes.bf() vtdataarray() tbytes.bf(3) vtdataarray(3) LSet tsingle tbytes lststatus.additem (CStr(tSingle.f)) End Select sservermsg "Received: " CStr(cmd) " " CStr(swingu_controller) CStr(balance_controller) " " " from " server_socket.remotehostip lststatus.additem (sservermsg) End Sub Timer function Private Sub timudatedata_timer() Dim errcode As Integer Dim data() As Single Dim i As Single i Dim reverse_float_array(3) As Byte Dim tsingle As MySingle

119 Aendix E Dim tbytes As MyBytes Get data to dislay from the RTSS rocess errcode SendCmdToRTSS_SendDataToVB(&H45, data()) If errcode <> Then MsgBox "Error sending command GetData, errcode " & errcode End If Dislay data lbldata.cation data() * 8 / i lbldotdata.cation data() lbldata.cation data() * 8 / i lbldotdata.cation data(3) lbltaudata.cation data(4) data(5) link angle osition error data(6) disc velocity error If server_socket.state <> sckclosed Then Send link angle tsingle.f CSng((data() * 8) / i) LSet tbytes tsingle reverse_float_array() tbytes.bf(3) reverse_float_array() tbytes.bf() reverse_float_array() tbytes.bf() reverse_float_array(3) tbytes.bf() On Error GoTo errorhandler server_socket.senddata reverse_float_array Send link velocity tsingle.f CSng(data()) LSet tbytes tsingle reverse_float_array() tbytes.bf(3) reverse_float_array() tbytes.bf() reverse_float_array() tbytes.bf() reverse_float_array(3) tbytes.bf() On Error GoTo errorhandler server_socket.senddata reverse_float_array Send disc angle tsingle.f CSng(data()) LSet tbytes tsingle reverse_float_array() tbytes.bf(3) reverse_float_array() tbytes.bf() reverse_float_array() tbytes.bf() reverse_float_array(3) tbytes.bf() On Error GoTo errorhandler server_socket.senddata reverse_float_array Send disc velocity tsingle.f CSng(data(3)) LSet tbytes tsingle reverse_float_array() tbytes.bf(3) reverse_float_array() tbytes.bf() reverse_float_array() tbytes.bf() reverse_float_array(3) tbytes.bf() On Error GoTo errorhandler server_socket.senddata reverse_float_array Send control effort tsingle.f CSng(data(4)) LSet tbytes tsingle reverse_float_array() tbytes.bf(3)

120 Aendix E reverse_float_array() tbytes.bf() reverse_float_array() tbytes.bf() reverse_float_array(3) tbytes.bf() On Error GoTo errorhandler server_socket.senddata reverse_float_array Send link angle error tsingle.f CSng(data(5)) LSet tbytes tsingle reverse_float_array() tbytes.bf(3) reverse_float_array() tbytes.bf() reverse_float_array() tbytes.bf() reverse_float_array(3) tbytes.bf() On Error GoTo errorhandler server_socket.senddata reverse_float_array Send disc velocity error tsingle.f CSng(data(6)) LSet tbytes tsingle reverse_float_array() tbytes.bf(3) reverse_float_array() tbytes.bf() reverse_float_array() tbytes.bf() reverse_float_array(3) tbytes.bf() On Error GoTo errorhandler server_socket.senddata reverse_float_array End If Exit Sub errorhandler: lststatus.additem ("Error writing to socket!") lststatus.additem ("Stoing Exeriment!") lststatus.additem ("Closing server_socket") Me.server_socket_Close Me.cmdSto_Click timudatedata.enabled False End Sub Private Sub otsinusoidal_click() txtswingk.visible True txtswingk.visible False txtswingk3.visible False txtswingk4.visible False lblswingk.visible True lblswingk.visible False lblswingk3.visible False lblswingk4.visible False End Sub Private Sub otnoncollocated_click() txtswingk.visible True txtswingk.visible True txtswingk3.visible False txtswingk4.visible False lblswingk.visible True lblswingk.visible True lblswingk3.visible False lblswingk4.visible False End Sub Private Sub otcollocated_click() txtswingk.visible True txtswingk.visible True txtswingk3.visible False txtswingk4.visible False

121 Aendix E lblswingk.visible True lblswingk.visible True lblswingk3.visible False lblswingk4.visible False End Sub Private Sub otpd_click() txtbalk.visible True txtbalk.visible True txtbalk3.visible False txtbalk4.visible True lblbalk.visible True lblbalk.visible True lblbalk3.visible False lblbalk4.visible True lblbalk.cation "K" lblbalk.cation "Kd" lblbalk4.cation "Kd" End Sub Private Sub otpid_click() txtbalk.visible True txtbalk.visible True txtbalk3.visible True txtbalk4.visible False lblbalk.visible True lblbalk.visible True lblbalk3.visible True lblbalk4.visible False lblbalk.cation "K" lblbalk.cation "Kd" lblbalk3.cation "Ki" End Sub Private Sub otstate_click() txtbalk.visible True txtbalk.visible True txtbalk3.visible True txtbalk4.visible True lblbalk.visible True lblbalk.visible True lblbalk3.visible True lblbalk4.visible True lblbalk.cation "K" lblbalk.cation "K" lblbalk3.cation "K3" lblbalk4.cation "K4" End Sub Private Sub otlinear_click() txtbalk.visible True txtbalk.visible True txtbalk3.visible True txtbalk4.visible True lblbalk.visible True lblbalk.visible True lblbalk3.visible True lblbalk4.visible True lblbalk.cation "K" lblbalk.cation "K" lblbalk3.cation "K3" lblbalk4.cation "K4" End Sub Private Sub otadatation_click() txtbalk.visible True txtbalk.visible True txtbalk3.visible True txtbalk4.visible False lblbalk.visible True 3

122 Aendix E lblbalk.visible True lblbalk3.visible True lblbalk4.visible False lblbalk.cation "K" lblbalk.cation "Kd" lblbalk3.cation "Ki" End Sub 4

123 Aendix E ############################################ # # # Visual Basic Module "mydatatyes.bas" Code # # # # Author: Adrian Lee, Henry Hahn # # Date: 3/3/ # # # ############################################ Tye MySingle f As Single End Tye Tye MyBytes bf(3) As Byte End Tye Tye MyInteger As Integer End Tye ############################################ # # # Visual Basic Module "myints.bas" Code # # # # Author: Adrian Lee, Henry Hahn # # Date: 3/3/ # # # ############################################ Tye MyIntegers i As Integer End Tye Tye MyIntBytes bf() As Byte End Tye 5

124 Aendix E ############################################ # # # Visual Basic Module "rtsscomm.bas" Code # # # # Author: Dan Block, Adrian Lee # # Date: 3/3/ # # # ############################################ Otion Exlicit Global variables Public Const GWL_WNDPROC -4 Global lprevwndproc As Long Global ghw As Long Global gcommstarted As Integer Ignore unless using RTSS events Ignore unless using RTSS events Handle to VB window Flag indicating if RTSS roc. started Win3 API functions for message handling For more information, see "Passing Function Pointers to Dll Procedures and Tye Libraries" - index entry function ointers in Books Online These Win3 API are used to setu a Message Que So the VB rogram can receive messages for the Mecha.dll DLL. Messages are sent from Mecha.dll when an event is sent from the RTSS rocess Declare Function CallWindowProc Lib "user3" Alias "CallWindowProcA" (ByVal lprevwndfunction As Long, ByVal hwnd As Long, ByVal Msg As Long, ByVal wparam As Long, ByVal lparam As Long) _ As Long Declare Function SetWindowLong Lib "user3" Alias "SetWindowLongA" (ByVal hwnd As Long, ByVal nindex As Long, ByVal dwnewlong As Long) As Long Declarations for the function calls in Mecha.dll Public Declare Function RunUtility Lib "mecha" (ByVal lcmd _ As String) As Long Public Declare Function InitRTSSCommunication Lib "mecha" (ByVal hwnd As Long, ByVal filename As String) As Long Public Declare Function DeleteRTSSCommunication Lib "mecha" () As Long Public Declare Function SendCmdToRTSS_Simle Lib "mecha" (ByVal Command As Long) As Long Public Declare Function SendCmdToRTSS_SendDataToVB Lib "mecha" (ByVal Command As Long, ByRef SentData As Single) As Long Public Declare Function SendCmdToRTSS_RecvDataFromVB Lib "mecha" (ByVal Command As Long, ByRef RecData As Single, _ ByVal buffer As String) As Long HOOK is used to setu Window Messaging in the VB rogram Public Sub Hook() Start using the message handler in this form lprevwndproc SetWindowLong(gHW, GWL_WNDPROC, AddressOf WindowProc) End Sub UNHOOK kills the Window Messaging Public Sub Unhook() Sto using this form s message handler Dim tem As Long tem SetWindowLong(gHW, GWL_WNDPROC, lprevwndproc) End Sub WindowProc is the function called in the event of a Message. You may need to modify this function if you are using events in your rtss rocess Function WindowProc(ByVal hw As Long, ByVal umsg As Long, ByVal wparam As Long, ByVal lparam _ As Long) As Long If this message is the Event message If umsg &H4 Then Check which Event Command was sent You could add additional If statements here to check for other wparam values and erform the necessary task in resonse to the Message If wparam Then MsgBox "Error, Event occurred,wparam " Else MsgBox "Error, Event occurred" 6

125 Aendix E End If End If Pass the message to the main message loo WindowProc CallWindowProc(lPrevWndProc, hw, umsg, wparam, lparam) End Function 7

126 Aendix F APPENDIX F JAVA APPLET CODE Page F..: Java Alet Interface Window..9 F..: Samling Otions and Disturbance Pulse Java Alet Windows 9 F.: F.3: F.4: F.5: F.6: F.7: F.8: F.9: F.: F.: F.: GyroA.java. GyroCanvas.java 3 GetDataThread.java 35 ExerimentSocket.java...4 ErrorWindow.java..4 ExerimentShell.java..44 OtionsWindow.java..45 ParameterPanel.java 48 SamleWindow.java...5 PulseWindow.java..54 Plotter.java

127 Aendix F F..: Java Alet Interface Window F..: Samling Otions and Disturbance Pulse Java Alet Windows 9