IP lab manual - GOOGOL TECHgoogoltech.com.hk/uploads/catalog/569/GLIP experiment and... ·...

©Googol 2006 1

INVERTED PENDULUM

EXPERIMENTAL MANUAL Suitable for GLIP Series

GOOGOL TECHNOLOGY

Second Edition, July, 2006

Be sure to give this instruction manual to customers!

Thank you very much for purchasing INVERTED PENDULUM (GIP Series) ofGoogol Tech.

Be sure to read this manual carefully before operation. For any technical trouble, call us or visit http://www.googoltech.com on the World

Wide Web for consultation. After reading this manual, keep it handy so that it can be referred to at anytime.

A-PDF Merger DEMO : Purchase from www.A-PDF.com to remove the watermark

http://www.a-pdf.com

Table of Content

I

Table of Content

TABLE OF CONTENT .................................................................................................... I

CHAPTER 1 OVERVIEW.........................................................................................1

1.1 INTRODUCTION ...................................................................................................1 1.2 INVERTED PENDULUM CLASSIFICATION ..............................................................1 1.3 INVERTED PENDULUM PROPERTIES .....................................................................3 1.4 CONTROLLER DESIGN METHOD..........................................................................4 1.5 INVERTED PENDULUM EXPERIMENTS..................................................................4

CHAPTER 2 MOTION CONTROL BASIC EXPERIMENT ................................6

2.1 ENCODER PRINCIPLE AND APPLICATION..............................................................6 2.1.1 Encoder Principle.........................................................................................6 2.1.2 Angle Conversion ........................................................................................7 2.1.3 Encoder Experiment.....................................................................................7

2.2 MOTOR CONTROL IN MATLAB SIMULINK..........................................................16

CHAPTER 3 LINEAR INVERTED PENDULUM MODELING, SIMULATION AND EXPERIMENT.................................................................................................25

3.1 1-STAGE LINEAR INVERTED PENDULUM SYSTEM MODEL .................................25 3.1.1 Differential Equations Methods.................................................................25

3.1.1.1 Newton’s Mechanics............................................................................25 3.1.1.2 Lagrange Method.................................................................................29

3.1.2 System Parameters .....................................................................................33 3.1.3 Real System Model ....................................................................................33 3.1.4 System Controllability Analysis ................................................................34 3.1.5 System Step Response Analysis.................................................................36

3.2 1-STAGE LINEAR IP ROOT LOCUS CONTROL EXPERIMENT................................38 3.2.1 Root Locus Analysis ..................................................................................38 3.2.2 Root Locus Method and Simulation ..........................................................40

3.2.2.1 Root Locus Method..............................................................................40 3.2.2.2 MATLAB Simulation...........................................................................43 3.2.2.3 MATLAB Simulink .............................................................................48

3.2.3 Root Locus Real Time Control Experiment...............................................54 3.2.4 Experiment Result and Report ...................................................................57

3.3 1-STAGE LINEAR INVERTED PENDULUM FREQUENCY RESPONSE ......................59 3.3.1 Frequency Response Analysis....................................................................59 3.3.2 Frequency Response Design and Simulation.............................................61 3.3.3 1-stage Linear IP Frequency Response Experiment ..................................70 3.3.4 Experiment Result and Report ...................................................................73

3.4 1-STAGE LINEAR IP PID CONTROL EXPERIMENT..............................................74

Table of Content

II

3.4.1 PID Control Analysis .................................................................................74 3.4.2 PID Control parameters and Simulation ....................................................75 3.4.3 PID Control Experiment ............................................................................80

3.4.3.1 Experiment steps in MATLAB experimental software........................80 3.4.4 Experimental Result and Report ................................................................83

3.5 STATE SPACE POLE PLACEMENT CONTROL EXPERIMENT ..................................84 3.5.1 State Space Analysis ..................................................................................84 3.5.2 Pole Placement and Simulation .................................................................85 3.5.3 Pole Placement Control Experiment..........................................................97 3.5.4 Experiment Result and Report .................................................................100

3.6 LINEAR QUADRATIC LQR OPTIMAL CONTROL EXPERIMENT..........................101 3.6.1 Linear Quadratic LQR Optimal Control Principle and Analysis .............101 3.6.2 LQR Control Parameters and Simulation ................................................102 3.6.3 1-stage Linear IP LQR Control Experiment ............................................106 3.6.4 Experiment Result and Report .................................................................109

CHAPTER 4 1-STAGE LINEAR PENDULUM MODELING AND EXPERIMENT ........................................................................................................ 110

4.1 1-STAGE LINEAR PENDULUM MODELING AND ANALYSIS ................................110 4.1.1 1-stage Linear Pendulum Modeling.........................................................110 4.1.2 Real System Model ..................................................................................114 4.1.3 System Controllability Analysis ..............................................................115

4.2 1-STAGE LINEAR PENDULUM ROOT LOCUS ANALYSIS ....................................116 4.3 1-STAGE LINEAR PENDULUM FREQUENCY RESPONSE ANALYSIS.....................117 4.4 1-STAGE LINEAR PENDULUM STEP RESPONSE ANALYSIS ................................118 4.5 1-STAGE LINEAR PENDULUM PID CONTROL SIMULATION AND EXPERIMENT .119

4.5.1 1-stage Linear Pendulum PID Control Analysis and Simulation.............119 4.5.2 1-stage Linear Pendulum PID Real Time Control ...................................123

4.6 1-STAGE LINEAR PENDULUM LQR CONTROL SIMULATION AND EXPERIMENT 126 4.6.1 1-stage Linear Pendulum LQR Control Analysis and Simulation ...........126 4.6.2 1-stage Linear Pendulum LQR Control Experiment ...............................130

CHAPTER 5 1-STAGE LINEAR IP SWING UP EXPERIMENT.....................134

5.1 ENERGY CONTROL IN IP SWING UP ................................................................134 5.2 1-STAGE LINEAR IP SWING UP EXPERIMENT...................................................135 5.3 SWING UP EXPERIMENT BY OTHER ALGORITHMS ...........................................137

CHAPTER 6 2-STAGE LINEAR INVERTED PENDULUM ............................140

6.1 SYSTEM PHYSICAL MODEL .............................................................................140 6.2 SYSTEM CONTROLLABILITY ANALYSIS ...........................................................146 6.3 2-STAGE LINEAR IP MATLAB SIMULATION...................................................148 6.4 LQR CONTROLLER DESIGN AND SIMULATION................................................150 6.5 2-STAGE LINEAR IP LQR CONTROL EXPERIMENT ..........................................155 6.6 EXPERIMENT RESULT AND REPORT .................................................................159 6.7 CONTROL EXPERIMENT BY OTHER ALGORITHMS............................................159

Table of Content

III

CHAPTER 7 3-STAGE LINEAR INVERTED PENDULUM ............................161

7.1 SYSTEM PHYSICAL MODEL .............................................................................161

CHAPTER 8 APPENDIX.......................................................................................172

8.1 LIST OF FIGURES .............................................................................................172 8.2 LIST OF PROGRAM...........................................................................................175

Chapter 1 Overview

©Googol 2006 1

Chapter 1 OVERVIEW

1.1 Introduction

Inverted pendulum (IP) problem is the combination of research area like robotics, control theory, computer control, etc. The IP system has the property of unstable, higher order, multi-variable and highly coupled, which can be treated as a typical nonlinear control problem. The research started from 50th, 20 century, control scientist in MIT developed the first 1-stage IP system based on the theory of rocket propeller. There are lots of new control algorithms appear in recent years. People apply them to IP systems to examine their ability to control multi-variable, nonlinear and unstable systems. As a relatively ideal experimental means for the teaching, experimentation and scientific research of control theories, IP system provides an excellent experimental platform for examining specific control theories or typical solutions and thus promoting the development of the new theories. Since they are widely applied in different fields such as semiconductors, delicate devices processing, robot control technology, artificial intelligence, missiles interception control systems, aviation docking control technology, perpendicularity control in rocket launching, gesture control in satellite circling and general industrial applications, they will have a broad prospects for development and utilization. Inverted pendulum can vividly simulate the flight control of rockets and the stabling control in walking robots etc.

1.2 Inverted Pendulum Classification

There are many series of IP systems extended from linear 1-stage IP, such as linear IP, circular IP, planar IP and configurable IP. IP is the system with pendulum plants placed on motion modules. Diverse pendulum plants and motion modules constitute different IP series. The following IP systems are classified by structure:

1) Linear Inverted Pendulum

Linear IP has pendulum plant on a linear motion module with one degree of freedom. The cart moves on the sliding shaft horizontally. There are different kinds of linear IP systems based on different pendulum plant structure such as the flexible IP, which has two carts on the sliding shaft with a spring connected. The linear IP series are shown in Figure 1-1.

2) Circular Inverted Pendulum

Circular IP system has the pendulum plant on a circular motion module with one degree of freedom. The pendulum is on the arm end are rotates around the center of the circle. Different IP system can be set up by varying the stage number

Chapter 1 Overview

2

in series or parallel as shown in Figure 1-2.

3) Planar Inverted Pendulum

Planar IP system has the pendulum plant on the planar motion module with two degrees of freedom. There are two classes of planar motion module: XY table and SCARA robotic arm. The pendulum can also be divided by stage like 1-stage or 2-stage as depicted in Figure 1-3.

4) Configurable Inverted Pendulum

Configurable IP is a new class of IP systems whose pendulum plant is composed of pendulum rod and connection rod. The connection rod can be configured to 3 modes: level, vertical upper and vertical down.

Classified by pendulum stages, there are 1-stage, 2-stage, 3-stage and 4-stage IP systems. 1-stage IP is used for basic experiment of control theory while others are mostly used for development of advanced control algorithms. Control complexity increase dramatically as the pendulum stage increase. The feasible maximal stage for IP system is 4 currently.

Figure 1-1 Linear IP Series

Figure 1-2 Circular IP Series

Chapter 1 Overview

3

Figure 1-3 Planar IP Series

Figure 1-4 Configurable IP

1.3 Inverted Pendulum Properties

Despite the different size and structure, all IP systems have the following properties in common:

1) Nonlinearity IP is a typical nonlinear system. In real control, the system model is usually

linearized. There are also nonlinear control methods applied to IP which is becoming a hot topic recently.

2) Uncertainty Most uncertainties come from model uncertainty, mechanical transmission error

and other resistances. In real control, uncertainties are reduced by controlling errors like, tighten the belt or screw to reduce the transmission error, or use ball bearing to reduce the friction.

3) Coupling There are coupling between each stage of IP and the motion module. We will

Chapter 1 Overview

4

decouple the IP near the equilibrium point and ignore some less important coupling variables.

4) Open loop instability There are two equilibrium states for IP systems: vertical upper and vertical down,

in which vertical upper is the unstable equilibrium point and vertical down is the stable equilibrium point.

5) Limitations The IP system performance is limited by mechanisms like motion module travel

distance, motor torque, etc. To make it convenient and reduce the cost, the structure size and the motor power of IP are required to be small. The effect of travel distance to IP swing up is especially evident: short travel distance easily gets the cart exceed the limit switch.

1.4 Controller Design Method

Controller design is the key content of IP systems. Controllers are used to stabilize the unstable system and make it robust to disturbances. The widely used control methods recently are: PID control, root locus and frequency response, state space, optimal control, fuzzy control, neural network, humanoid artificial intelligent, robust control, adaptive control and some more power algorithms which combine the above ones.

1.5 Inverted Pendulum Experiments

The experiments can be accomplished by IP platform in this book are as follows:

Table 1-1 Inverted Pendulum Experiments

Type No. Experiment Name Experiment Equipment

1. Encoder Principle and Application All IP Series Basic Experiment 2. Motor Control in Motor Control in

Matlab Simulink All IP Series

3. Linear Inverted Pendulum Modeling, Simulation and Experiment

1-stage IP

4. 1-stage Linear Pendulum Modeling and Analysis

1-stage IP

5. 2-stage Linear Inverted Pendulum 2-stage IP

Modeling Experiment

6. 3-stage Linear Inverted Pendulum 3-stage IP 7. 1-stage Linear IP Root Locus Control

Experiment 1-stage IP Control

Experiment 8.

1-stage IP

Chapter 1 Overview

5

9. 1-stage Linear IP PID Control Experiment

1-stage IP

10. 1-stage Linear Pendulum PID Control Simulation and Experiment

1-stage IP

11. 1-stage Linear Pendulum LQR Control Simulation and Experiment

1-stage IP

12. State Space Pole Placement Control Experiment

1-stage IP

13. Linear Quadratic LQR Optimal Control 1-stage IP

14. 2-stage Linear IP LQR Control Experiment

2-stage IP

Linear 3-stage IP can be taken apart to 2-stage IP and 1-stage IP.

Copyright 2004 Googol Technology (HK) Limited. All Rights Reserved

INVERTED PENDULUM (Educational products)

GLIP Series

INSTRUCTION MANUAL

Be sure to give this instruction manual to customers!

??Thank you very much for purchasing INVERTED PENDULUM (GIP Series) of Googol Tech. ??Be sure to read this manual carefully before operation. ??For any technical trouble, call us or visit http://www.googoltech.com on the world wide web

for consultation. After reading this manual, keep it handy so that it can be referred to at anytime.

GOOGOL TECHNOLOGY


2

CONTENTS CHAPTER 1 SAFETY PRECAUTION ......................................................3

1.1 CAUTIONS ON SAFETY..............................................3 1.2 CAUTIONS ON UNPACKING.....................................4

CHAPTER 2 INTRODUCTION ......................................................................5 2.1 System Configuration......................................................5 2.2 Mechanism and Parts Identification................................5 2.3 Electric Control box........................................................6 2.4 Control Platform..............................................................6

CHAPTER 3 INSTALLATION........................................................................7 3.0 Mechanical Assembly .....................................................7 3.1 Installation of GT-400-SV-PCI CARD...........................7 3.2 Wiring..............................................................................7 3.3 Software Installation .......................................................8

CHAPTER 4 GETTING STARTED IN DOS................................................11 4.1Starting the Software

...................................................................................................11 4.2Operation Procedure for Linear Pendulum System

...................................................................................................11 4.3 Software Operation for the Linear Pendulum System ..12 4.4 Operation procedure for Circular Pendulum System....14 4.5 Software Operation for the Circular Pendulum System15

CHAPTER 5 REALTIME CONTROL IN MATLAB SIMULINK ENVIRONMENT .................................................................................................16

5.1 Introduction...............................................................................................16 5.2Control Inverted pendulum in Matlab Simulink Environment ..................16

5.3 Modifying Inverted Pendulum Functional Blocks........18 5.4 Inverted Pendulum Functional Blocks Description. .....19 5.5 C-MEX Functions Library: Short Description..............23

CHAPTER 6 MAINTENANCE AND TROUBLESHOOTING....................25 6.1 Maintenance ..................................................................25 6.2 Troubleshooting: ...........................................................25

GOOGOL TECHNOLOGY


3

CHAPTER 1 SAFETY PRECAUTION

1.1 CAUTIONS ON SAFETY

Please note the cautions. Otherwise, a misoperation will lead to an unexpected accident or damage.

The following symbolic indication of “DANGER” and “CAUTION” may lead to serious results depending on situations.

The following symbols indicate what should not to be done and what should be done by all means.

PROHIBITION

COMPULSION

Incorrect operation may result in such a dangerous situation as death or serious injury.

Incorrect operation may result in such a dangerous situation as medium or slight injury.

DANGER

Before removing the control board from its conductive packaging, touch your hand on a grounded metal object for discharging static electricity. Avoid touching the components on the board. A pair of static electricity-proof glove is suggested.

Otherwise, the board may be damaged.

For your first use, please discharge the pendulum arm, run the test program to confirm if the product runs correctly. Don’t adjust the motor driver without permission. Otherwise, you shall bear responsibility for all the serious consequences caused.

Otherwise, you may be

injured or troubles may occur.

The Sequence to turn on/off the power: first to enter DOS environment (PC), then turn on the power of the Electric Control Box; contrarily, the sequence is contrary. Check the wiring carefully, wrong or disconnected wiring for the coder may cause collision.

Otherwise, the pendulum

may be damaged or you may be injured.

Don’t stand in the two side of the Linear Inverted Pendulum when it is running, but face at it. For the Circular Inverted Pendulum, don’t stand in the area of its running.

Otherwise, you may be injured.

CAUTION

GOOGOL TECHNOLOGY


4

1.2 CAUTIONS ON UNPACKING

?? When unpacking the product, check if what you have received are as your order; ?? Check if the components of the product are damaged during transportation, such as the limit

switch and so on; ?? Check if the components (including the connected cable) of the product are lacking; ?? If any abnormality is detected, please contact our dealer or authorized agency.

GOOGOL TECHNOLOGY


5

CHAPTER 2 INTRODUCTION

GLIP200X is a series of three state-of-the-art linear inverted pendulum systems from Googol Technology. Each of the system consists of a cart which slides on a parallel track and is powered by a servo motor, a DSP-based control system with interface electronics, and a chain of pendulums mounted to the cart through a passive joint. GLIP2001 contains a single pendulum, GLIP2002 a double pendulums and GLIP2003 a triple pendulums. GRIP20XX is a series of three state-of-the-art circular inverted pendulum systems. The common module of the GRIP20XX is a rotary servo system. If two single pendulums are parallel suspended from the instrumented joints that constructs a GRIP2011 inverted pendulum. If only a single pendulum is suspended from an instrumented joint, that constructs a GRIP2001. If a double pendulum is suspended, that constructs GRIP2002. High-resolution encoder sensors mounted on the servo motor and the joint axes of the pendulum provide position information of the cart and the pendulum, respectively.

2.1 System Configuration

In general each Inverted Pendulum (IP) System of Googol Technology consists of the following components: mechanism, the sensors and actuators, the control hardware, and the control software. In a linear IP system, the mechanism is a linear servo motion cart with a chain of pendulums, and in a circular IP system, the mechanism is a rotary servo system with a chain of pendulums. The types of systems presently available are briefly described in the table below.

Inverted Pendulums Type Main Model Description

GLIP2001 Linear single inverted pendulum GLIP2002 Linear double inverted pendulum Linear IP Systems GLIP2003 Linear triple inverted pendulum GRIP2001 Circular single inverted pendulum GRIP2002 Circular series double inverted pendulum Circular IP Systems GRIP2011 Circular parallel double inverted pendulum

2.2 Mechanism and Parts

Identification

2.2.1 Linear Inverted Pendulum

As shown in Fig. 2.1, the mechanical unit of linear inverted pendulum consists of the following parts:

Base Cart Sliding shaft Timing belt

Motor

Base

Pendulum rod

Encoder

Timing belt

Timing wheel

Cart Limit switch

Sliding shaft

Fig. 2.1

GOOGOL TECHNOLOGY


6

Timing wheel Optical encoder Limit switch Pendulum rod Motor.

2.2.2 Circular Inverted Pendulum

The entire view of the circular inverted pendulum is as shown in Fig. 2.2. The Mechanism is designed and fabricated without mechanical limit, and a slipring is used to output the signal from angle encoders, and eliminate damage-prone wires dangling from movable pendulums.

The movable pendulums are driven by an AC servo motor through gear transmission.

2.3 Electric Control box The control box of linear IP houses a power supply, a servo amplifier, circuit protection electronics and terminal interconnections. The power switch of the control box is found on the front side panel as shown in Fig. 3.3. Pull the level upward to turn on the machine. All electric components of circular IP are housed into the base of pendulum, so there is not independent control box.

2.4 Control Platform The Control Hardware consists of a PC for program development and user interface, a DSP-based motion control card (GT-400-SV-PCI) with advanced data I/O and storage capability, and several connection cables, connecting the PC to the control box and the control box to the pendulum base. The Control Software comes in with one of two options. The first option is a DOS based version in which all control algorithms are programmed in C. Sample programs and source codes for input/output are provided for educational purpose. The second option is a MATLAB based that runs under Windows 98/2000 environment. Simulink is used as an interactive tool for modeling, simulating, and analyzing pendulum systems. It enables you to build graphical block diagrams, evaluate system performance, and refine your designs.

Fig. 3.3

Fig. 2.2

GOOGOL TECHNOLOGY


7

CHAPTER 3 INSTALLATION

3.0 Mechanical Assembly

Carefully unpack the machine, and remove all packaging and securing material. To linear inverted pendulum systems, place the Mechanical Unit on a level surface of the table so that the pendulum can freely swing up and down, and allow sufficient room for the controller box, PC and keyboard. Ensure that the pendulum is clear of any obstructions when executing a complete revolution, and is not able to make contact with any observers. To circular inverted pendulum, place the mechanical Unit on a level surface of the floor, and allow the pendulum can freely rotate and swing up and down. Ensure that the pendulum is clear of any obstructions when executing a complete revolution, and is not able to make contact with any observers.

3.1 Installation of GT-400-SV-PCI CARD

?? Check the control card. If it is damaged during transfortation, DO NOT use, contact us immediately? Before the installation, DO read the User Reference Manual of GT-400 carefully. ?? In order to protect the motion control board from static electricity damage, please touch

your hand on a grounded metal object before touching or plugging it in/out the slot. ?? Before installing the GT-400 motion control card into your computer, make sure that the

switches and jumpers are set correctly. In general, the GT-400 card is usually delivered in default setting. You don’t need to change it. For detailed information, please refer to User Reference Manual of GT-400.

?? Turn off and disconnect the main power supply before opening the computer. ?? Open the computer and check to see if there is one spare PCI bus slot. ?? Carefully install the GT-400 card in a spare slot on the PC motherboard. Use screws to secure

the card. ?? Attach a 62-pin flat cable to a connection bridge included into the package and to the

connector CN2. Use screws to secure the bridge. ?? Close the PC case.

3.2 Wiring

3.2.1 Linear Inverted Pendulum ?? Make sure the power switch of the electric control box in OFF status; ??Use two shielded cables included into the package to connect the GT-400-SV-PCI card to the

electric control box. ?? Connect the 25-pin and 15-pin sockets of the electric control box to the sockets of the

mechanical unit correspondingly with the cable accessory with the pendulum system; ?? Connect the 4-pin aerial plug of the electric control box to the jack corresponding of the

mechanical unit with 4-pin cable; ?? Connect one end of the power cable into the jack of the electric control box, connect the other

GOOGOL TECHNOLOGY


8

end into 220V AC power supply; 3.2.2 Circular Inverted Pendulum For Circular Inverted Pendulum, the electric control part is intergrated with the pendulum mechanical unit. You only need to connect directly GT-400 card to the pendulum mechanical unit using two shielded cables included into the package.

3.3 Software Installation

3.3.1 Software Installation and Testing of the GT-400-SV-PCI Card ?? Once you have installed the control card in the computer, switch on the power of your

computer and enter Windows system. The Windows can browse the new hardware automatically and prompt you to install its Windows driver.

?? Insert CD-ROM into the drive, browse in the subdirectory \WinDriver\Setup-1.3 to find and select the associated driver program according to the used Windows version. Windows will automatically install the Windows driver.

?? After succees of driver installation, reset your computer. The windows driver library is distributed on a CD-ROM, which also includes the user manuals of GT-400 motion controller in “pdf” format. The available different Windows version drivers include Windows 98, Windows 2000, and Windows NT. After installation, start System Attribution and check Device manager. If you can find the hardware signed“ GoogolTech400” ,the driver program is successfully installed. Otherwise,you should re-install. If no address conflict or other troubles, do not change the factory default setting. ?? Find subdirectory \ENGTCmdPCI-011215 on CD-ROM, copy this subdirectory into your

computer. ?? Enter the subdirectory \ENGTCmdPCI-011215, and locate the file EGTCmdPCI.EXE,

which is the test program of GT-400-SV-PCI card. ?? Double click and run the test program. ?? See the Appendix B of the User Reference Manual of GT-400 to test the motion controller. 3.3.2 Demo Software Installation of the Inverted Pendulum System Demo/Test software is supplied to check the operation of the Inverted Pendulum System. DOS installation: ?? Type in the following DOS commands

C:>MD PEND (You create a sub-directory C:\PEND) ?? Insert the CD-ROM in CD drive, supposed the CD drive is G; ??Find the DOS demo/test software file on CD-ROM supplied ?? Type in the command “copy g:\GLIP2003_EN\I1VL-SFT-BEN-2.0\*.* c:\pend ”; Note: There are several subdirectories in “ g:\GLIP2003_EN” diretory for different type pendulum.

For example: “… \I1VL… En-2.0” is for single inverted pendulum, and “\...I2VL...En-2.0” for double

inverted pendulum. All source codes are included. These source codes are compiled in Borland C 3.1. Windows installation ?? Enter Source Manager, copy GLIP2003_EN in CD-ROM to any free hard disk space. 3.3.3 MATLAB based Software Installation for Windows 98 and Windows 2000

GOOGOL TECHNOLOGY


9

The installation programs can be used for all MATLAB based pendulum software in MS-Windows environment. The software is distributed on a CD-ROM, and is only compatible with version 6.1 of MATLAB or above. To finish the installation program, follow the next steps. ?? Insert CD-ROM into the drive ?? Find the MTALAB based software setup program “Setup.exe” ?? Double click the setup.exe program to start the installation program. ??Setup program will prompt you to enter MATLAB root directory name (See Fig

3.1). ??To activate the Setup button

you must point out the MATLAB root directory. For this purpose click the Browse button. You will see the dialoge box with drives and directory list displayed (See Fig. 3.2). Select the MATLAB root directory from the system directory, and click OK button. The directory you selected before will be displayed and Setup button will be activated. Click Setup button. The program automatically detects if the current directory is the MATLAB root. If no, setup program will prompt you to find MATLAB root directory once more( See Fig. 3.3). If you choose Yes, the installation process returns to the previous step (Fig. 3.2), otherwise the program will stop.

??The location for the files to be installed is the PEND directory. It will be automatically added as a subirectory under the MATLAB/toolbox directory. If the installed directory has existed, the installation program will fail.

??If all the settings are correct, the installation process continue, and you will see the progress window.

??After installing the software you will find the created subdirectory under the MATLAB/toolbox directory.

Fig. 3.1

Fig. 3.2

Fig. 3.3

GOOGOL TECHNOLOGY


10

Be sure to read the following warning before running the system.

??Switch off all the power supply before disassembly, installation and wiring. ??Check the wiring carefully, wrong or disconnected wiring for the coder may cause

collision. ??For your fiest use, please disassembly the belt and run the test software to

ensure correct operation.

??Stand in the area of the pendulum arm reached. ??Put your hand or other part of your body in the track when the system is

running.

COMPULSION

PROHIBITION

GOOGOL TECHNOLOGY


11

CHAPTER 4 GETTING STARTED IN DOS

4.1 Starting the Software

?? Locate the PEND.EXE program in your computer. Suppose it is in the directory C:\PEND. ?? Type in the following DOS command:

C:\PEND>PEND

4.2 Operation Procedure for Linear Pendulum System

1) Place the cart in the centre of the rail; 2) Make sure the pendulum rod in the free down static status; 3) Switch on the power supply to your PC and the electric control box; 4) Enter the directory of the test software, type Pend to start; 5) Type S, the following dialog window opens:

6) Press? button to swing up for single inverted pendulum. As the double or

triple inverted pendulums has no swinging-up proceeding, Type S, use your hand to bring the end tip of the highest grade pendulum arm to the upright position, when you feel the control force, let it go;

7) When in the balance position, press ? or ? button, move the cart to the left or to the right;

8) Press T button to stop Servo; 9) Press ALT+X to Exit; 10) Switch off the power of the electric control box and the computer.

?? Please operate the system according to the procedure above strictly. Any incorrect operate procedure may cause the pendulum can not swing and keep balance, also cause the cart strike the two endpoints of the rail.

?? If the cart strikes the two endpoints of the rail and can not return to the center position, Switch off the power of the electric control box, exit the control program and return to DOS environment. Then bring the cart to the center and re-start.

If there is no problem, press TAB button and select “ Yes” button, press Enter to open Servo ON. Otherwise, press Enter or ESC to cancel.

COMPULSION

GOOGOL TECHNOLOGY


12

4.3 Software Operation for the Linear Pendulum System

1) After starting the software, you will see the following screen presented.

2) Press C button to convert the screen into the following simulation interface. Press C button again to return the old screen.

3) Control algorithms selection (only available for single inverted pendulum) and control

parameters modification

GOOGOL TECHNOLOGY


13

Note: The default algorithm of the single pendulum system is LQR. When use the LQR to balance the single pendulum , you can use the above method to convert the balancing algorithm to PID or Transfer Function. Therefore, you can observe the different control effects with the different control algorithms. Because both PID and Tranfer function algorithms can not control the position of the cart, the conversion may cause the cart move in random direction. You can slightly impact the pendulum rod in the opposite direction by hand to make the cart stop. 4) Set the signal generator

5) Observe the output responding curve

Press ALT+F, pop out the menu as the left, move the cursor Up and Down to select different algorithms, then press Enter . Then system will prompt you to set the control parameters of the selected algorithms as shown in the the following dialog .

Input new control parameters Press Enter to save the parameters and the software immediately use the new algorithms to control the pendulum. At the same time the system saves 5-second data of dynamic responding processof the pendulum system. Press ESC or TAB button to select Cancel and press Enter, the changes will be canceled.

Press ALT+D, pop out the menu as shown in the left side. Select the Input Signal button and press Enter, pop out the dialog window below:

Input wave code, frequency, magnitude,press Yes to save and exit, press Cancel, not save and exit.

Wave code 0 No Signal 1 Pulse 2 Step 3 Square 4 Sin 5 Sawtooth

GOOGOL TECHNOLOGY


14

6) Zoom out / in the display curve

Four different kinds of colors are used to display the sampling data of pendulum angle, pendulum angle velocity, cart dispacement and cart velocity. All figures can zoom out or zoom in repectively. They are definited separately as the following: F1(F5)— — Zoom out ( Zoom in ) the pendulum angle F2(F6)— — Zoom out ( Zoom in ) the pendulum angle velocity F3(F7)— — Zoom out ( Zoom in ) the cart displacement F4(F8)— — Zoom out ( Zoom in ) the cart velocity 7) Other keyboard input

a. Press S to start Servo; b. Press? to swing up the single inverted pendulum; c. Press ? or ? to move the cart to the right or the left; d. Press T to return ZERO and shut off Servo; e. Press Spacebar, emergent STOP.

4.4 Operation procedure for Circular Pendulum System

The operation procedure is similar to the linear pendulum systems: ?? Make pendulum in the static position; ?? Make the pendulum arm in the free down static status; ?? Switch on your PC and enter DOS environment; ?? Switch on the power to the electric control box; ?? Press Pend to start.

Input the disturbance signal, the program will record 5-second data of responding process. Select the menu Cart responding or the Arm responding, and press Enter, the following dialog window appears.

Input File name such as “pend”, press OK, save the file to the current directory. Suppose the current diretory is C:\PEND. Start Windows and Matlab. In MatLab run the command below: S=load(‘c:\pend\pend’) plot(S) MatLab will re-draw the curve saved.

GOOGOL TECHNOLOGY


15

4.5 Software Operation for the Circular Pendulum System

1) Type Pend and enter, the following window opens. 2) The continous procedure is the same as linear inverted pendulum.

GOOGOL TECHNOLOGY


16

CHAPTER 5 REALTIME CONTROL IN MATLAB

SIMULINK ENVIRONMENT

5.1 Introduction

We write a Matlab Inverted Pendulum Toolbox for REALTIME CONTROL of the GoogolTech Inverted Pendulum in Matlab Simulink Environment.Three Demos are also provided to illustrate usages of the toolbox for realtime control of single inverted pendulum and double inverted pendulum. For single inverted pendulum, a digital PID control algorithm and a LQR algorithm are implemented to control the pendulum rod angle and the pendulum rod as well as cart position. As to double inverted pendulum, a LQR algorithm is implemented to control the two pendulum rods and the cart position.

5.2Control Inverted pendulum in Matlab Simulink Environment

After successfully installing the Inverted Pendulum Toolbox in Matlab (please refer Section 3.3.3 for how to install the toolbox), you will find a new created subdirectory “pendPCI” under the MATLAB/toolbox directory and an Inverted Pendulum Toolbox blockset in the Simulink Library Browser(Figure 5-1). In the right pane of the Simulink Library Browser window, there are ten functional blocks (light blue blocks), two simple example (dark green blocks) and three realtime control demos (orange blcoks).

The directory matlabroot/toolbox/pendPCI/ contains all the files required for building GoogolTech Inverted pendulum models based on GT-400 PCI board functions' set. These are:

- the Inverted Pendulum Matlab library file (invpend.mdl), - GT board dynamic link library (including GT400.dll, GT400.lib, user.h and

GT400data.h), - C-MEX source code files, including reference constants file (Pend.h), - several test examples of using the functions library (*.mdl files). IMPORTANT: As Windows is a multi-task operation system and inorder to control the

inverted pendulum steadily, Please close other software application when you start realtime control in Matlab Simulink environment. And during the process of realtime control, Do not start other software application.

5.2.1 Single pendulum PID control Demo

Double click the “Single pendulum PID control Demo” block in the right pane of the Simulink Library Browser window, a “SinglePID” Simulink model poped up (Figure 5-2). Here is the operation procedure:

1) Place the cart in the centre of the rail; 2) Make sure the pendulum rod in the free down static status;

GOOGOL TECHNOLOGY


17

3) Switch on the power supply of the electric control box; 4) Start the simulation; 5) As a simple demo, there is no swinging-up proceeding. Use your hand to bring the end tip

of the pendulum rod to the upright position in the clockwise direction, when you feel the control force, let it go;

6) PID algorithm does not control the cart position and when the cart reaches the edge of the sliding shaft, impact the pendulum rod slightly in the opposite direction by hand to make the cart stop.

7) Stop the simulation when you need.

Figure 5-1: Inverted Pendulum Toolbox in the Matlab Simulink Library

GOOGOL TECHNOLOGY


18

Browser

Figure 5-2: Single pendulum PID control Demo 5.2.2 Single pendulum LQR control Demo

The operation procedure is much like the “Single pendulum PID control Demo” execept that LQR algorithm has control force on the cart position and the cart will not move irregularly any more.

5.2.3 Double pendulum LQR control Demo Double click the “Double pendulum LQR control Demo” block in the right pane of the

Simulink Library Browser window, a “DoubleLQR” Simulink model poped up. Here is the operation procedure:

1) Place the cart in the centre of the rail; 2) Make sure the pendulum rod in the free down static status; 3) Switch on the power supply of the electric control box; 4) Start the simulation; 5) Use your hand to bring the end tip of the highest grade pendulum rod to the upright

position in the clockwise direction, when you feel the control force, let it go; 6) Stop the simulation when you need.

5.3 Modifying Inverted Pendulum Functional Blocks

The provided Inverted Pendulum toolbox for Simulink includes several functilonal blocks. These are so-called "S-functions" blocks that could be inserted directly into a new Simulink Model pane. NOTE: the library blocks are "masked" S-functions. It means that you cannot modify their parameters (it is usually desired axis number).

GOOGOL TECHNOLOGY


19

To do it, insert block into model, select it, then select from Menu Edit -> Link options -> Disable link. Otherwise it will be impossible to save the new block parameter value (after start of modeling it will take on the default value - "1").

In case of need to modify a behavior of the block, you should make some changes in

source C-code files of required block.

NOTE: do not recommended to modify these files for people that are not experts in C programming language. In addition you need to have any C-compiler on your computer (Microsoft, Watcom or other). To do it: 1. Open the InvPendulum functions library from Simulink library browser: right-click "Inverted

Pendulum toolbox" -> "Open the Inverted Pendulum Toolbox' Library". The pane with full library will be open.

2. Find necessary source file's label under "Source C-MEX files" board,then double-click it. The corresponding source code file and information Pend.h header file will be open in Matlab M-File Editor.

3. Save these files' copy in your current working directory (probably C:\Matlab\Work\) by choosing commands: File->SaveAs...

4. Make necessary changes in source code. 5. Save and then compile the file. It could be done by typing the 6. following string into Matlab command window: " mex FileName.c ". The DLL file with the

same name will be created. After these steps your function is ready to use.

5.4 Inverted Pendulum Functional Blocks Description.

- GetPos is intended for getting the current axis position value in encoder' counts. This value represents the actual position for the axis at the current sample time. This command operates for all profile modes. The value returned is a 32-bit signed number with units of counts. The range is -1,073,741,824 to 1,073,741,823. You can recalculate obtained value into meter units via multiplication by the scaler coefficient (ENCODE1 for the first axis, ENCODER2 for the second and so on) in file Pend.h.

Block parameter: axis number (between 1 and 4), default is 1 (or any number not in range). Block input: none. Block output: current encoder reading, counts.

- GetVel is intended for getting the current axis' averaged velocity value. The output units are encoder' counts per second. The velocity is calculated as derivative of position and equals (current position - saved position) / sample time, where sample time is MATLAB-defined parameters (assumed value 0.002 sec). You can recalculate obtained value into m/sec units via multiplication by the scaler coefficient (ENCODE1 for the first axis, ENCODER2 for the second and so on) in file Pend.h. Block parameter: axis number (between 1 and 4), default is 1 (or any number not in range).

Block input: none.

GOOGOL TECHNOLOGY


20

Block output: the velocity of axis , averaged by 5 last values. - GetError returns the current instantaneous position error of the current axis. The returned value represents the difference between the target position and the actual position (actual position minus target position), and is a signed 16-bit number. Number of axis whose status will be read is defined by S-function parameter (integer constant between 1 and 4). Note that any another values will cause that current axis number will be set to 1.

Block parameter: axis number (between 1 and 4), default is 1 (or any number not in range). Block inputs: none Block output: the actual position error for the current axis.

- GetStat is intended for getting the current axis status. Number of axis whose status will be read is defined by S-function parameter (integer constant between 1 and 4). Note that any another values will cause that you obtain the status of the first axis. For status word bit definition refer to GT board command reference.

Block parameters: 1: axis number (between 1 and 4), default is 1 (or any number not in range). 2: must be a mask constant that allows you to see either full status word ( in case of default

value: mask = -1) or separated bits status. For example, if you want to know when Carriage trips the negative limit switch, you need to check bit 6 of status word (1 indicates limit switch trip). Then mask value must equal 64 (decimal) (that means 1000000 binary). In case if the second parameter is omitted, then the mask assigned default vaue -1 and full status word will be set in block output.

Block inputs: none Block output: the current axis' masked status.

- InitPend function is intended for performing all necessary initializations of card and motion parameters. After doing it the servo drive will be enabled (also by this function). This function will be called only once, on modeling start. The necessary initial values are obtained from Pend.h file and could be modified in case of need.

Block parameter: none. Block input: must be one constant that defines initial profile mode: 0 - S-curve profile 1 - trapezoidal profile another values - velocity profile. Block output: none.

- The function SetPVAJ (PosVelAccJerk) is intended for setting motion parameters for different profile modes. Number of block inputs could be resized depend of profile mode: if current profile mode is velocity contouring, you can use only 1st and 2nd inputs (setting velocity & accelerarion), because other parameters make no sense in this case and so on.

Block parameter: 1st: axis number (between 1 and 4), default is 1 (or any number not in range).

2nd: desired number of input ports (must been between 2 and 5) Block input: must be several constants in following order: input 1: commanded velocity value input 2: commanded acceleration value (valid only in Velocity and Trapezoidal profiles

mode)

GOOGOL TECHNOLOGY


21

input 3: commanded position value (valid only in Trapezoidal and S-Curve profiles mode)

input 4: commanded maximal acceleration value (valid only in S-Curve profile mode) input 5: commanded jerk value (valid only in S-Curve profile mode) Block output: motion error bit of carriage servo drive's status word (if 1, motion error

occurs, otherwise 0).

Additional information: GT_SetVel() sets the maximum velocity magnitude used during the S-curve, Trapezoidal, and Velocity Contouring profile modes. The velocity is specified as an unsigned 32-bit number in units of counts/sample-time, with a scaling factor of 1/216. The range is 0 to +1,073,741,823 (230), hence the actual range of counts/sample-time is 0 to +16383 (214). The loaded velocity is not utilized until a parameter update occurs. GT_SetAcc() sets the command acceleration. The acceleration is specified as an unsigned 32-bit number in units of counts/sample2, with a scaling factor of 1/216. When in Trapezoidal point-to-point mode, the range is 0 to +1,073,741,823 (230). When in the Velocity Contouring mode, the range is -1,073,741,824 to +1,073,741,823. The loaded acceleration is not utilized until a parameter update occurs. GT_SetPos() sets the destination position when using the S-curve and Trapezoidal trajectory profile. The position is specified as a signed 32-bit number in units of counts, ranging from -1,073,741,824 to 1,073,741,823. The loaded position is not utilized until a parameter update occurs. GT_SetMAcc() sets the maximum acceleration. The acceleration is specified as an unsigned 16-bit number in units of counts/sample2 with a scaling factor of 1/216. The range is 0 to +32,767 hence the actual range is 0 to 0.5 in counts/samples2. The loaded max acceleration is not utilized until a parameter update occurs. GT_SetJerk () sets the command jerk used during the S-curve profile generation mode. The jerk is specified as an unsigned 32-bit number in units of counts/sample3 with a scaling factor of 1/232. The range is 0 to 2,147,483,647 and the actual range is 0 to 0.5 in counts/sample3. The loaded jerk is not utilized until a parameter update occurs.

- SetProf is intended for profile mode definition.

Block parameter: axis number (between 1 and 4), default is 1 (or any number not in range). Block input: must be one constant that defines profile mode: 0 - S-curve profile 1 - trapezoidal profile another values - velocity profile as default. Block output: none.

Additional information: GT_PrflS() sets the trajectory profile mode to S-curve point-to-point. In this mode, the host specifies the destination position using SET_POS, the maximum velocity using SET_VEL, the maximum acceleration using SET_MAX_ACC, and the jerk using SET_JERK. Once in this mode, the trajectory profile generator will drive the axis to the destination position at the specified jerk

GOOGOL TECHNOLOGY


22

while not exceeding the maximum velocity and maximum acceleration. The axis will stay in this profile mode until another profile mode is explicitly set. GT_PrflT() sets the trajectory profile mode to Trapezoidal point-to-point. In this mode the host specifies the destination position using SET_POS, the maximum velocity using SET_VEL and the acceleration using SET_ACC. Once in this mode, the trajectory profile generator will drive the axis to the destination position at the specified acceleration while not exceeding the maximum velocity. When in the profile mode, position and velocity may be changed on the fly, but acceleration may not. The axis will stay in this profile mode until another profile mode is explicitly set. GT_PrflV() sets the trajectory profile mode to Velocity Contouring. In this mode the host specifies the command acceleration using SET_ACC, and the maximum velocity using SET_VEL. Once in this mode, the trajectory profile generator will drive the axis at the specified acceleration while not exceeding the maximum velocity. The acceleration and the maximum velocity may be changed on the fly. The axis will stay in this profile mode until another profile mode is explicitly set. There are no limitations on changing the profile mode to Velocity Contouring while the axis is in motion. - SetFiltr is intended for setting dig ital filter's parameters. All these coefficients are specified as an unsigned 16-bit number. The range is 0 to 32,767.

Block parameter: axis number (between 1 and 4), default is 1 (or any number not in range). Block input: must be 4 constants in following order: input 1: KP - set proportional gain input 2: KD - set derivative gain input 3: KI - set integal gain input 4: KVFF - set velocity feed forward gain Block output: none.

- The function SetMode defines desired control mode (open loop/close loop).When input value is equal 1, the function SetMode enables closed loop servo control. When motor output is enabled, motor output values generated by the Digital Filter are output to the selected output hardware circuitry. Otherwise when input value is equal 0, the function SetMode disables closed loop servo operations. After this command is executed the motor output is taken from the motor command register, set using the GT_SetMtrCmd() command. This register is loaded with a value of 0 at the moment the motor is disabled. This command can be used for emergency shutdowns, for calibrating the motor amplifier, or for running an axis in open loop mode.

Block parameter: axis number (between 1 and 4), default is 1 (any number not in range). Block input: must be one constant that defines control mode: 0 - open loop mode 1 (or any other values) - closed loop mode as default. Block output: none.

- The function SetMtrCm is intended for writting a direct value to motor output. SetMtrCm loads the motor command register with the specified value. This register replaces the motor command value from the servo filter when the motor is shut off. The specified motor command is a 16-bit signed number with range -32,767 to +32,767. Regardless of the motor output mode, a value of

GOOGOL TECHNOLOGY


23

-32,767 represents the largest negative direction motor command, a value of 0 represents no motor (0) output command, and a value of 32,767 represents the largest positive motor command. IMPORTANT: For this command to work properly, the board must be in open loop mode (SetMode "0" command or after a motion error with automatic motor stop enabled).

Block parameter: axis number (between 1 and 4), default is 1 (or any number not in range). Block input: the motor command value in range between -32,767 and +32,767. Block output: none.

5.5 C-MEX Functions Library: Short Description

- GetPos.c contains C program that serves getting of the current axis' position value (in encoder' counts). It is possible (in case of need) to recalculate the obtained value into meter units via scaler coefficient ENCODER1. Value of ENCODER1 can be found in Pend.h. - GetVel.c contains C program that serves getting of the current axis' velocity value in counts per second. This value computed as difference between current and prevoius (saved) position value divided by Matlab sample time (time between function calls). Calculated by this way last 5 velocity values is stored in array. The average value of this array's elements is passed to the output. - GetStat.c contains C program that serves getting of the current axis status value as 16-bit word. This word can be masked by numerical constant (connected to block input). If the constant is -1 or 2^16 = 65535 (that's the same), then full status word will be passed to output. Otherwise it must be decimal constant which is desired power of 2 (the power will be a sequence number of interesting bit). - GetError.c contains C program that serves getting of the current axis' actual position error. The error is a difference between commanded and current position in encoder counts. - InitPend.c contains C program that performed all necessary initializations of card and motion parameters and enabled the first axis' servo drive. This function will be called once, at start of model execution. The main function in this file is mdlStart(). Initial values for the some functions are obtained from information header file Pend.h. These functions are:

set_smpl_time(SMPL_TIME), set_pos_err(POS_ERROR), set_kp(FILTER_KP), set_ki(FILTER_KI), set_kd(FILTER_KD), set_i_lm(INTEGR_LIMIT).

- SetPVAJ.c (SetPosVelAccJerk) contains C program that performed all settings of motion parameters (such as velocity, acceleration, position, max acceleration and jerk), depend of currently selected profile mode. The commanded values have counts (per corresponded time) units. Commanded values of velocity, acceleration and jerk could be re-calculated into standard physical units via scaler coefficients VEL_COF, ACC_COF and JERK_COF correspondingly; they are enumerated in file Pend.h. - SetProfile.c contains C program that serves the motion profile mode definition, depend of input constant value. - SetMode.c contains C program that serves the open loop/ closed loop mode definition, depend of input constant value.

GOOGOL TECHNOLOGY


24

- SetFiltr.c contains C program that serves the definition of digital filter parameters. - SetMtrCm.c contains C program that set the input command value to the current axis' motor.

GOOGOL TECHNOLOGY


CHAPTER 6 MAINTENANCE AND TROUBLESHOOTING

To ensure the pendulum system to work well, the user is expected to often maintain and inspect machine.

6.1 Maintenance

1. Periodically check if the belt is slacker or tauter. If so, slacken off the belt tension adjustment bolts to adjust the belt tension, then tighten the adjustment bolts.

2. Periodically check if the connection between pendulum axis and encoder axis is rigid. If no, please reassemble and tighten it.

6.2 Troubleshooting:

Faulty action Possible Cause Solution Servo Driver alarms. 1) Check the system servo wiring.

2) Check the encoder wiring. Fault in interface board resistance.

Replace resistance.

Fault in DC power supply. Replace DC power supply.

Start the control software, the cart and the pendulum arm does not move.

Servo OFF. Servo ON. Connection to the encoder is slack.

Tighten the fixing bolt.

Connection to the motor is slack.

Tighten the fixing bolt.

The pendulum rod can not arrive the normal upright position.

The distance between the cart and the limit switch is too close.

Befor the pendulum moves, put the cart in the middle of the rail.

IP lab manual - GOOGOL TECHgoogoltech.com.hk/uploads/catalog/569/GLIP experiment and... ·...

Documents

Transcript of IP lab manual - GOOGOL TECHgoogoltech.com.hk/uploads/catalog/569/GLIP experiment and... ·...