Servo-controller for a changing load

Servo-controller for a changing load

Citation for published version (APA):

Heren, J. A. (1987). Servo-controller for a changing load. (TH Eindhoven. Afd. Werktuigbouwkunde, Vakgroep Produktietechnologie : WPB; Vol. WPA0388). Technische Universiteit Eindhoven.

Document status and date: Published: 01/01/1987

Document Version:

Publisher’s PDF, also known as Version of Record (includes final page, issue and volume numbers)

Please check the document version of this publication:

• A submitted manuscript is the version of the article upon submission and before peer-review. There can be important differences between the submitted version and the official published version of record. People interested in the research are advised to contact the author for the final version of the publication, or visit the DOI to the publisher's website.

• The final author version and the galley proof are versions of the publication after peer review.

• The final published version features the final layout of the paper including the volume, issue and page numbers.

Link to publication

General rights

Copyright and moral rights for the publications made accessible in the public portal are retained by the authors and/or other copyright owners and it is a condition of accessing publications that users recognise and abide by the legal requirements associated with these rights. • Users may download and print one copy of any publication from the public portal for the purpose of private study or research. • You may not further distribute the material or use it for any profit-making activity or commercial gain

• You may freely distribute the URL identifying the publication in the public portal.

If the publication is distributed under the terms of Article 25fa of the Dutch Copyright Act, indicated by the “Taverne” license above, please follow below link for the End User Agreement:

www.tue.nl/taverne

Take down policy

If you believe that this document breaches copyright please contact us at:

openaccess@tue.nl

providing details and we will investigate your claim.

Jean HEREN UNIVERSITE DE

TECHNOLOGIE DE COMPIEGNE FRANCE

September 1986/ February 1987 WPA rapport nr.0388

ACKNOWLEDGEMENTS

-I want to express my acknowledgements to my coach, Mr. p.e. Mulders, who welcomed me into his team, for his advice and his kindness.

Greatest thanks also to Henk Smit, Leon Pijls, Henk Van Rooy, Rogier Mares, Arthur and Willy and so many others I can not mention here, who provided me help and advice in work, and others fields.

They all contributed to make the work atmosphere very nice and also the time I spent in Holland, that is such a nice and friendly country.

Eindhoven, february 1987

1. SUMMARY Page 5

2. THE WORK ENVIRONMENT 6

- Technical universities in Netherlands - The mechanical engineering department - The F.A.I.R project

- My traineeship project

3. THE LOAD SIMULATOR 9

4. THE CONTROL THEORY 14

5. HARDWARE AND EQUIPMENT

- Presentation of the iSBC186/03 19

- The iSBX standard 21

- The debouncer switch 22

- The development system 23

- The interface encoder/single board computer 25 - The interface single board/motor power amplifier 27 - Initialization of the hardware and 110 addresses 29

6. THE MOTOR CONTROL PROGRAM 32

7. CONCLUSION 36

APPENDIX CONTENTS

1. RESUME EN FRANCAIS

2. LISTING OF THE MOTOR CONTROL PROGRAM - Module INIT1

- Module CON SOL - Module MAIN

3. LISTING OF THE TRANFORMATION PROGRAM

4. LISTING OF THE MATLAB's PROGRAMS - The BDLQR program

- The simulation program 5. THE HARMONIC DRIVE

6. THE MOTOR

7. THE INCREMENTAL ANGLE ENCODER 8. THE 74L52000 DOCUMENTATION 9. THE DAC 811 DOCUMENTATION 10. REFERENCES

1. SUMMARY

Nowadays machine tools, flexible automation systems and robots use many servo-systems. A servo-system consists of a servomotor, its drive and control, and the load.

A servomotor-drive system is able to follow a path, reach a position with a desired velocity. It can do it with a certain accuracy, but for it to be optimal, the servo-system has to be adjusted according to the load. When the load changes are-adjustment of the servo controler may be necessary.

The load of a rotational servomotor can be defined as a combination of a mass of inertia, damping and torsional stiffness. In a robot working in polar coordinates, the mass of inertia of the load varies from to 4, when the robot moves its arm in- or outwards. In order to test these effects, a dynamic load-simulator has been developed at the university.

The goal of my work was to build a servo controller, used to drive a servomotor, connected to the load simulator. The parameters of the controller have to be optimal for all the loads covered through the simulator, and for the motor used with the maximum of effeciency.

A incremental angle encoder, an Intel single board computer, an amplifier, few interfaces, and simulation programs have been needed to achieve in this work. This report shows how they have been used, explain the control theory, and the final state of the work.

2. THE WORK ENVIRONMENT

T.U.E Technische Universiteit Eindhoven is now, in 1987, thirty one years old. It is already in the new system of studies for engineering of the Nederlands.

T.H.E Technishe Hogeschool Eindhoven, was the former name of this school, the change of name was a part of the school reform. Nowadays, students get only four years to end their studies, instead of five years in the old system. This one was appreciate by students, because they were free to manage their studies as they wanted to, and for most of them the average lenght of studies was seven years. Now, a maximum of six years is tolerated. That will maybe affect the associative life which was very active before, in every department.

There are three technical universities of that kind in the Nederlands: T.U. Delft, founded in 1842,

Eindhoven thousand. there is research.

T.U. Eindhoven, founded in 1956, T.U. Twente, founded in 1961.

Delft university is about twelve thousand students, and Twente universities are about half of this, six The first level of studies is the Master's degree, then the Doctor's degree that require few years of original A T.U.E's student has to choose his speciality into the nine that are offered: - Technology in its social application,

planning.

Industrial engineering and management science, - Mathematics, - Computer science, - Technical science, Mechanical engineering, Electrical engineering, - Chemical engineering,

Architecture, structura engineering and urban

Since T.U.E has been opened more than eight thousand

THE MECANICAL ENGINEERING DEPARTMENT

-This department is divided into four division: - Fundamentals of mechanical engineering, - Product design and development,

- Design for industrial processing,

- Production engineering and production automation it has about one thousand students and two hundred employees (teachers and technical personal). I worked in the production engineering division (WPA) for five and a half months.

THE F.A.I.R PROJECT

-The research project F.A.I.R (Flexible Automation and Industrial Robots) is financed and directed by the Dutch government. Its aims is to get some experience in flexible automation and in industrial robot systems.

The electrical and mechanical engineering departments are involved in this project as well as several private firms. The project is composed of five parts:

- The general aspect of automation, - The handling of parts,

- Kinematic and dynamic of mechanical strutures, The drive systems, the control systems and applications of the system,

- The arc-welding and the sensory systems.

MY TRAINEESHIP PROJECT

-During the first two weeks of my training period, several subjects have been proposed to me. In order to make a good choice, I spent some time to read some documentation on each subject.

The first proposition was to work with a linear robot arm that has been already used by two students of the UTC, Eric GALET and

Loic JANVIER. The second subject was to work on a robot-arm with compensation for bending, and the last one was to drive a motor related to a load simulator. This last one seemed very interesting to me, for many reasons, first of all because this subject included both, hardware and software work, the others were mainly software work, another reason was that Rogier MARES, the student who was doing his final project on the load simulator, was leaving soon, and somebody should continu his work.

The idea was to use a new single board computer to drive the motor. Thus, many things were to be done, equipe the board, with power supplies, cooling, Input/Output connections with the development system, the system itself, ... Then interfaces between the single board and the incremental angle encoder, and the power amplifier of the motor were realized. After this a control algorithm has been written and tested on a simulation software.

All kind of subjects that are going to be developed in the following parts of this report.

3. THE LOAD SIMULATOR

-The load is described as follow, a value of stiffness, a value of damping, and a certain quantity of mass of inertia. In the model developed in the laboratory, the aass of inertia can be a function of time but not the damping neither the stiffness. This is a rather good view of reality because in aany cases damping and stiffness stay constant meanwhile the mass of inertia moves, for exemple in this kind of aovement:

Here is the model of the load,

T - - - 4 ... T

=

T(t) 2

=

Q(t)

=

.(t) 21

=

21(t)

=

.1(t) tp

=

tp(t) q>1

=

q>1 (t) J

=

J(tp)

₌

J(tp(t) ) D C

r

-I

I

Q

I

'I'

...,

I

I

I

I

,

~ee.d

_ _ _ _

l,_J

torque (Na)

angular speed (rad/s) angular speed (rad/s) angular position (rad) angular position (rad) mass of inertia (Rgm2)

damping (Nas/rad)

stiffness (Na/rad)

0(0) = 0, 01(0) = 0, the equations related to load are

d(J*~1)

=

-D*~1+ T .

The load response is related to the torque that is measured as follow, the shaft of the harmonic drive is suspended by means of three springs in a ring. The deflection of a spring is proportional to the applied torque and is measured by strain gauges.

Those two schemes show how it works: Without any torque,

•

With a torque applied,

A servo-motor is coupled to the load simulator, by the means of an harmonic drive. This one reduces the rotation speed 80 times, and it can not be used to deliver more power than 100 Watts.

-That is the harmonic drive;

We wanted to control the position of the servo-motor, using an incremental angle encoder and a single board computer (see appendix:

MOTOR r- -~

LOAD

I - - - . . J . .

The motor used before has burned down a few weeks before I came at the TUE. Another motor should have been available during my traineeship, but some problems occured and the new motor did not arrive in time. For this reason, all the parameters used are those from the former servo-motor, and they should be modified when another motor will be installed.

The former motor was a TM 530, from AXEM-SERVALCO, it is a printed disc armature DC-motor. If we adopt this notation:

Um motor voltage coming from the power amplifier' Rm motor resistance

Ia intensity coming from the power amplifier

+

motor constante L motor self-induction

we can write this equation;

Um

=

Rm*Ia + +*2m + L*(dla/dt)

L*(dIA/dt) has a very small value compared to the other terms of the sum, 50 we decided to neglect it in the other

calculations.

The power amplifier is supposed to be ideal (no offset), this can be written in this equation;

Urn

=

K*Vout with, K : gain of the amplifier,

Vout : output voltage from the DAC MULTIMODULE board.

The motor and the load simulator are related by two harmonic drives, that are exactly the same, and an axis, for this first approach, we decided to do like if it was only an axis with a huge stiffness, to make the equations more simple.

A mechanical model of the system (encoder, motor and harmonic drive) has been found,

dp

With: J_pl J

m, Jhd the mass of inertia of the encoder, the motor and the harmonic drive,

Dpl _{Dm, Dhd the damping in the encoder, the motor} and the harmonic drive,

Cp' Cm' dp' dm the stiffness and the damping of the shafts between the encoder and the motor and between the motor and the harmonic drive.

This model has been made more compact with some new variables, J

stiffness of the shafts has been neglHted because in practice, the frequency range is limited, and the system is in fact a first order one.

The system is now very simple;

I

r

We can now relate it to the load simulator model,

f

Three equations are related to this system, Um

=

K*Vout

=

Rm*Ia + +*Qm

J *(dQm/dt) _t

=

+*Ia - 0 *Qm _t + T

d(J*Qm)/dt = -O*Qm - T

This gives after some calculations this differential equation;

K*+*Vout/Rm = (Jt + J)*dQm/dt + (Ot + 0 + dJ/dt + .2/Rm )*Qm

That is the model basis equation that we will use for the control theory.

4. THE CONTROL THEORY

As the load was not constant, it was not possible to find a control with parameters that were optimal for every kind of loads. According to this we had to make an adaptive control for the servo-motor

We tried to find in the control theory literature (see sources references) some advices about adaptive control. Some paper dealt with Model Reference Adaptive Control (MRAC), Self Tuning Regulator (STR) and Sub-optimal control. They give some algorithmes to implement on computer to apply those theor!4$. Unfortunatly, the sampletime needed for these algorithmes is rather long, some seconds and never less than 100 ms, for the fastest. These theories has been developped for chemical industry or for slow process. In our problem it was not bearable to have such a long sampletime, we should be able to reach a sampletime of about 1 ms.

To have an idea of adaptive control, we have below the block diagrams of a functional model reference adaptive control system;

r +

-.~

r • ref erence input

nt output d output U • • err 01' dlUl

v-_

"01 ofaaal PLANT -+ MODEL CONTllOLLER x e r p x _p x

..

of a Self Tuning regulator; u

.

y

-

PROCE'SS

-

-CONT F-Ol..

...

..

\." IN ,

VAL vE" ~ ~oF

'f.RA MfTfR.~

tJ

't.

-

~~Tit'\ArO~

of a Optimal or a Sub-Optimal regulator;

JJ Y

-

'PROC,"S::'

...

~.

----

.... ---~ c:..OIJT~OL.

-

)( rSTj""I\T~f\

..

LAW

-

_of E,.TATcS

-

-

'J

----To achieve, in having a small sampletime, and an adaptive control system, we decided to make the calculations of the optimal parameters off-line. Then, according to the parameters value of the load (J, dJ/dt, D) we can find the optimal parameters that have been calculated and saved in a memory space.

Using the model equation of the system,

Vout = (Rm/K*+) * «Jt + J)*dQm/dt + (Dt + D + dJ/dt + +2/ Rm )*Qm)

we decided to find the state space of it. With: X = ( x1, x2 )

{X1

=

f Qm dt we can write,

t

x2

=

Qm { X

=

A*X + B*Vout Y

=

C*X B =

(~)

C = ( 0, 1 ) with {a

=

(Dt + D + dJ/dt + .2/Rm )/(Jt + J)

a

=

(R*+)/(Rm*(Jt + J»

With the differential equation and the state space representation of the system, we have several ways to find the parameters for the controler.We decided to use PC-MATLAB, a program available at the TUE, that could provide many things we needed for this system control. PC-MATLAB is a new version of the original Mainframe-MATLAB that was written by Cleve MOLER in FORTRAN. This version was written in C, by several programmers, including Cleve MOLER.

PC-MATLAB has a rich collection of functions immediately useful to the controls engineer. Complex arithmetic, eigenvalues, root-finding, matrix inversion are some examples of important numerical tools that are available. More generally, PC-MATLAB's linear algebra, matrix computation, and numerical analysis capabilities provide a reliable foundation for control systems engeneering as well as many other disciplines.

The CONTROL SYSTEMS TOOLBOX, using PC-MATLAB matrix functions, builds where the foundation leaves off to provide functions specialized to control engineering. This toolbox is a collection of algorithms, expressed as .m files, or macros, that implement common control systems design, analysis, and modeling techniques.

Control systems can be modeled as transfer functions or in state-space form, allowing both ·classical" and "modern" techniques to be used. Both continuous-time and discrete-time systems are handled. Conversions between various model representations are provided. Time responses, frequency responses, and root-locus measures can be computed and plotted. Other functions allow pole-placement, optimal control, and estimation. Finally, and most importantly, tools that are not found in the toolbox can be created by writing new .m files.

System Representations

YlU. Y(.) H(s) dx/dt. Ax +Iu 'I.

ex.

Du Y(t), YCw' .. z) ... Ax,. +

8u,.

-Yn • CXn •

Dun

Some functions were very useful for us, such as;

~ c2d ~ that makes a conversion from continuous to discrete time, dlqr Discrete Linear Quadratic Requlator Desiqn, calculates the optimal feedback qain matrix L, such that the feedback lawt

U ::: -L*X

minimizes the cost function J :::

I

(X'*Q*X + U'*R*U) subject to the constraint equation X(n+1)

=

A*X(n) + B*U(n)

~ dimpulse ~ Unit sample response

dstep Step response, and ~ dlsim ~ Simulation with arbitrary inputs.

The system can be represented such like on this scheme;

WP r--SYS-TEM

i

Um SYSTEM

L~_--=-PO-=-S=-

_ _

---'-i~

} -

~

----;l'--~~ t - I _~~_-t~ " ' _ . . . . , ~ MODEL I 1 _ _ . _ _ •

1

POS U ERROR STATE RECONSTRUCTOR L=(l1, 12) ~---'

'L' optimal is determined with PC-MATLAB for every value of load we could have.

5. HARDWARE AND EQUIPMENT

PRESENTATION OF THE iSBC 186/03

-The iSBC 186/03 Single Board Computer is a general purpose, 16-bit computer system on a MULTIBUS-compatible 7.05*12.0 inch printed circuit card. The CPU, system clock, memory, sockets, I/O ports and drivers, serial communications interface, priority interrupt logic and programmable timers, all reside on the board.

The central processor is an Intel 80186, high integration 16-bit microprocessor, operating at 6 MHz, it combines several of the most common system components onto a single chip (i.e. Direct Memory Access, Interval Timers, Clock Generator and Programmable Interrupt Controller).The 80186 instruction set is a superset of the 8086. It maintains object code compatibility while adding ten new instructions (Block I/O, Enter and Leave subroutines, Push Immediate, Multiply Quick, Array Bounds Checking, Shift and Rotate by Immediate, and Push and Pop all). An optional 8087 Numeric Data Processor may be installed by the user to dramatically improve the 186/03 board's numerical processing power (the 8087 will increase the performance of floating point calculations by 50 to 100 times).

Here is the block diagram of the board:

eo1l. CPU I 0 .. " 118"T MIMOIIV I'TlI ,.OUtIJ IITI : I IX""NSION : I , '---...

---

....

_--..

,.

20 -A brief list of the board features, that have some interrest for us will be discussed now.

The on-board 80130 component adds several functions to the board: a subset of the iRMX 86 Operating System Nucleus in on-chip memory, three additional programmable timers and an interrupt controller on a single chip.

Up to 16 Megabytes of total system memory may be addressed by the iSBC 186/03, using a paging technique. Of this amount, a maximum of 256K-bytes of EPROM type local memory and a maximum of 32K-bytes of RAM type local memory may reside on-board.

Interrupts may originate from numerous on-board or off-board sources. All interrupts, except the 80186 non-masquable interrupts (NMI, that are used to signal catastrophic events such as power failure), are handled by the 80130, configured as the master interrupt controller, and three other slave interrupt controllers. The 8259A, 8274, and the interrupt controller portion of the 80186 are configured as slaves to the 80130 device. This table contains a list of devices and functions capable of generating interrupts.

Device Funcllon Nu ... ot

Interrupta MULTIBU~ bua Interface Requestl trom MULTIBUS buS I'tIIident pertpherWa

•

INTO -INT7 or other CPU

8274 Serial Controller Transmit buffer empty. receive buffer full and 8

channel errors

Internal 80186 Timar Timar O. 1, 2. outputs (function determined by 5

and OMA timer mode) and 2 OMA channel interrupts

80130 Timer Output iRM)(TM system timer (SYSTICKl 1

iSB)(TM bus connectors Function determined by ISBX MULTIMOOULETM 6

board (3 per

ISBX connector)

Bus fail-sal. timer Indicates addressed MULTIBUS bus rMident 1

device has no! rasponded to command within 10 msec

8255A Parallel I/O Parallel port control 2

Controller

J4 Connector External/Power-fail Interrupts 2

Serial I/O operation is handled by an Intel 8274 Multi-Protocol Serial Communications (MPSC) device. It supports two serial I/O channels for either RS232C or RS422/449 applications.

Two iSaX bus connectors are provided on the iSBC 186/03

board, these are designed to expand the board's 1/0 functions, using iSBX MULTIMODULE boards.

THE iSBX STANDARD

-The iSBX I/O Expansion Bus is one of a family of standard bus structures of Intel. The iSBX bus interface can be grouped into six funtional classes: control lines, address and chip select lines, data lines, interrupt lines, options lines, and power lines. The iSBX bus provides nine control lines that define the communications protocol between base board and the iSBX MULTIMODULE boards. These control lines are used to manage the general operation of the bus by specifying the type of transfer, the coordination of the transfer, and the overall state of the transfer between devices. The five address and chip select signal lines are used in conjonction with the commands

lines to establish the I/O port address being accessed, effectively selecting the proper iSBX MULTIMODULE. The data lines on the bus can number 8 or 16, and are used to transmit or receive information to or from the iSBX MULTIMODULE ports. Two interrupt lines are provided to make interrupt requests possible from the iSBX board to the base board. Two option lines are reserved on the bus for unique user requirements, while several power lines provide +5, -12 and +12 volts to the iSBX board. Here is the Signal/Pin assignments table:

"'"

... _1Ie a ...

_"'"

MMlllonic

D-'_'"

43 Moe MOATA Bit 8

....

Moe MOATA Bil 9

..

,

MOA MOATA Bit A .. 2 MOB MOATA Bit F

39 MOC MOATA Bit C <10 MOO MOATA 8,\ 0

31 Moe MOATA Bit E 38 MOF MOATA Bi\ F

35 GNO Signll Gnd 36 'SV +5 Volts

33 MOO MOATA Bit 0 34 MOROT M OMA RlIQUH\

31 MO' MOATA Bill 32 MQACKI M OMl< AcknOWledge

29 M02 MOATA Bil 2 30 OPTO Oplion 0

21 M03 MOATA Btt 3 28 OPT1 Option 1

25 MO" MOATA BII .. 26 TOMA Terminale OMA

23 MOS MOATA B,I 5 24 R~

21 MOe MOATA Bil 6 22 MeSO! M Chip Select 0

19 Mor MOATA B.t 7 20 MCSlf M Chip Selecl I

11 GND S.gnal Gnd 18 'SV -5 Volts

IS IOROI 110 Reid emd 16 MWAIT! MWlIII'

'3 IOWRTf tlO Wrile Cmd 14 MINTRO M Interrupt 0

I

II MAO M Address 0 12 MINTAI M Inlerrupl I

9 MAl M Addreu I 10 Aeserved

7 MA? M Address 2 8 MPSTI ,SBX Multomodule

Bo"rd Presen t

5 RESET Aea.1 6 MCLK M Clock

3 ONO S.gna' Gnd 4- +SV '5 Volts

THE DEBOUNCER SWITCH

-The Single Board Computer needs a hardware reset and interrupt signals, to provide these, we had to equiped the board with two debouncer switches.

drawing;

A normal switch gives a signal as the one shown on this

[V]

o

+5~~---.~

This signal is not clean enough to be used by the computer, that is the reason why we use debouncer switches that gives the signal shown on this drawing;

(V]

o

+5

1-..---11 ....

Those debouncer switches are made with a few NAND gates, resistors and capac!~ors, according to this scheme;

1

}

THE DEVELOPMENT SYSTEM

-In software developing, the -Intellec Serie III Microcomputer Development System was used. This is a very useful tool for designing micro-computer software implemented on the iAPX186 processor. We write programs, compile them, link and locate them, then we can debug them if it is necessary and run them on the system itself or on the single board computeT'o

lNT!llEC' SIRlES HI DEVELOPMENT SYSTEM

II!IIIAL 110 POM

This system offers the possibility to write source programs in high level languages, such as Pascal, Fortran, PLM or Assembly. These different languages are special Intel versions (Pasca186, Fortran86, PLM86, Assembly86) to allow specific functions such as Input/Output, interrupts management, ... The programmer is able to divide his work in several parts called 'modules', to write them in different languages and link them all together with other library files. The program is thus modular and several people can work on it separately. These modules are linked together by an interface specifications section. This part declares public all constants, types, variables, procedures and functions that can be used by other modules. In a complete program, the interface specifications part can be common to all the modules.

The compiler converts the user's instructions, source files, into object code files.

LINK86 combines 6066 or 80186 object modules and resolves references between independently translated modules. All the object files have to be compiled under the same memory addressing mode control (SMALL or LARGE). The SMALL control directs the compiler to perform a certain memory addressing technique that helps to reduce the amount of code produced and so minimize the necessary memory space. LARGE can be used for larger programs.

LOC86 changes a relocatable 8086 or 80186 object module into an absolute object module which contains the absolute address of each public part of the program.

SDM86 is used to make the connexion between the single board, a keyboard and a screen. This also allows the programmer to acceed to the iAPX186 Operating System.

Time needed for compilation, linking and relocation is rather long up to 15 minutes, but this development system is still one of the best tools for single board software developing.

fl··· .... r-- - - -.... •OPIII SYSTtM : .. - '

p.----_ ...

• • t - -... : CII"II : • I '---I---.1

p.-."---• •

r--··---..

:"'~~GI.: : _ _ .fOR : • ___ • ___ tI ... - - : v-I .~:::: _ _ _ _ _ _ _ _ _ _ _ _ _

.J

:..._ .... _.-' , \OAO,II , ...

.,

...

-_

...

·

_{• ...clllCutr'}

,

• t.U~Aro. :

....

---

....

p-.-._--, • •

·

•

_'---1---.1

-

:

•

.----

....

-

... : ",I : • \OAO!II , t ,

,---

..

~ 24

THE INTERFACE ENCODER / SINGLE BOARD COMPUTER

-The encoder is used to know the exact position of the motor axis. We use a bidirectional encoder, that deliver two 90 degrees phased (quadrature) square waves. Those two permit direction-sensing, signal 2 comes before signal 1 at clock-wise rotation (seen from shaft end). In the model used, one thousand pulses are deliver every rotation. One O-pulse is also delivered every rotation.

This is illustrated on the scheme below;

I

I

I

: I

!

..

..

UnO ReferenZIlTlPUI~ I impulslOll de reference I' reference pulse • I

In order to work properly, the encoder requires power supply of 5 volts and a ground signal (140 mA maximum are needed), the maximum pulse frequency is 50kHz. The power supply was available on the iSBX bus.

We had to transform the two signals into one, square wave, that was a picture of the motor shaft. This has been possible to do with the SN 74 LS 2000 DIRECTION DISCRIMINATOR chip. This chip can be used as a direction discriminator or for pulse width measurement or frequency measurement. The 74LS2000 features are :

direction discrimination to identify forward/backard direction,

- separate 16-bits cascadable up/down counter,

pulse width measurement with either forward/backward counting,

- frequency measurement,

- 8-bits parallel tri-states data bus, - all inputs and outputs TTL compatible, - single +5 volts supply.

A 74LS04 has been used also in this interface to make the reset signal from the iSBX bus suitabl,e for the 74LS2000. The iSBX bus gives a active high reset signal and the chip needs a active low reset signal, that is the reason why we had to invert the bus signal. The 0-pulse signal has been connected to one option line of the bu~ for later uses.

Scheme of the interface:

I-

- - - -

---CON N f'C: To,? .7:6

- - l

•

I

_,

L

T.5

8X }jUS A 30

I

L

~

Ii t'1 IS t.'L ~ +! " 35' t

.

1

DA T A lll'4E5

..

i I .--_{1'1 1.1 •} 1.3 '1.5 '11 t ij 31-:;)"

- -+-

-

--

.

- - -

- -

_r-r-- -

-

r

"-1

-

T

.1 ;;;~, .. ~ '''.fl."! _.D? .K ~C!~

1)5 .Pt <til> J?3 :n. lit DlI fji I:!

7+ LS

2000

•

I<ti

Q~

t~

J:Dii;j Y!i+ !J. ~ ~,~,~q, ~

..

~

_

~ ~

I'; ;

r •

TiT

T

i

T

~

;

Tt,c

r---~

_I,

I

U

r. .

.r . .

. ~~ .. • ~.." .. WI:

~'

74

lS 0+

W~

+=I= .. ~=t.

,

I

I

t

-;-

-

I

•

I r , L _ _ _ _ _ _ ..1 eN,OJ)E 1\ flUG.

- THE INTERFACE SINGLE BOARD I MOTOR POWER AMPLIFIER

A DAC811KP, a OPA27FZ and a 74LSOO chips has been used to make this interface.

The DAC811 is microcomputer-compatible

a complete single-chip integrated circuit 12-bit digital-to-analog converter. The chip includes a precision voltage reference, microcomputer interface logic, double-buffered latch,and a 12-bit D/A converter with a voltage output amplifier. Fast current switches and a laser trimmed thin-film resistor network provide a highly accurate and fast DIA converter. The DAC811 is fully specified for operation on +12V and -12V power supplies. However, in order for the output to swing to +10V and -10V, the power supplies must be +13.5V and -13.5V or greater.When operating with +12V and 12V, the ouput swing should be restricted to +8V and 8V in order to meet specifications.

The OPA27 is an ultra-low noise, high precision monolithic operational amplifier.Laser-trimmed thin-film resistors provide excellent long term voltage offset stability and allow superior voltage offset compared to common zener-zap techniques.

The 74L500 is a quadruple 2-input positive NAND gates. It is used to select the DAC8l1 and also to invert the MSB (most significant bit) of the 12-bit send. This has been done to have a nice conversion

~haracteri5tic, according to the two's complement notation for the negative numbers.

1. 6 :B

I

X

I

e

. 5

I

,

~

"

N f C T

o

~

The iSBX bus has been used also for this interface, that is very useful because it provides +12V and -12V, in addition to +5V.Two potentiometers are used for the OAe, one (100kQ) is for the gain

J

adjustment, the other one (100kQ) is for the offset adjustment. The output voltage range should be 20V (from +10V to -10V), because that is what the power amplifier of the motor needs.

Here is the scheme of this interface:

35

l

r'--'

A

- A

I~ ~~ w .

•

I

71-

LS 00

1

L ..•.. a a . • I I

,

I

,

_I I

L

l

r

J:lIt " " l ' N .}8 " ")10 )11 ~ I Nl> liiI l.)J.t ~ VDn

DAC

811

KP

t

GalA I~ Sf ~ri:t ~ J

GNo...J)l6.o\ ,])1 .9~ tIl 'It.-~~ &lID

ltr

'::l

~ r , K J\.

T

T

('.o'p.

l--=-

~

-

...

~~1J.

H~

28 -.+ 3'!> 31 .eD

'=t

1011..1\. 11

1T ,

.v ..,. ::r

,.

>-J

:Y3fAf7~

~ :>

t

~ our

"*'

!I . ~ ~ . -

•

~ 1

,.

I

100lC IIlOI(

i

-!--- -1~F = _~ ~ .. f I _'I'f

=

_f=

.

- INITIALIZATION OF THE HARDWARE AND I/O ADDRESSES -A. Initialization of the PIC 60130

Programming the 80130 interrupt controller is accomplished by accessing the control words in the I/O space located at the locations EOH and E2H. These registers consist of, Interrupt Request, In-service and Interrupt Mask registers, Initialization Control Words

(1, 2, 3, 4, 5, 6), and Operation Control Words (1, 2, 3).

The 80130 accepts two types of command words, Initialization Command Words (ICW), and Operation Command Words (OCW). The lCW must be sent before normal operation can begin, and the OCW are sent at any moment after initialization.

In our application, we had to send 4 initialization command words, ICW1, ICW2, ICW4 and ICW6i

ICW1 bits 7 6 5 4 3 2 1 0

o

0

1

o

0

written to I/O address EOH unused

no slave unit ICW1 indicator

edge triggered interrupts noICW5

ICW4 always needed

ICW2 bits 7 6 5 4 3 2 1 0 written to I/O address E2H

o

0 1 1

*

* *

five most significants bits of

the vector byte supplied to the CPU, 36H or 56D. From OOH to 37H, it is a reserved part for Intel.

ICW4 bits 7 6 5 4 3 2 1 0 written to I/O address E2H unused

0 0 0

o

no special fully nested mode

o

buffered mode, master, normal end of interrupt, 8066 mode.

ICW6 bits 7 6 5 1 1 4 3 1 1 2 1 1 1

o

1

written to I/O address E2H all local inputs

B. Initialization of the PIT 80130

The 80130 has restricted the programmability of each timer so that the control word is a single fixed byte value to initialize the corresponding timer. We use the timerO, that is a rate generator, with a 6 MHz clock.

The initialization word, 00110100B, has to be written to 110

address EEH. The sampletime is loaded to I/O address E8H, for example to have 1ms sampletime, we load 6000 (decimal).

C. Counter and DAC initialization

As the counter is equiped of a reset pin, we use it to initialize the chip. But this is only a hardware initialization and to make a software initialization we have to send OOH to the I/O address AOH, and OOH to the I/O address A2H, that are located on the iSBX bus.

The DAC has no reset pin, thus we have to initialize it always with software means: send OOOOH to the 1/0 address 80H.

-6. THE MOTOR CONTROL PROGRAM

This program is divided in three modules, and are written in Pascal, as it is possible in the Pascal-86, provided by INTEL. The modules are: INIT1, CONSOL, and MAIN.

Module INIT1: the aim of this module is to initialize the iSBC 186/03 computer, in particular the P.I.C and the P.I.T 80130.This module is made of three procedures, INITPIC, INITPIT, and INITIALIZATION1. The first send the lCW's to the 80130 P.I.C, simply using the command OUTBYT. The second procedure, INITPIT, has a parameter, COUNT, that is of the type PART, that means a word made of two bytes (low and high). This parameter is the value that is going to be load in the timer's counter, to determined the sampletime we want. This value is sent to this procedure by the mean of the last procedure, INITIALIZATION1, which has been declared public and calls the other two. The value of COUNT is established into the main module and comes through the public procedure.

Module CONSOL: this module establishes the relation between the user, through the terminal, and the program. The values of the parameters are brought in via this procedure. This parameters are:

simulator

- WP : number of turns of the shaft desired

J mass of inertia programmed on the load

- JO derivate of the mass of inertia

o

damping of the load simulated

- L2 proportional action gived to the regulator - L3 derivating action gived to the regulator. To realise this, the module uses several procedures, such as PRINTTR, REAOlNTEGERTR, ... , that use other basical procedures or functions, CHR, CO, CI , ORO, ... To have details about these procedures, a listing of them have been inserted in the appendices, as well as,for the procedures, LINE, TAB, TABS, .. " that are used for the presentation of the menu on the terminal screen.

The procedure INPUTVAR and LINE are declared public in this module, with all the parameters involved, to be able to be called in the module MAIN.

Module MAIN: it is declared as PROGRAM and not PRIVATE like the other modules. This module includes the interrupt procedures, the calculations functions, and the I/O procedures or functions.

Variables used in the whole program are, as defined in Pascal language, divided into three parts, CONST, TYPE, and VAR.

CONST are the constant values used in the program, MAXSTRING equal 30/ is the maximum length of a characters string we will print on the screen, TSAMPLE is the value of the sampletime we want, in milliseconds. The last constant is INTERRUPTMASK, that is the address, OE2H, of the interrupt mask of the 80130 P.I.C.

TYPE are new types of variables we defined, here we have STRING, that is a chain of characters.

VAR are the variables that are used in several procedures; INT and KEYPRESS are boolean, their values are settled through the interrupt procedures: SAMPLETIME and KEYPRESSINT.

A, PRESA, PREVA are integers, A is a software counter that is increased or decreased through two procedures: CARRY and BORROW that are related to the carry and borrow signals of the hardware counter Multimodule board. PREVA and PRESA means previous value and present value of A, those are used to know the previous and the present position.

PRESPOS and PREVPOS are of PART type to be easily entered with the INBYT command, they are the values read into the hardware counter, at the sampletime T and (T-1), on 16 bits.

PRESL and PREVL are LONGINT and are the exact numbers of pulses counted since the program started, at the sampletime T and (T-1). These two values are calculated as follow:

PRESL := (PRESA

*

65536) + PRESPOS.FULLWORD

PREVL := (PREVA

*

65536) + PREVPOS.FULLWORD

-WPOSPULS is declared as a LONGINT, it is the number of rotations given in turns by the user, converted in the number of pulses. One complete rotation of the ouput shaft of the harmonic drive, increase the counter value of 32000, that is why we need a LONGINT for this variable. J, JD, D, L2, and L3 are INTEGER [ -32767 ; 32767 ], they are the parameters of the load that have been programmed on the load simulator, and L2, L3 are the parameters of the regulator, proportional and derivating action.

UN, and DACVALUE are declared as LONGINT, UN is the nominal voltage that has to be sent to the motor and is calculated from the model equation of the system. DACVALUE is the value that is actually sent to the DAC, it includes UN and the correction made by the regulator. As we have a 12-bit DAC, this value has limits: -2048 to +2047 (decimal). The interrupt procedures are as short as possible, in order to lose no information. There are four of them:

CARRY: increases the software counter, A, with 1, for every carry signal coming from the iSBX bus,

BORROW: decreases A with 1, for every borrow signal coming from the iSBX bus,

KEYPRESSINT: sets the boolean, KEYPRESS, to TRUE. This presses the interrupt key on the front occurs

panel.

every time the user

SAMPLETIME: sets the boolean, INT, to true, updates A with PRESA, and PRESPOS by reading the counter value on the Multimodule board. This happens for every time the TimerO sends an interrupt pulse. Vector number 56 57 58 61 Interrupt procedure CARRY BORROW SAMPLETIME KEYPRESSINT Flag INT KEYPRESS

CARRY, BORROW, and KEYPRESSINT have been installed via jumpers on the board,

Jumpers 80130 interrupt number CARRY/ iSBX(intr1) BORROW/iSBX(intrO) KEYPRESSINT/switch E90--E84 E89--E83 Switch--E79 IRO IR1 IRS

There are three calculation functions, INTEGERTOSTRING, POSITION, and VELOCITY.

INTEGERTOSTRING is a procedure that allows the conversion of an integer or a long integer into a text string, to be printed using PRINTTR procedure.

POSITION is a function that calculates the global number of pulses counted since the beginning, using PRESA and PRESPOS.FULLWORD;

POSITION := PRESA*65636 + PRESPOS.FULLWORD.

VELOCITY is a function that calculates the velocity of the motor output shaft, making the difference between two positions, and dividing this by sampletimej

VELOCITY := (PRESL-PREVL) DIV TSAMPLE,

this value is given in pulses/millisecond.

There are three procedures for I/O communications with the user, the amplifier of the motor and the front panel switch; SENDDAC, PRINTTR, IFKEYPRESS.

SENDDAC is the procedure that outputs DACVALUE to the 1/0

address OaOH, some caculations are also executed in this procedure: the calculation of the nominal voltage sent to the motor, UN, according to the model equation,

- the calculation of DACVALUE, including UN and the control factors, a test made on the maximum and the mimimum values of DACVALUE, in order to not exceed the value of a 12 bits number.

Then DACVALUE is output, through OUTWRD command, to the DAC on the Multimodule board.

-PRINTTR is a very useful procedure that allows to print strings on the terminal, of a maximum length of 30 characters, ignoring the spaces of the right.

IFKEYPRESS is a procedure that displays on the screen the values of the position, of the velocity, of the nominal voltage, and of the value sent to the DAC, at the moment that the switch has been pressed.

A flowchart of the MAIN program is given here;

! \ \ MAIN INT:=FALSE

.

, A:=O KEYPRESS:=FALSE ; PRESA:=O COUNTERVALUE:=6000; PREVA:=O PREVPOS:=O ; PRESPOS:=O INITIALIZATION1 INITPIC INITPIT INITIALIZATION OF OAC

I

INPUTVAR (WPOSPULS, J, JO, D, L2, L3) INT:=TRUE NO YES INT:=FALSE SENODAC

l

KEYPRESS:=TRUE NO-

_.

YES I KEYPRESS:=FALSE

I

IFKEYPRESS

I

I

7. CONCLUSION

Five and a half mounths spent at the T.U.E, studying this servo-controller, were for me an unforgettable experience.

I had to deal with both, hardware and software problems, that were new to me. That time gave me the oportunity to learn a lot about programming and improve my knowledge in the hardware and control theory fields. It was also very good for the fluency of my english.

The work has never been tried out in reality, but is operational, few things have to be changed before utilization with a new motor, first, change the motor parameters, used in the program, such as the internal resistance of the motor, its constant ... Then, the new optimal parameters used in the control algorithm have to be determined, for every load that can be simulated. Finally, according to the way that these values can be stored, a procedure can be written to find them directly, for each sampletime, for the changing load values. A protection for the motor against the 'burn down', should be realised also to avoid problems.

1. RESUME EN FRANCAIS

'"

.

De nos jours, machines-outils, systemes automat1ques

flexibles et lea robots utilisent beaucoup de servo-systemes. Un servo-systeme consiste en un servomoteur, son systeme de pilotage et de control, et la charge a laquelle il est accouplee.

Un tel systeme est capable de suivre un chemin, atteindre une position, avec une vitesse desiree. II peut Ie faire avec une certaine precision, mais pour @tre optimal, Ie servo-system doit @tre

ajuste

a

la charge. Quand la charge change, un re-ajustement des

parametres du control leur peut etre necessaire.

La charge d'un servomoteur tournant peut ~tre definie comme

etant la combinaison d'une masse d'inertie, d'un amortissement, et d'une raideur. Dans Ie cas d'un robot travaillant en coordonnees

polaires, la masse d'inertie de la charge peut varier de 1 a 4,

suivant que Ie robot d~place son bras de l'interieur vers I'ext~rieur.

De mani~re

a

~tudier les effets de cela, un simulateur de charge dynamique a ete developpe a l'universite de technologie de Eindhoven.

Le but de mon travail dans ce cadre etait de concevoir un

servo-controlleur, utilise pour piloter un moteur connecte au

simulateur de charge. Les parametres du controlleur devant ~tre

optimaux pour toutes les charges rencontrees via Ie simulateur, de

mani~re _{a utiliser Ie moteur avec un rendement maximum.}

Un capteur incremental d'angles, un ordinateur complet implememte sur un seul circuit imprime, un amplificateur, quelques interfaces,

d'aboutir utilises, travail.

un logiciel de simulation, ... , ont etes necessaires afin dans ce projet. Ce rapport montre comment ils ont ete explique la theorie de control employee et l'etat final du

2. LISTING OF THE MOTOR CONTROL PROGRAM

SOURCE TEX'I: IUIT1.SRC

£: '.01( ::;: ~!.l4<: :~: :~;)!<)t( *: '~.: .~< »: )t.::¥. [: )K }'.OICKl}{ )IO~:;tOK)Ij( >K:«:<K:!i

£:

FILE ~ INIT1.SRC J

FUNCTION: INITIALISATION OF ISBC186/03 J

NODULE: I:NIT1)

r:

.:t.:;I(:f.<}I(:lI.: *: y'.~ )1( ){, ':;': 'i,.~ x.: )t.:':t:, J

[ :#:Y'{)lCiO~; iiOICIOK:·;O!("lj.: *;;-r ::1

PUDL..TC TNIT:!. f

FCm THE MClTOr: CONnmL :l

TYPE PARl .. R~CORD CASE 800LEAN OF

THUE ~ (FULLWORD : WDI:;':O ) 1

FALS~: (LOW, ~IGH : O •• 25~ );

END;

F'h:OCEUURE IiHTIAL12ALHHH ( COUNT! PAHT) t

t---J

PRJ}JATE :nUT 1 i-I:: ___ •• _ .. __ .. __ ••. __ •... ·_.· •• _ •• m • • • _ _ _ • • • • • • • • • • _ • • _ • • - . - - - • • • • • - . - • • • • - - •• - - . - - - . - • • • • • • -• • • • - . - - - - . - - - • • • - . - - - . - - - . - - - . J PROCEDURE INITPIC; Cx.:~~.*~**~~~.::¥.*.~~*J CON':>T

:r.U·u.;~U[+;: .- (JEOH~ ()i( lCl..Jl (..,1...1... ~3Lf;I)E '::)F~L NON--LOC(\L, NO ICW5, )I()

I:Ci'iJ, -.. 001:1. 00l:l E: f eli NO 8L~(t)E UNITt;? SO NO :rCW:1, ICW'i AL.WAYS NEEDED.)I<;:'

:t'::;1·1:2!~iL)DI;;: -" (JE.2H; TCH2. .- OOll.:l.OOOE:: IC~HAL)DR - OE2HI I£Wq - 000011018; lCW6ADDR - OE2H~ ICH6 ... :l.111:1.111B! BE:.GIN OUT8YT(ICW1ADDR~ leW1); OUTBYT(ICW2ADDR~ ICNZ)i OUTD'{T (:rCH':HltlXm v ICI--F+;'; OUT8YT<ICW6ADDR, ICW6);

();( NO !:;I:·'l:::CI(~lL. I~'ULLY NU:)TEO t'1ODE 1 BUFFERE:.D i-lODE ~)I.; )

(::.+:: Mf-~s)TEH r NORriAL EOI, 8086 MODE. :'::0

(;¥ (iLL. LOCAL. INPUT!:; )1( )

c ... -... _-... -... _ ... _ ... -... __ .. _

... -_ ... _-_ .... _.-... -_ ... -... -. __ ... -.-.---.-.. ----.---.-.-.. --...

J

CUNST

CLIi'n HHCClUNH·iDDh:··· OI::.E.H;

SOURCE TEX"'! INIT1.SRC

EJ·!DJ

DlJrH"(T ( CON'! m·lCLlUl,rr(H)DF;: ~ COl..JTRlrlCUUNlEI;: 0 ) ~

o!...rn::YT ( TH'iEI·:: 0 U_kIO('~DDi:;; I' COt..!!'.!"!",

u:n-..!;.

~

DLrn::YT ( l:IHLROU)(.'d)!~·d)[)F: r CDU(!T, HICH;'

;-- 40

09/01/80

I:: ... -... -...••... __ ... _ ... -... _ .... _. __ ... _ .•. , •. _ .. --_.---,_.-.. --_ ... - .... -.-.. --.:...-.-.--:-.---... ---.-•• -... J

\

Pt;:OCE[)UF~E: IN:r:TI?":jL:LZ{4TJ:UNl (COUNT; PriRT) t

i t*~~K*~~.**~

••

~.K.**.**.*

••

~.*****~z.****.**.**.******]

DEG:rN

li'nTP]:C j'

Ti'!lTPJ:T(CI]UNf")?

I:: ... " ... ' .... " .... " ... -... _ ... "._ ... _... .... ... .. ... _, .... """'- •.. -""". _._ ... ""-'--."-'-'''-''--'-'-' -- -_.-.. -... :J

C)F·F·E;E~··f· _{':,:ODE SIn~.} lA'iT(:~ ~:3 ::c ::.;~

E:

:3T(.iCI< SIZE

0030H 00:1.3H :t ,?t:. OOOBH

Em

OOOOH OOlCH .~·~·"tD OOUbH 61)

0" 'l'T' • !..\., C t~1 _001UI·: }."+C,. _0OO6H <~[)

OOOOOH (1)

SOURCE rEX1: CONSOL.SRC

\:: )KY.OiOIOt::)It}!()t;:~DIOK::\C>K ::I C ;r:iO';)I,~~:*".X·:):')lo.c>nc(x{] .

MCIDULE: CON SOL. ~ I:: ::~(;!( :*: X(;t;]:f.: :1:: x(';c, )f:; :?~ )lCV :'1 [ ;1' XOICl';I: )It )!Ol\:'l; y.<: »: :*::~: ::I

r:'Uf:L.TC ':3E:CICl!:;;

PROCEDURE C~ ( C :CHAR);

FUNCTTDN CT : CHt-',H! PUBLIC N1YIN;

CONr;)T Mf"'IX5.:rnnNG ::::30 :

TYPE SlRING =PACKO) ARRAY[l,.MAXSTRINGJ OF CHAR;

PFmCEDUF:E PF::n-tTTI~~ ( TEXT t STrUNG);

F:'UE:L .. IC CONSOL;

p·mJCEDl.n::E: :rNPUT~)(\P (~)~tI::: y-!p(}~)r)UL~) : l.DNGTNT:

INTEGER) ? [: ... ".-... : ...

_

...

_

... _-

_

...

-

'"

._

...

_

..

"._.--_._

...

_-_

...

_-_

...

_---

_._

...

_

...

__

... _ .... -_ ... __ ..

__

.-

.. --. :: PF:DCEDUi::E L.TNE;: r;:!~.GIN CO(Cfn; tAl (!_F") ~ EJ,![' ;

Pf::OCEt)Uh:E 'TAE: ( LENGTH t :I:NTEGEH ) ~

!·HtF: :[ DEC:n,!

FUR I ;~ ] TO LENGTH UO CU( I ' )

END:;

. \.H:ti~: I

l:::CCJ.:N

FOE 1:: .n. 1. TO LENC~TH DO CO ( , :~: ' :;.

SOURCE TEX'r~ CUNSOL.SRL

FUNCTTU/,I REf!,DJ.:NTEGERTf:.: ( L.ENGHT ~ XNTH;ER ) t INTEG!::!:;:;

c~*~*~~~*~w**w**~**.w*.*.*~**

••

*.****.********.*******

*J

[ 1~~E{~I[) :n·!TEGEF: NUi'iE1Ef;: FF;:QM TEPf'lnli~lL., FOH THE BEGIN:I:NG I!-tCCEPTS SPACES

{iN!:) .... ', ~iF'rEi::: FH·:~;n D:naT {,CCEPT~3 ONL.Y [):rGl~TE) (tNt) <cr,:> (-iT THE END

OF l"lUHBEF;:. ,. DOE~)N I T 1~\CCt:T'T NUivil~:EI:~~) L.ARGEr;: THI.':tN MAX1:NT. IF YOU TF<Y

TU PUT Lc'tj=;:GEH NU11E::I::J;: .. ( HE. L(.I~:il GOOD i,)('%L.LJE j,HLL BE: AUSIGNED ASD t.i 1:;:E~1DED TNTEGER.

INPUT F'i:·\F;:f~ll"lE:.TE:l~~~) ~

.... i'if}.>:: ~;:EC:HJEbTE::D LENGTH UF 1~:Ef,'.jI)EJ) ~;nRIN(; V~R SIGN, NUM8ER 1: CI .. 1I::: F ::t.i-l:I!;lH I' Er,:F: m~C:IN :u,rrn;I:]:;: ~

nrn::CER;

CHf.m: E:DDL.E()H i' F:V,!]SH i c' I: {~U~;E:: F NUi'l!:::Er ! ~~, O:l ~nGN : c', :l. t 1: ><l; F;:EPEtYI PEeE{1T CHF: :. :;;. CJ ~

jT' 1'''Ui''1BI:~F: 0.': 0 THI::N DEC1J·!

CI:,\~::;t:~ CHi:: Df

_.

, _, t::n.:t~l'! .-. .L A{~3JGN :-E.j,!L· ~ • tEI:;:F: i Fi~,LSE! UTHI;:.~':I,.!],~·)E E::I:;:C::U··! END; IF CHR IN [ '0' . , ' ? ' J THEN BEGIN E!~:!:':"~ F (~L ~:)E: ;

NUMt:::EH 1:" (1I:;~[) ( CHF: ) --

arm (

I 0' );

END ELSE E~~

:=

l'RUE~ EHI) ; [ CAb!?. :1

END t'::LSE ElEGIN

IF CHR IN [ '0' •• '9' J THEN BEGlN

J

',J::F 1,!UNE:EP

>

«( Iltj~)X:J]'!T --

(mn (

CHF: ) +

mm ('

0 I

»

DIV 1. 0

"('I-lEi'! E:ECl.H

I:: Ttl Ene !'RJ~It::E:r;::1

EF:F{ '. "" I'\·;:UI::: i EJ..j[) EL':3E:. E:l:::C:U..J

NUM8ER

:=

1U* NUMBER + ORD (CHR) - ORO ('0');

Ei41) El...f:.I.:, [:1::: (,.;: U,!

TF U .. \P :. CH n'IEi-!

Dcc:n:

EJ1J.: ; ,;;; (·~L.~::,L· ..

SOURC~ TE~T: CONSOL.SkC

Ei'!D l

END;

u;·,n:n.. I,!UT EJ·:H;

:IF CHf::

<>

Ct;~ HICN m::Gn·~

CO .: Cl-!i~: ); T : :., 1:+1;

END; [ELSE DON'T Dl~PLAY CHAHAC1ER J

UNr:U.. F:IN:tBH ClP ( :r>I..ENGHT ) t

END; CREADINTEGERfRJ

c· .. · .... · .... ·· .... · .. ·· .. · -... -... _

... _

... _

... _ ... - ... _ .... _

.... - ... --.-.. --.... - ... -... -.. ]

PF:iJCEDUF:E :n-..!PU·p.)tIH < ,)AF: wr':'O':lPULS 1 LDNGINT;

I)(.:II~: ,J r dD !' 0 ~ L.Z !' L3 : INn'::GEI~:);

[**********************.*******.* ••• **.***************.*.J

VAR WAN'fPOSI1ION, WP : IN1EGER; E:EG1J,1

LINI::, ; L.]]·~E : T?'-lBh(70);

'j' l~iE:';:) ("? (I ) ~ L.:IHE t L.l:NE ;.

Ti2_{iB ( 1. '3') ;} F'F::II'-!TTH (' _{'i\/H' i'1r)TC)R CfJNTh:OL PI:WGI:;':f~t-i +,''i'') ~}

Ti~iE:': 7.~O :. t LIHE; L:I:NE? TI~B~; (:; 0) i

Ei~1) ~

'f r~IU': :i. U :. i· p!:nj,rrTl~: ( • HFU:Tk: i'RJI"!E:L},: Ui~' ',UF·:NEi ~,U~N'1 EO , ' ) ; LINE; LINE;

TAB(9)~ F'RINTfR(' WAN1POSITION ');

'fA8(Zli PRlN1TR(' WP L:rHE! TA8(S)i PRlNTTR(' WP IN ROUND TAB(10);F'RINTTR('WP ~ HF·' ~ ;':. [:;:EJ)D:IJ-ffLGER! I~: ( (; ) i TAB(3); PRINTI'R('ROUNDS PI.::li,nTl:.: (' ,...\ :: PRJ.:;·,rT"n~ (' JD:::· PRI:NTTh: (' D ;:; p~(INTm.:' L2:;: PI,(!:j'.rr Tl:;:.: I 1-.:]:'. WANTF'OSITION:~ WP; L..:IN!::. ;; TI'iE:': (?:. i PRX:Nl·n~:.:· F:E(~l\)'''(

wr' m.iPULS : ~"WI·:·:¥.::l2 I.) 000 , L.U!E

');LINEt ·)~L.lNE;LINEj:

,

) ; • ) f ')F J != REA~INTEGERTR(6); , ) 'i "JD t ~= 1::!E:f'lOl:i"!TI::GERrR (6)

r

, :. i D :::;: Fi:E?,OINTEGERTF( (6) ; I ) t LZ t:::: F~EADIN"EGER l'R (6) ; , ); L.:3:;:;· fi:EADIt-ITEGERTR (6) ; I > ;

SUMMARY INFORMATIUN ₄₄

-Di="F'~;)ET CODE. S)::t..E Dt!:iTI'':j Sl.ZE: sn('~CI{ SIZE

O:tDOH 02011"1 ~;l::m 002BH .qOD

OOOOH OOl:tH :ZOD O()()fJH 81)

OO:t.':fH () 02i~I!-1 if~~D _OOOEH l.(m

o n:;~EH 002(:)1"1 _{420 OOOEI-I 14D}

006BH 01"::H:n'l :::12Br:' 00141-1 2UI)

00 :i.DflH ::5~)(} D

03f.::.LH S),,+ :~:j[)

o o

_{1 Ek:'H ::l90D 00611H 960}

J,

SOURCE TEXT: MAIN.SRC [ )K }IOIOf.:.K )10K )lCAOIC,!( ]

I:: )()lOK}y')f;)IO'!.)«}f.~::~' ::I

MODULE MAIN:; [ ',K){{)!CIOI()t;;'KlIOIC-K>:< :)

r:

)K)(OK::K)f.·'IOr.:.tOK)!()f J pum .. l:C ~:)BCI:m:;:·

L FILE MAIN.SRC J

(: F·UNCT :nJi'-! : j';i{dJ-I i'lUDUI...L 11Cnm;~ Cot-rnmL J

;:'i~:UCEUUr<L CD .-: L 4 CH(.-lh: ,_.

PUBL,LC INIT:I.;:

TYPE 1"'f:",F(f .... RECORD LASL 8001...~AN OF

THUE ~ (FLlLl..JKlf;·~D ~ HOfm );

FALSL: (LOW, HIGH: 0 •• 255 );

Ei'.!D;

PI~:(JCEl)UF~E Ti4:n ~[AL.IZAT:U:il":l. (CDUN'(: 1',(~I;(i);;

PUBLTC Cm .. !{30L ~

PI:mCEDUh:E INPl..rPh~,r;: 0: t}f!,I": ~·!PO~:)PLJl...b: LOI-ICU·n; PHOCEDUF:E \...Ti-,/E;

PUE::L.TL dr;~lh!;

Cm'!~::l r t'i1~XETRn'lG

TYPE ~;TRr:NC

VAR J,~ID,D,L2,L3 1 INTEGER)~

· .. :30 ;

;···e{~U<E]) r:.)I:;:!~:i~ YL )" ., i'j(..lX~;:;TI<IHG] OF CHf!~r,: t

PROCEDURE PRINTfR(TEXT;STRING); I:: ... -... _ ... _ .. _ .... --.-~-.-... _ ... _.-.- ... _ ... J !·:'fWG1;:f:\t-! \"\(.:·I:th!; 1:: ... _ ... -... _ ... _ ... - ... __ ... _ ... -... _._ ... - ... _ ... _.:1 CON'::;T IN·l·Eh:F:uF··rf~l::.lbl< T~:)(.~r'lPL.E f~ 9 F'F;:U:;I~l r Pf;:E::\H~ INT r:t;':E(JPD;~; ~ F'\·;.:ESPO:;:) CDUi'-n EJ;.:()f"LUE:. eF~C::;L. '1 PF:Et.)L.

... OI:~~:~H i r l)DC+:E::;b m· THE :n-rn.JmUP'l' MP,m< J

- 13 CMILLI SECOND]

; :n·!'iEGEr;:; L .... ::l:Ll {:,7

<""

:r:NlEGEI~: <::~ +~~27 67 ]

t E:UDL.E:r:\tN ;

; l~:OULl2.I~d··! ;;

: F·' i~·t! '(T t

46

-i~ ~ "~i::!+:I.

CJUTl:::·rr.; UEOH~ O:i.10()OOOU) [ Lcn. :]

ENDJ ::: ... ..-... -. ... _ ... __ ... _ ... " ... ::J ~i:.ri·rn::J·:I:~:jFT ;: CCW::F:CH,·! > PRUCEDUR~ BORRUW; E:ECJ:N I:~I ! ;: .. (:, ... :L :' OUT8YT( OEOH~ 011000018 E:i·!1) ; [ LUI :I t: ... _ ... -... -... _._ ... -... _ ... - ... -.... - ... -.. -.--- .... - ... --.-... - ... - ,--,---,-",,,,,., :t ':i':r:i·-rn:>:PUPT (~;)I~i"it:'LET:rNE:'

r

'I-:OLU)tJF~E ~)()i"IF'L[r:U-\E! CDUNTADORL UAZH? CUUi-.!T/::·d)C+:H ... Ut'l'';!'!; !::·L.r~ .• ll··l 1':'~;:E5i':I: ~:l~l;' pr··:L.lh·D~:):;;;: ;::-!":ESF'U~::,;

:n·!Er-{T ': C:Cll..Ji'H·?·',DDF:i·1 y Pl:;:F.:SI:·O'.:).; I-D:GH i ,

~:Ct· .. ~!::;"(·T~: C:·[~lJt\l~r~·,:·!I)[)f~L. ~ i::'i~:E:~:)r'[Jf) y L.()!·4,! >::

OU1'SYT( OEON ~ 011000108 ): ~::i~D ;

r: ... -... _ ... -... _ ... _ .. -... -... -... -... -...

~

... --.-.... -.... - ... --.-... --...

-~. J

POSITION

:=

PRESA-65536 + PRESPUS.FULLWURDi

.i ; .,' ~ .. ' UEC'Ir,l DISA8LEIN1~RkUP1S; 1··iPr;:ESi:-, ~ PFt::~:)(:) i 1·/1"' kEt\oI(:, : :;;.; PI~:I-.:.lh~) I HPRESPOS

:=

PRESPOS~ Hr-r,:L'"IPDb ~ :;:; j:'f;;E')F'O\3 I lNABLEINTERRUPTS;

Ph:I:;~'::;:L; :~; HPF:L.b(i-":.f,:55::k) + HPF:!:::SPDb i

r

UL.I..J.jCJl~:D ~

!·:'h:E:).)!...; ;', I'H::'m~~){1!;\(")'5:::;::j6 + j··H·>F~E(,IPClE:., FUL.U·KlHD;

')LUX::C'C( ~;;; (Fh:l::m .. ""PI·;:[:'..JL::' [<LV Tbi~\f/iFL.E:::;

E.ND:

c ... ,' ... "" ... __ .... -_ ... _ ... _. __ .... _._._ ... _

.. _ .... _ .. _ .... ]

PROCEDURE SENDDAC;

CDi'I~:n r;:;'

::J:::

i

.. rr ,;. ;;:: .

.L . "

[ TI"IUbE 1.J!.',LUE:S H()I')E TO 1:::[ LH('~NC;F ACCORD1:NG TO THE HOTO!;:]

[ n"\(.:,T lS co:n .. !C r'o I:::E U~:;ED ~ R 1:SrHE RES1:STANCE~ DPn:DI:'::c.:]

[: !::Y 'f1·:L >'iCJf Uh: CUl'!~:)TtjHTE (i'li"'Vt=:\) ]

.: \,.q:'Uhi:\.!L.'i .... j:'!:;:E::;;L:·;: I"JEL.DC:LTY· ;

(: ut-! HI..H"'iH·li::' I .. , '",tOLl'"G;E C(.:,L.CUl..f:t fI:::L' J,-.U: I'H ''CHE: HODEL ECiUATIlJN OF THE SYGTEM :I

l)':'IC;\H·\LUE ; :.: .; (t.H',! D:tl) (1 B/ DI.I) 10 () ;;) ... : L.~~iKX2 + L::i)l(X~:I»;

[: Di~\C~)('~l...UE :L'J bEi',!T TO THE Df~'C J:NCLUDINC lit'! AND THE CONTROL FEEDE:ACI-( :I

:rF I.)f:\CIJ(~LUL >:~, ?FFH THLN D(.·IUJf.',L.LJE::;;; 7FFH t

:IT" DACV(.:IL.UE <>: .~.;: 7FFH+:I.) '"i"I-Il::N Df~)C\"Jf1l...UE: ;~; .. - ( 7FFH+ 1. ) t END~

r: ... ' ... , .. , ... ' ... -... , ... -... _ ... _

... -. ... :]

[ F'~IN1·S STRING ON THEfERMINAL IGNORING SPACES FROM l'HE RIGHT. J

E: E: G .I i"!

LL.i'-!CI·n ~ ;.; i'if'IX~~;'1 FG:Nb ~

\ .. !HTI...E: ( TEXT I:: L.Ei..JCHT :I ;.:. ,

.

o )

DO BEG]],!

L.Ei'·!C:rn ',:; l...E:r'·~CiH'! ... t;·

END; I:: WHl1 .. E j

[ . . . -•••••••••••••••• ~ . . . • . . . _ . . . _ . . . m . . . _ _ . _ . . . - . . . _ . _ . . . . _ - . _ _ . . . _ . _ - .... _ . _ . . . - J

p!:ml.~EDuh:E INTEGt::RHHHRI:NG ( NUMBE:R t L.ClNG:I:i'!T t LEN: INTEGER; VAf< TEXT: STRING);

[ NUt-"iBER IS THE L.ONGI:NT TO 1:;:1:: Cm,p,)ERTEO ~

L.EN :1:'::' TI·II::. L.ENCHT 01·:' lHL TEXT STR£NG EXPECTED:3

VAR I ;INTEGER!

N ~ L.ONC:INT ~

E:Ec:n·!

FOR I :~ 1 TO MAXSTRING DO TEX1'[IJ :~ , I;

IF NUVIBEI~:<O THE:N DEeD·!

TEXT[ :l. ::I t ;.: ' ... ' ;.

N :~:: .... NUMBl:]~~;

Ei.JD EU3E E:EGIN

TEXTt: 1:1 : :::: ' • t N : ;;:: NUI1t:EH,

END,:'

IF N~O lHEN TEXTC2J ~~ '0' ELSE BEGIN

:r

i. ;::: L.EN ~

t:.HD~ Ei·'·!() ~

WHILE (N)O) AND (1)0) DO BEGIN

TEXTr.: :r:::I t:::; CHf~ (C)l~:.[) ( I 0 ' :. + (I'! HOD :I. () ) ) t

N ~ :c: 1,1 D:r\) 1 () ~

T ~.. 1: .. -:1. t D,!O; L \-!!··ITLE. ::I

.fF 1<··:: 1 THEN FUF~ I ~:;:::t TU LEh' DO TEX

rt:

I J t:::: ';4< I ;

I.: • __ ... _ ... _ ... _ •.. -._.-... _ ... - .... -... -.---... -.-... - •• --.. -... - - -... -.. -.-.----... --.-.-.-... --... --•. -:J

PI·mCE:[)UPE IFI<EYP\:;':ESS;

[ WAIT FOR INTERRUPT KEY PRESS J

TEXll, TEX1~, TEX13, TEXT4 : STRING;

:tt,!TEGERrm:nl:;::r.NC.: < PO~:;;:C{ TON

PFn:NT TH ( I POS1:T:[(lH .1:8

PRINTTR( TEXTl ); PRINT1~(' PULSES

1.:!. , Tt:::::<Tl);

::U'{l ECEJ~:'fD~nRIJ'H:~': lJi::.L.Dc:ny:, :!..1.

PRINTTR('; VELOCITY l~ PI~l:i'·!TTf~ (lE>::T;~) i

pr;::n~rrF: ':. • r:'uu:;c::) / hl1.LLSO ::Oi'·lDE'::~

!._~1:N[ ;:

:r:HTEGEtno~:; rrn:NG (UN \' :1..!. , lE::{TJ;';;

.

) ;

I ) ;

• ) t

I " _J

9

C TI-I1[-) PHOC;EDUI:..:E [)l~:;l'Li::'l

C ON THE TERMINAL;

C THE F-'05:irn:ON