A111D3 DflTbEl

E & TECHNOLOGY:

m mm

AN ARCHITECTURE

FOR A ROBOT

HIERARCHICAL

CONTROL SYSTEM

NBS Special Publication 500-23

U.S. DEPARTMENT OF COMMERCE

National Bureau of Standards

i
NATIONAL BUREAU OF STANDARDS

The National Bureau of Standards^ was established by an act of Congress March 3, 1901. The Bureau's overall goal is to strengthen and advance the Nation's science and technology and facilitate their effective application for public benefit. To this^ end, the Bureau conducts research and provides: (1) a basis for the Nation's physical measurement system, (2) scientific and

technological services for industry and government, (3) a technical basis for equity in trade, and (4) technical services to pro-i mote public safety. The Bureau consists of the Institute for Basic Standards, the Institute for Materials Research, the Institute for Applied Technology, the Institute for Computer Sciences and Technology, the Office for Information Programs, and the Office of Experimental Technology Incentives Program.

THE INSTITUTE FOR BASIC STANDARDS provides the central basis within the United States of a complete and consist- ent system of physical measurement; coordinates that system with measurement systems of other nations; and furnishes essential services leading to accurate and uniform physical measurements throughout the Nation's scientific community, industry, and commerce. The Institute consists of the Office of Measurement Services, and the following center and divisions: Applied Mathematics — Electricity — Mechanics — Heat — Optical Physics — Center for Radiation Research — Laboratory Astrophysics ° — Cryogenics^ — Electromagnetics'^ — Time and Frequency

THE INSTITUTE FOR MATERIALS RESEARCH conducts materials research leading to improved methods of measure-, ment, standards, and data on the properties of well-characterized materials needed by industry, commerce, educational insti- tutions, and Government; provides advisory and research services to other Government agencies; and develops, produces, and

distributes standard reference materials. The Institute consists of the Office of Standard Reference Materials, the Office of Air' and Water Measurement, and the following divisions:

Analytical Chemistry — Polymers — Metallurgy — Inorganic Materials — Reactor Radiation — Physical Chemistry.

THE INSTITUTE FOR APPLIED TECHNOLOGY provides technical services developing and promoting the use of avail- able technology; cooperates with public and private organizations in developing technological standards, codes, and test methods; and provides technical advice services, and information to Government agencies and the public. The Institute consists of

the following divisions and centers: I Standards Application and Analysis — Electronic Technology — Center for Consumer Product Technology: Product; Systems Analysis; Product Engineering — Center for Building Technology: Structures, Materials, and Safety; Building

Environment; Technical Evaluation and Application — Center for Fire Research: Fire Science; Fire Safety Engineering.

THE INSTITUTE FOR COMPUTER SCIENCES AND TECHNOLOGY conducts research and provides technical services designed to aid Government agencies in improving cost effectiveness in the conduct of their programs through the selection,! acquisition, and effective utilization of automatic data processing equipment; and serves as the principal focus wthin the executive branch for the development of Federal standards for automatic data processing equipment, techniques, and computer

languages. The Institute consist of the following divisions: Computer Services — Systems and Software — Computer Systems Engineering — Information Technology.

THE OFFICE OF EXPERIMENTAL TECHNOLOGY INCENTIVES PROGRAM seeks to affect public policy and process

to facilitate technological change in the private sector by examining and experimenting with Government policies and practices in order to identify and remove Government-related barriers and to correct inherent market imperfections that impede

the innovation process. THE OFFICE FOR INFORMATION PROGRAMS promotes optimum dissemination and accessibihty of scientific informa-i

tion generated within NBS; promotes the development of the National Standard Reference Data System and a system of in- formation analysis centers dealing with the broader aspects of the National Measurement System; provides appropriate services

to ensure that the NBS staff has optimum accessibility to the scientific information of the world. The Office consists of the following organizational units:

Office of Standard Reference Data — Office of Information Activities — Office of Technical Publications — Library —

Office of International Standards — Office of International Relations.

' Headquarters and Laboratories at Gaithersburg, Maryland, unless otherwise noted; maihng address Washington, D.C. 20234.

' Located at Boulder, Colorado 80302.

i
COMPUTER SCIENCE & TECHNOLOGY:

An Architecture for a Robot Hierarchical Control System

U.S. DEPARTMENT OF COMMERCE, Juanita M. Kreps, Secretary

Dr. Sidney Harman, Under Secretary

Jordan J. Baruch, Assistant Secretary for Science and Technology

NATIONAL BUREAU OF STANDARDS, Ernest Ambler, Acting Director

Issued December 1977

Anthony J. Barbera

Institute for Computer Sciences and Technology

National Bureau of Standards

Washington, D.C. 20234
Reports on Computer Science and Technology

The National Bureau of Standards has a special responsibility within the Federal

Government for computer science and technology activities. The programs of the NBS Institute for Computer Sciences and Technology are designed to provide ADP

standards, guidelines, and technical advisory services to improve the effectiveness of

computer utilization in the Federal sector, and to perform appropriate research and

development efforts as foundation for such activities and programs. This publication

series will report these NBS efforts to the Federal computer community as well as to interested specialists in the academic and private sectors. Those wishing to receive

notices of publications in this series should complete and return the form at the end

of this publication.

National Bureau of Standards Special Publication 500-23

Nat. Bur. Stand. (U.S.), Spec. Publ. 500-23, 227 pages (Dec. 1977) CODEN: XNBSAV

Library of Congress Cataloging in Publication Data

Barbara, Anthony J. An architecture for a robot hierarchical control system. (Computer science & technology) (NBS special publication ; 500-23)

Supt. of Docs, no.: CI 3. 10:500-23

I. Robots, Industrial. I. Title. II. Series. III. Series: United

States. National Bureau of Standards. Special publication ; 500-23. QC100.U57 no. 500-23 [T59.4] 602Ms 629.8'92 77-17960

U.S. GOVERNMENT PRINTING OFFICE

WASHINGTON: D.C.

For sale by the Superintendent of Documents, U.S. Government Printing Office, Washington, D.C. 20402

Price $4.25 - Stock No. 003-003-01874-1
TABLE OF CONTENTS

I. CONTROL SYSTEM ARCHITECTURE

1. INTRODUCTION 1-3

1.1 Industrial Robot Control Systems 1-4

1.2 Problem 1-4

1.3 Solution 1-5

1.4 Additional Control System Requirements 1-5

1.5 Research Control Systems 1-6

2. THE NBS ROBOT CONTROL SYSTEM 1-7

2.1 The Three Modules Of NBS Control System 1-7

2.1.1 Hierarchical Control Module (Module #1) 1-7

2.1.1-1 First Level Of The Control Hierarchy 1-9

2.1.1-2 Second Level Of The Control Hierarchy 1-15

2.1.1-3 Third Level Of The Control Hierarchy 1-19

2.1.2 Program Module (Module #2) 1-21

2.1.3 Location Table Module (Module #3) 1-21

3. THE NEXT LEVELS OF CONTROL 1-24

3.1 Fourth Level Control 1-24

3.2 Fifth Level Control 1-25

4. MODULAR DESIGN 1-28

5. NBS CONTROL SYSTEM PHILOSOPHY 1-29

6. SUMMARY 1-30

II. USER'S GUIDE

1. EXECUTING APPLICATIONS PROGRAMS (Module #1 - Control

Hierarchy) II-3

1.1 Control Hierarchy - Operator Interactions 1^-3

1.2 Initializing Program II-3

1.3 Executing Program

1.4 Interrupting Execution

1.5 Continuing After Interrupt II-6

2. ENTERING APPLICATION PROGRAMS (Module #2 - Program

Module) 11-10

2.1 Program Module - Operator Interactions 11-10

2.2 Indexed Location Name Entry 11-10

2.3 Editor Commands 11-11

iii
2.3.1 Table Of Editor Commands 11-11

2.3.2 Elemental Move Functions Il-n

2.3.3 INSERT and LOOP Commands 11-11

2.3.4 PRINT Command 11-12

2.3.5 AVOID Command 11-13

2.3.6 DELETE Command 11-13

2.3.7 RECORD Command 11-14

2.4 New Functions 11-14

2.5 Exit Program Module 11-15

3. ENTERING LOCATION POINTS (Module #3 - Location

Module) 11-17

3.1 Table Of Operator Interactions II-l?

3.2 Mode Of Entry 11-17

3.3 Array Entry If-17

3.4 Approach Path 11-19

3.5 Exit Location Module 11-21

III. CONTROL SYSTEM IMPLEMENTATION

1. CONTROL SYSTEM OPERATION III-3

1.1 Control Hierarchy (Module #1) III-3

1.2 Program Module (Module #2) III-5

1.3 Location Module (Module #3)

2. IMPLEMENTING A NEW FUNCTION III-ll

2.1 Subroutine Design III-ll

2.2 Control Hierarchy Modification III-ll

2.3 Program Module Modification III-15

3. PORTABILITY III-16

3.1 Input Channels III-16

3.1.1 Function Switch Panel III-16

3.1.2 Joystick Control Box III-16

3.1.3 Joint Position Indicator Input III-16

3.2 Output Channels III-16

3.2.1 Joint Position Command Outputs III-20

3.2.2 Brake Control Output III- 20

3.2.3 Interlock Channels III-20

4. ADVANTAGES OF HIERARCHICAL CONTROL SYSTEM III-21

REFERENCES III-24

iv
APPENDIX A

Interface Design ^"3

Comments

FIGURES

Figure # Descri ption

1 3-Level Hierarchy 1-8

2 6-Axis Manipulator I-IO

3 Transfer Task I-ll

4 Insertion Task 1-12

5 Proximity Sensors 1-13

6 Schematic of 1st Level Control 1-14

7 Chart of Transfer Times I-I6

8 Chart of Insertion Times I-17

9 Straight Line Coordinated Motion 1-18

10 3 Modules of Control System 1-20

11 5-Level Hierarchy I-23

12 Information Processing in Hierarchy 1-26

13 Array of Location Points 11-18

14 Approach Path Concept 11-20

15 Block Diagram of Control Hierarchy

16 Program Table Line III-6

17 Block Diagram of Program Module III-7

18 Block Diagram of Location Module III-9

19 Location Table Line III-IO

20 Program Modifications for New Function iii-]2

TABLES

Table 1 - Control Hierarchy - Operator Interactions II-4

Table 2 - Program Module - Operator Interactions II-7

Table 3 - Editor Commands II-8

Table 4 - Elemental Move Functions II-9

Table 5 - Location Module - Operator Interactions II-I6

Table 6 - Non-Portable Functions III-I7

Table 7 - Listing of Input Channels III-18

Table 8 - Summary of Control Systems Concepts and III-22

Advantages

V
PROGRAM DOCUMENTATION

Control Hierarchy

(Module #1)

EXPRO - Executive program to oversee entire

control system.

RDMOD ~ Reads in Program Table and Location

Table for disc.

RECORD - Stores Program Table and Location Table

on disc.

SAMPLE _ Allows operator to call up Program

or Location module.

3rd Level of Control Hierarchy

EMOVE

2nd Level of Control Hierarchy

Calls appropriate primitives for each

elemental move command.

1st Level of Control Hierarchy

SERVO - Outputs joint position commands through

ARMOUT.

ARMOUT - Output driver that sends output values

to interface.

ARMIN - Input driver that stores input values in a buffer.

DI-2

DI-8

DI-10

DI-12

DI-16

STLINE - Generates a straight line motion. DI-24

WAIT - Suspends execution and waits for an input signal

.

DI-30

CALD - Calculates the distance between two

location points. DI-32

POL - Calculates the delta joint values for an interpolated trajectory. DI-36

ACC - Executes interpolated trajectory using

values from POL. DI-40

DETECT -- Uses proximity sensors to detect presence

of object. DI-A4

BAL - Uses proximity sensors to center hand over

object. DI-48

GRASP - Causes fingers to close with a defined force. DI-52

RELEAS -- Causes fingers to open completely. DI-56

PTOUCH -- Uses proximity sensors to locate top of

an object. DI-58

COOR - Transforms external coordinates into joint

coordinates and vice versa. DI-62

DI-66

DI-70

DI-71

vi
Program Module

(Module #2)

PROGS - Reads in editor commands and calls

appropriate subroutine.

Editor Commands

INSERT - Causes elemental move commands to be entered

in program table.

PRINT - Causes the program table to be printed out

as elemental move commands.

DELETE - Deletes specified lines from program table.

AVOID - Creates avoidance paths by inserting additional

location points.

LOOP - Creates the additional lines in an indexed

repeat pattern.

Additional Support Subroutines

INDEX - Codes indexed location names into their

location table pointers.

LOCPT - Codes non-indexed location names into their

location table pointers.

LINE - Codes elemental move command into a line in

program table.

PTNAME - Decodes location table pointer into its

location name. PLINE - Prints out decoded line from program table

as elemental move commands.

NEXT - Advances through input character string to

next piece of information.

LIMIT - Decodes into integer from the lines specified

in the editor commands.

ADJUST - Verifies continuity in the specified motion

in the program.

Location Module

(Module #3)

LOCTAB - Requests data to completely specify a location point.

ARRAY - Requests data to specify the dimensions of

the array of points to be entered.

ARRLOC - Computes the coordinate values to specify

all of the locations in the array.

JOY - Uses input values from joystick to control

robot's motions.

POS - Calls in and scales the present joint

position values.

vii

DII-2

DII-6

DII-10

DII-14

DII-18

DII-22

DII-26

DII-30

DII-34

DII-40

DII-44

DII-48

DII-52

DII-56

DIII-2

DIII-10

DIII-14

DIII-18

DIII-24
ACKNOWLEDGEMENTS

I wish to express my thanks to Drs. James Albus and John Evans for

their invaluable help, advice and suggestions. Many of the ideas

presented in this report were a result of many hours of discussion with

them.

I wish to thank Ellen Lowenfeld for her able assistance in developing

the computer programs.

I am indebted to Debbie Ingram for the uncountable hours she spent

in the typing and preparation of this report.

viii
I. CONTROL SYSTEM ARCHITECTURE

I-l
I. CONTROL SYSTEM ARCHITECTURE

1. INTRODUCTION I_3

1.1 Industrial Robot Control Systems 1-4

1.2 Problem I_4

1.3 Solution 1-5

1.4 Additional Control System Requirements 1-5

1.5 Research Control Systems 1-6

2. THE NBS ROBOT CONTROL SYSTEM I-7

2.1 The Three Modules Of NBS Control

System I_7

2.1.1 Hierarchical Control Module (Module #1) i-y

2.1.1-1 First Level Of The Control Hierarchy 1-9

2.1.1-2 Second Level Of The Control Hierarchy 1-15

2.1.1-3 Third Level Of The Control Hierarchy 1-19

2.1.2 Program Module (Module #2) I_2i

2.1.3 Location Table Module (Module #3) 1-21

3. THE NEXT LEVELS OF CONTROL 1-24

3.1 Fourth Level Control l~2U

3.2 Fifth Level Control I_25

4. MODULAR DESIGN I_28

5. NBS CONTROL SYSTEM PHILOSOPHY I_29

6. SUMMARY I_30

1-2
AN ARCHITECTURE FOR A ROBOT

HIERARCHICAL CONTROL SYSTEM

Anthony J. Barbera

ABSTRACT

Complex automation systems, such as industrial robots, require a computerbased control system for the effective utilization of this advanced

technology. This report describes such a control system developed at

the National Bureau of Standards. The approach has been to partition

the control system into a hierarchy of different functional levels.

This has proven to be a powerful technique in obtaining sensor-controlled

robot behavior at a minimum cost of programming time and computer size.

Further, this partitioning has greatly simplified the implementation

of additional functions and sensors. This report discusses the control

system, its implementation and use, and provides a documented listing

of all of the control programs.

Keywords: Adaptive; automation; computer; control; goal-oriented;

hierarchical control; robot; sensors.

I. CONTROL SYSTEM ARCHITECTURE

1. Introduction

The National Bureau of Standards program in automation focuses NBS resources on developing a basic understanding of the technology of

computer based automation and then develops those standards and guidelines that will stimulate the diffusion of this technology to enhance

productivity in both Government and industry.

Specifically this program attempts:

1) to provide standards for the interfaces between modular

components of computer-aided manufacturing systems,

2) to provide standards for the computer control languages used

to program automation systems,

3) to provide performance measures for specification and

procurement of robots and numerically controlled machine tools,

and

4) to carry out research in dynamic measurement and computer

control for computer based automation systems.

1-3
It is work in this last area, the development of dynamic sensors and

computer control techniques that has led to the control system

architecture for robots described in this report.

1.1 Industrial Robot Control Systems

Industrial robots are proving themselves to be flexible, general

purpose automation systems that will contribute significantly to the

development of automatic factories. These industrial robots utilize

essentially the same control mechanisms that were developed for

numerically controlled (NC) machine tools. That is, the motions of an industrial robot are determined by numbers that are stored in the

memory of a computer or on a tape or some other storage device. Each

degree of freedom (each axis) of the robot has a position servo system

that drives the joints to the values commanded by the stored numbers

of the control program.

Robots have presently found use in a number of industries in such

diverse operations as removing parts from presses, spray painting,

spot welding automobile bodies and loading and unloading parts from

machine tools. All of the applications of these robots have two

characteristics in common. First, these operations involve a rather

high volume production where changes to the robots' programs do not

have to be made often. At present, industrial robots are not being

used in a batch type of environment (i.e. where parts are made in

small lots or batches and control programs must be quickly changed)

because of the time and difficulty required to program the robot to do

a new task. Second, these operations are also characterized by a highly constrained work environment, either due to the nature of the

work (e.g. unloading work pieces from a press) or by the installation

or redesign of positioning equipment (e.g. installing very accurate

indexing transfer lines to maintain repeatability in the positioning

of automobile bodies on a spot welding assembly line). Although

present industrial robots have a high degree of positional repeatability,

they cannot in any way sense slight misalignments of parts in their

environment. This lack of sensory feedback creates the requirement

that the position of the objects they are to work with must be

accurately maintained to match the position locations stored in the

robot's program.

1.2 PROBLEM

Thus the situation can be summarized as follows:

There exists a sophisticated piece of general purpose

manufacturing hardware - the industrial robot - whose effective

and widespread use is seriously hampered by the difficulty and

amount of time necessary to communicate even simple operations to

it, and by the requirement of a tightly constrained work

environment.

1-4
1.3 SOLUTION

The solution to this problem is the development of a higher level

control system that will make it faster and easier to program the

robot and that can interact with sensory data to modify the

robot's motions in real time to cope with misalignments in its

environment.

1,4 ADDITIONAL CONTROL SYSTEM REQUIREMENTS

Several additional requirements are identified here that are felt to

be important for promoting the effective use of computer controlled

robots within the environment of developing CAD/CAM systems

(Computer-Aided Design/Computer-Aided Manufacturing).

1) the control system should be as general purpose as possible

so that each type of task does not require a different unique

control system (e.g. the control system that causes the robot

to load parts in and out of a vice on a machine tool, should

also allow a robot to spot weld automobile bodies, spray

paint bath tubs, or drill holes in an aircraft wing panel).

2) The control system should be modular and partitioned to make

higher control levels independent from specific robot

designs. This allows the same higher levels of the control

system to be universally used to direct any robot. The

system should be designed to facilitate the addition of new modules at any level. This will enhance flexibility in new

appl i cations

.

3) The control system should allow for the creation of robot

application programs off-line. These programs should be

independent of an individual robot in much the same way that

the part program created by an APT programmer is independent

of a particular machine tool. Obviously, a robot or machine

tool with three degrees of freedom cannot execute a program

requiring six degrees of freedom; however, to the extent that

robots or machine tools are functionally equivalent, the same application programs should be useable.

4) The control system should allow for the entry of location

points in the robot's work space from other data bases in a computer-aided manufacturing system, such as the CLDATA file

from APT. In addition, these location points should be

defined in a relative manner so that the same set of location

points can be post-processed to specify these locations for a particular robot. This is a specific facet of the general

requirement that the control system should be as compatible

as possible with the Integrated Computer Aided Manufacturing

concept, i.e. the use of standard interfaces between modules

of well-defined functions that can interact with common data

bases. ^ ^
1.5 Research Control Systems

A number of groups have developed control systems (1, 2, 3, 4, 5) to

solve the basic problem stated above. These higher level control

systems rely on a computer to provide the information processing

necessary for real time control of the robot in accord with its

incoming sensory feedback data. These systems have some form of a higher level language, usually resembling a general purpose computer

language like FORTRAN. This provides a communication interface to the

robot so that the programmer can specify, off-line, a complex task

which may require the use of sensory feedback. However, none of these

control systems meets all of the additional requirements stated above.

1-6
2. THE NBS ROBOT CONTROL SYSTEM

The National Bureau of Standards (NBS), with its unique view of

standards, measurement science, numerical control machining, and

computer applications, together with input from manufacturing

industries, has developed an architecture for a hierarchical control (6, 7)

system. In carrying out this work, NBS has adopted the strategy of

modular design, well-defined interfaces, and integratabil ity into total

CAD/CAM systems. The architecture of this control system, its present

implementation, and future development are the subject of this report.

2.1 The Three Modules of the NBS Control System

The hierarchical control system for the execution of complex tasks

involving sensory feedback makes up module #1 of the complete NBS control

system shown in Figure 10.

Input to the highest level in the hierarchy of module #1 comes from

module #2. In module #2 a sequence of elemental moves ("GOTO" statements)

is produced. These commands are interpreted by module #1 as an executable program. This program is a procedural description of a task, and as such can be used to instruct any module #1 control hierarchy

to control a robot to carry out this specified task

Module #3, the location module, is used to record the coordinate values

for all the location names used in the program module. These are X,

Y, Z values in a standard coordinate system. This location table is

transformed by a postprocessor in module #1 to a table of coordinate

values that define these points in a particular robot's work space.

2.1.1 Hierarchical Control Module

The control system architecture (Figure 1) is based on a hierarchical

structure, where simple primitive operations are executed at the lowest

levels. A complex task command enters the hierarchy at the highest

level and results in the generation of a sequence of simple primitive

commands to the next lower level. Each primitive, in turn, produces a sequence of a joint position commands to the lowest level to

accomplish the task. The partitioning of the control system into this

hierarchy of control levels does much to simplify the control problem

and to allow complex-looking behavior to be commanded simply and quickly

using a small computing system. Each control level uses incoming data

(both higher level commands as well as sensory feedback) to branch to

the appropriate subroutine calls of the next lower control level.

Additional primitive operations or the implementation of another type

of sensor require only the addition of a new subroutine that will

accomplish this function.

1-7
iGO TO POS 12 GRASP PROXIMITY

\

\

\

\

3RD

LEVEL

CONTROL

GRASP

HIERARCHICAL

CONTROL

STRATEGY

2ND

LEVEL

CONTROL

i

PROXIMITY AND FORCE

SENSOR FEEDBACK

'^JOINT POSITION COMMANDS

\

1ST

LEVEL

CONTROL

JOINT POSITION S

INDICATOR FEEDBACK^

DRIVE SIGNALS

V TO JOINT ACTUATORS

Figure 1

This chart shows the three levels in the control hierarchy. Each level

receives commands from the next higher level and responds by generating

ordered sequences of simpler commands to the next lower level. Sensory

feedback is used to close control loops where appropriate.

The elemental move command GOTO POSITION 12 GRASP PROXIMITY causes the

third level to generate a sequence of primitive commands (INTERPOLATE TO

POSITION 12; MOVE X, Y, Z; SEARCH (with proximity sensors); BALANCE (with

proximity sensors); GRASP). As a result of these primitive function

commands interacting with the sensory feedback, the second level generates

the correct sequence of joint position commands to the first level. The

first level servos the joints to these positions by generating the necessary drive signals to the actuators.

1-8
The test bed for evaluating this control system has been a six axis

research manipulator (Figure 2) which is controlled by one of the

minicomputers in the Institute for Computer Sciences and Technology's

Experimental Computer Facility at NBS.

Two types of manipulative tasks were designed to evaluate the

performance of the different programming techniques at each level in

the hierarchy.

The first task is a simple transfer operation (Figure 3). It requires

the arm to move to a particular location, pick up an object, and place

it at another defined location.

The second task is an insertion operation (Figure 4). A peg is moved

to a hole and inserted. The diameter of the peg is 1.3 cm. The

diameter of the hole is 1.5 cm. The insertion part of this task

requires the hand to follow a straight line motion for a distance of

about 7.6 cm.

These two tasks are meant to simulate the type of transfer and simple

assembly operations performed by industrial robots.

Infrared proximity sensors (Figure 5) developed at NBS will be used to

demonstrate how the control system can interact with sensory feedback

data in real time. The sensors themselves are the subject of another

report.

2.1.1-1 First Level of the Control Hierarchy

The lowest level in the hierarchy (Figure 1) is where servo control

functions are computed. This is the level at which most industrial

robots in use today are programmed and controlled. The input commands

are joint positions which are compared to the feedback- from the joint

position indicators. If these values are different, a drive signal is

generated to move each joint until the position error is nulled.

The control system at this level is shown schematically in Figure 6.

During teaching or programming, a hand controlled unit is used that

allows the operator to control the motion of each individual" joint,

using rate control. When the joints of the robot are in a desired

configuration, the programmer presses a "record" button which stores

the positional value of all of the joints in the memory of the robot.

During playback, these position values are recalled from memory and

compared with the actual measured positions of the joints. If there

are any discrepancies, the servo system sends a command to the

actuator to move the joint until the error disappears.

Most of the robots being sold today have controls of this type. These

controls may be specially hardwired units, or they may be based on minicomputers or microcomputers, but the control concepts are the

same. A typical robot may be able to store 100 program steps or more. 1-9
I-IO
m <

(TUJ

U.cnz<

on co O -r-

+-> -->

CO 1 C

fO -Q -I— ^ rC +-> C CO -a O O) C CO "O ^ O) QJ

CD -a o o ^ E

00 E

03 CO

-4->

-i4J CO •!- ^ > +J

CO

O 03

03 S- CL

•4-> Q.

SO

0) +J

CO 4-> c e

rO •>-o

+-> Cl e

03 CO 03

03

s- -o

4-> (D "O

CO S-

•rO ^ U

+-> OJ

SO -1-1 T3

-a

03

OJ T3 >

O)

O

S-

•1- 03

r3a-4->

<D c

S- -1-o C Q. o

•I- 03

+-> O

O E -M

CU O)

jz: >

-M

OE

CO +j ^ E C

O)O

CO •-

OJ +-> S- fO Q. O

O)O

S- 1— > ' 00 O +-> E

SC

O

CL-r- -1- O O Q. 03 e o C O r— O -r- •r- +-) "O

-M 03C

03 £= O) C TI- +-> -a

-l-J CO c

CO <D 03 o a>

CO c

O) T- -r- x: ^ c:

-4-> +-> c

S- +J CD

OJ 03 OJ > JD o eS O) +j o -sr c -a I—

•rC

t-> o

03E E

OJ

O

x: sco -M

COu

•r- O)

-c: -1-3 I— J=> o

03Q.

s-CD

U

O +-> Q. U

CU

03 -i-O J3 O O +J

OJ

O) -C > 4-> OE 4J

o; - 00

03 r— Q-r—

03 CD e C -r-

•I- 4-

4-> M- -a oOJ T3

S-ou

OJ

$-

CU

COo

Q.

-a

OJ

CO "4-

+-> -r- c: o

•>- O) O Q.

Q. CO

I-ll
INSERTION TASK

Figure 4 - This schematic represents the motion required for an insertion task. The hand is to move to a point over the hole,

follow a straight line path into the hole, which required

recording two additional points between the beginning of the

approach path and the destination point.

1-12
Figure 5 - Proximity Sensor affixed to a robot's gripper enables a robot to locate and grasp objects that are not precisely positioned.

Infrared radiation, produced by light-emitting diodes and carried

by fiber-optic bundles, is projected downward in two beams. Radiation

reflected from the target enters a parallel set of fiber-optic bundles.

Strength of reflected radiation acts as a feedback signal that informs

robot how close gripper is to its target. Experimental system in

photograph is under development at the National Bureau of Standards.

1-13
QJ +-> 1

o•1—5

CU

peat ^ cu +J S- o

4- o o o \—

s_ 1 CJfO E +^

<L) cuO +J

(/) C/1 o

(/) o to

cu

cu -C

•1

CT>4->

03

S- cu o o >

+J -o to •1— O) s -o

t/) >1 ^ o o o +-> E -Q E

O) cu to 4J CJ) E +J

(/) c

>> •1— to o o Q.

fO o o <D "D

S- 4-> T3 CU

QJ -a

c: 03 -o s- o s- o o >4- o CJ O u cu

cu s- cu CO %- > cu

cu 03 <u O) i- OJ E 03

+-> +-> CO >, 1/1 s- +-> cu

ir- e to

<+- to

+-> o 03 c Q. a

CJ o cu o

a. to Q. cu E

+J o h- MO

fO CO

CD ^- -M o z. o

cu +J to

CO fO o S- 03 CU -E

+-> a. o o -M

s- CD

+-> zs cu *S

LO o JO. to

13 t- 4-> to -M >> •r— 1—1

ed

J3"O th 1 ve E

VD CO o O

•I

E

OJ +J

S- 4-> CU

13

O J3 scu

•r—

O

O Q.

U_ s- -M

O

1-14
Some controls allow branching from one set of program steps to

another, depending on external conditions. This feature is useful,

for example, in spot welding of automobile bodies, where the control

program must be changed to accommodate a mix of body styles on a typical automobile assembly line.

Some tasks require the path of the robot hand to follow a straight

line (such as inserting a cutter into the spindle of a machine tool).

Programming a straight line motion with the rate control box is a particularly time consuming job since most industrial robots have one

or more rotary joints that cause the hand to travel along a circular

arc instead of a straight line (Figure 9).

Thus, a simple insertion task requires the operator to continually

readjust a number of joints in order to program the hand to move along

a straight line.

The transfer and insertion tasks described above were programmed by

three different operators using a rate control box. Their times for

the first level of control are shown in Figure 7 and 8.

This first level of control does not require the use of a computer.

However, a computer is necessary to implement the second and third

levels of this hierarchical control system. These higher control

levels are required for increased capabilities in speed and ease of

programming, and for real time interaction of the robot with the

environment through the use of sensory feedback.

2.1.1-2 Second Level of the Control Hierarchy

The commands to the second level of the control system are calls to

primitive function subroutines to be executed. These low level

primitives are the basic, general purpose, operations that can be

sequenced together to accomplish more complicated tasks. They are called, one at a time, by the different input commands such as GRASP,

or RELEASE, or MOVE X, Y, Z, etc. A command call like GRASP will,

together with whatever feedback is appropriate for this primitive,

cause the second level to generate the correct sequence of joint

position outputs to the next lower level (servo level) to accomplish

this operation.

Programming at this second level is much enhanced over the first level

since coordinate transformations are now possible with the computer.

Inputs to the robot, to move it to the desired positions and

orientations to be recorded, can now be in the form of values in an external coordinate system. Thus, the arm can be commanded in terms

of X, Y, Z coordinate space through the use of a joystick. Moving the

joystick in a desired direction provides inputs into the computer that

are the delta X, Y, Z coordinate offsets from the present position.

This X, Y, Z command becomes the input to the second level. The

1-15
CO

CO<

CO

Xo

CD

a

Lf3 o

LU

00

CO

CD

^'O

CO I I

ro -p U CO O X

?HO COoE O H -P OU

Ki -P

H o c . . CD E

(1) -p (D ft cti H O ?-i CD O

-P

(DO

CD t:) 0

CD

CO

IS o

CO

CD

-P

CD %i bOpL,

CD •

Ct CO

t3OO

CD

ClO CO •H CD E Xi UU -P

bO CO O

HCD

!>CD

•H

-P o

1^

CD 0

c; CO I -P

O

•H

-P

COO

•1-3 0) _ -P

CO ;=i

£>- CD

CDU

-P

CO . ^

bO-H

CO CD

CO -P

Oo

COo

•H

T}

CD

1-16
oouuCO

CO

cc

CO<

CO

ID

GO LO

LO o o

Oh

H

O

O

-P ?H -P

ino o

-p

CO 0)

?H -P

(D CO ft f-i o

CD -P

CD :s

toOs

'^^0B

(D

CD

CD CO u

-po

COCD

CO

•H

-P

noO

CO^ .

a) ^ !> CO CO CO -P O O

0) -H M-P

c3 0 oo

CD

CO M-p

-p

CO

HCD>H ^ -P oH (D -P

•1-3 CO

CO CD --P

CD

O

H o

CO

1^

CO (D

-P

CO

?H

O

•H 4^

CD bO

•H

Ph CO 0

+:> CD

CO +3

1-17
Figure 9 - This figure illustrates the required coordination of a number of joints in order to obtain a typical straight line motion of

the hand. For each incremental move along the straight line path,

joints 2 (elevation), 3 (boom), and 5 (wrist flex) must be adjusted.

1-18
coordinate transformation routine is then called to calculate all of

the joint motions required to cause the robot's hand to move along the

commanded straight line. The operator is one level removed from the servo system and, therefore, no longer has to worry about moving the

individual joints. This is the power of a hierarchy. As higher

levels are added, the input commands become simpler and more procedure

oriented, while the sequences of the detailed operations required to

accomplish the tasks are generated by the lower levels in response to

these commands.

If the transfer and insertion tasks are programmed at the second

level, using a joystick, there is a significant reduction in programming

time (Figures 7 & 8). In addition, the programming becomes easier and

less tedious since the operator no longer has to concern himself with

what series of joint motions is required to move the arm to the new location.

In addition, the coordinate transformation routine makes it possible

for the control system to interact with sensory data. Most sensors provide information that will require the robot to move along vectors

in the sensor-based coordinate system, not in the joint coordinate

system of the robot.

The sensor generated commands for motions of the arm in terms of the

sensor's coordinate system are transformed into the proper joint

coordinate values. Thus, causing real time dynamic interaction of the

robot with its environment through sensor controlled movement.

2.1.1-3 Third Level of the Control Hierarchy

The third level in the control hierarchy receives its input commands

in the form of elemental move commands. The elemental move is a basic

unit building block in the description of a task. It is in the form

of a motion and an operation. Most, if not all tasks, can be broken

down into a sequence of these elemental move commands, where the hand

of the robot executes some trajectory through space to a destination

point and performs some function. Of course, the trajectory or the

function may default to a null value. These elemental moves are programmed by the operator in the form of "GOTO" statements. An

example of an elemental move command would be "GOTO PALLET (04),

GRASP." This command, along with any appropriate sensory data, would

generate a sequence of calls to the second level to execute the

required primitive functions.

At this third level, the operator is programming in a much more task

procedural language as opposed to the robot joint position language of

the first level. The joint positions of the robot that define the

location PALLET (04) still have to be recorded in a table of points.

However, these points can be entered under joystick control or as the

X, Y, Z coordinates of the location. Once a location is stored under

1-19
MODULE 2 MODULE 3

PROGRAMMING

MODULE

LOCATION

MODULE

CONTROL

HIERARCHY

MODULE 1

Figure 10 - The three modules of the control system in their relation

to each other and the robot.

1-20
some arbitrary name (like PALLET (04)), it can be used in any number

of elemental move statements. Of course, the stored locations can be

programmed in any sequence, not just the order in which they were

entered.

Recording and programming the transfer and insertion tasks at the third

level shows a further reduction in programming time (Figure 7 & 8).

The programming is even easier for the operator to accomplish since he

is giving elemental move assignments which describe the task in much

the same way as it would be described by one person to another. Large

reductions in recording and programming time occur when arrays of

locations are used as will be explained later in the report.

2.1.2 Program Module

The application program is entered in the program module (module #2)

with the control system editor. The application program takes the form

of a sequence of GOTO statements using arbitrarily named locations

such as PALLET (04), NEUTRAL, VISE, etc. Each statement (elemental

move) may also include a list of desired functions such as GRASP,

INSERT, etc. These elemental move commands might be entered through a computer terminal by the operator, by an APT part programmer at che same time that he is writing the part programs for a NC tool, or through a function button programming box on the shop floor.

Thus, the program module produces a procedural description of the tasks

to be performed in terms which are as independent as possible of the

particular robot that may perform it.

^ 2.1.3 Location Table Module

' The values in the location table provide a relative description of the

positions of all of the location points. These values can be

j postprocessed to specify the location points in the coordinate space

of any particular industrial robot that is sufficiently flexible to

carry out the task (i.e., in most cases, a six-axis robot). The

concept is to allow these relative locations to be entered by the

jj programmer as X, Y, Z coordinate locations at the same time he is creating

I the program module, or to be processed from some data base such as a

I CLDATA file of an APT program. The CLDATA file (the X, Y, Z and

surface normal angle data) has a certain level of machine independence

and can be postprocessed to provide the location points for a particular

robot as well as for a particular machine tool. Another source of

these location points is from a particular robot trained in its own workspace. These specific location points can be converted into

standard coordinates for use by another robot through an inverse

i postprocessor.

Thus, an attempt has been made to separate the description of the task

as much as possible from the particular robot that might carry out its

operation.

1-21
The control system interfaces to the particular robot through that

robot's own coordinate transformation subroutine. The coordinate

transformation routine can be used with a post processor to generate

the robot- specific location table from a robot-independent location

table. It is also used during execution of the program for real time

transformation between external or sensor-based coordinate systems an the robot's joint coordinate system.

1-22
\ INPUT \coTMnds

SYSTEM CONTROL

HIERARCHICAL CONTROL

Level 5 Control

FUNCTION

. controls a system of work stations (robots/tools)

. integrates with data bases and management information systems

in higher level computers

INPUT

. system task command - (e.g. 'bake a differential")

. feedback from work station - {e.g. "lathe tf3 is broken")

OUTPUT

. assign tasks to work stations - e.g. "station #6- mill flanges on differential coverplate"

"station #2- turn input shaft" "station cut gear If 3b"

.

etc.

WORK STATION

CONTROL

Level 4 Control

. controls a single work station (robot/tool)

. monitors sensors and handles certain error conditions

by branching to corrective procedur s.

INPUT

. work station task command - {e.g. "mill flanges")

. feedback from the work station - (e.g. "end mill broken")

OUTPUT

. sequence of elemental moves to accomplish task

e.g. "GOTO MACHINE REMOVE" "GOTO TRASH RELEASE" "GOTO SPARE MILL (11) GRASP PROXIMITY"

etc.

ELEMENTAL

MOVE CONTROL

Level 3 Control

FUNCTION

. specifies trajectory segments and primitive operations

. uses sensory feedback for branching to primitives to cope with environment

JNPUT

. elemental move corunand - (e.g. "GOTO MACHINE REMOVE")

. sensory data - (e.g. "sensors not balanced on end mill")

OUTPUT

. the sequence of primitive commands to accomplish the elemental move

e.g. "APPROACH"

"DETECT" "BALANCE" "GRASP"

etc.

PRIMITIVE

FUNCTION CONTROL

level 2 Control

. generates trajectory segments and executes primitives

. modifies trajectory on basis of sensory feedback

INPUT

. primitive command - (e.g. "BALANCE")

. sensory data - (for the "BALANCE" primitive the feedback is voltage levels from proximity sensors)

OUTPUT

. the sequence of coordinated joint position commands to execute the

primi ti ve . - (for the "BALANCE" primitive - these cornnands would cause the robot to move in the correct direction to equalize the proximity

sensor feedback signals).

SERVO CONTROL

FEEDBACK

Level 1 Control

. controls position and velocity of individual actuator

INPUT

. the commanded joint position value

. position and velocity feedback

OUTPUT

. the proper drive signals to the actuator

Figure 11 - The 5 Level Control Hierarchy

1-23
3. THE NEXT LEVELS OF CONTROL

Ongoing work at the Bureau includes the addition of the fourth and

fifth levels of the control hierarchy (Figure 11), and the completion

of the modular design, its standard interfaces and the postprocessors

to provide the general purpose control system described above.

The next sections will discuss the basic conceptual functions of the

fourth and fifth levels of control and an overall view of the complete

control system to be integrated into the computer-aided manufacturing

operation.

3.1 Fourth Level Control

This level of control takes care of the complete operation of a robot

in its associated work station. Its input is in the form of a task to

be completed, such as "SPOT WELD A CAR BODY" or "CUT 50 GEARS OF TYPE

#36" etc. Sensory feedback data comes not only from the robot's sensors,

but also from sensors throughout the work station. These work station

sensors provide the additional feedback required to allow the robot to

cope with all of the error conditions that are within its capability

to correct. This reduces the need for external supervision and

intervention to a minimum.

The input task command, together with the sensory feedback from the

robot and the workstation, result in the fourth level sending out sequence

of elemental move commands to the third level. Different prerecorded

sequences of elemental move commands can be decided upon as a result

of the particular input task and sensory feedback.

As an example, consider the task to make a particular gear from a pallet

of gear blanks. To accomplish this task command, the fourth level

will send the sequence of elemental moves to the third level to cause

the robot to load and unload the parts from the pallet to the vise on

a machine tool table, and to change cutters in the machine tool for

the different cutting operations.

This sequence of elemental moves will be the main execution program.

In addition, a number of sequences of elemental moves can be programmed

and named to be used as subroutines. These will be sent to the third

level when certain conditions arise. For example, suppose one of the

cutters breaks while in the machine tool. A sensor on the tool or on the robot can report this data back to the fourth level. This

condition will cause a branch to a preprogrammed sequence of elemental

moves. This sequence will command the robot to remove the broken cutter

from the tool and replace it with a new cutter. The program then

returns control to the proper point in the execution program.

Thus, the fourth level control system has the responsibility of

accomplishing tasks for an entire work station. This level receives

1-24
sufficient sensory data to cope with problems that might hinder the

execution of that task, and outputs the preprogrammed sequences of

elemental moves to correct these situations.

3.2 Fifth Level Control

The fifth level of control has the responsibility of accomplishing a project that might involve assigning a number of tasks to a number of

different work stations; or scheduling a number of tasks to the same work station. Its feedback might consist of one of its fourth level

control stations reporting back that a task has been completed, or that a machine tool is inoperative. This fifth level would respond by

issuing a new task to the particular work station or rerouting

materials to another work station and assigning it the task that the

disabled station could no longer accomplish.

The fifth level interfaces to higher level computer-aided

manufacturing and management information systems' data bases to provide

them with information (processed feedback) about the work being done

on the factory floor, maintenance and repair situations, productivity,

efficiency of machine utilization, etc. Figure 12 gives an illustration

of the processing of information both up and down the hierarchy.

These five levels of control carry out the responsibility of the

actual manufacturing or production in a factory. They should integrate

into total CAD/CAM systems as well as have the capability of a stand

alone system at any level depending on the resources of the manufacturing

unit involved.

1-25
INPUT COMMAND

PREDEFINED

FUNCTION

SENSOR INPUT

raw sensor data

preprocessed sensor data

sensor data processed in

relationship to input command

(from lower levels) (b)

Figure 12 (a) & (b)

Each level in the hierarchy has information flowing into and out of it. The

output being a function of the inputs (a).

Each level's sensory input is composed of three types (b). There is raw sensor

data such as the voltage levels from proximity sensors indicating relative reflected

intensity.

There is preprocessed sensor data such as might come from a vision system where

sophisticated data manipulation and pattern recognition is performed by some sensor

unit and its output is a sensor input to the control system.

The other sensor feedback is information from the lower levels of the hierarchy

that is a reporting of their effectiveness in completing their input task.

This takes the form of an interpretation of their sensor data in light of the

input command. These are additional outputs from each level, not as commands

to the next lower level, but as inputs to the higher levels. As with the other

outputs they are also predetermined functions.

1-26
"extra hour required to complete

set of differentials"

reroute job to work station #2

"milling station #3 cannot finish j

"end mill cutter broken"

"no object at location X Y"

proximity sensor signal level

sensor feedback

Figure 12 (c) - Thus, each level not only generates information in a descending path

to the lower levels but also processes and reports information back up the hierarchy.

At each higher level, this reporting takes on a more and more sophisticated form such

that the fifth level may report to a management information system that an extra hour

will be required to produce a set of differentials while the original raw sensor data

to the second level might have been a proximity sensor signal level.

1-27
4. MODULAR DESIGN

Modular design offers several very important advantages. It provides

a partitioning of the system into identifiable units, each of which is

comprehensible and lends itself more easily to a solution. This

modularization also aids in the separation of the control system into

those sections that are of a general nature and therefore robotindependent, and those sections that are specific to the individual

robot (e.g. the coordinate transformation routine, the servo system

etc.). This allows the creation of a general purpose, universal

control system that can run any robot if the robot-dependent modules

are supplied.

Modularization encourages the use of standard interfaces which aids in

the set up of the functional organization of the system and simplifies

the information links between the modules themselves, and between

these modules and the other components of the CAD/CAM system. It clearly

defines where the responsibility for each function lies, which

simplifies the writing and debugging of code.

The concept of modularization has been extended to the level of

partitioning the control system module, the program module, and the

location table module into a number of simple subroutines. In the

control system, each primitive operation (like GRASP) is a separate

subroutine which is called when the appropriate condition occurs. Therefore, once the control system architecture is supplied, the

addition of new primitive functions is merely the addition of the new subroutines. Interaction with sensors is handled in the same way.

A sensor subroutine is written and added into the system. Whenever

the system is commanded to interact with the sensor's feedback, a call

to its subroutine is made. This usually includes calls to the coordinate

transformation routine to provide the necessary joint position commands

to cause motion of the robot in the sensor's coordinate system. In

this way, sensors of any type can be implemented and the control system

made to interact with their feedback by the addition of simple subroutines

This makes the control system flexible and easily modifiable for new applications.

1-28
5. NBS CONTROL SYSTEM PHILOSOPHY

This control system has been designed as a completely deterministic

hierarchy of input - output patterns. The hierarchical architecture

has provided a separation of responsibilities into different function

levels. Each level becomes a group of preprogrammed function generators

that respond to a limited set of input states with a defined set of

output states.

If a response to a certain set of input conditions is not programmed,

the control system can neither decide on the proper output nor "learn"

the correct response. All of the intelligence to cope with the

environment must come from human intervention in the form of preprogrammed functions. The location points and procedure must be specified

for each task. A human operator must construct the appropriate subroutines to recognize and respond to the input patterns.

However, within this set of defined input states, this deterministic

system exhibits goal directed, adaptive behavior, responding to sensory

feedback to modify the robot's motions in real time. In this way, the

assigned task is accomplished in spite of perturbations in the

environment.

This use of a hierarchical system of simple deterministic functions

has demonstrated large increases in capabilities as each new level is

added. At each higher level, input patterns that correspond to more

complex task assignments and more sophisticated processed sensory

data, are used to generate sequences of simpler commands to the next

lower level. The degree of "intelligent" behavior that can eventually

be exhibited by a control system based on this concept is an important

area of continuing research and development.

1-29
6. sunmRY

The partitioning of the control system Into simple modules and the

interaction of these modules in a hierarchical fashion has proven to

be a powerful technique in obtaining sophisticated sensor-controlled

robot behavior at a minimum cost of programming time and computer

size. This architecture also provides a flexible framework for the

incorporation of additional functions and sensors.

1-30
USER'S GUIDE

II-l
USER'S GUIDE

1. EXECUTING APPLICATIONS PROGRAMS (Module #1 - Control Hierarchy)

1.1 Control Hierarchy - Operator Interactio

1.2 Initializing Program

1.3 Executing Program

1.4 Interrupting Execution

1.5 Continuing After Interrupt

2. ENTERING APPLICATION PROGRAMS (Module #2 - Program Module)

2.1 Program Module - Operator Interactions

2.2 Indexed Location Name Entry

2.3 Editor Commands

2.3.1 Table Of Editor Commands

2.3.2 Elemental Move Functions

2.3.3 INSERT and LOOP Commands

2.3.4 PRINT Command

2.3.5 AVOID Command

2.3.6 DELETE Command

2.3.7 RECORD Command

2.4 New Functions

2.5 Exit Program Module

3. ENTERING LOCATION POINTS (Module #3 - Location Module)

3.1 Table Of Operator Interactions

3.2 Mode Of Entry

3.3 Array Entry

3.4 Approach Path

3.5 Exit Location Module

II2
II. USER'S GUIDE

1. EXECUTING APPLICATIONS PROGRAMS Ulodule #1 - Control Hierarchy)

The compiling and linking of subroutines into an executable load

module are the only operator interface to the host computer's operating

system. All other functions are maintained internally to the control

system programs.

Real time control over the execution of the different control system

modules is accomplished by a set of function switches. These switches

are polled at periodic intervals (approximately every 20 milliseconds).

Branches to different subroutine calls are made on the basis of these

switch values. Thus, the operator can interact with the control system

in real time. He can, for example, stop the arm in the middle of a trajectory modify the application program, and/or change a location

point using either the joystick control or entering the X, Y, Z coordinate values, and then continue the execution of the new application

program either at the point it was interrupted or at any other designated

step in the program.

1.1 Control Hierarchy - Operator Interactions

See Table 1 for a listing of the panel switches and their interactions

with the control system.

1.2 Initializing Program

Once execution begins (i.e. execution of the compiled and linked subroutines on the user's computer) the control system initially reads in

from the storage unit the assigned application program and location

table and prints the following messages on the terminal: —-SW 28 UP--- INTERRUPTS RUN PROGRAM —-SW 29 UP--- TEACH LOCATION TABLE —SW 30 UP—- ENTER PROGRAM SEQUENCE —SW 31 UP—- RECORD LOCATION TABLE ON DISC

*** TYPE [CR] *** TO CONTINUE

This identifies the functions of switches 28 thru 31. If an existing

application program and location table is available, then, by typing

in a carriage return, this application program will be initiated.

After the entry of a carriage return the next message is sent to the

terminal

:

—SW 35 DOWN-— AND

*** TYPE [CR] *** TO INITIALIZE THE SERVOS WITH THE STARTING LOCATION

This reminds the operator to be sure that switch 35 is in the down

position. This switch will start the execution of the program. When

II-3
Table 1

Control Hierarchy - Operator Interactions

Input Channel

Number Function

26 Velocity Control

28 Interrupt (abort)

29 Enter Location

30 Edit Program

31 Store on disc

Description

Voltage readings from a potentiometer used to control

relative velocity of arm by

specifying the number of interpolation points to be used.

When set, immediately stops

execution and provides the

operator with the option of

editing program or location

table or recording them on disc

When set, this switch calls up

the location module to allow

entering of points in the

location table. When reset, causes an exit from location

module.

When set, this switch calls up

the program module to allow

editing of program table. When

reset, causes an exit from program module.

When set, causes the location

table to be stored on disc.

34 Pause Temporarily suspends all

execution so long as it is set.

When reset, execution continues

35 Start, Repeat After initialization procedure,

this switch is set to begin

execution. The program will

continue to repeat as long as this swtich remains set. If

reset, it stops the program

after execution of last line

of program table.

II-4
"able 1 cont.

Input Channel

Number

36

Function

Straight

Velocity

Line

Descri ption

Voltage readings from a potentiometer used to control the

velocity of the straight line

portions of the trajectories.

11-5
the carriage return is typed in, the program sends the joint position

values of the starting location to the servo system.

TURN ON ShRVOS

---SW 35 UP— TO EXECUTE PROGRAM. —-SW 35 REMAINS UP— FOR REPEATED EXECUTION.

'

1.3 Executing Program

The servo system can now be turned on since a known set of joint

position values has been conmanded. The program cycles here, testing

switch 35 until this switch is flipped up. When this switch is in the

up position, the application program is executed. Switch 35 will not

be polled again until after the last executable line of the application

program. If the switch is still up at this time, the application

program is repeated. If it is down, the control system cycles here,

continuously polling switch 35.

Once switch 35 is flipped to the up position, the application program

is executed without any further messages being printed on the terminal

except error conditions.

1.4 Interrupting Execution

At any time, the interrupt (abort) switch (switch 28) can be set.

This causes an immediate halt in the executing program, stopping the arm at its present position. The following message is printed on the

terminal

:

YOU ARE AT PROGRAM STEP 11

11 GOTO B0X(03) GRASP PROXIMITY VEL(50) SEND(O) WAIT(O) —SW 29 UP— TEACH LOCATION TABLE —SW 30 UP— ENTER PROGRAM SEQUENCE —-SW 31 UP—RECORD ON DISC

*** TYPE [CR] *** TO CONTINUE

In this instance, the control system was executing line 11 of the

application program when interrupted. The elemental move statement

(GOTO BOX (03) GRASP PROXIMITY) that is in line number 11 is also

printed out.

1.5 Continuing After Interrupt

After the desired changes have been made, a carriage return is entered

and the system responds with:

WHICH PROGRAM STEP DO YOU WANT?

The operator can respond with any program line number, and execution

will begin at that point in the program. If a zero or carriage return

is entered, the initialization phase is carried out and the system

awaits switch 35 to be set to begin executing the program.

II-6
Table 2

Program Module - Operator Interactions

Input Channel

Number

28

Function

Interrupt (abort)

Description

When set immediately

stops execution of the

program and allows the

program module to be

cal 1 ed.

30 Edit Program When set, calls up the

program modul e to

al low editing of the

program table. When

reset, causes an exit

from the program module.

II-7
Table 3

Editor Commands

Command Symbol

insert (XX)*

Print (XX-YY)

Delete (XX-YY)

Start NNN

L^oop

Finish

Name

Avoid XX ^1^2" ''^10 "^"^

Function

Specifies the program table line number

XX where new line(s) are to be inserted.

Specifies the first (XX) and last (YY)

line of the section of the program table

to be printed. Once print command given,

a minus sign will cause previous line

to be printed, a carriage return will cause the next line to be printed.

Specifies the first (XX) and last (YY)

line of the section of the program table

to be deleted. This section will be

printed out as it is deleted.

Once the insert command has been called,

this command enters the name (NNN) of

the starting location in the program.

Specifies that all of the following

elemental move inputs between this command

and the command FINISH are to be repeated

the number of times required by the

indexed values of the location names used.

Causes the termination of the LOOP sequence

Causes the names of the indexed locations

to be printed out on terminal with the

option to change them.

Causes the insertion of up to 10 (I^^q)

additional intermediate location points

between the specified location points

XX and YY. Used to create special

trajectories using detailed path segments.

* All editor commands default to just their first letter for faster

editing.

II-8
Table 4

Elemental Move Functions

Command Symbol

Goto XX(YY-ZZ)*

G^rasp

Release

Grasp Proximity

Release P^roximity

l^nstack

Touch

Detect

Bal ance

Une

Send (XX)

Wait (XX)

* All elemental move commands

faster entry.

Function

Specifies a destination point (XX)

for this trajectory. If it is an indexed location, the value of the

particular index (YY) is specified. If

it is used in a loop, then the range

of indexed val ues (YY-ZZ) is specified.

A grasp command - the fingers close on

an object with a predefined amount of

force.

A release command - the fingers open

compl etely

.

Center hand over object using the

proximity sensors, then pick it up.

Locate an object with proximity sensors

and place the held object on top &f~4jt^

Locate top of a stack with the proximity

sensors, then pick up the top object.

Locate the top of an object of undefined height using proximity sensors.

Locate an object using the proximity

sensors

.

Center hand over an object using the

proximity sensors.

Move to the destination point through

a straight line trajectory rather

than an interpolated trajectory.

Cause channel number XX to output a +5 vol tage 1 evel

.

Suspend execution until channel number

XX receives +5 voltage signal.

default to just their first letter for

II-9
2. ENTERING APPLICATION PROGRAMS jHodule #2 - Program Module)

If a new program is to be entered or the present one modified, then

switch 30 is set and a carriage return typed.

2.1 Program Module - Operator Interactions

See Table #2 for a summary of the operator interactions through the

switch panel

.

2.2 Indexed Location Name Entry

When the editor is called, the names that have been assigned indexed

values are printed out on the terminal.

THESE ARE THE CURRENT INDEXED LOCATIONS

BOX STACK

DO YOU WANT TO ENTER NEW INDEXED NAMES?

If the answer is yes, then the program responds with:

ENTER INDEXED NAMES

(E.G. PALLET CUTTER STACK)

As an example, consider a pallet with an array of parts to be machined

by a milling tool. The program to be entered should cause the robot

to load and unload these parts from the vise, insert the correct end

mill cutter in the tool at the beginning and remove it at the end, and

air brush off the chips after each operation. For this task, PALLET

will be set up with indexed locations corresponding to the array of

parts on the pallet. Therefore, in answer to the request for indexed

names made above, the operator enters the name PALLET:

% PALLET*

The system responds with the message:

EDITOR IS NOW AVAILABLE FOR PROGRAM ENTRY

*A11 operator entered commands will be preceded by a percent sign (%).

11-10
2.3 Editor Commands

The editor that has been written for this control system is line

oriented and has commands to insert, delete or print specified line

numbers

.

2.3.1 Table of Editor Commands

See Table #3 for summary of all of the editor commands.

2.3.2 Elemental I^ove Functions

See Table #4 for a description of all of the presently implemented

function commands for writing an application program.

2.3.3 INSERT and LOOP Commands

To enter the application program that will accomplish the machining

task described above, the following commands are typed in:

% INSERT 01

% START NEUTRAL

% GOTO ENDMILL GRASP

% GOTO TOOL RELEASE

% LOOP

% GOTO PALLET (01 - 06) GRASP PROX

% GOTO VISE RELEASE

% GOTO NEUTRAL SEND (03) WAIT (13)

% GOTO AIRHOSE GRASP

% GOTO CHIP VELOCITY (10)

% GOTO AIRHOSE RELEASE

% GOTO VISE GRASP

% GOTO PALLET (01 - 06) RELEASE

% FINISH

% GOTO TOOL GRASP

% GOTO ENDMILL RELEASE

% GOTO NEUTRAL SEND (04)

The above program, which is 14 statements long, will cause the robot

to execute the following actions - start at some initial location named

"neutral" - move from this location to the location of the end mill

cutter - pick up the end mill cutter and insert it into the spindle of

the milling tool - move over to the first work piece on the pallet,

locate it with proximity sensors, pick it up and put it in the vise - go back to its neutral safe position -send a voltage level out on channel three to cause the machine tool to begin cutting - wait until

the interlock signal from the machine tool on channel 13 goes high,

indicating that the machining operation is finished - go to the

location of the air hose - pick it up and blow off the chips by taking

the air hose to a position named "chip" which is above the vise - replace the air hose - go to the vise and remove the machined part -

II-ll
return the part to the pallet - locate and pick up the next part - repeat all of the above procedures for all six parts - then remove the

end mill cutter from the tool and replace in the tool rack - go back

to the neutral position and send out a signal on output channel four

indicating that the job is finished.

The advantage of indexing names is obvious here, where the task has to

be described only one time for one part. The "loop" command will cause

all of the statements between it and the "finish" command to be repeated,

for each of the six pallet positions.

2.3.4 PRINT Command

The edit program immediately generates the complete sequence of elemental move commands for all of the indexed positions. Thus, if a print command

is given to display lines one thru 20, the following would occur:

% PRINT 01-20

START N*

01 GOTO E GRASP VEL(.50) SEND .0) WAIT :o)

02 GOTO T RELEASE VEL([50) SEND!:o) WAIT :o)

03 GOTO PALLET (01) GRASP PROX VEL(;50) SEND :o) WAIT .0)

04 GOTO V RELEASE VEL(:50) SENDI.0) WAIT :o)

05 GOTO N VEL(.50) SEND!.03) WAIT .13)

06 GOTO A GRASP VEL([50) SEND :o) WAIT :o)

07 GOTO C VEL(:io) SENDI:o) WAIT :o)

08 GOTO A RELEASE VEL([50) SEND :o) WAITI:o)

09 GOTO V GRASP VEL([50) SENDI:o) WAIT :o)

10 GOTO PALLET(Ol) RELEASE VEL('50) SENDI:o) WAIT :o)

11 GOTO PALLET 02 GRASP PROX VEL( 50) SENDI:o) WAITI.0)

12 GOTO V RELEASE VEL( 50) SENDI.0) WAITI:o)

13 GOTO N VEL(.50) SENDI.03) WAITI.13)

14 GOTO A GRASP VEL( 50) SENDI:o) WAITI:o)

15 GOTO C VEL( 10) SENDI:o) WAITI:o)

16 GOTO A RELEASE VEL('50) SENDI:o) WAITI 0)

17 GOlO V GRASP VEL( 50) SENDI:o) WAITI:o)

18 GOTO PALLET(02) RELEASE VEL( 50) SENDI 0) WAITI'0)

19 GOTO PALLET(03) GRASP PROX VEL( 50 SENDI 0) WAITI.0)

20 GOTO V RELEASE VEL(50) SEND(O) WAITI 0)

As can be seen, the program has been expended to its full sequence of

elemental moves. The channel numbers for the interlocking commands

"SEND" and "WAIT" are listed for each program line. A velocity value

is also listed for each line. This is the maximum velocity that the

hand is to reach for that particular move. If it is not specified

during programming then a default value of 50 cm/sec is entered in the

program sequence.

*A11 location names other than indexed names, default to their first letter

11-12
2.3.5 AVOID Command

The program entered above is a minimum description of the task to be

completed. There might, however, be peculiarities about the work

environment that will prevent the trajectories from being executed as described. For example, suppose there is a building support column

between the "NEUTRAL" position and the "VISE" position. To prevent

the arm from colliding with the column, the following entry can be

made:

% AVOID VISE POINT NEUTRAL

From the single entry of this one command, the entire application

program will be scanned for all instances where the trajectory to be

executed is between the vise and the neutral position. At every one

of these points in the program, the additional elemental move - "GOTO

POINT" - will be inserted. Thus, if a print out of lines three to seven is called for:

% PRINT 03 - 07

03 GOTO PALLET (01)

04 GOTO V

05 GOTO P 06 GOTO N 07 GOTO A

2.3.6 DELETE Command

Once a program is written it can easily be modified by the "DELETE"

and "INSERT" editor commands. As an example, suppose this pallet of

parts was to go to a work station to have a number of holes drilled in

the parts. The only difference in the description of the task is that

now a drill bit instead of an end mill cutter is to be inserted into

the machine tool

.

To accomplish this program change, line one will be deleted and a new

line will be inserted. This new line will require the robot to go to

a position named "DRILL" to pick up the drill bit to be used for this

machining operation. To delete line one, the following command is

given:

% DELETE 01

01 GOTO E GRASP VEH50) SEND(O) WAIT(O)

When a delete command is given, the system deletes the specified line(s)

and prints it (them) out on the terminal. The remainder of the program

is closed up around the deleted line so that a print out of lines one

to three would look like this:

GRASP PROX VEL(50) SEND(O) WAIT(O)

RELEASE VEL(50) SEND(O) WAIT(O)

VEL(50) SEND(O) WAIT(O)

VEL(50) SEND(03) WAIT(13)

GRASP VEL 50 SEND 0) WAIT 0)

11-13
% PRINT 01-03

START NEUTRAL

01 GOTO T RELEASE VEL(50) SEND(O) WAIT(O)

02 GOTO PALLET(Ol) GRASP PROX VEL(50) SEND(O) WAIT(O)

03 GOTO V RELEASE VEL(50) SEND(O) WAIT(O)

To enter a new line, the "INSERT" command is given:

% INSERT 01

START NEUTRAL

01 GOTO T RELEASE VEL(50) SEND(O) WAIT(O)

The "INSERT" command causes the line presently at the specified line

number to be printed out. Any new line(s) inserted here will be placed

in the program in front of the displayed line. The new line can now

be entered:

% GOTO DRILL GRASP

If a print out of lines one to three is called for:

% PRINT 01 - 03

START NEUTRAL

01 GOTO D GRASP VEL(50) SEND(O) WAIT(O)

02 GOTO T RELEASE VEL(50) SEND(O) WAIT(O)

03 GOTO PALLET(Ol) GRASP PROX VEL150) SEND(O) WAIT(O)

The new line has been inserted and the other program lines moved back

to accommodate it. This change is also to be made at the end of the

program where the drill bit is removed from the machine tool and

returned to its holder.

Thus, by two "DELETE" and "INSERT" commands, the program has been

modified to cause the next machining operation to be accomplished on this pallet of parts.

2.3.7 RECORD Command

Once the application program is written, it can be stored on disc for

future use. The editor command "RECORD" accomplishes this:

% RECORD

THE PROGRAM HAS BEEN STORED ON DISC

2.4 New Functions

If there are additional functions that are required, for example, a special insertion technique is required to place a cutter into a spindle, then these special motions and perhaps the use of touch or force sensing to adjust the positioning of the cutter as it is

11-14
inserted into the spindle, can be defined in a new FORTRAN subroutine

or combination of existing subroutines. Then this subroutine(s) can

be specified by a single function call like INSERT so that a command

to insert a cutter into a tool might be programmed as:

% GOTO TOOL INSERT

The addition of these extra functions and their programming code words

is a simple modification with this control system and will be

explained in some detail in the next chapter.

2.5 Exit Program Module

To exit the program module, switch 30 is flipped down and a carriage

return is typed in. The control system responds with the following

list of options: —SW 29 UP—- TEACH LOCATION TABLE —SW 30 UP—- ENTER PROGRAM SEQUENCE —SW 31 UP—- RECORD LOCATION TABLE ON DISC *** TYPE [CR] *** TO CONTINUE

The operator can now set switch 29 and type a carriage return to call

up the location table module with its associated routines.

11-15
Table 5

Location Module - Operator Interactions

Input Channel

Number Function Description

28 Interrupt (abort) Stops execution of program so location module can be called

29 Enter Location Causes location module to be

called up so location points

can be entered. Resetting this

switch causes an exit from the

location module.

31 Coordinate Printout

32 Joystick Control

34 X, Y, Z Entry

While under joystick control,

this switch causes X, Y, Z coordinate values of the present

location of the arm to be

printed out. Resetting switch

returns control of joystick.

Causes the signals from the

joystick box to control the

motions of the arm. Resetting

this switch causes the present

location of the arm to be

stored in the location table.

Allows location point to be

entered as an X, Y, Z coordinate

position.

II-16
3. ENTERING LOCATION POINTS (Mule #3 - Location Module)

Once the program is written, it only remains that those location names referenced in the program have their corresponding position values

stored in the location table. This section explains how the user is

to enter these location points and what data the system will request

to completely describe these points.

3«1 Location Module - Table of Operator Interactions

See Table #5 for a summary of the operator's interactions through the

function switch panel.

3.2 Mode of Entry

As in the programming module, the names of the indexed locations are printed on the terminal and the option to change them is given.

The system then prints out the following message to indicate which

panel switches are to be used for the different modes of location

point entry: —SW 32 UP— FOR JOYSTICK CONTROL —-SW 34 UP— FOR XYZ ENTRY —SW 32 & 34 DOWN— FOR MANUAL ENTRY *** TYPE +1 *** TO RECORD AN ARRAY OF INDEXED LOCATIONS

*** TYPE [CR] *** TO RECORD LOCATIONS ONE AT A TIME

3.3 Array Entry

To record the array of location points for the six positions on the

pallet in the previous example and to use the joystick to maneuver the arm to the proper positions, requires panel switch 32 to be set and

a +1 to be typed on the terminal.

The system responds with a series of requests. The operator's responses

will exactly specify the array of locations to be recorded:

ENTER LOCATION NAME AND INDEX NUMBERS

E.G. PALLET (01-20)

The array to be specified is the six positions on the pallet as shown

in Figure 13a, so the following is entered:

% PALLET (01-06)

ENTER ARRAY DIMENSIONS

(E.G. 4,5)

The array of parts on the pallet, as shown in Figure 13a, is a 3 by 2

array therefore the entry is:

11-17
>-

CSC cr

cn

CM

—1

1-

——

H1 — -

CM— 

CDO o(XI o O

CDO o o

CD

>-cr

etc a:cc

cn

"O

OJ (O 'o

-t-J Sc: cn o

t_) to J_

CD fO CU "O +j c~

to +J

CO • p— to cu

O) to CD

4-> r3 03

C/) o E

s_ •r- >

^-

u -o

•r- (U

CD S- ^ S Z3 1— cu +-> E o , "1

CL

>,

s_ CD >, to

+-> 03 c

cu "O S- cu c 03

s_ cu o u CO

O

M- to 4-> fO CVJ T3 (A

03 -M o E q to '1

fO S- o CL

S- "O

O- O) E

t/) cu •1

O

CL) -M •1

-M o fC

CT) O) CJ E E Cl o O (/) a

03 CL) cu

to 4-> 5- >> o

S- ft3 t/1 M-

(C

O E to

fO -a CU

to =3

to •1

>^ o E u > S O

<u to cu

•4-> E X

fO CU s- cu c E 03 -o

s- •1

E

cu -o cu •1

+J E

+-> 03 (U

ea to to

S- +-> o •r— to 2 M- •r- cu

+-> E

CU 1—

O) l/l x: h- CO 1— 03 -a >,o

ra 1 Z3 CO %- O

t—l ora 1 O) I—1 E

S- o CU o +-> CJ)CO to

I—)

Li_ 03 o to

11-18
% 3,2

ENTRY X,Y,Z DELTAS (DECIMAL CM) BETWEEN ARRAY POSITIONS ALONG

FIRST DIMENSION.

The first dimension is along the sequential ascending index values

(i.e. along the row 01-02-03), therefore the delta X, Y, Z offsets

will be +10. cm in the X, 0.0 cm in the Y and 0.0 cm in the Z (given

that the pallet lies in an X-Y plane) and will be entered as:

% 10., 0,0

ENTER X,Y,Z DELTAS (DECIMAL CM) BETWEEN ARRAY POSITIONS ALONG

SECOND DIMENSION.

% 0,-15.,0

Control is now turned over to the joystick box to move the arm to the

correct position for PALLET(Ol). The following message is also sent

out to the terminal to provide further information.

TO RETURN TO THE TEACH LOCATION MODE FLIP SW 29 TO THE DOWN POSITION.

FLIP SW 31 UP TO DISPLAY XYZ VALUES.

At any time, while the arm is under joystick control, switch 31 can be

flipped up for a print out on the terminal of the the x,y,z coordinate

values of the present location of the hand. This information can be

used by the operator for future entry of locations in terms of their

X, Y, Z coordinate values. Once the arm has been positioned at the

location PALLET (01) using joystick control, switch 32 is flipped down.

This causes the system to record this position as PALLET(Ol).

The remaining five positions in the array are computed- from the position

of this location and the array dimension data supplied above.

! As mentioned earlier in the report, it is with the recording and

programming of arrays of locations that large savings in time can be

obtained.

3.4 Approach Path

The next request from the location module is:

ENTER DELTA X,Y,Z (DECIMAL CM) TO START OF APPROACH PATH

This data is required for the specification of the approach path to be

I used for this location. The approach path concept simplifies much of

1 the trajectory calculation. It is illustrated in Figure 14. Location

' "PALLET(Ol)" has the coordinate values X, , Y, , Z, . The direction in

11-19
o

I—o :

Q_

cc: ~ UJ o

I—

UJ

cc

Q_

(X

Q_

_J 31 cr CJO

cc •-• ot—

cc—.

Q_ a: Q-O

cc---

"O

er

1/1

+-> OJ 03Q. CO >- •r- -M o +-> X o o Q. o

CL -a

o. o

fD th cu CM ^- 03 OJ 1 o Q. th

to sz

I/) E +J s- 03 o

CI CL)

O

+-> S- C|- Q.

CT) CL ^—,

OJ '1

03M

CO o O) <] ro OJ «\ \— >- O o <] +j +-) CO CO X

-o <1

CO c: (U n3

03 CO -o > OJ o CD 13 "D

4-> 03

d) o 03 > '1—5 u E

03 cu X3 OJ

S- CO

O)O MQJ o Mf~ 03 u O

CO +-> OJ CO -l-> +J

03 o •1

o o •>-^ •1

4_> Q. -a

-o s- 4J o c o

S_ c 03 o

fO O) o

f)1

<L) 1—

4_> >-

CO _j

>) >-

CO >,

X o —103 o 4-5 X 4-> S- o

4-> a> CO c 03 -a o 03 o s- CO -M CO 03 0) 0) -o J- 1— O! o +J 03 +-> 03 > 03 1 s- O O) (D a.-i-> Q.

I—1

er na

O

OJ +-> (U

i- c: -o ^ :3 •1

S- +J

CJ> o c o >>

Ll_ 03

u

11-20
which the hand approaches this point is critical. If the object shown

is sitting on top of a table, then it would be disastrous to approach

it from below.

The operator designates the delta X, delta Y, delta Z offsets from the

location point to the start of the desired approach path (for the

example in Figure 14 delta X=0, delta Y=0 delta Z=10cm). These values

are stored in the location table. During execution, the hand moves quickly through space to the start of the approach path. It decelerates

at this point and follows a straight line path from here to the actual

location point. During this approach path maneuver, the orientation

of the hand in space is maintained.

The non-critical portion of the trajectory, from one approach path to

the other, can be executed at high speed by simple interpolation

between all the joint values without regard for close tolerances on this motion. The use of an approach path, therefore, modularizes the

trajectory into segments that can be easily executed by simple

subroutines.

The operator's response to the above request for the delta offset

values would be:

% 0,0,10. *** jYPE +1 *** IF HAND WILL STOP AT THIS LOCATION

*** TYPE [CR] *** IF HAND DOES NOT STOP HERE.

This information is requested to provide a flag for each location. If

the flag indicates that the hand is to stop at that location then an acceleration profile is created for the trajectory to this point. If

the flag indicates that the hand does not stop at that location, then

no acceleration profile is created and the trajectory passes through

that point at the velocity specified in the elemental move command.

This completes the data required to totally describe a location. The

operator is now given the opportunity of entering another location

point or displaying the location table itself on the terminal for his

inspection:

*** TYPE[CR] *** FOR ANOTHER POINT

*** TYPE +1 *** TO DISPLAY TABLE

3.5 Exit Location Module

To exit from the teach-location portion of the control system, switch

29 is flipped down and a carriage return is entered.

IIa

CONTROL SYSTEM IMPLEMENTATION

III-l
III. CONTROL SYSTEM IMPLEMENTATION

1. CONTROL SYSTEM OPERATION III-3

1.1 Control Hierarchy (Module #1) III-3

1.2 Program Module (Module #2)

1.3 Location Module (Module #3) III-8

2. IMPLEMENTING A NEW FUNCTION III-ll

2.1 Subroutine Design III-ll

2.2 Control Hierarchy Modification III-ll

2.3 Program Module Modification III-15

3. PORTABILITY III-16

3.1 Input Channels III-16

3.1.1 Function Switch Panel III-16

3.1.2 Joystick Control Box III-16

3.1.3 Joint Position Indicator Input III-16

3.2 Output Channels III-16

3.2.1 Joint Position Command Outputs III-20

3.2.2 Brake Control Output III-20

3.2.3 Interlock Channels III-20

4. ADVANTAGES OF HIERARCHICAL CONTROL SYSTEM . Ill-21

REFERENCES III-24

III-2
III. CONTROL SYSTEM IMPLEMENTATION

This chapter will concern itself with the manner in which information

is processed by the subroutines to accomplish all of the responsibilities

of the control system. Included here will also be an example of the

procedure used to incorporate a new subroutine into the control system.

The system documented here, is presently implemented on a 16 bit word

length minicomputer, running under DOS (Disc Operating System), with

32K words of core and a 1.2 million byte disk storage system. All of

the subroutines (except the I/O drivers) have been written in FORTRAN,

compiled on this computer and their object modules linked to form a single run-time load module. If a prerecorded robot application

program and table of location points are to be used, then these two

files are assigned to the load module at run time. These files are read in from the disc by the subroutine RDMOD.

The load module contains all of the control system modules - the

program module, the location module and the control system hierarchy

including all of the support subroutines.

The operator communicates with the control system through a CRT terminal,

the previously described switch panel and the joystick box.

1 . CONTROL SYSTEM OPERATION

The architecture of this control system is based on a non-intelligent,

deterministic system. Essentially, all data that is an input to the

control system (control flags, location table pointers, sensory data,

etc.) is treated as a form of a pointer (or address) to a file location,

subroutine call, or section of code. The control system has been

partitioned into different control levels which have been further

partitioned into simple general purpose subroutines. A high level

command input is in the form of pointers and flags which cause control

to branch to the appropriate sequences of subroutine calls. Incoming

sensory feedback data is typically tested against a threshold level.

The section of code to be executed is determined by whether the

sensory data is above or below the threshold value. Thus, this

hierarchical system executes real time control over the robot by testing

and branching to sequences of simple subroutines.

1.1 Control Hierarchy (Module #1)

The control hierarchy receives input commands as individual lines

(elemental moves) from the program table. Each line is an input to

the third level ("EMOVE") to generate the correct sequence of calls to

the second (primitive) level (see Figure 15 for a diagram of the subroutines in the hierarchy). The pointers to the location table address

the proper position values of the end points of the trajectories.

III-3
t CO > 13 0 Q. 0 +-> c:

113 •r— O

S- >- .0 0 0

13 +-) a.<c LJl. (/) S- 0

+j

=-1. E cu C/1 O) CL CU CU 0 >-

+-> 0 > QJ

I/) CU Z> 4- LlJ

I/) Cn4- C/1 c +-> o CU 0 E /1

1

s- 0 CU •I

4-> S- a

C 4- OJ +-> 0 X 0

1/1 ai (/) s- CD -D Z3 O) e c 0

x: <T3 03 1/) >—

+-> E cn U

E OJ CU 0 ^

0 0

0

VE -M

s- 0 0

0 0 +J 4- S- LU 0

-)-> 0 -0

dj S_ >) 03 c 03 CO >-

c: 0 eC U 0

0 1— 03 0 03 0 0 S- ID +-> _J cu

+-> r3 C 0

•1

(/I <U

CU 1 % CU ^ ^ a +-> CD

+-> +-> 0 in E 4- 0

_ M- (/) 0 > 0 s- c: s- 0 0 cu -M CU </) 0 •r— +j >

SZ (C CU CU fT3 4-> 0 CJ

03 E 0 4-> E

•4-> -0 CU -M S- 0 4-> O) 03 +-> (/)

(/) sz >>

CU 4-> +-> -t-> CO

S- 0

Q. E CU Q-r—

CU 03 Cl_ +J O

S- S- — I— 13 SCT) O +-> U O CU C

•r- S- .— . ^ O

+-> Q. :3 >^ 0 0

03 "D ^ -I— E 0 0 SZ CU E S- S s:

03 -M 0 E S- I—

CO 03 CU CU C

S- •1- > CU

CU CU cn^ CU CU X 0

1— CU S- 4-> Q. 0 T3 CU

CU S- E JD

CU +-) 0

C U +-> cn +-> 0 CU Z3

1—1 CO CO Q.

•1

Q. +J

(U 13 CU CU 0 ^ ^ 0

q; 4-> 4-> CDQ- -0 X 03 C 4-

U- UJ 0 •.- 0 03
Trajectories are modularized into 3 path segments (Figure 14), a critical path segment at each end of the trajectory (the approach paths)

and a non-critical segment that is the portion of the trajectory between

the two approach paths. The approach paths are executed at a relatively slow velocity with accurate control of the path and orientation

of the hand. This is accomplished through a call to "STLINE." The

non-critical middle section of the trajectory is executed at

relatively high velocity with simple liner interpolation between the

joint values that specify the end points of the two paths. A call to

"CALD" determines the distance between these two end points. This

value is used by "POL" and "ACC" to generate the joint position commands

to move the arm through this trajectory segment at the designated

velocity.

Sensory feedback for real time control is obtained through calls to

"ARMIN" to read in the values from the A/D converter. The subroutines

that process this information use simple algorithms to generate new position commands, in many instances requiring coordinate transformation

(a call to "COOR") to command motion in joint coordinate space in accord

with feedback in sensor coordinate space. These joint position

commands are sent to the arm by a call to "SERVO" which calls the driver

"ARMOUT" to output these values to the D/A interface which is connected

to the hardware servo system.

1.2 Program Module (Module #2)

When an elemental move command is entered by the operator, the

programming module codes these English-word-like statements into a number of pointers and flags (see Figure 16). All of the subroutines

contained in the programming module are used to generate or edit this

table of pointers and flags which will form the input commands to the

third level of the control hierarchy.

The relationships between the subroutines of the programming modules

is shown in block diagram form in Figure 17.

The detailed workings of these subroutines are covered by the next

chapter where the functional flow charts and the documented FORTRAN

code is listed.

The functional activities of the programming routines can be

summarized in the following manner. Editor commands (INSERT, DELETE,

PRINT, AVOID, RECORD, LOOP, NAME) are decoded by a test on the ASCII

value of their first letter by the subroutine PROGS. This determines

which of the editor subroutines is to be called. Other subroutines

(LIMIT, NEXT) search the operator entered commands for the key

information symbols. The subroutines "LINE" and "LOCPT" code the Englishlike elemental move commands into the proper location table pointers

and function flags. The subroutines "PLINE" and "PTNAME" decode these

pointers and flags into the English-like elemental move commands and

print them on the terminal.

III-5
i^*^ line in program table

Poi nter

for present

location

Pointer

for

destination

Velocity

indicator

in cm/sec

Interlock

channel

numbers

Optional

,

not presently

used

Function

code

number

Specific example for the elemental move coimand GOTO PALLET (03) GRASP PROX VEL0CITY(15)

SEND(03) WAIT(IO). The line in the program table would look like (assuming the present

position was neutral):

74 03 15 310* 3

The following is the corresponding function code number for

the presently implemented functions.

Function Code Number

grasp release

grasp proximity

release proximity

detect

balance

unstack

touch

line

*The output channel number is multiplied by 100 and added to the

input channel number.

Figure 16 - The program table representation of an elemental move command,

III-6
QO

cr

CO

cr

O)

c_i

Cl. oo

UJ

z:

cr

OJ

-l->

O fO

•1

OJ CL

c: o

S-

+-> Q.

Q. o fO

S- JD <D

=3to -M

OJ

O

+-> +-> to

O)

O)

O)a 34-> O) S- XJ

(/)

ac o•1

+-> to u -a

O) c:

c: 03E o E o o

s- o

OJ

+-> +->=3ClC

<D • 1

to 4- _

sO o

-l-J • (C to o -o

•I

O)

+-> Q. 03

(t5

O E

+-> E C o

O) ^ u

CO +-> OJ 0) to to Q. S- (U

OJ o

S- -M o

•r— o +->

+-> E Z5

03u E oo cu

OJX o OJ

u

to D_ o+J

cu 1—

03 -oo OJ E I—1 E »-> OJ 03 o

s- cn O -O

sLl_ Cl to

III-7
The output from the program module (in the form of a program table)

becomes the sequence of commands to the third level of the

hierarchical control system. Each line in this table corresponds to

an elemental move command and contains six positions for pointers and

flags (Figure 16).

1.3 Location Module (Module #3)

The subroutines of the location module perform the function of

generating and storing the required information for each location

point so it can be used by the control system in the execution of the

elemental move commands. A block diagram of these subroutines is

shown in Figure 18. Translation of the location name into the correct

pointer value is accomplished by "LOCPT." When arrays of locations are

to be entered, "ARRAY" requests the necessary dimensional information

and "ARRLOC" generates the required values for each location of the

array. When joystick control is to be used, "LOCTAB" makes a call to

"JOY" which reads in the present joint position values of the arm by a call to "POS." "JOY" also reads in the commanded x,y,z offsets from

the joystick by a call to the assembly language input driver, "ARMIN"

and generates the new joint position commands by a call to "COOR" to

perform the coordinate transformations. These joint position commands

are sent to the arm by a call to "SERVO" which outputs these values to

the hardware servos by a call to the assembly language output driver

"ARMOUT." In the present system, the location table can store information

on 86 different locations. Indexed location names are assigned pointer

values of one to 60 (three indexed names are allowed - each containing

twenty locations). One arbitrary name for each letter of the alphabet

is allowed and assigned pointer values 61 through 86. The last two

pointer locations (89 and 90) do not store position information. Instead,

the ASCII code for the complete names of the indexed locations reside

in these positions.

A pointer to the location table designates a group of four sequential

lines of seven positions each. The information stored in these four

lines is detailed in Figure 19. Two of the lines are particular for

the specific robot executing the application program. They contain

the joint indicator values that characterize the position of the arm

for that location point.

The other two lines contain information that describe the location in an external X, Y, Z coordinate system. This information is robotindependent in that it can be post-processed by another robot's coordinate

transformation routine into joint position values specific for that

robot. Therefore, the table of location points, stored in terms of

this external coordinate system, can reside in a central control computer

to be post processed by the specific coordinate transformation routine

of the robot that is to use this location table.

III-8
O

_J

ctr

or: cr

oroo

CJ

CDO

a_ ct: cr o

CD

CJ

>-

az

cr

QD

TRCJ

CD oo

CJ

ct: cr o

"DX

LU

CDZ

COo

Q_

CE

cc: en

CE o eu +J

l/l ra CD •1

S-

•r- CL

+-> o

S- o Ci

CL

n3

CO o

+-> CD

(/) +-> CU

-c:u

CD

d) n3 S J- -M JD

CU

ac:nD

do CO -a

+-> c: u roEEo o CJ CJ +J

OJ Z3 -M C2. c

'1

*rCU CO

-(-> o

M- -)-> O rn

CU o CLo

-M

fO CU

+->

-)-> OJ

(/) 00

CD Ss- o

Q.-l-> <U

S- c:o u E •1

+-> OQ

fc =3: E 1—

O) o

-c ou _I

to

OJ CD 1— -o ! oE

COI—1 co

CU •r— S- +J

(T3 CD CJO

LiIII-9
Pointer XX locates a set of four - 7 element-lines in the location table

(LTAB( XX, 1-4, 1-7)) to completely describe a single location point.

Line 1 JIlp J2lP ^\p J\p J5lp J6lp

Line 2 \P \P i j k STOP FLAG

Line 3 jh AX ______

AY AZ

Line 4 ^^APP ^^APP ^Vp JS^pp ^^APP

Line 1 and line 4 contain the position values of the six joints to describe the

location point and the starting point of the approach path. These two lines will

contain values unique to a particular robot's position servos.

The other 2 lines (line 2 and line 3) contain the description of the location

point in an external coordinate system system. Line 2 contains a description

of the location point in the same format as used by the CLDATA file to store

location values. These are the coordinates of (X|^, Yj^, Z|^^) the location

point and the i , j , k unit vectors to describe the direction of the handwrist axis (surface normal). This provides a 5 axis description. The 6th

axis, the direction of a vector perpendicular to the hand wrist axis and

passing through the two finger tips (the hand rotation) is described by the

unit vectors i, , j, , k, in line 3. The delta X, Y, Z distances (AX, AY, AZ)

from the locatfon Poin"? to the start of the approach path are also contained

in line 3. The stop flag, to indicate if the hand comes to a stop at this

location (used to decide on acceleration-deceleration profiles) is stored

in the seventh position of line 2.

Pointer Value

1 - 20

21 - 40

41 - 60

61 - 86

Figure 19 - The

Name Description

20 pointers allocated for the first indexed location name

20 pointers allocated for the second indexed location name

20 pointers allocated for the third indexed location name i

26 pointers allocated for 26 names each starting with

a different letter of the alphabet.

Location Table representation of a location point.

III-IO

j
2. IMPLEMENTING A NEW FUNCTION

2.1 Subroutine Design

Functions, specified by elemental move commands entered in the program

module, are executed by branches to the appropriate subroutine (or

sequence of subroutines). Therefore, a new function, such as implementing

a new operation using sensor feedback, requires the addition of the user supplied subroutine(s) to accomplish it. In addition, modifications

have to be made to two subroutines (LINE, PEINE) in the program module

and one subroutine (EMOVE) in the control hierarchy module.

The approach in developing these subroutines that are the primitive

functions has been to make them as general as possible. This allows

them to be more easily used in sequences to perform other operations

as well. To this end, it is also advisable to limit their scope to a very simple action. Again, using sequences of these simple actions to

perform more complex actions.

As an example, consider a user requirement to insert a part into a vise where the part is to be pushed against a bottom plate within the

vise to properly seat it. A subroutine to accomplish this seating

action using a force sensor in the wrist would be written by the user.

It might employ a simple algorithm such as:

(a) read in the sensor value

(b) if its above a predptermined threshold value go to (e) otherwise

continue

(c) move a defined incremental distance in the -z direction

(d) go to (a)

(e) exit subroutine.

This subroutine might be called FORCE. It performs a very limited,

simple action. This action in itself is insufficient to accomplish

the actual operation (which will be called INSERT). The primitive

(RELEAS) to cause the hand to release the part must also be called.

This releasing action could have been written into the primitive FORCE

but this would have increased the complexity of its actions and

thereby narrowed its range of application to other operations.

2.2 Control Hierarchy Modification

To implement this new subroutine requires a modification in the control

hierarchy's third level (EMOVE) to test for its function code number

in the elemental move command input and to branch to its calling

statement. Figure 16 shows that code numbers 1 through 9 are presently being used. Therefore, this INSERT operation could be assigned

number 10. The value of the function code number is used to control

two multiple "GOTO" statements in EMOVE. In this way, the function

number acts much as an address to the location of code to execute it.

Figure 20 shows how the relevant code is to be modified for this example.

III-ll
I

EMOVE before modifications (changes handwritten in)

C FUN=1+£LE(6J

GaTOC20»20,20,3#a,20,20»7,8,20)FUN

C

C IF THERE IS AM APPROACH PATH, IT IS GENtRATEO AND EXECUTED «Y

C THIS CALL TO ' STLINE ' , C20 CALL STLINE(-LTA3CELEC2),3,a),-LTA8(ELE(2),3,!5),-LTABCELE(2),3,6),l}

IF (EAeORT,LT,-2000)GOTO HO CALL WAIT(3a) ^/

C

C THIS MULTIPLE 'GOTO' STATEMENT CAUSE^THE BRANCHING TO THE APPROPRIATE

C COUt TO CARRY OUT THE FUNC T I ON/^PEC I F I ED ,^ ,0 Call Force.

C GOTQ(110,l,2,3,a,5,6,7,8,110)FUN

CC THIS IS THE CODE FOR A 'GRASP PROXIMITY' FUNCTION. THE HAND IS

( Call <?ele3seCc>

^ ^

V A Call 5-ii''<^'^C-off0)rO<rC2)^-0ff(3)^i) <_ GOTO ¥3S'

I

EMOVE after modifications

C FUN=1+ELE (6)

GOTO(?0,20,20,3,4,20,20,7,6,20,203FUN

C

C IF THERE IS AN APPROACH PATH, IT IS GENtRATED AMD EXECUTED BY

C THIS CALL TO 'STLINE '

,

C20 CALL 5TLlNEC-LTAb(ELE(2),3,a3,-LTAB(ELE(2),3,5),-LTABCELE(2),3,6),n

IF CEAbORT.LT,-2000)GOTO UC CALL i^'AlT(3a)

C

C THIS MULTIPLE 'GOTO' STATEMENT CAUSES THE BRANCHING TO THE APPROPRIATE

C CODE TO CARRY OUT THE FUNCTION SPECIFIED,

C GOTG(t\0,l,2/3,u,5,6,7,8,110,lO)FUN

10 CALL FORCE

CALL RELtASCO;

CALL ST LI NEC -OFF (1), -OFF (2), -OFF C 3 ),

n

GOTO ^435 C

C THIS IS THE CODE FOR A 'GRASP PROXIMITY" FUNCTION. THE HAND IS

Figure 20 (a) - Shows the section of code in "EMOVE" before and

after the modifications required for the addition of a new function.

In this example, the new function code number (10) plus one gives

the value of the variable "FUN" used to specify the section of code

to be executed through a multiple "GOTO" statement. The code required

to execute the "INSERT" function is, first, a call to the user written

"FORCE" subroutine to seat the part, a call to "RELEAS" to let go of the

part, a call to "STLINE" to move back the distance covered during the

"FORCE" subroutine and to branch to statement 435 to test for interlock

signal s.

III-12
^

LINE before modifications

C

CbU

C

C

C65

• TOUCH'

C

C

IF(CS(PN) ,NE,8276)G0T0 65

PR0G(PLN,6)=8

GOTO 81

'LINE'

IF ( CS (PN) ,NE, 8268) GOTO (85

PR0G(PL'^/6)39

GOTO 81

ASCLL Code fo- f<lc

PROG (P^ N, <i>) / O

Qoro 81

TEST FOR A 'VELOCITY' COMMAND, IF THERE IS ONE THEN DECODE THE

ASCII FORM OF THE SPECIFIED VELOCITY (CM/SEC) BY A

LINE after modifications

w

C 'TOUCH'

C6« IF(CS(PN) .NE,8276)G0T0 65

PR0G(PLN,ft;=8

GOTO 81

C

C 'LINE'

C65 IF(CSCPN) ,NE,8266)G0T0 66

PROGCPLN, 6)=9

GOTO 81

i ^

C 'INSERT'

' 66 IF(CS(PN) .NE.e265)G0T0 85

PHOG(PLN, 6)510

GOTO 81

C

, C TEST FOR A 'VELOCITY' COMMAND, IF THERE IS ONE THEN DECODE THE

i _C ASCII FORM OF THE SPECIFIED VELOCITY CCM/SEC) BY A

Figure 20 (b) - Shows section of code in "LINE" before and after

I modifications to code and store the new function command in program

I table. The additional code is a test for the first letter ("I") of

the new function command (INSERT). If an "I" is detected then the

appropriate function code number (here, it is a 10) is stored in the

sixth position of the present line of the program table.

III-13
1

PLINE before modifications

C 0 M M 0 N / C M D / C S ( 5 01j_PNi_^^LJi

DIMENSION FNC6,(1^3

DATA FN/6*

2 •GR' ,

' AS »

' P ,3* '

' f 3 ' R£ •

I 'LE • AS , 'E ,2*' ',

a 'GR' , 'AS " P , 'PR , 'OX' ,

'

' 5 'R£' (

' LE t

' AS , 'E , ' PR ' ,

' OX

'

6 'OE •

,

' TE 1' CT » 3* '

IF THE FIRST LIME OF THE PROGRAM MODULE IS TO BE PRINTED OUT,

PLINE after modifications

CC^^MON/C^^D/CS (50 ) , PN, PLM

D1^^ENSI0N FrJC6,U)

DATA FN/6*' ', ? 'GR 1

' AS 1 P ,3* ' ' , 3 'RE • LE 'AS , 'E ,2* ' a 'GR t •AS ' P , 'PR , 'OX • ,

1 5 'RE • LE 'AS , 'E , 'PR' ,

• OX

6 'OE ' TE 'CT ,3*' ' , 7 ' BA t 'LA /

' NC , 'E »2* ' 8 ' UN t

' ST ' AC , 'K ,2* '

' f 9 ' TO ' UC • H ,3* '

' ,

I 'LI • NE a*

'

' , 1 'IN t

' SE ' RT # 3* '

' /

IF THE FIRST LI^E OF THE PROGkAf^ MODULE IS TO BE PRINTED OUT,

Figure 20 (c) - Shows section of code in "PLINE" before and after

modifications to cause a print out of the new function name. The

array that contc^ins all of the possible function names is enlarged

by one line and the letters to be printed out are stored.

789

BA

UN

TO

LI

3*

III-U
2.3 Program Module Modification

Two subroutines (LINE, PLINE) in the program module are to be altered

so that the new function command, INSERT, can be recognized and coded

into its function number (LINE), and decoded and printed on the

terminal (PLINE) by the editor PRINT command. The modifications for

this example are illustrated in Figure 20.

s.

III-13
3. PORTABILITY

The partitioning of the entire control system into a number of modules

with well-defined interfaces to the rest of the system results in a large number of the modules being independent of both the robot they

are controlling and the computer system they are running on. Those

instances where the control system must interface to either the host

computer system or to a particular robot can be contained in a limited

number of modules. These modules have been identified and it is only

these modules that require alteration for portability of the control

system (Table 6)

.

3.1 Input Channels

As mentioned before, the operator communicates with the control system

through a terminal, a switch panel, and a joystick control box. The

values of the switches and the joystick control box are read into an input buffer through a call to the assembly language driver ARMIN.

Thus subroutine reads in 64 channels through a 14 bit A/D (Analog to

Digital) converter and stores these values in the common variable

INBUF(64)= This call takes approximately 4 milliseconds to execute

(the maximum conversion frequency of the A/D unit is 20 kilohertz).

Table 7 provides a summary of the functions of the 64 input channels.

3.1.1 Function Switch Panel

The switch panel consists of eight switches which can be set to +5.0

volts or reset to 0.0 volts. These values are read into the computer

through the D/A converter with a value -4000 corresponding to a +5.0

volts and zero corresponding to 0.0 volts.

3.1.2 Joystick Control Box

The joystick control box uses a joystick in two dimensions actuating

two potentiometers to provide the delta offsets in the X and Y direction.

The Z motion is provided by two buttons, one for up and one for down.

Push buttons also provide control of the wrist roll, wrist flex, hand

roll and opening and closing of the fingers. A detailed listing is

provided in Table 7.

3.1.3 Joint Position Indicator Input |

The joint position indicators are also read in through the A/D converter

and range from +10 to -10 volts resulting in values of -8383 to +8383

into the computer.

3.2 Output Channels

A call to the output driver ARHOUT will cause the values in the first

16 locations in the common variable 0UTBUF(64) to be sent to the interface.i

The system allows 64 output channels but only 16 are presently implemented;

III-16
Table 6

Non-Portable Functions

These are the subroutines whose functions are tailored to either the

host computer's system or the particular robot, and must be modified

to allow the control system to be implemented with a different computer

or robot.

Subroutine Name Function

RDMOD

RECORD

ARMIN

ARMOUT

COOR

Used to interface control system to

host computer's file accessing method.

Causes the previously recorded

location table and program table to be

read from host computer's mass storage

into the control system's programs.

Causes the program table and location

table to be written into the host

computer's mass storage system.

An assembly language input driver

to read the input values from the

system interface and store in a input buffer to be accessed by the

control system subroutines.

An assembly language output driver

that sends the values stored in an output buffer to the system interface.

This is the coordinate transformation

routine that allows conversion of

X, Y, Z coordinate values into the

corresponding joint coordinate

values and vice versa for a particular

robot. As provided here, only a three

axis (two shoulder rotational and a boom prismatic joint) coordinate

transformation is done, with the

remaining 3 wrist joints pantographing

the shoulder moves. This was all

that was required for the demonstration

using workpieces on a bench top.

However, the complete 6 axis coordinate

transformation routine is being

implemented and is required for complete

general purpose programming of the robot,

III-17
Table 7

Listing of the input channels read into the command variable "INBUF(64)"

by the call to ARMIN.

INPUT

CHANNEL

NUMBER

12

3

4

5

6

7

8

9

10

ir

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26*

27*

23**

29**

30**

31**

32**

33**

34**

35**

FUNCTION

INPUT

VOLTAGE

RANGE

CORRESPONDING

COMPUTER

VALUES

joint #6 position

joint #2 position

joint #3 position

joint #4 position

joint #5 position

joint #1 position

joint #7 position

trajectory velocity

control

finger grasp-release

interrupt-abort

enter location

edit program

store on disc, or display XYZ values

joystick control

pause, or enter

location with XYZ

val ues start, repeat

•10 to +10

•10 to +10

10 to +10

10 to +10

10 to +10

10 to +10

10 to +10

2.7 to 4.0

0, 4.6

0, 4.6

0, 4.6

0, 4.6

0, 4.6

0, 4.6

0, 4.6

0, 4.6

8191 to -8191

8191 to -8191

8191 to -8191

8191 to -8191

8191 to -8191

8191 to -8191

8191 to -8191

-2160 to -3160

0, -3700

0, -3700

0, -3700

0, -3700

0, -3700

0, -3700

0, -3700

0, -3700

III-18
Table 7 cont.

INPUT

CHANNEL

NUMBER FUNCTION

36

37*

38*

39*

40*

41*

42*

43

44

45

46

47

48

49

50

straight line

velocity control

wrist roll

wrist flex

hand roll

X motion

Y motion

Z motion

proximity sensor proximity sensor

not used

64

* Joystick Box Input Channels

** Switch Panel Input Channels

INPUT

VOLTAGE

RANGE

CORRESPONDING

COMPUTER

VALUES

0, 4.6

0, 4.6

0, 4.6

0,

.9

.9

4.6

to 1.9

to 1.9

0, 4.6

0, -3700

0, -3700

0, -3700

0, -3700

-730 to -1450

-730 to -1450

0, -3700

0 to -lOv

0 to -lOv

0 to 8191

0 to 8191

III-19
3.2.1 Joint Position Command Outputs

The position command output values to the six joints and the fingers

of the robot are sent on the first seven output channels. These channels

in the interface unit (see Appendix A for a detailed description of

the interface unit) are connected to a digital to analog converter to

provide analog signals to the hardware servo system. Output values

from the computer range from 0 to 32767 corresponding to the range of

+10 volts to -10 volts respectively (with 16383 corresponding to zero

volts).

3.2.2 Brake Control Output

Output channel number 8 is used as a digital output for individual

control of the seven sets of brakes (one for each joint and one for

the fingers). A fifteen bit word is sent out on channel 8 with the

first seven bits used as switch values for setting the brakes. With

all of the bits equal to one (i.e. the value of the word's 32767), the

brakes are released. To set the brake on the fingers, the seventh bit

is set to zero. This requires the number 32703 (i.e. 32767-64) to be

sent.

3.2.3 Interlock Channels

There are eight additional output channels (numbers 9 thru 16) that

can be used either as digital or analog outputs to control other

devices or send interlocking signals on. The "SEND" command presently

outputs a value(8192) on the specified channel number that results in an analog signal level of +5 volts.

III-20
4. ADVANTAGES OF HIERARCHICAL CONTROL SYSTEM

A summary listing of the advantages afforded by the hierarchical

control system architecture described in this report is given in Table

8.

III-21
Table 8

Summary of Control System

(1) The system has been modularized, resulting in :

(a) easily defineable functional specifications, interfaces, and

communications requirements;

(b) the defining and isolation of those parts of the control

system that are specific to the host computer and particular

robot;

(c) relative independence of the robot programs and their standard

coordinate location tables from the particular robot, thereby

allowing off-line programming as well as location table

generating from other data bases such as CLDATA files;

(d) a control system compatable with the addition or deletion of

extra functions, sensors etc. (accomplished with addition

or deletion of subroutines along with minor modifications to

the existing system);

(e) quick and simple control system code generation and debugging.

(2) The system has been structured as a deterministic hierarchy of

functional levels resulting in :

(a) sophisticated programming techniques using simple higher level

language commands to ease and speed program entry, where the

hierarchy provides amplification of a single command into the

large sequences of detailed steps required to accomplish it;

(b) minimal information processing and transfer within and between

levels due to the deterministic feature where only a limited

set of states are allowed; the hierarchy that provides a fast and efficient method of chosing between these states based

on simple tests of input information flags;

(c) the ability for real time sensor control of a multiaxis robot

on a small computing system; this is due to the simplicity

of information processing (described above) which requires

a relatively small number of computations allowing the trajectories to be modified in real time in accord with the sensory

feedback.

m-22
(3) The control system has been written in FORTRAN IV resulting in :

(a) relative portability of the system to other host computers

(where the computer-dependent modules have been identified);

(b) an ease in understanding of the code due to the simplicity of

FORTRAN and its universal use;

(c) the ability of the user to add additional features easily by

writing his own FORTRAN subroutines;

(d) the ability to implement the control system in assembly

language on microprocessor or minicomputers since it is a rather straight forward task to cross code FORTRAN statements

into assembly language.

Ill- 23
REFERENCES

1. R. Finkel, et. al . , AL, A Programming System for Automation,

Stanford Artificial Intelligence Laboratory Memo. AIM-243,

STAN-CS-74-456, Stanford University, November, 1974.

2. P. M. Will and D, D. Grossman, An Experimental System for Computer

Controlled Mechanical Assembly, IEEE Trans, on Computers, Vol.

C-24, No. 9, 879-888, September, 1975.

3. J. Nevins, et. al . , Exploratory Research in Industrial Modular

Assembly, C. S. Draper Lab Report No. R-921, August, 1975.

4. C. Rosen, et. al . , Exploratory Research in Advanced Automation,

First Semi-Annual Report, SRI Project 2591, Stanford Research

Institute, Menlo Park, California, December, 1973.

5. R. M. Spencer, High Level Control of Reprogrammable Automatic

Assembly Machines, Master's Thesis, Electrical Engineering

'- Department, Massachusetts Institute of Technology, 1975.

6. J. S. Albus, and J. M. Evans, Jr., Robot Systems, Scientific

American, Vol. 234, No. 2, pp 76-86b, February, 1976.

7. J. S. Albus, Data Storage in the Cerebellar Model Articulation

Controller (CMAC), Journal of Dynamic Systems, Measurement, and

Control Trans. ASME, Series G, Vol. 97, No. 3, pp 228-233,

September, 1975.

III-24
APPENDIX A

A-1
I

1
APPENDIX A

Interface Design

Data transfer between the laboratory hardware and the PDP-n/45 minicomputer is handled via a DRll-C interface card in the computer. This

card has 16 data input lines, 16 data output lines, two interrupt

lines and two sync lines, one (data transmitted) for input synchronization, and one (data present) for output synchronization.

The DRll-C is connected to the laboratory interface by forty parallel

wires powered by 7406 line drivers. These are open collector circuits

with resistors connected to the power supply voltage at the receiving

end.

The laboratory interface hardware consists of two parts: an input section

and an output section.

Input:

A sixty four channel analog multiplexer followed by a 14 bit analog to

digital converter forms the heart of the input section. This unit has

a range -10 volts to +10 volts. Negative voltages are converted to

2's complement form.

Output from the A/D converter is carried to the computer on the 16

data input lines. Since the A/D converter has only a 14 bit output,

the most significant (sign) bit is connected to the three most

significant computer input lines. This causes the computer to read the

correct values for both positive and negative (2's complement) numbers.

Data transfer between the A/D converter and the computer is signaled

by a pulse on the data transmitted line. This pulse, initiated by the

computer, indicates that the data lines have been sampled. This

data transmitted pulse is used to step the analog multiplexer to the

next input and to initiate the analog to digital conversion cycle on the new input input voltage. The input software is timed to sample

the input data at a rate slower than the conversion rate of the A/D

system. Thus, a "conversion completed" signed is not required by the

computer, and only one control line is needed to advance the converter

to the next channel

.

The multiplexer is operated in a sequential mode. It is reset to the

zeroth channel by a reset pulse derived from the output section. Each

data transmitted pulse steps the multiplexer to the next sequential

channel. The input section is therefore completely under computer

control

.

A-3
Output:

A sixteen channel digital demultiplexer routes output signals from the

DRll-C to sixteen holding registers. These provide either digital

output directly, or are connected to digital to analog converter so as to provide analog voltages. Data transfer between the DRll-C and the

output section is initiated by a pulse on the data present line. The

data present pulse occurs simultaneously with the appearance of data

on the DRll-C output lines. This pulse is delayed in the laboratory

interface hardware for two microseconds before actuating the digital

demultiplexer so as to allow the data lines time to settle. The

delayed data present pulse gates the output data into the selected

holding register and then steps the address counter to the

next address.

The most significant (sign) bit of the output is decoded as a reset

signal and is used to set the demultiplexer address counter to Channel

1 as well as to reset the analog multiplexer address in the input

section.

Using the sign bit for control means that only 15 bits of output are available for data. Numerical output is restricted to the range 0 to

32,767. The digital to analog converters are adjusted such that numerica

0 corresponds to +10 volts; 16,384 corresponds to zero volts, and

32,767 corresponds to -10 volts.

A-4
Comments

This report describes an architecture for a control system. Its

emphasis has been on the partitioning of the control system into a hierarchy of functional levels. This approach provides a way of thinkin

about the problem that has been very instructive to us. Of course,

the decomposition of a problem or task into sequences of simpler

actions is a very old technique. Indeed, it is an implicit

fundamental method used by human beings in almost all things they do.

So, in that sense, the use of a hierarchical design for a robot

control system is hardly a novel idea.

However, instead of the hierarchy being implicit in our approach we

have made it the explicit starting frame work. The benefits that we

feel have derived from this are many and several have been discussed

in the report.

The control system actually implemented in this report is a minimum

configuration for a robot performing simple manipulative tasks. It is

provided here as an example of how to construct a sensory

interactive, goal directed control system when one uses a hierarchical

framework to structure the problem. It also demonstrates the ease of

greatly increasing control capabilities by the addition of higher

levels in the hierarchy while retaining the already developed lower

levels. Thus, the system is upwards compatible to almost any degree

of complexity of behavior.

Increases in complexity of behavior will usually require increasingly

complex sensory data processing such as might occur with force or low

level vision. We view the sensory processing required occuring in an ascending hierarchy parallel to the descending control hierarchy with

the exchange of differing degrees of processed information between

these two hierarchies at all levels. Thus, the control hierarchy will

continue to make simple tests on sensory feedback input and branch to

appropriate programmed responses regardless of the complexity of the

sensory data processing.

This sensory data processing will be occuring in the sensor feedback

hierarchy not in the control hierarchy.

USE OF MICROPROCESSORS

One of the major points of this approach has been the partitioning of

a sensory interactive control system into a number of functional ' levels. Implicit in this, is the idea that each of these function

modules is an independent system, operating all the time, communicating

to other control or sensory modules through standard interfaces

anytime it needs to receive or send information. The use of a single

computer severely restricts this type of system.
These modules can be made to appear to be independent and parallel

through complex software manipulation such as real time multi-tasking

operating systems on a serial computer. But even a fast computer soon

runs out of time to perform all the functions as the requirements of

the control system increase.

The obvious solution is for each level of the hierarchies to reside in an inexpensive microprocessor. Information to be exchanged would be

placed in common memory. Several levels might be combined into one microprocessor depending on the simplicity of and number of functions

to be handled. A separate microprocessor could be allocated for the

coordinate transformation routine and one for safety and error checking routines. Future development work will address the use of

microprocessors in these hierarchical network structures.

PROGRAMMING STRATEGY

This report presents not only a philosophical approach to the design

of a control system but also the documented FORTRAN programs that

implement a minimum configuration sensory interactive control system.

This has been done to provide both a concrete example of a hierarchical control system and a starting point for anyone wishing to

pursue robot control system development.

Both the programs and techniques of data handling can be improved and

will be. The programming approach has been to make the system as table driven as possible, to make the applications programs as totally

separate from the data as possible. We are updating the system in an attempt to realize these goals.

As new levels, functions, and sensors are added and programming

techniques are improved, additional reports will be written and

distributed.

A-6
Control Hierarchy

(Module #1)

EXPRO - Executive program to oversee entire

control system.

RDMOD - Reads in Program Table and Location

Table for disc.

RECORD - Stores Program Table and Location Table

on disc.

SAMPLE - Allows operator to call up Program

or Location module.

3rd Level of Control Hierarchy

EMOVE - Calls appropriate primitives for each

elemental move command.

DI-2

DI-8

DI-10

DI-12

DI-16

2nd Level of Control Hierarchy

STLINE -- Generates a straight line motion. DI-24

WAIT - Suspends execution and waits for an input si qnal

.

DI-30

CALD - Calculates the distance between two

location points. DI-32

POL - Calculates the delta joint values for an interpolated trajectory. DI-36

ACC - Executes interpolated trajectory using

values from POL. DI-40

DETECT -- Uses proximity sensors to detect presence

of object. DI-44

BAL - Uses proximity sensors to center hand over

object. DI-48

GRASP -- Causes fingers to close with a defined force. DI-32

RELEAS - Causes fingers to open completely. DI-56

PTOUCH -- Uses proximity sensors to locate top of an object. DI-58

COOR - Transforms external coordinates into joint

coordinates and vice versa. DI-62

1st Level of Control Hierarchy

SERVO - Outputs joint position commands through

ARMOUT.

ARMOUT - Output driver that sends output values

to interface.

ARMIN - Input driver that stores input values in a buffer.

DI-66

DI-70

DI-71

DI-1
DI-2
C PROGRAM : EXPRO

C

C ARGUMENTS : C

C CALLED BY : C

C CALLS SUBROUTINES : RDMOD SAMPLE ARMIN SERVO EMOVE WAIT

CC INPUT DATA : LTAB(90,3,6) = THE PRESENT LOCATION TABLE MODULE

C THAT IS EITHER READ IN FROM THE DISC

C BY 'RDMOD' OR IS UPDATED BY A CALL TO

C 'SAMPLE'.

C PROG(105,6) = THE PRESENT PROGRAM MODULE THAT

C IS EITHER READ IN FROM THE DISC BY

C 'RDMOD' OR IS UPDATED BY A CALL TO

C 'SAMPLE'.

C SW-35 = INPUT SWITCH -35- WHOSE VALUE IS EITHER

C 0 OR -4000 (0 TO +5 VOLTS). THIS SWITCH

C IS USED TO HOLD THE PROGRAM IN AN INITIAL

C STATE UNTIL EXECUTION WHICH IS SIGNALED

C BY FLIPPING THIS SWITCH TO THE UP POSITION

C THIS SWITCH IS ALSO USED TO INDICATE

C WHETHER THE PROGRAM IS TO BE EXECUTED

C ONCE OR REPEATEDLY.

C SW-28 = THE VALUE OF THIS SWITCH IS SET EQUAL TO

C THE VARIABLE 'EABORT' AND IS USED AS AN

C ABORT SWITCH WHICH STOPS THE EXECUTION

C OF THE PROGRAM AND CALLS 'SAMPLE'. THIS

C ALLOWS THE MODIFICATION OF THE PROGRAM

C MODULE, THE LOCATION TABLE MODULE, THE

G STORING OF THESE TWO MODULES ON DISC,

C AND THE RETURN TO EXECUTION OF THE

C PROGRAM AT ANY SPECIFIED LINE IN THE

C PROGRAM MODULE.

C LL = THE SPECIFIED LINE OF THE PROGRAM

C MODULE TO BE EXECUTED NEXT [ F ROM ' SAMPLE ']

.

C KK = FLAG RETURNED FROM A CALL TO 'SAMPLE'

C TO INDICATE IF A RE-ENTRY TO A

C PARTICULAR LINE IS DESIRED.

CC OUTPUT DATA : ELE(6) = THE VALUES OF THE LOCATION TABLE.

C POINTERS AND THE FUNCTION FLAGS FOR

C LINE 'LL' OF THE PROGRAM MODULE (AN

C 'ELEMENTAL MOVE' COMMAND).

C KK = FLAG USED TO INDICATE WHETHER THE

C CALL TO 'EMOVE' IS FOR THE EXECUTION

C OF A LINE IN THE PROGRAM MODULE (KK=0),

C OR THAT EXECUTION IS TO BEGIN AT

C • AT LINE 'LL' IN THE PROGRAM MODULE

C WITH THE ARM STARTING AT SOME ARBITRARY

C LOCATION. COMES FROM THE CALL TO 'SAMPLE

C WHERE RE-ENTRY INTO THE EXECUTION OF THE

C PROGRAM MODULE IS SPECIFIED TO BE AT

C LINE 'LL' AND THE PRESENT LOCATION

C OF THE ARM MIGHT BE ANYWHERE IN ITS

C WORKING SPACE. (KK=1).

DI-3
c

C FUNCTION: SUPPLIES THE THIRD LEVEL OF THE HIERARCHY WITH

C AN ELEMENTAL MOVE COMMAND, IE. REQUESTS THE

G EXECUTION OF A SINGLE LINE OF THE PROGRAM MODULE.

"C ALSO, ALLOWS THE ENTERING OF NEW PROGRAM LINES AND

C .. LOCATION POINTS, AND INITIALIZING THE SYSTEM TO

G BEGIN EXECUTION.

G IMPLICIT INTEGER (B-Z)

COMMON/ARMBUF/INBUF( 64 )

, OUTBUF( 64

)

COMMON/STEP/LL

C0MMON/LMOD/LTAB(90 , 4, 7 ) ,PRES ,DEST

COMMON/PMOD/PROG( 105 ,6) , ENDP , BRNCH ( 6 ) ,M

C0MM0N/0UT/JP0S(8)

COMMON/EM/ELE ( 6

)

EQUIVALENCE(EAB0RT,INBUF(28)

)

C

C INITIALIZE THE FLAG 'KK'.

C KK = 0 CC THIS SUBROUTINE ( ' RDMOD ' ) READS THE PROGRAM MODULE AND THE LOCATION

C. TABLE MODULE FROM THE DISC AND STORES THEM IN THE MATRICES

g' PR0G(105,6) AND LT AB ( 9 0 , 3 , 6 )

.

G CALL RDMODC 1 ,0)

C

C THIS ROUTINE ALLOWS THE ENTRY OF NEW PROGRAM LINES

C AND LOCATION POINTS, AND ALLOWS A RE-ENTRY INTO THE EXECUTION OF

C THE PROGRAM AT A SPECIFIED LINE 'LL'.

G CALL SAMPLE(I)

GOTO 4000

3400 CALL SAMPLE(O)

IF(LL.LE.O)GOTO 4000

KK= 1 GOTO 301

G

C A REQUEST IS MADE FOR THE OPERATOR TO FLIP SW-35 TO THE DOWN

C POSITION SO THAT THE STARTING LOCATION JOINT POSITION

G VALUES CAN BE SENT TO THE SERVOS.

G4000 WRITE(6 , 400 1

)

4001 F0RMAT(8X' SW 35 DOWN AND',/

1 8X'»»»TYPE [CR]«»»TO INITIALIZE THE SERVOS WITH THE ',/

1 24X'STARTING LOCATION.')

READ(6 , 5000)FHH

5000 F0RMAT(I3)

C

C THE INPUTS ARE READ INTO THE COMPUTER AND SW-28 (THE 'ABORT'

C SWITCH IS CHECKED TO SEE IF IT HAS BEEN SET. IF IT HAS,

G THEN RETURN TO THE ROUTINE 'SAMPLE' TO DETERMINE WHAT ACTION

G SHOULD BE TAKEN.

G CALL ARMIN

IF(EABORT.LT.-2000)GOTO 3400

DI-4
cC INSTRUCT THE OPERATOR TO NOW TURN ON THE SERVOS, AND THAT

C SW-35 IS TO BE FLIPPED UP TO BEGIN EXECUTION OF THE PROGRAM.

C WRITE(6

,

H002 ) 4002 F0RMAT(8X'TURN ON SERVOS,',/

1 8X' SW 35 UP TO EXECUTE PROGRAM.',/

1 8X'LEAVE SW 35 UP FOR REPEATED EXECUTION.')

CC SET THE VARIABLE 'M' EQUAL TO THE NUMBER OF EXECUTABLE LINES IN THE

C PROGRAM.

C SET THE VARIABLE 'FR' EQUAL TO THE LOCATION TABLE POINTER

C FOR THE STARTING LOCATION OF THE PROGRAM. SEND OUT THE

C JOINT POSITION VALUES FOR THIS LOCATION TO THE SERVOS, AND

C WAIT UNTIL SW-35 IS FLIPPED UP BEFORE CONTINUING WITH

C THE EXECUTION OF THE PROGRAM.

C FR=PROG( 1

, 1

)

DO 200 FRR= 1 , 6 200 JPOS(FRR)=LTAB(FR, 1 ,FRR)

CALL SERVO(O)

CALL ARMIN

IF(EAB0RT.LT.-2000)G0T0 3^00

CALL WAIT(35)

C

C PLACE THE VALUES (LOCATION TABLE POINTERS AND FUNCTON FLAGS)

C OF THE PROGRAM LINE 'LL' INTO THE MATRIX 'ELE(6)'. THIS IS AN

e 'ELEMENTAL MOVE' THAT WILL BE USED AS A COMMAND TO THE THIRD

C LEVEL OF THE CONTROL SYSTEM BY A CALL TO 'EMOVE'.

C305 LL=1

301 DO 202 J = 1 , 6 202 ELE(J)=PROG(LL, J)

C

C TEST TO ASCERTAIN THAT BOTH A PRESENT LOCATION (ELE(1)) AND A

C DESTINATION (ELE(2)) POINTER ARE AVAILABLE. IF NOT, THEN

C PRINT OUT AN ERROR MESSAGE ON THE TERMINAL AND RETURN TO

C THE SUBROUTINE 'SAMPLE' TO DETERMINE WHAT IS TO BE DONE NEXT.

C IF(LTAB(ELE( 1 ) , 1 , 1 ) .EQ.OGOTO 100

IF(LTAB(ELE(2) ,1,1).NE.0)G0T0 300

100 WRITE(6,101)

101 FORMAKBX'PROGRAM HAS NO LOCATION VALUE ',/

1 2X ' «»»»»»*»»«»»»«PROGRAM ABORTED»»»»»»»*»»»»»»»» '

)

GOTO 3400

CC THIS SUBROUTINE CALL CAUSES THE EXECUTION OF A SINGLE PROGRAM

C MODULE LINE ('ELEMENTAL MOVE' COMMAND).

C300 CALL EMOVE(KK)

KK = 0 C

C THE INTERRUPT SWITCH 28 ('EABORT') IS CHECKED TO

C SEE IF IT IS SET.

DI-5
c IF(EABORT.LT.-2000)GOTO 3^00

CC THE PROGRAM LINE COUNTER ('LL') IS INCREMENTED. IF THIS VALUE DOES

C NOT EXCEED THE LAST EXECUTABLE LINE, THEN JUMP BACK TO

C STATEMENT 301 TO CARRY OUT ITS EXECUTION.

C LL=LL+1

IF(PR0G(LL,2) .NE.O)GOTO 301

C

C ONCE THE ENTIRE PROGRAM HAS BEEN EXECUTED, THEN TEST SWITCH 35. IF

C IT IS UP THEN REPEAT THE PROGRAM. IF IT IS DOWN, THEN SIT

C HERE AND CONTINUE MONITORING IT.

C CALL WAIT(35)

CC BRANCH BACK TO STATEMENT 305 TO BEGIN EXECUTION.

C GOTO 305

.END

DI-6

DI-8
C SUBROUTINE : RDMOD

C

C ARGUMENTS : LTM = IF LTM=1 READ THE LOCATION TABLE MODULE

C OFF THE DISC AND STORE IN THE

C MATRIX • LTAB( 90 , 4 , 7 )

' . C IF LTMrO DO NOT READ IN THE LOCATION

C TABLE MODULE FROM THE DISC.

C PM = IF PM=1 READ THE PROGRAM MODULE IN

C FROM THE DISC AND STORE IN THE

C MATRIX ' PROG( 105,6)'.

C IF PM=0 DO NOT READ IN THE PROGRAM

C MODULE FROM THE DISC.

C

C CALLED BY : EXPRO

C

C CALLS SUBROUTINES : C

C INPUT DATA : THE PROGRAM MODULE ASSIGNED TO DEVICE NUMBER 7,

C AND THE LOCATION TABLE MODULE ASSIGNED TO DEVICE

C NUMBER 4.

C

C OUTPUT DATA : PROG(105,6) = THE MATRIX CONTAINING THE

C PROGRAM MODULE.

C NTAB(90,4,7) = THE MATRIX CONTAINING THE

C LOCATION TABLE MODULE.

C ENDP = THE PROGRAM MODULE LINE NUMBER

C OF THE LAST ELEMENTAL MOVE

C

C FUNCTION: READS IN THE PROGRAM MODULE AND THE LOCATION

C TABLE MODULE FROM THE DISC AND STORES THEM

C IN THEIR PROPER MATRICES IN THE COMMON BLOCK.

C SUBROUTINE RDMOD ( LTM , PM

)

IMPLICIT INTEGER (B-R),(U-Z)

COMMON /LMOD/LTAB( 9 0,4,7) ,PRES,DEST

COMMON /PMOD /PROG (105,6), ENDP , BRNCH(6 ) ,M

C

C TEST IF 'PM' SET EQUAL TO ONE. IF IT IS, THEN READ IN

C THE PROGRAM MODULE ASSIGNED TO DEVICE NUMBER 7 FROM THE

C DISC.

C

IF(PM.NE. 1 )GOTO 400

DO 15 JK = 1 , 105

READ (7 , 15,END=100) (PROG( JK, J) , J=1 ,6)

15 F0RMAT(6(2X,I6) ) 100 CONTINUE

CC TEST IF 'LTM' SET EQUAL TO ONE. IF IT IS, THEN READ IN

C THE LOCATION TABLE MODULE ASSIGNED TO DEVICE NUMBER 4 C FROM THE DISC.

C400 IF(LTM. NE. 1 )GOTO 40

DO 14 JB=1,90

DO 14 JC=1 ,4

READ(4, 14, END = 40) ( LTAB ( JB , JC , J ) , J=1 ,7)

14 F0RMAT(7(2X,I6)

)

40 RETURN

END

DI-9
DI-10
C SUBROUTINE : RECORD

C

C ARGUMENTS : LTM = IF LTM=1 STORE THE LOCATION TABLE MODULE

C (LTAB(90,4,7) ) ON THE DISC UNDER

C THE NAME ASSIGNED TO DEVICE 2.

C IF LTM=0 DO NOT STORE THE LOCATION TABLE

C ON THE DISC.

C PM = IF PM=1 STORE THE PROGRAM MODULE

C ( PROG( 1 05 , 6) ) ON THE DISC UNDER

C THE NAME ASSIGNED TO DEVICE 3-

C PM=0 DO NOT STORE THE PROGRAM MODULE ON

C ' THE DISC.

C

C CALLED BY : PROGS SAMPLE

C

C CALLS SUBROUTINES : C

C INPUT DATA : NTAB(90,4,7) = THE MATRIX CONTAINING THE LOCATION

C TABLE MODULE.

C PROG(105,6) = THE MATRIX CONTAINING THE PROGRAM

C MODULE.

C

C OUTPUT DATA : THE LOCATION TABLE MODULE STORED ON DISC UNDER

C THE NAME ASSIGNED TO DEVICE 2.

C THE PROGRAM MODULE STORED ON THE DISC UNDER THE

C NAME ASSIGNED TO DEVICE 3-

C

C FUNCTION: STORES THE LOCATION TABLE MODULE ( LTAB ( 9 0 , 4 , 7 ) ) AND

C THE PROGRAM MODULE ( PROG ( 1 05 , 6 ) ) ON DISC UNDER

C ASSIGNED NAMES.

C SUBROUTINE RECORD ( LTM , PM

)

IMPLICIT INTEGER(B-Z)

COMMON /LMOD /LTAB (90, 4,7) ,PRES,DEST

COMMON /PMOD /PROG (105, 6) , ENDP , BRNCH ( 6 ) ,M

C

C TEST IF 'PM' SET EQUAL TO ONE. IF IT IS, THEN WRITE THE

C PROGRAM MODULE ONTO THE DISC.

C IF(PM.NE. 1 )GOTO 400

DO 15 JK=1,105

WRITE(3, 15) (PROG (JK, J) ,J=1 ,6)

15 F0RMAT(6(2X,I6)

)

END FILE 3 CC TEST IF 'LTM' SET EQUAL TO ONE. IF IT IS, THEN WRITE THE

C LOCATION TABLE MODULE ONTO THE DISC.

C400 IF(LTM.NE. 1 )GOTO 40

DO 14 JB=1 ,90

DO 14 JC=1 ,4

WRITE(2, 14) (LTAB( JB, JC, J) , J=1 ,7)

14 F0RMAT(7(2X,I6)

)

END FILE 2 MO RETURN

END

DI-11
DI-12
C SUBROUTINE : SAMPLE

C

C ARGUMENTS : KG = USED AS A FLAG TO CHOOSE THE MESSAGES TO BE

C PRINTED OUT ON THE TERMINAL.

C

C CALLED BY : EXPRO

C

C CALLS SUBROUTINES : ARMIN LOCTAB PROGS RECORD PLINE

C

C —- INPUT DATA : LL = THE PRESENT PROGRAM LINE NUMBER THAT IS

C BEING EXECUTED.

C INBUF(28) = THE ABORT SWITCH VALUE.

C INBUF(29) = THE VALUE OF THE SWITCH THAT IS USED FOR

C CALLING UP THE TEACH LOCATION POINT

C PROGRAMS.

C INBUF(30) = THE VALUE OF THE SWITCH THAT IS USED FOR

C CALLING UP THE ROUTINES FOR ENTERING

C A PROGRAM.

C INBUF(31) = THE VALUE OF THE SWITCH USED TO

C INDICATE IF THE PROGRAM MODULE AND THE

C LOCATION TABLE MODULE ARE TO BE STORED ON

C DISC.

C

C OUTPUT DATA : LL = THE NUMBER .OF THE PROGRAM LINE TO BE

C EXECUTED NEXT.

C

C FUNCTION: THIS PROGRAM IS CALLED BY THE ABORT SWITCH AND ALLOWS

C THE ENTRY OF NEW OR MODIFIED LOCATION VALUES, THE

C EDITING OF THE PROGRAM MODULE, THE STORING OF THE NEW

C LOCATION TABLE MODULE AND PROGRAM MODULE ON DISC, AND

C THE ABILITY TO RETURN TO THE EXECUTION PROGRAM

C AT ANY LINE IN THE PROGRAM MODULE.

C SUBROUTINE SAMPLE(KG)

IMPLICIT INTEGER(B-Z)

COMMON/ARMBUF/INBUF( 64) , OUTBUF( 64

)

COMMON /PMOD /PROG (105, 6) ,ENDP,BRNCH(6),M

C0MMON/IND/IN(30) ,FL(6) ,NAME(3) ,NPT(3,20)

COMMON/STEP/LL

EQUIVALENCE(EABORT,INBUF(28)

)

C

C TEST ARGUMENT 'KG', IF EQUAL TO ZERO, THEN SKIP NEXT MESSAGE.

C5000 IF(KG. EQ. OGOTO 6000

WRITE(6 , 6240)

6240 F0RMAT(8X' SW 28 UP INTERRUPTS RUN PROGRAM')

GOTO 6235

C

C PRINT OUT PRESENT PROGRAM LINE NUMBER AND PRINT OUT THE ENGLISH

C TEXT OF THIS LINE AS THE ORIGINAL 'GOTO' STATEMENT.

C6000 WRITE(6 , 6200)LL

6200 F0RMAT(8X • YOU ARE AT PROGRAM STEP', 14)

IF(LL.EQ.O)GOTO 425

MR = 0 DO 204 J=101 , 105
DO 204 L= 1

,

6

MR=MR+1

.204 IN(MR)=PROG( J,L)

CALL INDEXC 1

)

CALL PLINE(LL)

C

C PRINTS OUT ON TERMINAL THE VARIOUS OPERATIONS AVAILABLE TO THE USER.

C425 WRITE(6 , 6201

)

6201 F0RMAT(8X'---SW 28 UP RETURNS PROGRAM TO BEGINNING')

6235 WRITE(6 , 6225)

6225 F0RMAT(8X' SW 29 UP TEACH LOCATION',/

1 8X' SW 30 UP RECORD PROGRAM SEQUENCE',/

1 8X' SW 31 UP STORE LOCATION TABLE ON DISC',/

1 8X'»»»TYPE (CR)»»» TO CONTINUE')

READ(6 , 6001 )TR

6001 F0RMAT(I4)

C

C READS IN THE SWITCH VALUES TO DETERMINE WHICH IS THE PROPER SUBROUTINE

C TO BRANCH TO.

C CALL ARMIN

DO 6203 J= 1,

4

K = 27 + J CHKV=IABS(INBUF(K)

)

IF(CHKV.GT.2000)G0T0(45,11,12,13)J

6203 CONTINUE

GOTO 45

11 CALL LOCTAB

GOTO 5000

12 CALL PROGS

GOTO 5000

13 CALL RECORD( 1 ,0)

WRITE(6 ,225)

225 F0RMAT(2X ' LOCATION TABLE STORED')

GOTO 5000

45 IF(KG.EQ. 1 )GOTO 40

WRITE(6,6220)

6220 F0RMAT(8X 'WHICH PROGRAM STEP DO YOU WANT?')

READ(6 , 6221 )LL

6221 F0RMAT(I4)

40 RETURN

END

LlJ^02_J1-

DI-16
C SUBROUTINE : EMOVE

CC ARGUMENTS : KK = IF KK=1 THEN THE STRAIGHT LINE DISTANCE

C FROM THE PRESENT LOCATION OF THE

C ARM WHICH MIGHT BE POSITIONED

C ANYWHERE IN THE WORK SPACE, TO THE

C DESTINATION SPECIFIED IN THE GIVEN

C- LINE OF THE PROGRAM MODULE IS CALCULATED

C AND USED AS THE TRAJECTORY FOR THIS

C COMMANDED 'ELEMENTAL MOVE*.

C IF KK=0 THE TRAJECTORY IS ASSUMED TO BE BETWEEN

C THE TWO LOCATION TABLE POINTERS SPECIFIED

C BY THE 'ELEMENTAL MOVE' COMMAND (ELE(6)),

C IE. THE ACTUAL LOCATION OF THE ARM IS

C CONSIDERED TC BE THE SAME AS THAT DEFINED

C BY THE JOINT POSITION VALUES OF

C THE 'PRESENT LOCATION ' POINTER

C IN THIS ELEMENTAL MOVE COMMAND.

C

C CALLED BY : RUNPRO

CC CALLS SUBROUTINES : STLINE WAIT GRASP POS POL DETECT

C BAL RELEAS PTOUCH COOR SERVO CALD ACC

C

C INPUT DATA : KK = THE FLAG THAT INDICATES WHETHER OR

C NOT THE ACTUAL LOCATION OF THE ARM

C IS AT THE LOCATION SPECIFIED BY THE

C 'PRESENT LOCATION' POINTER (ELE(1))

C FROM THE ELEMENTAL MOVE COMMAND.

C SW-28 = THE ABORT SWITCH ('EABORT') USED TO

C BRANCH THE CONTROL SYSTEM BACK TO

C THE SUBROUTINE 'SAMPLE' TO ALLOW

C MODIFICATION OF THE PROGRAM OR

C LOCATION TABLE MODULES AND RE-ENTRY INTO

C THE PROGRAM AT A SPECIFIED LINE.

C ELE(6) = THE MATRIX THAT CONTAINS A SINGLE

C 'ELEMENTAL MOVE' COMMAND, IE. THE LOCATION

C TABLE POINTERS FOR THE 'PRESENT LOCATION',

C THE 'DESTINATION', THE VALUE OF THE

C VELOCITY INDICATOR, AND THE VARIOUS

C FUNCTION FLAGS.

C

C OUTPUT DATA : THE PROPER SEQUENCE OF CALLS TO THE NECESSARY

C SUBROUTINES ('PRIMITIVES') TO CARRY OUT THE

C 'ELEMENTAL MOVE' COMMAND.

C

C FUNCTION: FROM THE INFORMATION CONTAINED IN A SINGLE LINE

C FROM THE PROGRAM MODULE, THIS SUBROUTINE,

C WHICH IS THE THIRD LEVEL OF THE CONTROL HIERARCHY,

C CALLS THE SUBROUTINES (PRIMITIVES) TO GENERATE A C TRAJECTORY BETWEEN THE TWO LOCATIONS SPECIFIED,

C CAUSES THE ARM TO MOVE ALONG THIS TRAJECTORY AT A C GIVEN VELOCITY, AND TESTS ON THE FUNCTION

C FLAGS AND CALLS SUBROUTINES IN ACCORD WITH

C THE VALUES DETECTED.

C

DI-17
SUBROUTINE EMOVE(KK)

IMPLICIT INTEGER(B-Z)

COMMON/ARMBUF/INBUF( 64 ), OUTBUF( 64)

COMMON/GRIP/HAND

COMMON/LMOD/LTAB(90,4,7) ,PRES,DEST

C0MM0N/0UT/JP0S(8)

COMMON/EM/ELE (6 ) COMMON/DIS/ADIST

C0MM0N/0FFS/0FF(3)

COMMON / CORT/ AC(6) , AXE P, A YEP, AZEP

COMMON/END/PNT

EQUIVALENCE(NPO,INBUF(33) ) EQUI VALENCE (EABORT,INBUF( 28)

)

C

C A CALL TO 'SERVO(-I)' IS MADE TO RESET ALL OF THE OUTPUT CHANNEL

C LEVELS TO ZERO.

C CALL SERV0(-1

)

C

C THE COORDINATE OFFSET VALUES THAT ARE USED IN THE SENSOR

C SUBROUTINE CALLS ARE SET EQUAL TO ZERO,

C DO 70 J=1 ,

3

70 OFF(J)=0

C

C TESTS ON THE FLAG 'KK' AND IF SET, JUMPS THE APPROACH PATH CALCULATION

C IF(KK. EQ. 1 )GOTO 220

C

C TESTS THE FLAG TO SEE IF THE ARM STOPPED AT THE PRESENT LOCATION

C IF IT DID (LTAB(ELE( 1 ) ,2,7)=1 )

, THEN CONTINUE. IF IT DID

C NOT STOP(LTAB(ELE( 1 ) ,2,7)=0) , THEN JUMP OVER THE APPROACH PATH

C CALCULATION (IT IS ASSUMED THAT THERE IS NO APPROACH PATH FOR

C THOSE LOCATIONS THAT THE ARM DOES NOT STOP AT).

C

IF(LTAB{ELE( 1 ) ,2,7) .EQ.OGOTO 220

C

C THE FOLLOWING SUBROUTINE ('STLINE') GENERATES A STRAIGHT LINE PATH

C FROM ITS PRESENT LOCATION TO A POINT A DELTA X, DELTA Y, DELTA Z

C DISTANCE AWAY (THE DELTAS ARE LISTED AS ARGUMENTS). HERE IT IS

C USED TO GENERATE THE SPECIFIED APPROACH PATH FOR THE 'PRESENT

C LOCATION' POINT.

C CALL STLINE (LTAB(ELE( 1 ) , 3 , 4) ,LTAB(ELE( 1 ) , 3 , 5) ,LTAB(ELE( 1 ) , 3, 6) , 1 ) C

C 'EABORT' IS THE EQUIVALENT VALUE OF SWITCH 28 WHICH IS USED TO

C ABORT THE EXECUTION OF THE PROGRAM AND RETURN TO THE SUBROUTINE

C 'SAMPLE'. THIS VALUE IS CHECKED CONSTANTLY IN THE SUBROUTINE

C 'SERVO' AND AFTER EVERY SUBROUTINE CALL THAT RESULTS IN A CALL

C TO 'SERVO' (IE. EVERY TIME THE ARM IS COMMANDED TO MOVE)>

C IF(EAB0RT.LT.-2000)G0T0 40

C

C THE FUNCTON FLAG IS CHECKED TO SEE IF IT IS A 'GRASP PROXIMITY'

C (ELE(6)=3). IF IT IS, THEN CALL 'GRASP' TO CLOSE THE FINGERS

C FOR A SEARCH.

DI-18
c220 IF(ELE(6) .NE.3)G0T0 230

CALL GRASP(I)

C

C IF THE FUNCTION 'STRAIGHT LINE' IS CALLED FOR (ELE(6)=9), THEN

C BRANCH TO THE SUBROUTINE CALLS 'POS' AND 'STLINE'.

C230 IF(ELE(6) .EQ. 9)G0T0 301

C

C THE ARGUMENT 'KK' IS CHECKED TO SEE IF THE TRAJECTORY IS TO BE

C A STRAIGHT LINE FROM THE PRESENT LOCATION OF THE ARM

C TO THE DESTINATION CALLED FOR IN PROGRAM LINE 'LL' (KK=1),

C OR IF IT IS TO BE LINEAR INTERPOLATION PATH BETWEEN

C THE TWO LOCATIONS SPECIFIED IN THE PROGRAM LINE 'LL' (KK=0).

C

IF(KK. NE. 1 )GOTO 300

C

C TO GENERATE A STRAIGHT LINE TRAJECTORY FROM THE PRESENT LOCATION

C OF THE ARM, THE SUBROUTINE 'POS' IS CALLED TO READ IN THE PRESENT

C JOINT POSITION VALUES TO BE USED BY 'STLINE'.

C

301 CALL POS

CC THIS SUBROUTINE (' STLINE ( DX , DY , DZ , I )' ) GENERATES AND EXECUTES A

C STRAIGHT LINE TRAJECTORY BETWEEN TWO LOCATION POINTS.

C IF THE ARGUMENT 'I' IS SET EQUAL TO ZERO, THEN THE STRAIGHT LINE

C IS GENERATED BETWEEN THE PRESENT LOCATION OF THE ARM (WHICH MAY OR

C MAY NOT BE DEFINED IN THE 'LOCATION TABLE MODULE') AND THE

C START OF THE APPROACH PATH OF THE 'DESTINATION' POINT LISTED

C IN THE 'ELEMENTAL MOVE' COMMAND (LINE 'LL' OF THE PROGRAM

C MODULE). IF 'I' IS SET EQUAL TO ONE, THEN THE STRAIGHT LINE

C TRAJECTORY IS FROM THE PRESENT LOCATION OF THE ARM TO A

C LOCATION POINT 'DX', 'DY', 'DZ' AWAY.

C CALL STLINE(0 ,0,0,0)

CC IF A STRAIGHT LINE TRAJECTORY HAS BEEN USED THEN THE

C INTERPOLATION CALCULATION IS SKIPPED.

C

GOTO 325

CC IF A STRAIGHT LINE IS NOT GENERATED, THEN THE INTERPOLATION

C ROUTINE IS USED. HERE, 'CALD' IS CALLED TO CALCULATE

C THE STRAIGHT LINE DISTANCE (IN CENTIMETERS) BETWEEN THE PRESENT

C LOCATION OF THE ARM AND THE START OF THE APPROACH PATH

C FOR THE DESTINATION POINT. 'POL' IS THEN CALLED TO CALCULATE

C THE CORRECT DELTAS FOR EACH JOINT TO BE USED BY 'ACC TO CAUSE

C THE JOINT POSITION COMMANDS TO BE INCREMENTED BY THESE

C DELTAS AT PERIODIC TIMES TO GENERATE THE SPECIFIED VELOCITY

C AND ACCELERATION AND DECCELERATION WHERE REQUIRED.

C

300 CALL CALD

CALL POL

CALL ACC

C

C THIS SWITCH IS EXPLAINED ABOVE.

DI-19
c325 IF(EABORT.LT.-2000)GOTO 40

C

C AT THIS POINT IN THE EXECUTION, THE ARM HAS ALREADY ACCOMPLISHED ITS

C TRAJECTORY MOTION BY THE ABOVE SUBROUTINE CALLS.

C IT NOW REMAINS TO MOVE THROUGH THE FINAL APPROACH PATH IF ONE IS

C CALLED FOR, AND TO DETERMINE WHAT FUNCTIONS ARE TO BE

C ACCOMPLISHED. FIRST, IT IS DETERMINED IF THE ARM

C WILL STOP AT THE END OF THIS TRAJECTORY ( LTAB ( ELE ( 2 )

, 2 , 1 ) = 1 )

.

C . IF NOT, THEN RETURN TO THE CALLING PROGRAM.

C400 IF(LTAB(ELE(2) ,2,7) .EQ.OGOTO 40

C

C THE COMMON VARIABLE 'PNT' IS SET EQUAL TO ONE. THIS FLAG IS

C USED BY THE SUBROUTINE 'SERVO'. WHEN IT IS SET TO ONE,

C 'SERVO' WILL MAINTAIN CONTROL UNTIL THE POSITION OF ALL OF THE

C JOINTS IS WITHIN SOME SPECIFIED MINIMUM DISTANCE OF THE

C COMMANDED POSITION. THEREFORE, THE CONTROL PROGRAM DOES NOT

C INITIATE THE NEXT STEP UNTIL THE ARM HAS ARRIVED AT THIS

C END POINT OF THE TRAJECTORY.

C PNT = 1

• CALL SERVO(O)

C

C THE VARIABLE 'FUN' IS SET EQUAL TO THE VALUE OF THE FUNCTION FLAG

C ('ELE(6)') PLUS ONE, TO BE USED TO COMMAND A MULTIPLE 'GOTO'

C . STATEMENT TO THE APPROPRIATF CODE TO CARRY OUT THE SPECIFIED

C FUNCTION. (•FUN'=1,N0 FUNCTION; =2 'GRASP' =3 'RELEASE'

C =4 'GRASP PROXIMITY' =5 'RELEASE PROXIMITY' =6 'DETECT'

C =7 'BALANCE' =8 'UNSTACK' =9 'PTOUCH').

C

FUN=1+ELE(6)

GOTO (20 ,20,20,3,4,20,20,7,8,20,20)FUN

C

C IF THERE IS AN APPROACH PATH, IT IS GENERATED AND EXECUTED BY

C THIS CALL TO 'STLINE '

. C20 CALL STLINE(-LTAB(ELE (2 ) ,3,4) , -L TAB ( ELE ( 2 ) ,3,5) , -L TAB ( ELE ( 2 ) ,3,6) , 1

)

IF(EABORT.LT.-2000)GOTO 40

CC THIS MULTIPLE 'GOTO' STATEMENT CAUSES THE BRANCHING TO THE APPROPRIATE

C CODE TO CARRY OUT THE FUNCTION SPECIFIED.

C GOTOd 10,1,2,3,4,5,6,7,8,110, 10) FUN

C

C THIS IS THE' CODE FOR A 'GRASP PROXIMITY' FUNCTION. THE HAND IS

C MOVED DOWN THE APPROACH PATH TO WITHIN SIX CENTIMETERS OF THE

C DESTINATION POINT BY A CALL TO 'STLINE'. IT HAS TO FIRST

C STOP ABOVE THE OBJECT TO USE ITS PROXIMITY SENSORS TO DETECT IT.

C IT DETERMINES THAT THE OBJECT IS UNDER, AT LEAST, ONE OF

C THE SENSORS BY A CALL TO 'DETECT' WHICH MOVES THE HAND

C UNTIL ONE OF THE SENSORS REACHES A THRESHOLD VALUE. THE HAND IS

C THEN MOVED IN THE DIRECTION TO EQUALIZE THE TWO SENSOR INPUTS BY

C A CALL TO 'BAL'. NOW THAT THE HAND IS CENTERED OVER THE OBJECT,

C THE FINGERS ARE OPENED BY A CALL TO 'RELEAS'. THE HAND DESCENDS

C THE REMAINING SIX CENTIMETERS BY A CALL TO 'STLINE', AND THE

DI-20
C FINGERS ARE CLOSED ON THE OBJECT BY A CALL TO 'GRASP'. IF THE

C FINGER POSITION INDICATOR ('HAND') IS LESS THAN 7200, THEN THE

C FINGERS ARE CLOSED TOO FAR. TO BE HOLDING AN OBJECT. IF THIS IS

C THE CASE THEN THE HAND IS RAISED BACK UP SIX CENTIMETERS

C BY A CALL TO 'STLINE' AND THE SEARCH BEGUN AGAIN.

C

3 NZ=600-LTAB(ELE(2) ,3,6)

CALL STLINE(-LTAB(ELE(2) ,3,4) , -LTAB ( ELE ( 2 ) ,3,5) ,NZ, 1 ) 480 IF(EABORT.LT.-2000)GOTO 40

CALL DETECT

IF(EABORT.LT.-2000)GOTO 40

CALL BAL

IF(EABORT.LT.-2000)GOTO 40

CALL RELEAS(O)

IF(EABORT.LT.-2000)GOTO 40

CALL STLINE(0,0,-600, 1)

IF(EABORT.LT.-2000)GOTO 40

CALL GRASP(O)

IF(EABORT.LT.-2000)GOTO 40

IF(HAND.GT.7200)G0T0 110

CALL STLINE(0 , 0 , 600 , 1

)

GOTO 480

C

C THE FOLLOWING CODE PERFORMS THE 'RELEASE PROXIMITY' FUNCTION.

C FIRST, THE HAND IS MOVED TO A POSITION ONE CENTIMETER ABOVE

C AND FIVE CENTIMETERS TO THE SIDE (SINCE THE PROXIMITY SENSORS

C ARE LOCATED ON THE OUTSIDE OF RATHER THICK FINGERS, THIS SIDEWAYS

C MOTION IS NECESSARY TO BRING ONE OF THE SENSORS TO A POSITION

C WHERE IT WOULD BE OVER THE EXPECTED LOCATION OF THE UNDERLYING

C OBJECT) OF THE DESTINATION POINT BY A CALL TO 'STLINE'. THE

C HAND IS MOVED SIDEWAYS UNTIL ONE OF THE SENSORS HAS REACHED THE

C THRESHOLD LEVEL BY A CALL TO 'DETECT'. THE CALL TO 'BAL' CAUSES

C THE HAND TO MOVE IN THE DIRECTION OF THE SENSOR THAT WAS AT

C THRESHOLD UNTIL THE SENSOR VALUE DECREASES BELOW A CERTAIN

C VALUE, INDICATING THAT THE EDGE OF THE UNDERLYING

C OBJECT HAS BEEN REACHED. THE HAND IS THEN MOVED THE ADDITIONAL

C DISTANCE OF THE THICKNESS OF THE FINGER TO CAUSE THE SIDE OF THE

C OBJECT IN THE HAND TO LINE UP FLUSH WITH THE SIDE OF THE

C UNDERLYING OBJECT. THE CALL TO 'STLINE' CAUSES THE HAND TO

C MOVE DOWN THE REMAINING ONE CENTIMETER AND THE FINGERS ARE

j C OPENED AND THE OBJECT RELEASED BY THE CALL TO 'RELEAS'.

C

4 NZ=100-LTAB(ELE(2) ,3,6)

NY= 500 -LTAB (ELE (2) ,3,5)

CALL STLINE (-LTAB (ELE (2) ,3,4),NY,NZ,1)

IF(EABORT.LT.-2000)GOTO 40

CALL DETECT

IF(EABORT.LT.-2000)GOTO 40

CALL BAL

IF(EABORT .LT. -2000)G0T0 40

CALL STLINE(0,0,-100, 1

)

IF(EABORT.LT.-2000)GOTO 40

CALL RELEAS(O)

GOTO 110

C

C THE 'GRASP' FUNCTON IS ACCOMPLISHED BY A CALL TO 'GRASP'.

DI-21
c

c

c10

740

CALL GRASP(O)

GOTO 110

THE 'RELEASE' FUNCTION IS ACCOMPLISHED BY A CALL TO 'RELEAS'.

CALL RELEAS(O)

GOTO 110

THE 'DETECT' FUNCTION IS ACCOMPLISHED BY A CALL TO 'DETECT'.

CALL DETECT

GOTO 110

THE 'BALANCE' FUNCTION IS ACCOMPLISHED BY A CALL TO 'BAL'.

CALL BAL

GOTO 110

THE FOLLOWING CODE IS USED TO GENERATE THE 'UNSTACK' FUNCTION.

THE FINGERS ARE CLOSED BY A CALL TO 'GRASP' TO MAKE CERTAIN

THAT THE SENSOR TO DETECT THE TOP OF THE STACK IS OVER THE

EXPECTED LOCATION OF THE STACK. THE CALL TO 'PTOUCH' CAUSES

THE HAND TO DESCEND WHILE CONSTANTLY MONITORING THE SENSOR

INPUT TO SEE IF THE THRESHOLD VALUE HAS BEEN REACHED. WHEN

THE THRESHOLD IS REACHED THE HAND STOPS. THE CALL TO 'STLINE'

IS USED TO MOVE THE HAND BACK UP ONE CENTIMETER TO MAKE

CERTAIN THAT THE HAND DOES NOT COLLIDE WITH THE TOP OF THE

STACK DURING THE SEARCH OPERATION. THE HAND IS CENTERED OVER

THE TOP OF THE STACK BY A CALL TO 'BAL'. THE CALL TO 'RELEAS'

OPENS THE FINGERS AND THE CALL TO 'STLINE' CAUSES THE HAND

TO DESCEND AROUND THE OBJECT. THE FINGERS CLOSE ON THE OBJECT

DUE TO THE CALL TO 'GRASP'. THE HAND IS RETURNED TO ITS INITIAL

STARTING POINT BY A CALL TO 'STLINE' . CALL GRASP(O)

IF(EABORT.LT.-2000)GOTO 40

CALL PTOUCH

IF(EABORT.LT.-2000)GOTO 40

CALL BAL

IF(EABORT.LT.-2000)GOTO 40

CALL RELEAS(O)

IF(EABORT.LT.-2000)GOTO 40

CALL STLINE(0 , 0 , -600 , 1 ) IF(EABORT.LT.-2000)GOTO 40

CALL GRASP(O)

IF(EABORT.LT.-2000)GOTO 40

OFF(3)=OFF(3)-600

CALL STLINE ( -OFF ( 1 ) ,-0FF(2) ,-0FF(3) , 1 ) GOTO 435

THE 'EDGE' COMMAND

RP= 1 CALL EDGE(O)

CALL WAIT(32)

DI-22
IF(RP.NE.2)G0T0 738

OFF(2)=OFF(2)-INBUF(3 6)

CALL STLINE(0,0,-INBUF(36) , 1)

738 CALL EDGE(RP)

CALL STLINE(0 , 0 , 500 , 1 ) OFF(3)=OFF(3)+500

CALL STLINE(-OFF( 1 ) ,-0FF(2) ,-0FF(3) , 1 ) IF(INBUF(31 ) .GT.-2000)GOTO 435

IF( RP. EQ. 2)G0T0 435

RP = 2 GOTO 740

CC THE 'TOUCH' FUNCTION IS ACCOMPLISHED BY A CALL TO 'PTOUCH'.

C

8 CALL PTOUCH

110 IF(EABORT.LT.-2000)GOTO 40

C

C THE FOLLOWING CODE RE-DEFINES THE 'DESTINATION' OF THE ELEMENTAL

C MOVE' COMMAND AS THE PRESENT LOCATION OF THE ARM AND ENTERS

C THE PRESENT JOINT POSITION VALUES INTO THE LOCATION

C TABLE UNDER THIS (ELE(2)) DESTINATION NAME.

C DO 420 J=1 ,

6

AC( J)=JPOS(J)

420 LTAB(ELE(2) , 1

, J)=JPOS( J)

C

C THE START OF THE APPROACH PATH MUST BE CALCULATED FROM THIS RE-DEFINED

C LOCATION AND ITS JOINT POSITION VALUES ENTERED UNDER THIS LOCATION

C NAME IN THE LOCATION TABLE.

C AX = LTAB(ELE(2) ,3, 4) /I 00.

AY=LTAB(ELE(2) ,3,5)/100.

AZ=LTAB(ELE(2) ,3, 6) /I 00.

CALL COOR(AX,AY,AZ,0,0)

DO 430 J=1 ,

6

LTAB(ELE(2) , 4 , J ) = JPOS ( J

)

430 JPOS( J)=LTAB(ELE(2) , 1

, J)

C

C IF A VOLTAGE LEVEL IS TO BE SENT OUT ON A SPECIFIED CHANNEL (THIS

C ALLOWS THE ARM TO BE INTERLOCKED WITH AN EXTERNAL DEVICE) THEN

C THE CHANNEL NUMBER IS DECODED AND A SIGNAL SENT OUT.

C

I 435 SEN=ELE(4)/100

IF(SEN . EQ. 0)GOTO 440

CALL SERVO(SEN)

C

C IF THE ARM IS TO WAIT AT ITS PRESENT LOCATION UNTIL AN INPUT CHANNEL

C RECEIVES A VOLTAGE LEVEL (THIS ALLOWS THE ARM TO BE SYNCHRONIZED

C WITH AN EXTERNAL DEVICE) THEN THE INPUT CHANNEL NUMBER IS DECODED

C AND THE CONTROL SYSTEM WAITS UNTIL THIS CHANNEL RECEIVES A

C VOLTAGE LEVEL BEFORE CONTINUING.

; C

i 440 WA=ELE(4)-SEN«100

IF( WA. EQ. 0)GOTO 40

CALL WAIT(WA)

40 RETURN

DI-23
DI-24
C SUBROUTINE : STLINE

C

C ARGUMENTS : DX,DY,DZ = A STRAIGHT LINE TRAJECTORY IS TO BE

C GENERATED FROM THE PRESENT LOCATION OF

C THE ARM TO A POSITION A DELTA 'X'CDX),

C DELTA 'Y' (DY), DELTA 'Z' (DZ) DISTANCE

C AWAY.

C I IF(I=0) THEN THE STRAIGHT LINE IS TO BE

C GENERATED FROM THE PRESENT LOCATION

C TO THE DESTINATION POINT (ELE(2))

C

' SPECIFIED IN THE 'ELEMENTAL MOVE'

C COMMAND.

C IF(I=1) THEN THE STRAIGHT LINE TRAJECTORY

C IS TO BE GENERATED FROM THE PRESENT

C POSITION OF THE ARM TO A POINT 'DX',

C 'DY', 'DZ' DISTANCE AWAY AS SPECIFIED

C BY THE ARGUMENTS OF THE SUBROUTINE

C CALL.

C

C CALLED BY : EMOVE

C

C CALLS SUBROUTINES : COOR SQRT SERVO

C

C INPUT DATA : DX,DY,DZ,I = THE ARGUMENTS AS DESCRIBED ABOVE.

C JP0S(1-6) = THE VALUE OF THE JOINT POSITION

C INDICATORS.

C ELE(2) = THE DESTINATION POINTER VALUE FOR

C THE PRESENT 'ELEMENTAL MOVE'

C COMMAND.

C INBUF(36 ) = THE VALUE OF A POTENTIOMETER

C WHICH IS USED TO CONTROL THE SPEED

C OF THE STRAIGHT LINE TRAJECTORY.

C

C OUTPUT DATA : JP0S(1-6) = THE JOINT POSITION COMMAND VALUES

C SENT TO 'SERVO' TO CAUSE THE HAND

C TO MOVE ALONG THE CALCULATED STRAIGHT

C LINE TRAJECTORY.

C

C FUNCTION: TO GENERATE A STRAIGHT LINE TRAJECTORY FROM THE PRESENT

C POSITION OF THE HAND TO A SPECIFIED LOCATION.

C SUBROUTINE STLINE ( DX , DY , DZ , I

)

IMPLICIT INTEGER(B-R)

C0MM0N/ARMBUF/INBUF(6i») ,OUTBUF(64)

COMMON/LMOD/LTAB(90,4,7) ,PRES,DEST

C0MM0N/0UT/JP0S(8)

COMMON/CORT/AC( 6 )

, AXEP , AYEP , AZEP

C0MM0N/EM/ELE(6)

EQUIVALENCE (EABORT , INBUF(28) ) DIMENSION ADD(6) ,COTAB(50,6)

C

C PLACE THE CURRENT JOINT POSITION VALUES IN THE COMMON VARIABLE

C 'AC(1-6)'. THE POSITION OF THE ARM DESCRIBED BY THESE

C VALUES WILL BE USED AS THE STARTING POINT OF THE

C TRAJECTORY. THE SUBROUTINE 'COOR' WILL CALCULATE

C THE CORRECT JOINT POSITION VALUES OF NEW POSITIONS THAT

DI-25
C • ARE SPECIFIED BY DELTA 'X', 'Y', 'Z' DISTANCES

C AWAY FROM THIS REFERENCE POSITION ('AC(1-6)').

C DO 15 J=1 , 6 15 AC( J)=JPOS( J)

CC THE ARGUMENT 'I' IS TESTED IN ORDER TO BRANCH TO THE PROPER

C CODE.

C IFd.EQ.OGOTO 10

CC FOR (1=1), THE END POINT OF THIS STRAIGHT LINE TRAJECTORY IS DEFINED C AS A POINT 'DX', 'DY', 'DZ' DISTANCE FROM THE PRESENT POSITION.

C THESE DELTA DISTANCES ARE CONVERTED TO CENTIMETERS BY DIVIDING BY

C 100 AND THEN STORED IN THE VARIABLES 'AX', 'AY', 'AZ'.

C AX=DX/100.

AY=DY/100

.

AZ=DZ/ 1 GO

C

C ONCE THE VALUES 'AX', 'AY', 'AZ' HAVE BEEN DETERMINED, THEN BRANCH

C TO STATEMENT 20.

C GOTO 20

C

C FOR (1=0), THE END POINT OF THIS STRAIGHT LINE TRAJECTORY IS DEFINED

C BY THE DESTINATION POINTER ('ELE(2)') FOR THE PRESENT 'ELEMENTAL

C MOVE' COMMAND. THE DELTA 'X', 'Y', 'Z' DISTANCE OF THIS

C DESTINATION FROM THE PRESENT LOCATION IS CALCULATED BY, FIRST,

C CALCULATING THE •X,Y,Z' COORDINATES OF THE PRESENT LOCATION BY

C A CALL TO 'COOR'. THE 'X,Y,Z' COORDINATES OF THE BEGINNING OF

C THE APPROACH PATH ARE OBTAINED FROM THE LOCATION TABLE MODULE

C (IE. THE COORDINATES ( LTAB ( ELE ( 2 ) , 2 , 1 -3 ) ) OF THE END POINT OF THE

C DESTINATION ARE ADDED TO THE DELTA OFFSETS ( LTAB ( ELE ( 2 ) , 3 , 4-6 ) ) C OF THE START OF THE APPROACH PATH FROM THIS END POINT,

C THUS, OBTAINING THE COORDINATES 'X,Y,Z' OF THE START OF THE

C APPROACH PATH). THE DIFFERENCE BETWEEN THESE VALUES AND THE

C' COORDINATES FOR THE PRESENT LOCATION ARE STORED IN 'AX',

C 'AY' , ' AZ •

.

C10 CALL COOR(0. ,0. ,0. ,0, 1 ) AX= (LTAB(ELE (2) , 2 , 1 ) +LTAB ( ELE ( 2 ) ,3,4) )/100 .-AXEP

AY=(LTAB(ELE(2) , 2 , 2 ) +LTAB ( ELE ( 2 ) ,3,5))/100.-AYEP

AZ=(LTAB(ELE(2) , 2 , 3 ) +LTAB ( ELE ( 2 ) ,3,6))/100.-AZEP

C

C THE DISTANCE OF THE STRAIGHT LINE PATH IS CALCULATED.

C20 DV=SQRT( AX»»2+AY»»2+AZ»«2)

C

C IF THE DISTANCE OF THE TRAJECTORY IS ZERO, THEN NO FURTHER

C COMPUTATIONS ARE MADE AND THE CONTROL IS RETURNED TO THE

C CALLING PROGRAM.

C IF(DV. EQ. 0)GOTO 40

0 THE STRAIGHT LINE TRAJECTORY IS TO BE 'DV CENTIMETERS LONG. THIS

DI-26
TRAJECTORY WILL BE GENERATED BY FIRST, CALCULATING A NUMBER OF

INTERMEDIATE LOCATIONS THAT LIE ALONG THIS STRAIGHT LINE AND THEN

CALCULATING THE ADDITIONAL POINTS BY INTERPOLATING BETWEEN THESE

INTERMEDIATE LOCATIONS. THE INTERMEDIATE LOCATIONS WILL LIE

APPROXIMATELY TWO CENTIMETERS APART ALONG THE STRAIGHT LINE.

THE NUMBER OF THESE INTERMEDIATE LOCATIONS IS CALCULATED BY

DIVIDING THE LENGTH OF THE TRAJECTORY ('DV') BY TWO. THIS VALUE

IS INCREMENTED BY ONE TO MAKE CERTAIN THAT THE NUMBER OF POINTS IS

A NON-ZERO VALUE IF THE TRAJECTORY IS LESS THAN TWO CENTIMETERS

LONG.

NDV=DV/2+1

THE DELTA 'X.Y.Z' COORDINATE VALUES BETWEEN EACH OF THE INTERMEDIATE

LOCATIONS IS CALCULATED AND STORED IN 'ANX', 'ANY', 'ANZ'.

ANX=AX/NDV

ANY=AY/NDV

ANZ=AZ/NDV

THE JOINT POSITION VALUES OF THE INTERMEDIATE LOCATIONS ARE CALCULATED

BY CALLS TO 'COOR' AND ARE STORED IN SEQUENCE IN THE TABLE

'COTAB(50,6) •

.

KD=NDV+1

DO 1 1 0 J= 1, KD

KLL=J-1

ATX=KLL»ANX

ATY=KLL»ANY

ATZ=KLL»ANZ

CALL COOR(ATX,ATY,ATZ,0,0)

DO 110 KB= 1,

6

) COTAB( J,KB)=JPOS(KB)

THE VALUE OF THE POTENTIOMETER (INBUF(36)) IS READ IN AND USED TO

DEFINE THE NUMBER OF ADDITIONAL INTERPOLATION POINTS ('PC') TO BE

CALCULATED BETWEEN EACH PAIR OF INTERMEDIATE LOCATIONS.

PC=10-INBUF(36) /30

ASP=PC

A LOOP IS SET UP TO PROVIDE THE INTERPOLATED JOINT POSITION COMMANDS

FOR ALL 'NDV OF THE INTERMEDIATE LOCATIONS.

DO 150 P=1,NDV

FOR EACH INTERMEDIATE LOCATION, THE DELTA JOINT VALUES FOR THE

SPECIFIED NUMBER ('ASP') OF INTERPOLATION POINTS IS CALCULATED

AND STORED IN 'ADD(1-6)'.

DO 140 MP=1 ,

6

jl40 ADD(MP) = (C0TAB(P+1 , MP ) -COTAB ( P , MP ) ) /ASP

jC THIS LOOP CAUSES THE ARM TO MOVE FROM ONE INTERMEDIATE LOCATION TO

C THE NEXT BY INCREMENTING THE JOINT POSITION COMMANDS BY THE

C PROPER DELTA INTERPOLATION VALUE ('ADD(1-6)') EACH TIME.

DI-27
DO 150 LM=1,PC

DO 160 JB=1,6

JPOS( JB) =COTAB(P, JB)+LM«ADD( JB)

CALL SERVO(O)

IF(EAB0RT.LT.-2000)G0T0 40

CONTINUE

RETURN

END

DI-28

DI-30
C SUBROUTINE : WAIT

CC ARGUMENTS : CH = THE INPUT CHANNEL NUMBER THAT IS TO

C SAMPLED FOR A VOLTAGE LEVEL.

CC CALLED BY : EXPRO EMOVE

CC CALLS SUBROUTINES : ARMIN

C

C INPUT DATA : CH = THE ARGUMENT AS EXPLAINED ABOVE.

C

C OUTPUT DATA : C

C FUNCTION: SUSPENDS OPERATION OF THE CONTROL SYSTEM UNTIL A

C SPECIFIED INPUT CHANNEL DROPS TO A ZERO VOLTAGE

C LEVEL OR THE ABORT SWITCH IS THROWN.

C SUBROUTINE WAIT(CH)

IMPLICIT INTEGER(B-Z)

COMMON /ARMBUF/INBUF( 64) , 0UTBUF(64)

EQUI VALENCE (EAB0RT,INBUF( 28)

)

C

C THE INPUT CHANNELS ARE READ IN, AND THE VALUE OF THE SPECIFIED

C CHANNEL ('CH') IS TESTED TO SEE IF IT IS A POSITIVE OR NEGATIVE

C VALUE, INDICATING THAT THE PROGRAM IS TO WAIT UNTIL THE

C SPECIFIED INPUT CHANNEL RECEIVES A HIGH

C OR A ZERO VOLTAGE LEVEL, RESPECTIVELY.

C20 CALL ARMIN

IF(EABORT .LT. -2000 )GOTO 40

IF(CH.LT.O)GOTO 25

C

C THE VALUE OF THE SPECIFIED INPUT

C CHANNEL IS TESTED. AS LONG AS THERE IS ZERO V-OLTAGE ON THIS

C CHANNEL, THE SUBROUTINE WILL CONTINUE SAMPLING IT. WHEN THE

C VOLTAGE LEVEL GOES ABOVE ZERO, CONTROL IS RETURNED TO THE CALLING

C PROGRAM.

C IF(INBUF(CH) .GT.-2000)G0T0 20

GOTO 40

C

C THE VALUE OF THE SPECIFIED INPUT

C CHANNEL IS TESTED. AS LONG AS THERE IS A VOLTAGE LEVEL ON THIS

C CHANNEL, THE SUBROUTINE WILL CONTINUE SAMPLING IT. WHEN THE

C VOLTAGE LEVEL DROPS TO ZERO, CONTROL IS RETURNED TO THE CALLING

C PROGRAM.

C25 IF(INBUF(-CH) .LT.-2000)GOTO 20

40 RETURN

END

DI-31
DI-32
C SUBROUTINE : CALD

C

C ARGUMENTS : C

C CALLED BY : EMOVE

C

C CALLS SUBROUTINES : COOR SQRT

C

C INPUT DATA : JP0S(1-6) = THE PRESENT JOINT POSITION VALUES.

C ELE(2) = THE VALUE OF THE DESTINATON POINTER

C TO THE LOCATION TABLE.

C AXEP = THE 'X' COORDINATE VALUE FOR THE HAND

C FOR THE SPECIFIED JOINT VALUES (AC(1-6))

C [FROM 'COOR'].

C AYEP = THE 'Y' COORDINATE VALUE FOR THE HAND

C FOR THE SPECIFIED JOINT VALUES (AC(1-6))

C [FROM 'COOR'].

C AZEP = THE 'Z' COORDINATE VALUE FOR THE HAND

C FOR THE SPECIFIED JOINT VALUES (AC(1-6))

C [FROM 'COOR'].

C

C OUTPUT DATA : AC(1-6) = THE JOINT POSITION VALUES TO BE USED

C BY THE COORDINATE TRANSFORMATION

C ROUTINE ('COOR').

C ADIST = THE DISTANCE IN CENTIMETERS BETWEEN

C THE PRESENT LOCATION OF THE ARM AND

C THE SPECIFIED DESTINATION

C POINT (ELE(2)).

CC FUNCTION: CALCULATES THE DISTANCE (IN CENTIMETERS) FROM THE PRESENT

C LOCATION OF THE END POINT OF THE HAND TO ITS POSITION

C AT THE DESTINATION POSITION.

C SUBROUTINE CALD

IMPLICIT INTEGER(B-R)

COMMON/LMOD/LTAB(90,4,7) ,PRES,DEST

C0MM0N/0UT/JP0S(8)

C0MM0N/EM/ELE(6)

COMMON/DIST/ADIST

C0MM0N/C0RT/AC(6 ), AXEP , AYEP , AZEP

C

C STORE THE JOINT POSITION VALUES IN THE VARIABLE (AC(1-6))

C SO THAT THE 'X', 'Y', 'Z' VALUES FOR THIS POINT CAN BE

C CALCULATED BY THE COORDINATE TRANSFORMATION ROUTINE.

C DO 20 J = 1 ,6

20 AC( J)=JPOS( J)

CC THE CALL TO • COOR ( X , Y , Z , D , J

)

' IS MADE WITH 'J' = 1 TO INDICATE

,C THAT ONLY THE 'X'.'Y'.'Z' VALUES ARE TO BE RETURNED (IE. THE

C JOINT VALUES ARE NOT TO BE CALCULATED).

C CALL COOR (0 . , 0 . , 0 .

, 0 , 1 ) CC THE 'X.Y.Z' VALUES OF THE PRESENT LOCATION OF THE ARM ARE STORED

C IN THE VARIABLES 'APX', 'APY', AND 'APZ'.

DI-33
c APX=AXEP

APY=AYEP

APZ=AZEP

C

C CALCULATE THE 'X,Y,Z' VALUES OF THE DESTINATION POINT (ELE(2)) BY

C PLACING ITS JOINT POSITION VALUES IN THE COMMON VARIABLE

C AC(1-6) AND CALLING 'COOR'.

C DO 21 J = 1 , 6 21 AC( J)=LTAB(ELE(2) ,1, J)

CALL COOR (0 ., 0. , 0. , 0 , 1

)

CC THE DISTANCE BETWEEN THESE TWO POINTS IS CALCULATED AND STORED IN

C THE COMMON VARIABLE 'ADIST'.

C ADX=APX-AXEP

ADY=APY-AYEP

ADZ=APZ-AZEP

ADIST=SQRT{ADX««2+ADY»»2+ADZ*»2)

40 RETURN

END

DI-34

DI-36
C SUBROUTINE : POL

CC ARGUMENTS : C

C CALLED BY : EMOVE

C

C CALLS SUBROUTINES : ARMIN

C

C INPUT DATA : INBUF(26) = THE VALUE OF A POTENTIOMETER USED TO

C SPECIFY THE VELOCITY OF THIS TRAJECTORY

C BY DEFINING THE NUMBER OF INTERPOLATION

C POINTS TO BE USED PER UNIT DISTANCE.

C ADIST = THE DISTANCE BETWEEN THE TWO END POINTS

C OF THE TRAJECTORY IN CENTIMETERS

C [FROM 'CALD'].

C ELE(1) = THE LOCATION TABLE POINTER FOR THE

C PRESENT POSITION OF THE ARM.

C ELE(2) = THE LOCATION TABLE POINTER FOR THE

C DESTINATION OF THIS TRAJECTORY.

C JP0S(1-6) = THE JOINT POSITION VALUES OF THE

C LOCATION OF THE ARM.

C LTAB(ELE(2) ,4, 1-6)= THE JOINT POSITION VALUES OF THE

C START OF THE APPROACH PATH FOR THE

C SPECIFIED DESTINATION POINT.

C LTAB(ELE(1 ) ,2,7)= THE FLAG THAT INDICATES IF THE ARM

C HAS STOPPED AT THE PRESENT LOCATION.

C LTABCELE ( 2 ) , 2 , 7 ) = THE FLAG THAT INDICATES IF THE ARM

C IS TO STOP AT THE DESTINATION POINT.

C

C OUTPUT DATA : NACV = THIS IS THE NUMBER OF COMMANDED

C JOINT POSITION OUTPUTS FOR THE

C ACCELERATION (OR DECCELERATION ) PORTION

C OF THE TRAJECTORY.

C NVPT = THIS IS THE NUMBER OF COMMANDED JOINT

C POSITION OUTPUTS FOR THE CONSTANT

C VELOCITY PORTION OF THE TRAJECTORY.

C KJ = A FLAG THAT CODES THE INFORMATION OF

C WHETHER AN ACCELERATION OR DECCELERATION

C PROFILE IS TO BE USED.

C ACDIF(1-6) = THE DELTA JOINT POSITION VALUES THAT

C WOULD CAUSE THE ARM TO MOVE 1/400 OF

C A CENTIMETER ALONG THE INTERPOLATED

C TRAJECTORY. USED AS THE ACCELERATION

C FACTOR FOR THE ACCELERATION PORTION OF

C THE TRAJECTORY.

C AVDIF(1-6) = THE DELTA JOINT POSITION VALUES THAT

C WOULD CAUSE THE ARM TO MOVE 'NACV

C TIMES 1/400 CENTIMETERS ALONG THE

C CONSTANT VELOCITY PORTION OF THE

C TRAJECTORY. THUS, PROVIDING THE PROPER

C SIZE STEP REQUIRED FOR THE ARM TO MOVE

C AT THE SPECIFIED VELOCITY.

C

C FUNCTION: GENERATES THE DELTA JOINT POSITION VALUES AND SPECIFIES

C THE NUMBER OF THEM REQUIRED TO CAUSE THE ARM TO MOVE

C THROUGH THE TRAJECTORY AT THE CORRECT VELOCITY. THE

DI-37
C ACTUAL EXECUTION IS ACCOMPLISHED BY THE SUBROUTINE 'ACC

C WHICH USES THIS INFORMATION FROM 'POL'.

C SUBROUTINE POL

IMPLICIT INTEGER(B-Z)

COMMON/ARMBUF/INBUF(64) ,OUTBUF(64)

COMMON /LMOD/LTAB( 9 0,4,7) ,PRES,DEST

C0MM0N/0UT/JP0S(8)

COMMON/INT/NACV ,NVPT , KJ

C0MM0N/DELT/ACDIF(6) ,AVDIF(6)

COMMON/DIST/ADIST

C0MM0N/EM/ELE(6)

EQUI VALENCE (EABORT,INBUF( 28 )

)

C

C THE POTENTIOMETER VALUE THAT WILL CONTROL THE VELOCITY IS READ

C IN ON 'INBUF(26)'. IT IS NORMALIZED TO A VALUE BETWEEN

C ONE AND TEN AND MULTIPLIED BY THE SPECIFIED VELOCITY 'ELE(3)'

C FOR THIS PARTICULAR TRAJECTORY. THE VALUE 'NACV IS SET

C EQUAL TO THIS NUMBER DIVIDED BY 5. THIS NUMBER SPECIFIES HOW

C MANY INCREMENTAL STEPS ( ' ACDIF (

1 -6 )

' ) ARE TO BE SENT OUT

C AT A TIME TO THE SERVOS IN ORDER TO CAUSE THE ARM TO MOVE AT THE

C CORRECT VELOCITY.

C

CALL ARMIN

NACVr ( (-INBUF(26)-2 100)/100+1 )«ELE(3)

NACV=NACV/10

C

C THE VALUE 'ANTRJP' IS SET EQUAL TO 100 TIMES THE TOTAL DISTANCE OF

C THE TRAJECTORY IN CENTIMETERS. THE DIFFERENTIAL JOINT VALUES FROM

C THE PRESENT LOCATION TO THE DESTINATION ( ' LTAB ( ELE ( 2 ) , 4 , 1 -6 )

' ) C ARE DIVIDE BY 'ANTRJP'. THIS CREATES THE DELTA JOINT VALUES THAT

C THAT WOULD CAUSE A MOVEMENT OF THE ARM OF 1/100 OF A CENTIMETER.

C

ANTRJP=ADIST»100

.

DO 20 H= 1

,

6

20 ACDIF(H)=(LTAB(ELE(2) , 4 , H

) - JPOS ( H ) ) /ANTRJP

C

C THE POSITION LTAB(J,2,7), CARRIES A FLAG THAT INDICATES IF THE ARM

.0 IS TO STOP AT THAT LOCATION. IF IT IS A ONE THEN THE ARM STOPS,

C THEREFORE, AN ACCELERATION PROFILE IS NEEDED FOR THAT POINT.

C IF IT IS ZERO, IT DOES NOT STOP AND NO ACCELERATION PROFILE IS

C TO BE USED. THE VALUE 'KJ' IS SET EQUAL TO THE SUM OF THE FLAGS

C OF THE PRESENT LOCATION AND THE DESTINATION PLUS ONE. THE

C VALUE 'KJ' IS THEN USED TO POINT TO THE APPROPRIATE CODE

C FOR THE CORRECT ACCELERATION PROFILES.

C

KJ = 2»LTAB(ELE(2) ,2,7)+LTAB(ELE( 1 ) ,2,7) + 1 GOTO ( 1 10 , 1 1 1

, 1 1 1

, 1 12 )KJ

C

C THIS IS THE CONDITION FOR NO ACCELERATION OR DECCELERATION PROFILES.

C110 NACCP=0

GOTO 115

C

C THIS IS THE CONDITION FOR EITHER AN ACCELERATION OR A DECCELERATION

C PROFILE. 'NACCP' IS THE TOTAL NUMBER OF INCREMENTAL STEPS

DI-38
C ( • ACDIF( 1 -6) ' ) THAT THE ARM WILL MOVE THROUGH DURING THE

C ACCELERATION OR DECCELER ATION . THEREFORE, 'NACCP' IS A MEASURE

C OF HOW MUCH OF THE TRAJECTORY WILL BE USED FOR THE ACCELERATION OR

C DECCELERATION REGION.

C111 NACCP= (NACV««2+NACV)/2

GOTO 115

C

C THIS IS THE CONDITION FOR BOTH AN ACCELERATION AND A DECCELERATION

C REGION FOR THIS TRAJECTORY, THEREFORE, THERE WILL BE TWICE AS MANY

C INCREMENTAL STEPS ( ' ACDIF (

1 -6 )

• ) INVOLVED IN THESE REGIONS.

C112 NACCP=NACV»»2+NACV

C

C THE NUMBER OF INCREMENTAL STEPS USED IN THE ACCELERATION-DECCELERATION

C PROFILES IS SUBTRACTED FROM THE TOTAL NUMBER OF STEPS IN THE

C TRAJECTORY TO YIELD THE VALUE 'ANVELP'. THIS NUMBER IS DIVIDED BY

C 'NACV (THE NUMBER OF STEPS TO BE SENT TO THE SERVOS AT ONE

C TIME IN ORDER TO CAUSE THE ARM TO MOVE AT THE CORRECT VELOCITY)

C TO GIVE THE NUMBER 'NVPT' (THE NUMBER OF TIMES THAT THE DELTAS

C 'AVDIFd-S)' ARE TO BE SENT TO THE SERVOS FOR THE CONSTANT

C VELOCITY PORTION OF THE TRAJECTORY.

C115 ANVELP=ANTRJP-NACCP

NVPT=ANVELP/NACV

IF(NVPT.NE.0)GOTO 4?

NVPT=

47 AMV=ANVELP/NVPT

DO 200 J = 1 , 6 200 AVDIF( J)=ACDIF( J)«AMV

C WRITE(8, 320) NACV, NACCP, ANTRJP , ANVELP , AMV

320 F0RMAT(2X,2( 1X,I6) ,3(1X,F10.M))

C WRITE(6 , 321 ) ADIST , ELE ( 3 ) ,INBUF(26)

321 FORMAT(2X,F10.4,2J(,I5,2X,I6)

40 RETURN

END

DI-39
DI-40
C SUBROUTINE : ACC

C

C ARGUMENTS : C

C CALLED BY : EMOVE

C

C CALLS SUBROUTINES : SERVO

C

C INPUT DATA : NACV = THIS IS THE NUMBER OF COMMANDED

C JOINT POSITION OUTPUTS FOR THE

C ACCELERATION (OR DECCELERATION ) PORTION

C OF THE TRAJECTORY.

C NVPT = THIS IS THE NUMBER OF COMMANDED JOINT

C POSITION OUTPUTS FOR THE CONSTANT

C VELOCITY PORTION OF THE TRAJECTORY.

C KJ = A FLAG THAT CODES THE INFORMATION OF

C WHETHER AN ACCELERATION OR DECCELERATION

C PROFILE IS TO BE USED.

C ACDIF(1-6) = THE DELTA JOINT POSITION VALUES THAT

C WOULD CAUSE THE ARM TO MOVE 1/200 OF

C A CENTIMETER ALONG THE INTERPOLATED

C TRAJECTORY. USED AS THE ACCELERATION

C FACTOR FOR THE ACCELERATION PORTION OF

C THE TRAJECTORY.

C AVDIF(1-6) = THE DELTA JOINT POSITION VALUES THAT

C WOULD CAUSE THE ARM TO MOVE 'NACV

C TIMES 1/200 CENTIMETERS ALONG THE

C CONSTANT VELOCITY PORTION OF THE

C TRAJECTORY. THUS, PROVIDING THE PROPER

C SIZE STEP REQUIRED FOR THE ARM TO MOVE

C AT THE SPECIFIED VELOCITY.

CC OUTPUT DATA : JP0S(1-6) = THE COMMANDED JOINT POSITION VALUES

C TO THE SERVOS THAT WILL MOVE THE

C ARM THROUGH THE PROPER TRAJECTORY IN

C SPACE.

C

C FUNCTION: CALCULATES AND OUTPUTS TO 'SERVO' THE CORRECT JOINT

C POSITION COMMANDS TO CAUSE THE ARM TO MOVE THROUGH ITS

C SPECIFIED TRAJECTORY USING ACCELERATION AND DECELERATION

C ROUTINES WHERE NECESSARY.

C SUBROUTINE ACC

IMPLICIT INTEGER(B-Z)

C0MM0N/EM/ELE(6

)

COMMON /LMOD/LTAB( 9 0,4,7) ,PRES,DEST

COMMON /ARMBUF/INBUF( 64) ,OUTBUF(64)

C0MM0N/0UT/JP0S(8)

C0MM0N/INT/NACV,NVPT,KJ

COMMON/DELT/ACDIF( 6 ) ,AVDIF(6)

EQUI VALENCE (EABORT,INBUF( 28)

)

DIMENSION AJP0S(6)

C

C THE JOINT POSITION VALUES ARE CONVERTED TO FLOATING POINT VALUES,

C SO THAT THEY CAN BE INCREMENTED BY VALUES LESS THAN ONE IF

C NECESSARY.

DI-41
c DO 10 K=1 ,

6

10 AJPOS(K)=JPOS(K)

C

C THE FLAG 'KJ' (WHOSE VALUE IS DETERMINED BY WHETHER OR NOT

C ACCELERATION AND/OR DECCELERATION PROFILES ARE TO BE USED)

C IS USED TO POINT TO THE APPROPRIATE CODE TO BE EXECUTED.

C GOTO (11,12,11, 12)KJ

C

C THIS IS THE CODE THAT CAUSES AN ACCELERATION TO BE PROVIDED AT THE

C BEGINNING OF THE TRAJECTORY. THE ACCELERATION LASTS FOR 'NACV

C OUTPUTS TO THE SERVOS. EACH TIME THE JOINT POSITION COMMANDED

C OUTPUT IS INCREMENTED BY A DELTA THAT IS 'ACDIF(J)' LARGER THAN

C THE LAST ONE, THEREBY PROVIDING AN ACCELERATION UP TO

C THE SPECIFIED VELOCITY.

C12 DO 400 LD=1 ,NACV

DO 401 J=1 ,

6

AJPOS( J)=AJPOS( J)+LD»ACDIF( J)

401 JPOS(J)=AJPOS( J)

CALL SERVO(O)

IF(EABORT.LT.-2000)GOTO 40

400 CONTINUE

CC THIS IS THE SECTION OF THE CODE THAT PROVIDES THE CONSTANT

C VELOCITY PORTION OF THE TRAJECTORY. THE JOINT POSITION COMMANDED

C VALUES ARE INCREMENTED BY THE DELTA VALUES 'AVDIF(1-6)' FOR

C 'NVPT' TIMES TO CAUSE THE ARM TO MOVE AT A CONSTANT VELOCITY

C THROUGH THIS PORTION OF THE TRAJECTORY.

C

1 1 DO 500 LD= 1

, NVPT

DO 501 J=1 ,

6

AJPOS( J)=AJPOS( J)+AVDIF( J)

501 JPOS(J)=AJPOS( J)

CALL SERVO(O)

IF(EAB0RT.LT.-2000)GOTO 40

500 CONTINUE

C

C THE FLAG 'KJ' IS USED TO RETURN TO THE CALLING PROGRAM ('EMOVE') IF

C NO DECCELERATION IS TO BE USED OR EXECUTE THE NEXT SECTION OF CODE

C IF A DECCELERATION IS CALLED FOR.

C GOTO (40,40, 13, 13)KJ

C

C THIS SECTION OF CODE CREATES A DECCELERATION PROFILE. THE

C DECCELERATION LASTS FOR 'NACV OUTPUTS TO THE SERVOS.

C FOR EACH OUTPUT, THE JOINT POSITION COMMANDED VALUES ARE

C INCREMENTED BY A DELTA THAT IS 'ACDIF(J)' SMALLER THAN THE LAST

C ONE UNTIL THE DESTINATION POSITION HAS BEEN REACHED.

C13 DO 600 LD=1 ,NACV

LLD=NACV+1 -LD

DO 601 J=1 ,

6

AJPOS( J)=AJPOS( J)+LLD»ACDIF( J)

601 JPOS(J)=AJPOS(J)

DI-42
CALL SERVO(O)

IF(EABORT.LT.-2000)GOTO 40

600 CONTINUE

C47 WRITE (6, 48) (LTAB(ELE(2) ,4,J),J=1,6) , ( JPOS(H) ,H=1 ,6)

48 F0RMAT(2X, 6(2X,I6)

)

40 RETURN

END

DI-43
DI-44
C SUBROUTINE : DETECT

C

C ARGUMENTS : C

C CALLED BY : EMOVE

CC CALLS SUBROUTINES : COOR ARMIN SERVO

C

C INPUT DATA : JP0S(7) = THE PRESENT JOINT POSITION VALUES

C PI = THE PRESENT VALUE OF THE SIGNAL

C FROM THE PROXIMITY SENSOR ON

C ONE OF THE FINGERS [FROM 'ARMIN'

C ON INPUT CHANNEL NUMBER 49].

C P2 = THE PRESENT VALUE OF THE SIGNAL

i C FROM THE PROXIMITY SENSOR ON

C THE OTHER FINGER [FROM 'ARMIN'

C ON INPUT CHANNEL NUMBER 48].

CC OUTPUT DATA : JP0S{7) = THE COMMANDED JOINT POSITION VALUES

C THAT WILL CAUSE THE ARM TO MOVE

C ALONG A SEARCH PATH IN THE 'Y'

C DIRECTION [FROM 'COOR'].

C

C FUNCTION: TESTS THE PROXIMITY SENSORS FEEDBACK FOR A LARGE ENOUGH

C SIGNAL TO INDICATE THAT THERE IS AN OBJECT IN FRONT OF

C AT LEAST ONE OF THE SENSORS. IF THIS THRESHOLD SIGNAL

C IS NOT DETECTED BY EITHER SENSOR, THEN THE HAND

I C IS MOVED IN A SEARCH PATTERN UNTIL THE THRESHOLD

I C LEVEL IS DETECTED.

i Cc SUBROUTINE DETECT

IMPLICIT INTEGER(B-Z)

COMMON/ARMBUF/INBUF( 64 ) ,OUTBUF(64)

C0MM0N/0UT/JP0S(8)

C0MM0N/C0RT/AC(6) , AXEP , AYEP , AZEF

C0MM0N/0FFS/0FF( 3

)

EQUIVALENCE(EAB0RT,INBUF(28)

)

EQUIVALENCE(P1 ,INBUF(49) ) , (P2,INBUF(48)

)

CALL ARMIN

C

C PLACE THE PRESENT JOINT POSITION VALUES IN THE MATRIX AC(6) TO BE

Ij C USED BY THE COORDINATE TRANSFORMATION {'COOR') CALL.

I C DO 51 JJ=1 ,

6

51 AC( JJ ) rJPOS ( JJ

)

C

C SET THE COUNTER FOR MOVEMENT IN THE 'Y' DIRECTION EQUAL TO ZERO.

C AY = 0.

C

I C ZERO THE FLAG ('MIN') USED TO INDICATE THAT THE HAND SHOULD BE

I C MOVING IN THE MINUS 'Y' DIRECTION.

C60 MIN=0

C

DI-45
C TEST EACH PROXIMITY SENSOR FOR THE THRESHOLD VALUE AND RETURN

C TO THE CALLING PROGRAM ('EMOVE') IF THIS VALUE IS DETECTED.

C20 IF(P1 .GT.20G0)GOTO 45

IF(P2.GT.2000)GOTO 45

C

C IF MOVING IN THE '-Y' DIRECTION, THEN BRANCH TO THE SECTION OF

C CODE TO DECREMENT THE DELTA 'Y' COUNTER ('AY').

C IF(MIN.EQ. 1 )GOTO 30

C

C ONCE THE HAND HAS MOVED 15 CENTIMETERS IN THE PLUS DIRECTION,

C THEN BRANCH TO THE CODE TO MOVE THE HAND IN THE MINUS

C DIRECTION.

C IF(AY.GT. 15. )GOTO 31

C

C THIS SECTION OF CODE ASSIGNS THE DELTA MOTION ('AYSIGN') AS

C A POSITIVE .2 CENTIMETER.

C25 MIN=0

AYSIGN=.2

GOTO 50

C

C IF THE HAND HAS MOVED 15 CENTIMETERS IN THE MINUS DIRECTION FROM

C THE INITIAL LOCATION, THEN BRANCH TO THAT CODE TO REVERSE

C THE DIRECTION OF MOTION TO THE '+Y' DIRECTION.

C30 IF(AY.LT.-15. )GOTO 25

C

C THIS SECTION OF CODE ASSIGNS THE DELTA MOTION ('AYSIGN') AS

C A NEGATIVE .2 CENTIMETER. THE FLAG ('MIN') INDICATING

C MOVEMENT IN THE NEGATIVE 'Y' DIRECTION IS SET EQUAL TO ONE.

C

31 MIN=1

AYSIGN=-.2

C

C HERE, THE DELTA MOTION IN THE 'Y' DIRECTION ('AYSIGN') IS ADDED

C TO THE TOTAL DELTA 'Y' ('AY') FROM THE ORIGINAL STARTING POINT

C TO DETERMINE WHAT THE NEW DELTA IS.

C50 AY=AY+AYSIGN

C

C THIS DELTA 'Y' OFFSET FROM THE STARTING POINT IS TRANSFORMED

C INTO THE PROPER JOINT POSITION VALUES BY A CALL TO

C 'COOR'. THESE JOINT POSITION VALUES ARE THEN

C SENT TO THE SERVOS TO BE EXECUTED BY A CALL TO 'SERVO'. IN

C THIS WAY THE HAND MOVES IN THE 'Y' DIRECTION ALONG A PATH

C THAT EXTENDS 15 CENTIMETERS ON EITHER SIDE OF THE STARTING

C POINT MOVING IN INCREMENTS OF .2 CENTIMETERS AND CHECKING FOR

C THE THRESHOLD LEVEL FROM THE SENSORS.

C CALL COOR(0. ,AY,0. ,0,0)

CALL SERVO(O)

IF(EABORT.GT.-2000)GOTO 20

DI-46
0FF(2)=0FF(2)+100»AY

40 RETURN

END

DI-47
DI-48
C SUBROUTINE : BAL

C

C ARGUMENTS : C

C CALLED BY : EMOVE

CC CALLS SUBROUTINES : COOR ARMIN SERVO

CC INPUT DATA : JP0S(7) = THE PRESENT JOINT POSITION VALUES

C PI = THE PRESENT VALUE OF THE SIGNAL

C FROM THE PROXIMITY SENSOR ON

C ONE OF THE FINGERS [FROM 'ARMIN'

C ON INPUT CHANNEL NUMBER 49].

C P2 = THE PRESENT VALUE OF THE SIGNAL

C FROM THE PROXIMITY SENSOR ON

C THE OTHER FINGER [FROM 'ARMIN'

C ON INPUT CHANNEL NUMBER 48].

C

C OUTPUT DATA : JP0S(7) = THE COMMANDED JOINT POSITION VALUES

C THAT WILL CAUSE THE ARM TO MOVE

C ALONG A SEARCH PATH IN THE 'Y'

C DIRECTION [FROM 'COOR'].

C

C FUNCTION: ASSUMES THAT AT LEAST ONE OF THE PROXIMITY SENSORS

C IS ABOVE THE THRESHOLD VALUE, IE. AN OBJECT HAS BEEN

C DETECTED). THIS ROUTINE THEN INCREMENTS THE HAND'S

C MOTION IN THE DIRECTION SO AS TO CAUSE THE TWO PROXIMITY

C SIGNALS TO BE EQUAL. IF BOTH SENSORS DO NOT REACH

C THE THRESHOLD VALUE, THIS ROUTINE CAUSES THE

C HAND TO MOVE UNTIL THE OBJECT DETECTED IS

C WITHIN THE GRASP OF THE HAND (IE. IT ALIGNS THE INSIDE

C EDGE OF ONE FINGER WITH THE OUTSIDE EDGE OF THE DETECTED

C OBJECT),

C SUBROUTINE BAL

IMPLICIT INTEGER{B-Z)

COMMON/ARMBUF/INBUF( 64 ), OUTBUF( 64)

C0MM0N/0UT/JP0S(8)

C0MM0N/C0RT/AC(6) , AXEP , AYEP , AZEP

C0MM0N/0FFS/0FF(3)

EQUI VALENCE (EABORT,INBUF( 28) ) EQUIVALENCE (P 1 ,INBUF (49) ), (P2,INBUF(48)

)

CALL ARMIN

CC PLACE THE PRESENT JOINT POSITION VALUES IN THE MATRIX AC(6) TO BE

C USED BY THE COORDINATE TRANSFORMATION ('COOR') CALL.

C DO 51 JJ=1 ,6

51 AC( JJ)=JPOS( JJ)

C

C SET THE COUNTER FOR MOVEMENT IN THE 'Y' DIRECTION EQUAL TO ZERO.

C AY = 0 . C

C THE FLAGS ('M' AND 'P') TO INDICATE IF THE HAND IS BEING MOVED

C IN A MINUS OR PLUS 'Y' DIRECTION DURING THE BALANCE FUNCTION,

DI-49
C ARE SET EQUAL TO ZERO.

C M = 0 P = 0

CC THE DIFFERENCE BETWEEN THE VALUES OF THE TWO PROXIMITY SENSORS IS

C CALCULATED AND STORED IN 'DS'.

C30 DS=P1-P2

C

C THE ALGEBRAIC SIGN OF 'DS' IS AN INDICATION OF WHICH OF THE TWO

C SIGNALS IS THE LARGER AND IS USED TO DETERMINE WHICH

C CODE SHOULD NOW BE EXECUTED.

C IF(DS.GT.O)GOTO 20

C

C IF 'P2' IS THE LARGER SIGNAL, THEN THE HAND SHOULD MOVE IN THE

C '-Y' DIRECTION, THEREFORE, THE DELTA 'Y' MOTION VALUE ('AYSIGN')

C IS ASSIGNED THE VALUE OF -.05 CENTIMETERS. THE MINUS 'Y'

C DIRECTION FLAG ('M') IS SET EQUAL TO ONE.

C AYSIGN=- . 05

M=1

C

C THE PLUS 'Y' DIRECTION FLAG ('P') IS TESTED. IF IT IS EQUAL TO

C ONE, THIS MEANS THAT THE HAND WAS MOVING IN THE PLUS 'Y'

C DIRECTION AND NOW THE SENSORS INDICATE IT SHOULD MOVE IN THE

C MINUS 'Y' DIRECTION. FOR THIS SITUATION TO OCCUR, THE

C SENSORS MUST HAVE PAST THROUGH A BALANCE POINT (IE. 'PI' WAS

C THE LARGER, NOW 'P2' IS THE LARGER. BETWEEN THESE TWO

C CONDITIONS, 'PI' MUST HAVE EQUALED 'P2'), THEREFORE, RETURN TO

C THE CALLING PROGRAM ( ' EMOVE ' ) BECAUSE THE BALANCE POINT HAS BEEN

C OBTAINED.

C IF(P. EQ. 1 )GOTO 45

C

C IF THE CONDITION OCCURS THAT 'P2' STARTS DECREASING

C IN VALUE BEFORE THE 'BALANCE' CONDITION IS REACHED, THEN

C BRANCH TO THE SECTION OF CODE THE 'EDGE' DETECTION. SINCE 'P2'

C WAS ABOVE THE THRESHOLD LEVEL AND IS NOW DECREASING, THE SENSOR

G MUST BE PASSING ACROSS THE EDGE OF THE UNDERLYING OBJECT.

C IF(P2.LT. 1500)G0T0 75

GOTO 50

CC IF 'PI' IS THE LARGER SIGNAL, THEN MOVE IN THE PLUS 'Y' DIRECTION

C BY ASSIGNING A VALUE OF +.05 CENTIMETERS TO 'AYSIGN'.

C20 AYSIGN=.05

C

C THE SAME TYPE OF TESTS MADE FOR THE MINUS 'Y' MOTION ABOVE

C ARE ALSO MADE HERE FOR THE PLUS 'Y' MOTION.

C P= 1 IF(M. EQ. 1 )GOTO 45

IF(P1 .LT. 1500)GOTO 80

DI-50
c

C THE ACTUAL COMMANDED MOTION OF THE ARM IS CALCULATED BY THE

C FOLLOWING CODE. FIRST 'AY', THE TOTAL DELTA 'Y' MOTION FROM

C THE STARTING POINT IS CALCULATED USING THE DELTA 'Y' VALUE

C ASSIGNED ABOVE. THEN A CALL TO 'COOR', THE COORDINATE

C TRANSFORMATION ROUTINE, CALCULATES THE CORRECT JOINT POSITION

C VALUES FOR THE LOCATION 'AY' DISTANCE AWAY FROM THE STARTING

C POINT. THEN A CALL TO 'SERVO' SENDS THESE VALUES OUT TO THE SERVO

C SYSTEM OF THE ARM. 'ARMIN' IS CALLED TO BRING IN THE CURRENT

C VALUES OF THE PROXIMITY SENSORS FOR ANOTHER TEST, STARTING

C BACK AT STATEMENT 30.

C50 AY=AY+AYSIGN

CALL COOR(0. ,AY,0. ,0,0)

CALL SERVO(O)

IF(EABORT.LT.-2000)GOTO 40

GOTO 30

CC IF AN EDGE IS DETECTED BY 'P2', THEN THE FOLLOWING CODE ADVANCES

C THE HAND AN ADDITIONAL DISTANCE IN THE '-Y' DIRECTION (WHICH

C DISTANCE IS EQUAL TO THE THICKNESS OF THE FINGER THAT THE

C SENSOR 'P2' IS ATTACHED TO ) TO PLACE THE INSIDE SURFACE OF

C THE FINGER FLUSH WITH THE UNDERLYING DETECTED EDGE.

C75 DO 77 GT= 1

, 48

AY=AY- . 05

CALL COORCO. ,AY,0. ,0,0)

77 CALL SERVO(O)

GOTO 45

C

C IF 'PI' DETECTS AN EDGE, THIS CODE MOVES THE HAND IN THE '+Y'

C DIRECTION BY AN AMOUNT EQUAL TO THE THICKNESS OF THE FINGER AS

C ABOVE.

C80 DO 81 GGT= 1

, 40

AY=AY+.05

CALL COORCO. ,AY,0. ,0,0)

81 CALL SERVO(O)

45 0FF(2)=0FF(2)+100«AY

40 RETURN

END

DI-51
DI-52
C SUBROUTINE : GRASP

CC ARGUMENTS : D IF D=0 THIS IS THE QUICK CLOSE COMMAND.

C THE POSITION VALUE OF THE FINGERS IN

C THE CLOSED POSITION IS SENT TO THE

C SERVOS. CONTROL IS IMMEDIATELY

C RETURNED TO THE CALLING PROGRAM

C WITHOUT WAITING FOR THE FINGERS TO

C COMPLETE THE CLOSING OPERATION.

C IF D=1 HERE, THE FINGERS ARE CLOSED AN

C INCREMENT AT A TIME UNTIL A DIFFERENCE

C IS DETECTED BETWEEN THE COMMANDED

C POSITION AND THE ACTUAL POSITION AS

C READ IN FROM THE POSITION INDICATOR.

C

C CALLED BY : EMOVE

C

C CALLS SUBROUTINES : ARMIN SERVO

CC INPUT DATA : INBUF(19) = THE INPUT CHANNEL THAT CONTAINS

C THE PRESENT VALUE OF THE POSITION

C INDICATOR FOR THE FINGERS.

C

C OUTPUT DATA : JP0S(7) - THE COMMANDED OUTPUT POSITION VALUE

C FOR THE FINGERS.

C JP0S(8) = THE DIGITAL (16 BITS) OUTPUT CHANNEL

C THAT CONTROLS THE SETTING

C OF THE BRAKES ON ALL OF THE JOINTS.

C

C FUNCTION: CAUSES THE FINGERS TO CLOSE UNTIL THEY EXERT A PREDEFINED

C FORCE ON AN OBJECT, THEN IT SETS THE BRAKE ON THE

C FINGERS (D=0). OR SENDS OUT THE POSITION COMMAND THAT

C WILL RESULT IN THE FINGERS CLOSING ALL OF THE WAY (D=1).

C SUBROUTINE GRASP(D)

IMPLICIT INTEGER(B-Z)

COMMON/ARMBUF/INBUF(64) ,OUTBUF(64)

C0MM0N/0UT/JP0S(8)

EQUI VALENCE (EABORT,INBUF( 28) ) COMMON/GRIP/HAND

CC READ IN THE PRESENT POSITION OF THE FINGERS AND SET THE OUTPUT

C POSITION COMMAND FOR THE FINGERS (JP0S(7)) EQUAL TO THIS

C VALUE.

C CALL ARMIN

JP0S(7)=2»INBUF( 19)+16383

C

C SEND OUT THE PROPER CODE NUMBER TO SET UP THE BIT PATTERN

C ON OUTPUT CHANNEL 8 (JP0S(8)) TO RELEASE THE BRAKE ON THE

C FINGERS.

C JP0S(8)=32767

CALL SERVO(O)

C

C TEST 'D'. IF D=1, THEN SEND THE POSITION VALUE OF 6600 TO THE

DI-53
C FINGER SERVO (THIS IS USED TO CLOSE THE FINGERS

C SO THAT THE SENSORS CAN BE USED IN SOME OF THE SEARCH ROUTINES.

C THE OUTPUTTING OF THIS VALUE TO THE SERVOS CAUSES THE FINGERS TO

C CLOSE COMPLETELY, HOWEVER, THIS ROUTINE

C DOES NOT WAIT FOR THIS ACTION TO BE FINISHED BEFORE RETURNING TO

C THE CALLING PROGRAM. AS A RESULT, THE FINGERS WILL STILL BE IN

C MOTION WHILE THE OTHER SUBROUTINES OF THE CONTROL SYSTEM ARE

C IN EXECUTION.). IF D=0 THEN BRANCH TO STATEMENT 20.

0

IF(D.NE. 1 )GOTO 20

JP0S(7 )=6600

CALL SERVO(O)

GOTO 40

C

C THE COUNTER 'KB' WHICH IS USED IN THE DETECTION OF THE GRIPPING FORCE

C OF THE FINGERS IS SET EQUAL TO ZERO.

C20 KB=0

C

C THE JOINT POSITION COMMAND FOR THE FINGERS (JP0S(7)) IS DECREMENTED

C WHICH WILL CAUSE AN INCREMENTAL CLOSING WHEN SENT TO THE SERVO.

C JPOS(7)=JPOS(7)-100

100 CALL SERVO(O)

C

C THE INPUT CHANNELS ARE SAMPLED TO DETERMINE IF AN ABORT HAS BEEN

C CALLED, AND TO DETERMINE THE PRESENT VALUE OF THE POSITION

G OF THE FINGERS ('FINGER').

C

IF(EABORT.LT.-2000)GOTO 40

FINGER=2»INBUF( 1 9 )+1 6383

C

C THE DIFFERENCE BETWEEN THE PRESENT POSITION ('FINGER') OF THE

C FINGERS AND THE COMMANDED (JP0S(7)) POSITION OF THE FINGERS

C IS CALCULATED AND STORED IN TST.

C TST=FINGER-JPOS (7

)

C

C AS LONG AS THIS DIFFERENCE ('TST') IS LESS THAN 800, THEN

C WE CONTINUE TO DECREMENT THE COMMANDED POSITION VALUE, THUS

C CONTINUING TO CLOSE THE FINGERS. IF, HOWEVER, THE DIFFERENCE

C BECOMES GREATER THAN 800, WE THEN USE 'KB' AS A COUNTER AND

e SAMPLE THE PRESENT POSITION FOR .2 SEC (KB=50) TO MAKE CERTAIN

C THAT THE FINGERS ARE NO LONGER CLOSING. THIS INDICATES THAT

C THE FINGERS HAVE GRASPED AN OBJECT WITH A CERTAIN FORCE.

C IF(TST.LT.800)GOTO 20

KB=KB+1

IF(KB.LT.50)G0T0 100

CC ONCE AN OBJECT IS GRASPED, THE BRAKE ON THE FINGERS IS SET BY

C SETTING THE OUTPUT CHANNEL 8 (rP0S(8)) EQUAL TO 32703.

C310 JP0S(8 )=32703

CALL SERVO(O)

C

DI-54
C THE PRESENT POSITION VALUE OF THE FINGERS IS READ IN AND THE

C COMMANDED POSITION VALUE (JP0S(7)) OF THE FINGERS SET EQUAL

C TO IT, THIS ZEROS THE SERVO ERROR, THEREFORE, THE

C ACTUATOR IS NO LONGER DRIVING THE FINGERS TO CLOSE, SINCE THE

C BRAKE WILL NOW HOLD THE FINGERS CLOSED ON THE OBJECT.

C CALL ARMIN

JP0S(7)=2»INBUF(19)+16383

C

C THE ACTUAL POSITION VALUE OF THE FINGERS IS STORED IN THE COMMON

C VARIABLE 'HAND* SO THAT IT CAN BE ACCESSED BY OTHER SUBROUTINES

C IF NECESSARY.

C HANDzJPOS (7

)

CALL SERVO(O)

40 RETURN

END

DI-55
DI-56
C SUBROUTINE : RELEAS

C

C ARGUMENTS : C

C CALLED BY : EMOVE

C

C --- CALLS SUBROUTINES : ARMIN SERVO

C

C INPUT DATA : INBUF(19) = THE INPUT CHANNEL THAT CONTAINS

C THE PRESENT VALUE OF THE POSITION

C INDICATOR FOR THE FINGERS.

C

C --- OUTPUT DATA : JP0S(7) = THE COMMANDED OUTPUT POSITION VALUE

C FOR THE FINGERS.

C JP0S(8) = THE DIGITAL (16 BITS) OUTPUT CHANNEL

C THAT CONTROLS THE SETTING

C OF THE BRAKES ON ALL OF THE JOINTS.

C

C FUNCTION: CAUSES THE FINGERS OF THE HAND TO FULLY OPEN.

C SUBROUTINE RELEAS

IMPLICIT INTEGER(B-Z)

COMMON/ARMBUF/INBUF(64) ,OUTBUF(64)

C0MM0N/0UT/JP0S(8)

EQUIVALENCE(EAB0RT,INBUF(28)

)

C

C SEND OUT THE PROPER CODE NUMBER TO SET UP THE BIT PATTERN

C ON OUTPUT CHANNEL 8 (JP0S(8)) TO RELEASE THE BRAKE ON THE

C FINGERS.

C JPOS(8)=32767

CC SEND THE POSITION VALUE OF 31000 TO THE SERVO. WHEN THE FINGERS

C ARE SERVOED TO THIS POSITION THEY WILL BE FULLY OPENED.

C JPOS(7)=31000

CALL SERVO(O)

C

C

C THE INPUT CHANNELS ARE SAMPLED TO DETERMINE IF AN ABORT HAS BEEN

C CALLED, AND TO DETERMINE THE PRESENT VALUE OF THE POSITION

C OF THE FINGERS ('FINGER').

C100 CALL ARMIN

IF(EABORT.LT.-2000)GOTO 40

FINGER=2*INBUF( 19)+16383

C

C IF THE FINGERS HAVE NOT YET REACHED THE FULLY OPENED POSITION

C THEN CONTINUING SAMPLING THEIR POSITION (GOTO STATEMENT 100).

C IF(FINGER.LT.30400)GOTO 100

C

C ONCE THE HAND IS FULLY OPENED, SET THE BRAKE (JP0S(8)), AND

C SET THE COMMANDED POSITION VALUE OF THE FINGERS EQUAL TO

C THEIR PRESENT POSITION.

C JPOS(8)=32703

JP0S(7 ) =FINGER

CALL SERVO(O)

40 RETURN

END DI-57
DI-58
C SUBROUTINE : PTOUCH

C

C ARGUMENTS : C

C CALLED BY : EMOVE

C

C CALLS SUBROUTINES : ARMIN SERVO COOR

C

C INPUT DATA : INBUF(48) = 'P2' THE VALUE OF ONE OF THE PROXIMITY

C SENSORS.

C JP0S(1-7) = THE JOINT POSITION VALUES AT THE

C BEGINNING OF THE ROUTINE.

C JP0S(1-7) = THE JOINT POSITION COMMANDS NECESSARY

C TO MOVE THE ARM TO THE POSITION

C DELTA 'Z' FROM THE STARTING POINT [FROM

C 'COOR'].

CC OUTPUT DATA : ADY = THE PRESENT DELTA 'Y' OFFSET VALUE FROM

C THE INITIAL POSITION OF THE ARM.

C ADZ = THE PRESENT DELTA 'Z' OFFSET VALUE FROM

C THE INITIAL POSITION OF THE ARM.

C JP0S(1-7) = THE JOINT POSITION COMMANDS TO BE

C SENT TO THE SERVOS.

C 0FF(1-3) = THE TOTAL OFFSET IN THE X.THE Y,

C AND THE Z DIRECTIONS FROM THE ORIGINAL

C STARTING LOCATION.

C

C FUNCTION: CAUSES THE HAND TO DESCEND IN A STRAIGHT LINE IN THE 'Z'

C DIRECTION WHILE TESTING THE VALUE OF THE SENSOR

C FOR A THRESHOLD LEVEL EVERY .2 CENTIMETER. WHEN THE

C THRESHOLD LEVEL IS DETECTED, THE HAND IS

C STOPPED AT THAT LOCATION AND CONTROL IS RETURNED TO THE

C CALLING PROGRAM.

C SUBROUTINE PTOUCH

IMPLICIT INTEGER(B-Z)

COMMON/ARMBUF/INBUF( 64) .OUTBUF(64)

C0MM0N/0FFS/0FF(3)

C0MM0N/0UT/JP0S(8)

C0MM0N/C0RT/AC(6 ) , AXEP , AYEP , AZEP

EQUIVALENCE(EAB0RT,INBUF(28)

)

EQUIVALENCE(P1 ,INBUF(49) ) . ( P 2 , INBUF ( 4 8 )

)

C

C SET THE DELTA 'Y' (ADY) AND THE DELTA 'Z' VALUE EQUAL TO ZERO.

C ADY=0.

ADZ=0

.

CC PLACE THE PRESENT JOINT POSITION VALUES IN THE COMMON

C VARIABLE 'AC(1-6)' FOR USE BY 'COOR'.

C DO 20 J= 1 ,

6

20 AC( J)=JPOS( J)

C

C MOVE THE HAND 3-0 CENTIMETERS IN THE '+Y' DIRECTION TO

C POSITION THE SENSOR ON THE OUTSIDE EDGE OF THE FINGER

DI-59
C OVER THE EXPECTED CENTER OF THE STACK.

C DO 30 J = 1 , 12

ADY=ADY+.25

CALL COOR(0. ,ADY,0. ,0,0)

30 CALL SERVO(O)

C

C TEST THE VALUE OF THE PROXIMITY SENSOR TO DETERMINE IF THE TOP OF

C THE STACK HAS BEEN DETECTED.

C50 IF(P2.GT. 1500)G0T0 35

C

C MOVE THE HAND ANOTHER .2 CENTIMETERS IN THE '-Z' DIRECTION, THEN

C TEST THE SENSOR VALUE AGAIN.

C ADZ=ADZ- .

2

36 CALL COOR (0 . , ADY , ADZ , 0 , 0)

CALL SERVO(O)

IF(EABORT.LT.-2000)GOTO 40

GOTO 50

C

C ONCE THE TOP OF THE STACK HAS BEEN DETECTED, MOVE THE HAND DOWN

C SLOWLY UNTIL THE THRESHOLD LEVEL IS DETECTED.

C35 ADZ=ADZ-.03

IF(P2.GT.4000)GOTO 45

GOTO 36

C

C UPDATE THE TOTAL OFFSET VALUES BY THE AMOUNTS MOVED DURING THIS

C SUBROUTINE CALL.

C45 0FF(2)=100»ADY+0FF(2)

OFF(3)=100»ADZ+OFF(3)

40 RETURN

END

DI-60


C SUBROUTINE : COOR

C

C ARGUMENTS : AX = THE 'X' COORDINATE VALUE

C AY = THE 'Y' COORDINATE VALUE.

C AZ = THE 'Z' COORDINATE VALUE.

C DA = THE FLAG TO INDICATE WHETHER THE

C 'AX.AY.AZ' VALUES ARE ABSOLUTE COORDINATE

C POSITIONS (DA=1), OR DELTA COORDINATE OFFSETS

C FROM THE PRESENT POSITION OF THE ARM (DA=0).

C NA = THE FLAG TO INDICATE WHETHER THE JOINT

C COORDINATE VALUES ARE TO BE CALCULATED (NA=0),

C OR NOT (NA=1).

C

C CALLED BY : JOY LOCTAB ARRLOC EMOVE STLINE CALD DETECT

C BAL PTOUCH

C

C CALLS SUBROUTINES : CC INPUT DATA : AX , AY , AZ , D A , NA AS EXPLAINED ABOVE

C AC(1-6) = THE VALUES OF THE SIX JOINT COORDINATES

C USED AS A REFERENCE POINT FOR THIS

C TRANSFORMATION.

C A04,A05,A06 = THE VALUES OF THE OFFSETS FOR THE FOURTH,

C FIFTH, AND SIXTH JOINTS (FROM 'JOY').

C

C OUTPUT DATA : AXEP , AYEP , AZEP = THE NEW •X,Y,Z' COORDINATE POSITION

C CALCULATED FROM THE REFERENCE

C POINT (AC(1-6)) AND THE INPUT 'X,Y,Z'

C VALUES (AX,AY,AZ).

C JP0S(1-6) = THE JOINT POSITION VALUES THAT

C ARE CALCULATED FROM THE REFERENCE

C POINT (AC(1-6)) AND THE INPUT •X,Y,Z'

C VALUES (AX,AY,AZ).

C

C FUNCTION: TRANSFORMS THE •X,Y,Z' COORDINATE VALUES FOR A POSITION

C IN SPACE INTO THE CORRESPONDING JOINT COORDINATE VALUES,

C AND VISE VERSA. (THIS IS ONLY A THREE AXIS TRANSFORMATION,

C NOT THE ENTIRE SIX AXIS TRANSFORMATION. THE FOURTH,

C FIFTH AND SIXTH AXIS PLANOGRAPH THE MOTIONS

C OF THE FIRST THREE.)

C SUBROUTINE COOR ( AX , A Y , AZ , DA , NA

)

IMPLICIT INTEGER(D-R)

COMMON /XYZ0/A04 , A05 , A06

C0MM0N/C0RT/AC(6 )

, AXEP , AYEP , AZEP

C0MM0N/0UT/JP0S(8

)

COMMON/ARMBUF/INBUF( 64) , OUTBUF( 64)

DIMENSION A0M(6)

C

C TEST IF AX,AY,AZ ARE TO BE DELTA OR ABSOLUTE COORDINATES.

C IF(DA)5,5,6

C

C IF ABSOLUTE, SET ' AXEP ',' AYEP ',' AZEP ' EQUAL TO THEM AND BRANCH

C TO STATEMENT 10 TO CALCULATE THE CORRESPONDING JOINT POSITION

C VALUES.

DI-63
c

6 AXEP=AX

AYEP=AY

AZEP=AZ

GOTO 10

CC IF AX,AY,AZ ARE DELTA COORDINATE OFFSETS, CALCULATE COORDINATES

C OF PRESENT POSITION (PRESENT POSITION JOINT INDICATOR

C VALUES ARE IN 'AC(1-6)').

C

5 AR=(34552.-AC(3) )/358.7

AT1=(15018.-AC(6) )/5l82.

AT2=(17821 .-AC(2) )/5190.

ARR=AR»COS ( AT2

)

AB=SQRT(275 .56+ARR»»2)

AA=ATAN(ARR/16.6)

AG=AT1+AA-1 . 5708

AAZ=AR«SIN ( AT2

)

AAX=AB»COS ( AG)

AAY=AB»SIN (AG)

CC ADD PRESENT POSITION X,Y,Z COORDINATE VALUES TO THE COMMANDED

C DELTA COORDINATE OFFSET VALUES.

C AXEP=AAX+AX

AYEP=AAY+AY

AZEP=AAZ+AZ

C

C IF JOINT POSITIONS NOT TO BE CALCULATED (NA=1), THEN RETURN TO THE

C CALLING PROGRAM.

C IF(NA. EQ. 1 )GOTO 40

C

C CALCULATE THE JOINT POSITION VALUES THAT CORRESPOND TO THE POSITION

C IN SPACE DESIGNATED BY THE COORDINATES ' AXEP '

,

• A YEP • , ' AZEP '

.

C

-10 ARRPS = AXEP»»2+AYEP»»2-275 .56

IF(ARRPS) 1323, 1323, 1324

1323 WRITE(6 , 1325)ARRPS

1325 F0RMAT(2X'ERR0R»» ARR PS =

' , F 1 0 . 3

)

READ(6 , 1 326 )KKR

1326 F0RMAT(I3)

1324 ARRP=SQRT( ARRPS)

IF(AXEP)202 ,201 ,200

202 IF( AYEP)203 , 204 , 204

203 AT1P=ATAN(AYEP/AXEP)-1 . 570 8-ATAN ( ARRP/ 1 6 . 6

)

GOTO 205

204 AT1P=ATAN(AYEP/AXEP)-1 .5708-ATAN(ARRP/l6.6)

IF(AT1P.GT.-1 . 60)GOTO 205

AT1P=ATAN(AYEP/AXEP)+4.7124-ATAN(ARRP/16. 6)

IF(AT1P.LT.2.54)GOTO 205

WRITE(6,220)

220 F0RMAT(2X • THIS IS FORBIDDEN!!')

DI-64
GOTO 40

201 AXEP=.0001

20 0 AT1P=ATAN(AYEP/AXEP)+1 . 570 8-ATAN ( ARRP/ 1 6 . 6

)

205 ARP=SQRT( ARRPS+AZEP»«2)

AT2P=ATAN( AZEP/ARRP)

1000 A0M(6) = 15018.-(AT1P»5182. ) A0M(2 ) = 1782 1

. -( AT2P»5 1 90 . ) A0M(3)=345 52 .-(ARP»358.7)

AOM(4)=17437.+A04

A0M(5)=1 .202«A0M(2)- 16461 .+A05

AOM( 1 )=- . 8837*A0M(6 )+35 380 .+A06

C

C TEST TO MAKE CERTAIN THAT ALL THE NEW JOINT VALUES

C (JP0S(1-6)) ARE WITHIN THEIR ALLOWABLE LIMITS.

C

720 DO 265 JB=1 , 6

IF(A0M(JB) .GT. 30000 . )G0T0 266

IF( A0M( JB) .LT. 2000 . )G0T0 267

JP0S( JB) =A0M( JB)

GOTO 265

266 JPOS(JB) =30000

GOTO 265

267 JPOS( JB)=2000

265 CONTINUE

40 RETURN

END

!'

I

DI-65

SUBROUTINE

ARGUMENTS

SERVO

SD IF SD=-1

IF SD=0

SD =

ZERO THE EIGHT OUTPUT CHANNELS

FOR THE 'SEND' FUNCTION COMMAND.

SEND OUT TO THE SERVOS, THE SEVEN

JOINT POSITION COMMANDS AND THE VALUE

IN JP0S(8) FOR CONTROLLING THE BRAKES.

IF 'SD' IS A NON-ZERO VALUE, IT IS THE

OUTPUT CHANNEL NUMBER ON WHICH A VOLTAGE

LEVEL IS TO BE SENT AS REQUESTED BY THE

•SEND' FUNCTION.

CALLED BY EMOVE RELEAS GRASP STLINE BAL DETECT

PTOUCH ACC JOY

CALLS SUBROUTINES ARMOUT ARMIN

•- INPUT DATA SD

PNT

THE ARGUMENT AS EXPLAINED ABOVE.

THE FLAG IN COMMON MEMORY THAT IF

IT IS SET EQUAL TO ONE CAUSES THIS

SUBROUTINE 'SERVO' TO MAINTAIN CONTROL

UNTIL THE ACTUAL POSITION OF THE JOINTS

IS WITHIN SOME MINIMUM DISTANCE OF THE

COMMANDED JOINT POSITIONS.

OUTPUT DATA : OUTBUF ( 1

) -OUTBUF ( 8 ) =

0UTBUF(9 )-OUTBUF( 16)

THE SEVEN COMMANDED JOINT

POSITION VALUES, AND THE

15-BIT DIGITAL CHANNEL FOR

CONTROLLING THE BRAKES.

THE EIGHT OUTPUT CHANNELS THAT

CAN BE USED BY THE 'SEND'

FUNCTION COMMAND. THIS IS USED

FOR THE INTERLOCKING OF THE ARM

EXTERNAL DEVICES.

•- FUNCTION: USES THE ARGUMENT TO DETERMINE WHICH OUTPUTS SHOULD BE

SENT TO THE ROBOT WORK STATION. ALL OUTPUTS FROM

THE CONTROL SYSTEM ARE MADE THROUGH THIS CALL. THESE

INCLUDE THE JOINT POSITION COMMANDS, THE CONTROL OF

ALL OF THE JOINT BRAKES, AND THE VOLTAGE LEVEL SIGNALS

ON THE SPECIFIED OUTPUT CHANNELS.

SUBROUTINE SERVO(SD)

IMPLICIT INTEGER(B-Z)

COMMON/ARMBUF/INBUF(64) ,OUTBUF(64)

COMMON/END/PNT

C0MM0N/0UT/JP0S(8)

DIMENSION ERRT(12) ,ERR(6) ,INP(6)

EQUIVALENCE(EAB0RT,INBUF(28)

)

DATA ERRT/700, 700, 1500, 700, 1000, 700, 200, 100, 360, 200, 280, 100/

THE FLAG 'PNT' IS TESTED TO SEE WHICH GROUP OF DELTA JOINT VALUES

FROM 'ERRT' IS TO BE USED. IF 'PNT' IS EQUAL TO ZERO, THEN

THE ONLY REQUIREMENT ON THE ACTUAL POSITION OF ALL

OF THE JOINTS IS THAT DIFFERENCE BETWEEN THEIR PRESENT POSITION

AND THEIR NEXT COMMANDED POSITION NOT BE GREATER THAN THE

DI-67
C DISTANCE THAT THEY CAN MOVE IN THAT NEXT INCREMENT. IF 'PNT' IS

C EQUAL TO ONE, THEN THIS SUBROUTINE WILL LOOP, SENDING THE SAME

C COMMANDED POSITION VALUES UNTIL THE ACTUAL POSITION OF

C THE JOINTS IS WITHIN SOME MINIMUM DISTANCE OF THESE VALUES

C (THESE MINIMUM VALUES ARE IN ' ERRT ( 7 - 1 2

)

• )

.

C KS = 0 IF(PNT.EQ.O)GOTO 55

PNT = 0 KS = 6 55 CALL ARMIN

KP = 0 C

C WHEN THE ACTUAL POSITION VALUES OF THE JOINTS ARE READ IN,

C A CORRECTION FACTOR IS USED TO COMPENSATE FOR THE ERRORS

C INTRODUCED IN THE CONVERSION ELECTRONICS IN THE INTERFACE

C AND THE HARDWARE SERVO SYSTEM.

C INP( 1 )=INBUF(5)»2.+ 16361

INP(2)=INBUF(8)»1 .975 8+16394

INP(3)=INBUF( 1 1 )»2.0 0 2+l6 369

INP(4)=INBUF( 13)*2. 033+16370

INP(5)=INBUF( 15)* 1 .9808+16270

INP(6)=INBUF( 17)»1 .994+16349

DO 710 KKK= 1 , 6 ERR (KKK)=INP (KKK)-OUTBUF(KKK)

KM=KS+KKK

IF(ERR(KKK) .GT.ERRT(KM) )GOTO 58

IF(ERR(KKK) .LT.-ERRT(KM) )GOTO 58

710 CONTINUE

GOTO 77

58 KP=1

77 IF(EABORT.LT.-2000)GOTO 40

C

C A CALL IS MADE TO 'ARMIN' FOR SWITCH 34. IF THIS SWITCH IS

C UP, THE ARM WILL STOP AT ITS PRESENT POSITION AND REMAIN

C THERE UNTIL THE SWITCH IS FLIPPED DOWN.

IF(INBUF(34) .LT.-2000)GOTO 55

CC THE VARIABLE 'SD' IS USED TO BRANCH TO THE APPROPRIATE CODE

C IF(SD) 1 0 , 20 , 30

C

C FOR 'SD':-!, ZERO ALL OF THE OUTPUT CHANNELS USED BY THE 'SEND'

C FUNCTION COMMAND.

C10 DO 1 5 K = 9 ,16

15 0UTBUF(K)=16383

GOTO 35

CC FOR 'SD'=0, OUTPUT THE SEVEN JOINT POSITION COMMANDS AND

C THE VALUE (JP0S(8)) FOR CONTROLLING THE BRAKES.

C20 DO 25 JK=1,8

25 OUTBUF( JK)=JPOS( JK)

CALL ARM0UT(8)

DI-68
GOTO 45

C

C FOR 'SD'r SOME NUMBER BETWEEN ONE AND EIGHT, SET 'CH' EQUAL TO THIS

C NUMBER PLUS EIGHT AND OUTPUT A +5 VOLT SIGNAL ON THIS CHANNEL.

C HERE, THE NUMBER 'SD' WILL HAVE BEEN SPECIFIED BY THE

C 'SEND' FUNCTION COMMAND.

C30 CH=8+SD

0UTBUF(CH)=8192

35 CALL ARM0UT(16)

45 IF(KP.NE.O)GOTO 55

40 RETURN

END

DI-69
.TITLE ARMOUT FOR LOW LEVEL ARM OUTPUT

.IDENT /RMR01/

CALLING SEQUENCE IS:

CALL ARMOUT(COUNT, [BUFFER] ) WHERE [BUFFER] IS OPTIONAL

IF NOT THERE, OUTBUF IS USED.

GLOBL VAL ;THE 11/45 IS TOO FAST, ARMIN AND

;ARMOUT MUST SLOW DOWN, VAL IS A iWORD TO HELP US DO THAT.

ARMOUT:

1$:

2$:

MOV

MOVB

MOV

MOV

DEC

BEQ

MOV

MOV

MOV

DEC

BPL

MOV

MOV

DEC

BGT

MOV

RTS

VAL, RO

@R5 , R4

@2(R5) R3

#0UTBUF,R2

R4

1$

4(R5) ,R2

#167772, R4

#-1 ,@R4

RO

2$

VAL, RO

(R2)+,@R4

R3

2$

#-1

R5

§R4

GET THE NUMBER OF ARGUMENTS

IN THE CALLING SEQUENCE

GET THE NUMBER OF POINTS TO

SEND TO THE ARM.

ASSUME OUTBUF IS USED

FOR ARM OUTPUT

THE NUMBER OF ARGUMENTS WAS ONE

SO USE OUTBUF, OTHERWISE

USE THE ARRAY SPECIFIED.

POINT TO THE ARM INTERFACE

AND RESET IT

11/45 IS TOO FAST, SO, TIMEOUT

OUTPUT FROM THE BUFFER

COUNT DOWN ON NUMBER OF POINTS

TO SEND TO THE ARM

THEN SEND THEM

RESET AFTER

THE WRITE (6-MAY-74)

THIS CAUSES LESS CURRENT

DRAIN ON THE POWER SUPPLIES IN THE

D/A CONVERTER

THEN GO HOME

IMPLICIT INTEGER(A-Z)

COMMON/ARMBUF/INBUF(64 ), OUTBUF ( 64

)

.CSECT ARMBUF

INBUF=

.

OUTBUFr .+200

. = . +400

. CSECT

. END

DI-70
.TITLE ARMIN FOR LOW LEVEL ARM INPUT

•IDENT /RMR01/

ARMIN AND ARMOUT HAVE SIMILAR DOCUMENTATION. THE TWO PROGRAMS

SHOULD BE READ TOGETHER AS SOME DOCUMENTATION EXISTS IN

ONE AND NOT THE OTHER.

. CSECT

THE ARM IS CONTROLLED BY A DR11-C INTERFACE TO THE PDP-11/45

UNIBUS. ALTHOUGH ALL THE INTERRUPT FACILITY AVAILABLE WITH

THE DR11-C IS INTACT, NO INTERRUPTS ARE USED WITH THIS VERSION

OF THE ARM SOFTWARE. THE GENERAL PHILOSOPHY OF THE SOFTWARE

IS SUCH THAT DATA TRANSFER TO AND FROM THE ARM WILL OCCURE

WHENEVER A PROGRAM WANTS AND/OR HAS THE TIME.

THE DR11-C HAS A UNIBUS ADDRESS OF 167770

THIS IS THE CONTROL REGISTER. 167772 IS THE TRANSMIT REGISTER

AND 167774 IS THE RECEIVE REGISTER. THE A/D CONVERTER

IS RESET WHENEVER IT SEES BIT 15 SET. AT LOCATION 24, THE A/D CONVERTER

IS RESET.

ALL 64 CHANNELS ARE READ INTO THE COMMON BLOCK NAMED ARMBUF

THIS IS FORTRAN COMPATIBLE COMMON. ALSO THE SUBROUTINE LINKAGE

IS COMPATIBLE .

VAL:

ARMIN;

.WORD 25 TIME OUT VALUE

IS SET AT 10 WHICH CAUSES THIS

ROUTINE TO WAIT (SEE LOOP 1$)

LONG ENOUGH FOR THE A/D CONVERTER

TO SETTLE DOWN BETWEEN READS.

MOV VAL, RO ; THE 1 1 /45 RUNS TOO FAST

;S0 WE WILL USE RO TO

;TIMEOUT BETWEEN READS.

MOV #167772 , R2 ;LOAD THE CSR OUTPUT REG

MOV #-1 , (R2)+ ;WITH A RESET AND POINT

;T0 THE INPUT SIDE

MOV #64. , R4 ;COUNT UP TO 64 POINTS

MOV #INBUF,R3 ;POINT TO THE FIRST WORD

DEC RO ;COUNT DOWN FOR 11/45 TIMEOUT

BPL 1$

MOV VAL, RO

MOV §R2, (R3)+ ;LOAD THE INBUF WITH 64 POINTS

DEC R4

BGT 1$

RTS R5 ;THEN RETURN

IMPLICIT INTEGER(A-Z)

COMMON /ARMBUF/INBUF(64) ,OUTBUF(64)

WHEN USED WITH FORTRN, COMPILE WITH THE /ON SWITCH

.CSECT ARMBUF

INBUF=

.

OUTBUF= . +200

. = .+400

.CSECT

. END

DI-71

Program Module

(Module #2)

PROGS

Editor Commands

Reads in editor commands and calls

appropriate subroutine. DII-2

INSERT -- Causes elemental move commands to be entered

in program table. DII--6

PRINT -- Causes the program table to be printed out

as elemental move commands. DII--10

DELETE - Deletes specified lines from program table. DII--14

AVOID -- Creates avoidance paths by inserting additional

location points. DII--18

LOOP Creates the additional lines in an indexed

repeat pattern. DII--22

Additional Support Subroutines

INDEX - Codes indexed location names into their

location table pointers. DII-26

LOCPT - Codes non-indexed location names into their

location table pointers. DII-30

LINE - Codes elemental move command into a line in

program table. DII-34

PTNAME - Decodes location table pointer into its

location name. DII-40

PLINE - Prints out decoded line from program table

as elemental move commands. DII-44

NEXT - Advances through input character string to

next piece of information. DII-48

LIMIT - Decodes into integer from the lines specified

in the editor commands. DII-52

ADJUST - Verifies continuity in the specified motion

in the program. DII-56

DII-1
DII-2
C SUBROUTINE : PROGS

C

C ARGUMENTS : C

C CALLED BY : OPRO

CC CALLS SUBROUTINES : INDEX INSERT DELETE PRINT RECORD

C AVOID ARMIN

C

C INPUT DATA : PROG(105,6)= THE PROGRAM MODULE THAT IS TO BE MODIFIED

C CS(50) = THE ASCII CHARACTER STRING THAT CONTAINS

C THE 'EDITOR' COMMANDS.

C

C OUTPUT DATA : PROG(105,6)= THE EDITED PROGRAM MODULE.

C

C FUNCTION: EDITS AN OLD PROGRAM FROM THE DISC, OR ENTERS A

C NEW PROGRAM THROUGH THE USE OF THE PROPER

C EDITOR COMMANDS.

C SUBROUTINE PROGS

IMPLICIT INTEGER(B-Z)

COMMON /PMOD /PROG (10 5, 6) , ENDP , BRNCH ( 6 ) ,M

COMMON/IND/IN(30) ,FL(6) ,NAME(3) ,NPT(3,20)

COMMON/CMD/CS(50) ,PN,PLN

C0MM0N/ARMBUF/INBUF(6iO , OUTBUF( 64

)

EQUIVALENCE ( RET, INBUF( 30)

)

CC THE SPECIFIED PROGRAM MODULE IS READ IN FROM THE DISC BY THE CALL

C TO 'RDMOD' IN 'EXPRO'. THE INDEXED NAMES ARE STORED IN THE

C CHARACTER STRING IN(30).

C500 M=0

DO 204 J=101 , 105

DO 204 L= 1,

6

M = M+1

204 IN(M)=PROG(J,L)

PLN = 1 C

C THE CALL TO 'INDEX' PRINTS THE PRESENT INDEXED LOCATION NAMES

C AND ALLOWS THE ENTERING OF NEW INDEXED NAMES.

C CALL INDEX(O)

WRITE(6 , 125

)

125 F0RMAT(8X ' EDITOR IS NOW AVAILABLE FOR PROGRAM ENTRY',/)

C

C THE CURRENT INDEXED NAMES ARE STORED IN THE LAST

C FIVE LINES OF THE PROGRAM MODULE.

C M = 0 DO 210 J=101 , 105

DO 210 L= 1

,

6

M = M+1

210 PROG( J,L)=IN(M)

C

C THE COMMAND FROM THE TERMINAL IS READ INTO THE

C CHARACTER STRING CS(50) AND TESTED TO SEE IF THE FIRST

DII-3
C LETTER IS THE ASCII CODE NUMBER FOR I ( INSERT ), D ( DELETE ), P ( PRINT )

,

C R(RECORD),OR A(AVOID). THE PROPER SUBROUTINE IS CALLED FOR

C THE IDENTIFIED CODE LETTER. IF NO VALID LETTER IS FOUND,

C THE MESSAGE 'ILLEGAL COMMAND" IS PRINTED AND THE PROGRAM WAITS

C FOR THE NEXT COMMAND.

C99 READ(6,200) (CS(N) ,N=1 ,50)

200 F0RMAT(50 ( A1 )

)

CALL ARMIN

IF(RET.GT.-2000)GOTO 40

PN= 1 77 IF(CS(PN) .NE.8265)GOTO 220

CALL INSERT

220 IF(CS(PN) .NE.8260)G0T0 221

CALL DELETE

GOTO 99

221 IF(CS(PN) .NE.8272)G0T0 222

CALL PRINT

GOTO 77

222 IF(CS(PN) .NE.827M)G0T0 223

CALL RECORD (0 , 1

)

GOTO 405

223 IF(CS(PN) .NE.8270)GOTO 224

GOTO 500

224 IF(CS(PN) .NE.8257)GOTO 255

CALL AVOID

GOTO 99

255 IF(RET.GT.-2000)GOTO 40

100 WRITE(6,101)

101 F0RMAT(8X' ILLEGAL COMMAND')

GOTO 99

405 WRITE(6,225)

225 F0RMAT(8X, 'PROGRAM HAS BEEN STORED ON DISC)

GOTO 99

40 RETURN

END

DII-4


C SUBROUTINE : INSERT

C

C ARGUMENTS : C

C CALLED BY : PROGS

C

C CALLS SUBROUTINES : NEXT LIMIT PLINE LOOP LOCPT LINE ADJUST

C

C INPUT DATA : CS(50) = THE CHARACTER STRING THAT CONTAINS THE

C INPUT COMMAND STATEMENT.

C BEG = THE LINE NUMBER IN THE PROGRAM MODULE

C WHERE THE INSERTION OCCURS [LIMIT].

C PLN = THE PRESENT PROGRAM LINE NUMBER,

C HERE, SET EQUAL TO 'BEG'.

C BN = THE LOCATION TABLE POINTER FOR THE

C FIRST LOCATION IN THE PROGRAM MODULE,

C ENTERED BY 'START' COMMAND [LOCPT].

C PR0G(PLN,6)= THE DECODED NEW LINE IN THE PROGRAM

C MODULE [LINE].

C

C OUTPUT DATA : PROG(105,6) = THE UPDATED PROGRAM MODULE

\ ^

I C FUNCTION: TO INSERT NEW LINES AT THE SPECIFIED PROGRAM LINE,

C SLIDING BACK THE REST OF THE PROGRAM TO

C ACCOMMODATE THESE ADDITIONAL LINES.

C

SUBROUTINE INSERT

IMPLICIT INTEGER(B-Z)

COMMON /PMOD /PROG (105, 6) , ENDP , BRNCH ( 6 ) ,M

COMMON/PTN/BN, EN

^ COMMON/BED/BEG, FIN, DIF, ERR

COMMON/CMD/CS(50) ,PN,PLN

DIMENSION TEMP(100,6)

ERR = 0 C

C STORE PRESENT PROGRAM MODULE ( PROG ( 1 05 , 6 ) ) IN A TEMPORARY MATRIX

C CALLED TEMP( 100 , 6) . C DO 50 K=1 , 100

DO 50 F = 1 , 6 50 TEMP(K,F)=PROG(K,F)

C

C IDENTIFY AND DECODE THE PROGRAM LINE NUMBER WHERE THE

C INSERTION IS TO OCCUR.

C500 CALL NEXT

IF(PN.GT.50)G0T0 27

CALL LIMIT

IF(ERR.EQ. DGOTO 100

PLNrBEG

27 STAR=PLN

GOTO 160

C

C ERROR MESSAGE TO BE PRINTED OUT IF A INCORRECT COMMAND IS

C ENTERED IN THIS SUBROUTINE.

C

DII-7
100 WRITE(6,101)

101 F0RMAT(8X, • ILLEGAL FORMAT FOR »»» INSERT (J) «»»')

GOTO 40

CC PRINT OUT ON THE TERMINAL THE PRESENT PROGRAM LINE (PLN).

C160 CALL PLINE(PLN)

C

C READ IN A NEW LINE (CS(50)) AND TEST ON THE FIRST CHARACTER,

C RETURN TO CALLING PROGRAM IF IT IS AN EDITOR COMMAND (INSERT,

•C NAME, AVOID, DELETE, PRINT, OR RECORD).

170 READ(6 , 171 ) (CS(L) ,L=1 , 50)

171 F0RMAT(50(A1 ) ) PN = 1 IF(CS(PN) .EQ.8265)G0T0 400

IF(CS(PN) .EQ.8270)GOTO 400

IF(CS(PN) .EQ.8257)GOTO 400

IF(CS(PN) .EQ.8260)G0T0 400

IF(CS(PN) .EQ.8272)G0T0 400

IF(CS(PN) .EQ.8274)G0T0 400

C

C TEST FOR A 'LOOP' COMMAND.

C IF(CS(PN) .NE.8268)GOTO 182

CALL LOOP

PLN=PLN+1

GOTO 170

0

C TEST FOR A 'START' COMMAND, IE. THE ENTERING OF THE FIRST

C LOCATION POINT IN THE PROGRAM MODULE.

C182 IF(CS(PN) .NE.8275)GOTO 183

CALL NEXT

CALL LOCPT( 1 ) PROG (1,1) =BN

STAR=1

PLN=1

GOTO 170

C

C TEST FOR A 'GOTO' ELEMENTAL MOVE COMMAND, CALL 'LINE' TO

C RECORD AND DECODE THE CORRECT VALUES FOR THE PROGRAM MODULE.

C183 IF(CS(PN) .NE.8263)G0T0 100

CALL LINE

PLN=PLN+1

GOTO 170

C

C PUSH BACK THE REST OF THE PROGRAM LINES TO ACCOMMODATE THE

C NEWLY INSERTED LINES, AND STORE UPDATED PROGRAM IN THE

C PROGRAM MODULE PROG(105,6).

C400 IF(PLN.NE. 1 )GOTO 401

GOTO 40

401 END=PLN-STAR

PT=101-PLN

IF(PLN.NE. 1 )GOTO 60

DII-8
PROGd , 2)=TEMP( 1 , 1 ) END= 1 PT = 99

60 DO 450 MB=1 ,PT

FD=101-MB

DIF=FD-END

DO 450 MT=1,6

450 PROG(FD,MT)=TEMP(DIF,MT)

451 CALL ADJUST

PLN=STAR

40 RETURN

END

DII-9
DII-10
C SUBROUTINE : PRINT

C

C ARGUMENTS : CC CALLED BY : PROGS

C

C CALLS SUBROUTINES : NEXT LIMIT PLINE

C

C INPUT DATA : CS(50) = CHARACTER STRING THAT CONTAINS THE

C EDITOR COMMAND TO PRINT A LINE(S)

C PLN = THE NUMBER OF THE PRESENT PROGRAM LINE

C (MAY NOT BE THE LINE SPECIFIED IN THE

C 'PRINT' COMMAND).

C BEG = THE NUMBER (IF SPECIFIED IN THE PRINT

C COMMAND) OF THE FIRST LINE TO BE

C PRINTED [FROM 'LIMIT'].

C DIF = THE NUMBER OF LINES TO BE PRINTED STARTING

C FROM LINE 'BEG' (IF MORE THAN ONE LINE

C HAS BEEN SPECIFIED IN THE COMMAND).

C

C OUTPUT DATA : PLN = THE NUMBER OF THE PROGRAM LINE TO BE

C PRINTED OUT ON THE TERMINAL (SENT

C TO THE SUBROUTINE 'PLINE').

C

C FUNCTION: DECODES THE PRINT COMMAND TO DETERMINE HOW MANY LINES ARE

C TO BE PRINTED AND WHAT ARE TH"IR LINE NUMBERS IN THE

C PROGRAM MODULE.

C SUBROUTINE PRINT

IMPLICIT INTEGER(B-Z)

COMMON/ARMBUF/INBUF(64) ,OUTBUF(64)

COMMON /BED /BEG, FIN , DIF , ERR

COMMON/CMD/CS(50) , PN , PLN

ERR = 0

C

C TEST TO SEE IF THE LINES HAVE BEEN SPECIFIED IN THE 'PRINT'

C COMMAND (IE. IS THEIR ANY CHARACTERS AFTER THE WORD

C 'PRINT' IN THE CHARACTER STRING CS(50)). IF SO, THEN

C GO TO STATEMENT 20.

C60 CALL NEXT

IF(PN.LT.51 )GOTO 20

C

C IF NO LINE NUMBERS ARE SPECIFIED IN THE 'PRINT'

C COMMAND THEN THE NUMBER OF LINES TO BE PRINTED (DIF) IS SET

C EQUAL TO ONE . C PLN=PLN-1

DIF=1

GOTO 70

C

C THIS IS THE ERROR MESSAGE DISPLAYED ON THE TERMINAL IF AN INCORRECT

C FORMAT IS USED.

C100 WRITE(6,101)

101 F0RMAT(8X'F0RMAT ERROR IN »*» PRINT (J-K) «»» STATEMENT')

DII-11
GOTO 40

C

C IF LINES ARE SPECIFIED IN THE 'PRINT' COMMAND SET PLN EQUAL

C TO THE FIRST LINE TO BE PRINTED , (IE. SET PLN EQUAL TO

C BEG FROM 'LIMIT') AND CALCULATE THE NUMBER OF LINES TO BE

C PRINTED 'DIF' [FROM 'LIMIT'].

C20 CALL LIMIT

IF(ERR.EQ. 1 )GOTO 100

PLN=BEG-1

C

C EACH TIME THROUGH THIS LOOP A LINE FROM THE PROGRAM MODULE IS

C PRINTED OUT (BY A CALL TO 'PLINE') AND THE LINE NUMBER 'PLN'

C IS INCREMENTED.

C70 DO 50 K=1 ,DIF

PLN=PLN+1

50 CALL PLINE(PLN)

CC THE NEXT COMMAND IS READ INTO CS(50) FROM THE TERMINAL AND THE

C FIRST LETTER OF THE COMMAND WORD TESTED TO SEE IF IT IS

C A D(DELETE), I(INSERT), N(NAME), P(PRINT), R(RECORD),

C A(AVOID). IF ONE OF THESE EDITOR COMMANDS, THEN

C RETURN TO THE CALLING PROGRAM 'PROGS'.

C65 READ(6 , 66) (CS(RN) , RN=1 , 50)

66 F0RMAT(50(A1 )

)

CC IF THE CALLING SWITCH (SWITCH 30) IS RESET, THEN RETURN TO

C CALLING PROGRAM.

C CALL ARMIN

IF(INBUF(30) .GT.-2000)GOTO 40

PN = 1 IF(CS(PN) .EQ.8260)GOTO 40

IF(CS(PN) .EQ. 8265)GOTO 40

IF(CS(PN) .EQ.8270)G0T0 40

IF(CS(PN) .EQ.8272)GOTO 40

IF(CS(PN) .EQ.8274)GOTO 40

IF(CS(PN) .EQ.8257)GOTO 40

CC TEST IF A MINUS SIGN WAS ENTERED INDICATING THE PREVIOUS LINE IS

C TO BE PRINTED. THIS IS DONE BY SUBTRACTING ONE FROM THE

C PRESENT LINE NUMBER PLN.

C IF(CS(PN) .NE.8237)GOTO 51

PLN=PLN-1

GOTO 31 C

C TEST TO SEE IF ONLY A CARRIAGE RETURN WAS ENTERED INDICATING THAT

C THE NEXT LINE OF THE PROGRAM MODULE IS TO BE PRINTED. THIS

C IS DONE BY ADDING ONE TO THE PRESENT PROGRAM LINE NUMBER AND

C CALLING 'PLINE'.

C51 IF(CS(PN) .NE.8224)G0T0 100

PLN=PLN+1

DII-12
CALL PLINE(PLN)

GOTO 65

RETURN

END

DII-13
DII-14
-- SUBROUTINE : DELETE

-- ARGUMENTS : - CALLED BY : PR-GS - CALLS SUBROUTI^ES :

C

C

CC

C

C

C

C

C INPUT DATA

NEXT LIMIT PLINE ADJUST

Pi?OG( 105,6):

CSf50)

BEG

DIF

FIN

PN

THE PRESENT PROGRAM MODULE

THE CHARACTER STRING THAT CONTAINS

THE 'DELETE' COMMAND.

THE NUMBER OF THE FIRST LINE TO BE

DELETED [FROM 'LIMIT'].

THE NUMBER OF LINES TO BE DELETED AS

SPECIFIED IN THE 'DELETE' COMMAND [FROM

'LIMIT ' ]

.

THE NUMBER OF THE LAST LINE TO BE DELETED

[FROM 'LIMIT'].

A POINTER THAT INDICATES THE POSITION

IN THE CHARACTER STRING CS(50) OF THE NEXT

PIECE OF INFORMATION [FROM NEXT].

OUTPUT DATA : PROG(105,6) =

PLN

THE MODIFIED PROGRAM MODULE AFTER

THE SPECIFIED LINES HAVE BEEN DELETED

AND THE REST OF THE PROGRAM CLOSED

UP AROUND THE DELETED LINES.

THE POINTER TO THE PRESENT PROGRAM

LINE . FUNCTION: DELETES THE SPECIFIED LINES FROM THE PROGRAM, PRINTS

OUT THE DELETED LINES ON THE TERMINAL, AND CLOSES

UP THE PROGRAM MODULE AROUND THE DELETED LINES.

CC

C

C100

101

C

C

C

SUBROUTINE DELETE

IMPLICIT INTEGER(B-Z)

COMMON /PMOD /PROG (105, 6) , ENDP , BRNCH ( 6 ) ,M

COMMON /BED /BEG, FIN , DIF , ERR

C0MM0N/CMD/CS(50) , PN , PLN

ERR = 0 ADVANCE THROUGH THE CHARACTER STRING CS(50) UNTIL THE NUMBER OF THE

FIRST LINE OF THE PROGRAM TO BE DELETED IS REACHED BY

CALLING 'NEXT'.

CALL NEXT

IF(PN.LT.51 )GOTO 20

ERROR MESSAGE THAT IS PRINTED OUT IF AN INCORRECT

FORMAT IS DETECTED IN THE 'DELETE' COMMAND.

WRITE(6 , 101 ) F0RMAT(8X'F0RMAT ERROR IN »»• DELETE (J-K)»»» STATEMENT')

GOTO 40

THE FIRST LINE SPECIFIED TO BE DELETED IS DECODED BY A CALL TO

'LIMIT' AND RETURNED IN THE VARIABLE 'BEG'. THE NUMBER

DII-15
C OF LINES TO BE DELETED IS ALSO DETERMINED BY 'LIMIT' AND

C RETURNED IN THE VARIABLE 'DIP'.

C20 CALL LIMIT

IF (ERR . EQ. 1 )GOTO 100

C

C AN INTERMEDIATE VARIABLE 'NEN' IS CREATED TO BE USED IN

C THE CLOSING UP PROCEDURE WHERE THE REMAINDER OF THE PROGRAM

C LINES ARE SLID FORWARD TO CLOSE THE GAP CREATED BY THE

C DELETION OF THE SPECIFIED LINES.

C NEN= 1 00-DIF

IF(BEG.NE. 1 )GOTO 70

C

C THE STARTING LOCATION OF THE PROGRAM IS SAVED IN THE VARIABLE 'TEM'

C TO BE REINSERTED IN THE PROGRAM MODULE AFTER ALL OF

C THE ADJUSTMENTS HAVE BEEN MADE.

C TEM = PROG ( 1 , 1 ) C

C THE FOLLOWING 'DO LOOP' DELETES THE SPECIFIED LINES AND CLOSES

C UP THE PROGRAM AT THE SAME TIME BY MOVING ALL OF THE PROGRAM

C LINES FORWARD 'DIF' NUMBER OF LINES, STARTING WITH LINE

C NUMBER 'BEG' + 'DIF'.

C70 DO 50 K=BEG,NEN

JMP=K+DIF

IF(K.GT.FIN)GOTO 30

PLN = K

C

C EACH LINE, AS IT IS DELETED, IS PRINTED OUT ON THE TERMINAL

C BY A CALL TO 'PLINE ' . C CALL PLINE(PLN)

30 DO 50 F = 1 , 6 50 PROG(K,F)=PROG(JMP,F)

IF(BEG.NE. 1 )GOTO 71

PROG( 1

, 1 ) =TEM

CC ANY INCOMPLETE LINES THAT MIGHT HAVE BEEN CREATED ARE

C ELIMINATED BY THE SUBROUTINE 'ADJUST'.

C71 CALL ADJUST

MO RETURN

END

DII-16
i

I
DII-18
C SUBROUTINE : AVOID

C

C ARGUMENTS : C

C CALLED BY : PROGS

C

C CALLS SUBROUTINES : NEXT LOCPT ADJUST

C

C INPUT DATA : PROG(105,6) = THE PRESENT PROGRAM MODULE.

C CS(50) = THE CHARACTER STRING THAT CONTAINS

C THE PROPER SEQUENCE OF LOCATIONS TO

C BE USED IN THE AVOIDANCE PATH.

C BN = THE LOCATION TABLE POINTER FOR

C EACH LOCATION SPECIFIED IN THE

C CHARACTER STRING CS(50) [FROM 'LOCPT'].

C OUTPUT DATA : PROG(105,6) = THE MODIFIED PROGRAM MODULE WITH

C ALL OF THE ADDITIONAL LINES REQUIRED

C TO CREATE THE AVOIDANCE PATHS THROUGH

C THE SPECIFIED LOCATIONS.

C

C FUNCTION: IF A TRAJECTORY FROM LOCATION 'A' TO LOCATION'S'

C IS TO PASS THROUGH ADDITIONAL INTERMEDIATE LOCATIONS,

C THIS SUBROUTINE INSERTS THOSE ADDITIONAL LOCATIONS IN

C THE CORRECT SEQUENCE FOR EVERY OCCURRENCE OF 'A-B' OR

C 'B-A' IN THE PROGRAM MODULE BY MEANS OF A SINGLE

C 'AVOID' COMMAND.

C SUBROUTINE AVOID

IMPLICIT INTEGER(B-Z)

COMMON/PMOD/PROG( 105 , 6) , ENDP , BRNCH ( 6 ) ,M

COMMON/PTN/BN,EN

COMMON/CMD/CS(50) ,PN,PLN

COMMON/TEMPOR/TEMPdOO ,6) ,DET( 10) ,NDET( 10)

C

C A FLAG ('REV') IS SET EQUAL TO ZERO. IT WILL BE USED TO

C INDICATE IF THE PROGRAM MODULE HAS BEEN CORRECTED

C FOR THE AVOIDANCE PATH IN THE REVERSE SEQUENCE ALSO.

C REV = 0

C

C THE FOLLOWING LOOP DETERMINES THE NUMBER OF LOCATION POINTS IN THE

C AVOIDANCE PATH (AS SPECIFIED IN THE CHARACTER STRING CS(50))

C AND DECODES THEM INTO THEIR PROPER LOCATION TABLE POINTERS

C BY CALLING 'LOCPT ' . C DO 30 JK=1 , 10

CALL NEXT

IF(PN.GT.50)G0T0 20

CALL LOCPT( 1

)

30 DET(JK)=BN

GOTO 77

20 JK=JK-1

C

C THE FOLLOWING LOOP SEQUENTIALLY TESTS EVERY LINE IN THE PROGRAM

C MODULE FOR THE PRESENCE OF BOTH OF THE END POINTS OF

C THE AVOIDANCE PATH. IF BOTH IN THE SAME LINE, THEN BRANCH

DII-19
C TO CODE TO INSERT ADDITIONAL LINES.

C77 DO 25 JB=1 , 100

IF(PROG( JB, 1 ) .EQ.DET( 1 ) )GOTO 22

GOTO 25

22 IF(PROG ( JB , 2) . EQ.DET ( JK) )GOTO 50

25 CONTINUE

GOTO 35

C

C THE FOLLOWING LOOP STORES THE PROGRAM MODULE IN A TEMPORARY

C TABLE WHICH ACTS AS A SCRATCH PAD UNTIL ALL OF THE REQUIRED

C MODIFICATIONS HAVE BEEN MADE.

C50 DO 75 K=1 , 100

DO 75 F=1 ,

6

75 TEMP(K,F)=PROG(K,F)

CC THIS LOOP INSERTS THE ADDITIONAL LINES FOR THE AVOIDANCE PATH,

C ZEROS THE FUNCTION FLAGS FOR THESE INTERMEDIATE LOCATIONS AND

C SETS THE VELOCITY (PR0G(NV,3)) EQUAL TO 50 CM/SEC.

C VE=PR0G(JB,3)

DO 76 FT=2,JK

NV=JB+FT-2

DO 79 RD=3,6

PROG(NV, RD ) =0

79 PROG(NV, 3)=VE

76 PR0G(NV,2)=DET(FT)

C

C THE REMAINDER OF THE PROGRAM IS NOW ADDED ON TO THE NEWLY INSERTED

C LINES.

C END=NV-JB

PT=101-NV

DO 450 MB= 1

, PT

FD=101-MB

DIF=FD-END

DO 450 MT=1 ,

6

450 PROG(FD,MT)=TEMP(DIF,MT)

C

C THE SUBROUTINE 'ADJUST' IS CALLED TO INSURE THAT

C ALL OF THE LOCATIONS ARE IN THE PROPER SEQUENCE.

C CALL ADJUST

C

C RETURN TO THE LOOP TO CONTINUE TESTING THE PROGRAM MODULE

C FOR ANY OTHER PLACES WHERE THE AVOIDANCE PATH IS TO BE USED.

C GOTO 77

CC IF THE REVERSE FLAG ('REV') HAS NOT BEEN SET EQUAL TO ONE, THEN

C SET IT EQUAL TO ONE AND REVERSE THE SEQUENCE OF THE LOCATION

C POINTS IN THE AVOIDANCE PATH (DET(10)>, AND GO BACK AND

C TEST THE PROGRAM MODULE FOR ANY PLACES WHERE THE REVERSE

C PATH IS TO BE USED. AFTER ALL OF THESE MODIFICATIONS

C HAVE BEEN DONE, THE REVERSE FLAG WILL BE TESTED AGAIN,

DII-20
C FOUND EQUAL TO ONE AND CONTROL WILL RETURN TO THE CALLING

C PROGRAM.

C35 IF(REV. EQ. 1 )GOTO 40

REV= 1 DO 60 LL= 1

, JK

60 NDET(LL)=DET(LL)

DO 61 LL=1,JK

0L=JK+1 -LL

61 DET(LL)=NDET(OL)

GOTO 77

UO RETURN

END

DII-21
E

truit— X

O—» OQOS

a _] CO UJ UJ a: I Q CD UJ 113:0=:

(XcrrDujt—

I— _jar , UJ CEO*— OD^

era: —ujo^

aiancli iri UJCbzoujcEjt: z o•-• t— q: >

UJOrCQ o

OCL.ID UJ

Sis

_ c^^-z.

. coz I •— 2: ^OZUJUJUJ _—*ujz3c_)a:

, ,QCJ_JUJO ^zcocrai—

'J.-.cr^ CO

tr, ,coo.

DO" 3:cr

^r^JUJ""

DII-22
C SUBROUTINE : LOOP

C

C ARGUMENTS : C

C --^ CALLED BY : INSERT

C

C CALLS SUBROUTINES : LINE

CC INPUT DATA : CS(50) = THE CHARACTER STRING THAT CONTAINS

C AN ELEMENTAL MOVE ('GOTO') COMMAND.

C PLN = THE NUMBER OF THE PRESENT LINE IN THE

C PROGRAM MODULE.

C BN = LOCATION TABLE POINTER FOR AN ENTERED

C LOCATION (IF INDEXED NAME THIS IS

C THE FIRST POINTER OF THE INDEXED

C SEQUENCE [FROM LINE].

C EN = THE LOCATION TABLE POINTER FOR THE LAST

C LOCATION SPECIFIED BY THE INDEXED

C SEQUENCE [FROM LINE].

C PROG(105,6) = THE PROGRAM MODULE.

n

\^C OUTPUT DATA : PROG(105,6) = THE MODIFIED PROGRAM MODULE

C WITH THE ADDITIONAL LINES GENERATED

C BY THE 'LOOP' COMMAND.

C PLN c THE NUMBER OF THE LAST LINE ENTERED

C IN THE PROGRAM MODULE BY THE 'LOOP'

C COMMAND.

CC FUNCTION: TO ALLOW ENTRY OF A REPEATED SEQUENCE OF ELEMENTAL

C MOVES WHERE ONLY THE LOCATIONS ARE

C DIFFERENT AND ARE DESIGNATED BY INDEXED NAMES.

C SUBROUTINE LOOP

IMPLICIT INTEGER (B-Z)

COMMON/PMOD/PROG ( 1 05 , 6) , ENDP , BRNCH ( 6 ) ,M

COMMON/PTN/BN , EN

C0MM0N/CMD/CS(50) ,PN,PLN

DIMENSION STR(50)

K=1

C

C ZERO THE ARRAY STR(50)

C DO 55 H=1,50

55 STR(H)=0

C

C STORE THE FIRST PROGRAM LINE NUMBER OF THE 'LOOP' SEQUENCE

C IN THE VARIABLE 'BEGIN'.

C BEGIN=PLN

C

C READ IN A COMMAND FROM THE TERMINAL.

C99 READ(6,90) (CS(J) , J=1 ,50)

90 FORMAT(50(A1 )

)

PN = 1 C

DII-23
C IF THE COMMAND IS A 'FINISH' STATEMENT THEN BRANCH TO

C STATEMENT 20 TO GENERATE THE ADDITIONAL REPEATED LINES.

C IF(CS(PN) .EQ. 8262)G0T0 20

C

C IF THE COMMAND IS A 'GOTO' STATEMENT, THEN BRANCH TO STATEMENT

C 30 TO DECODE IT.

C IF(CS(PN) .EQ.8263)GOTO 30

C

C IF THE COMMAND IS NEITHER OF THE ABOVE, THEN PRINT OUT THE

C FOLLOWING ERROR MESSAGE ON THE TERMINAL AND RETURN TO THE

C CALLING PROGRAM.

C100 WRITE(6,101)

101 F0RMAT(8X'ERR0R IN «»»L00P»»» INSTRUCTION')

PLNzBEGIN

GOTO 40

C

C CALL THE SUBROUTINE 'LINE' TO DECODE THE 'GOTO' STATEMENT INTO

C THE CORRECT LOCATION TABLE POINTERS AND FUNCTION FLAGS

C AND STORE THEM IN THE PROPER LINE IN THE PROGRAM

C MODULE.

C30 CALL LINE

C

C IF A RANGE OF INDEXED LOCATIONS WAS NOT GIVEN FOR THIS 'GOTO'

C STATEMENT (IE. ONLY ONE LOCATION WAS SPECIFIED, RESULTING

C IN 'BN' BEING EQUAL TO 'EN') THEN BRANCH TO STATEMENT

C 31 WHICH WILL HAVE THE EFFECT OF SETTING THE CORRESPONDING

C VALUE OF THIS LINE IN 'STR(50)' EQUAL TO ZERO.

G IF(EN.EQ.BN)GOTO 31

C

C IF A RANGE OF INDEXED LOCATIONS WAS GIVEN, THEN THE LOCATION

C TABLE POINTERS FOR THE STARTING (BN) AND ENDING (EN) INDEX

C SPECIFIED ARE USED TO CALCULATE THE NUMBER OF REPEATS THROUGH

C THE LOOP 'SDIF=EN-BN'

.

C SDIF=EN-BN

DIFrlABS (SDIF)

C

C THE SIGN OF THE DIRECTION THROUGH THE INDEXED NAME (IE. IS THE INDEX

C INCREASING OR DECREASING) IS STORED AT THE APPROPRIATE POSITION

C IN THE MATRIX •STR(50'.

C STR(K) =SDIF/DIF

C

C THE PROGRAM MODULE LINE INDICATOR (PLN) IS INCREMENTED. THEN

C BRANCH TO STATEMENT 99 TO READ IN ANOTHER COMMAND.

C31 K=K+1

PLN=PLN+1

GOTO 99

C

C THIS SECTION OF CODE GENERATES THE ADDITIONAL REPEATED LINES,

DII-24
C INCREMENTING (OR DECREMENTING) THE INDEXED LOCATIONS FOR

C EACH REPEAT.

C

C CALCULATE THE NUMBER OF LINES TO BE REPEATED AND STORE IN 'LINES'.

C20 LINES=PLN-BEGIN

C

C REPEAT THE NUMBER OF LINES (' L INES ' ) IN THE REPEATING GROUP 'DIF'

C TIMES, INCREMENTING THE INDEXED VALUES BY THE APPROPRIATE AMOUNT

C (STR(KM)»LB) EACH TIME.

C DO 21 LB=1 ,DIF

DO 21 KM=1 , LINES

0RIG=BEGIN+KM-1

NXT=BEGIN+LINES»LB+KM-1

DO 22 JK= 1,

6

22 PROGCNXT, JK)=PROG(ORIG, JK)

21 PR0G(NXT,2)=PR0G(NXT,2)+STR(KM)»LB

CC SET THE PROGRAM LINE INDICATOR ( ' PLN ' ) TO THE LAST LINE ENTERED IN

C THE 'LOOP' SEQUENCE AND RETURN TO THE CALLING PROGRAM.

C PLN=NXT

40 RETURN

END

DII-25
DII-26
C SUBROUTINE : INDEX

C

C ARGUMENTS : I = IF 1=1 DO NOT PRINT OUT INDEXED NAMES ON

C TERMINAL AND DO NOT ALLOW THE ENTRY

C OF NEW INDEXED NAMES.

C IF 1=0 PRINT OUT INDEXED NAMES ON THE

C TERMINAL AND ALLOW THE ENTRY OF

C NEW INDEXED NAMES.

C

C CALLED BY : PROGS LOCTAB

CC CALLS SUBROUTINES : C

C INPUT DATA : IN(30) = THE CHARACTER STRING THAT CONTAINS THE

C ASCII FORMATTED INDEXED LOCATION NAMES.

C

C OUTPUT DATA : NPT(3,20) = THE ARRAY THAT CONTAINS THE LOCATION

C TABLE POINTERS FOR ALL OF THE POSSIBLE

C INDEXED NAME LOCATIONS.

C FL(6) = THE POSITION IN THE STRING IN(30)

C OF THE FIRST AND LAST LETTER OF EACH

C INDEXED NAME.

C NAME(3) = THE FIRST LETTER OF EACH INDEXED NAME.

C

C FUNCTION: INITIALIZES THE ARRAY NPT(3,20) WHICH IS USED TO DECODE

C AN INDEXED NAME AND ITS INDEX INTO ITS CORRECT LOCATION

C TABLE POINTER. STORES THE INDEXED NAMES IN THE STRING

C IN(30) AS WELL AS DECODING PERTINENT INFORMATION INTO

C VARIABLES IN A COMMON BLOCK FOR USE BY OTHER PROGRAMS

C FOR IDENTIFICATION PURPOSES.

Cc SUBROUTINE INDEX(I)

IMPLICIT INTEGER(B-Z)

C0MM0N/IND/IN(30) ,FL(6) ,NAME(3) ,NPT(3,20)

C

C INITIALIZE THE ARRAY NPT(3,20) SO THAT IT CONTAINS ALL OF THE

C CORRECT LOCATION TABLE POINTERS FOR ALL POSSIBLE

C INDEXED NAMES.

C DO 10 G=1 ,20

NPT(1 ,G)=G

NPT(2 ,G)=G+20

10 NPT(3,G)=G+40

C

C TEST FLAG (I), IF 1=0 THEN PRINT OUT THE INDEXED NAMES FROM IN(30),

C AND REQUEST IF NEW INDEXED NAMES ARE TO BE ENTERED.

C IFd.EQ. 1 )GOTO 120

300 WRITE(6,199)

199 F0RMAT(8X 'THESE ARE THE CURRENT INDEXED LOCATIONS')

WRITE(6 , 200) (IN( J) ,J=1 ,30)

200 F0RMAT(2X, 30(A1 ) ) WRITE(6,201 ) 201 F0RMAT(8X'D0 YOU WANT TO ENTER NEW INDEXED NAMES ?')

READ(6 ,202) JA

DII-27
202 FORMAT(AI)

IF( JA.EQ. 1HY)G0T0 70

IF( JA.EQ. 1HN)G0T0 120

WRITE(6,125)

125 F0RMAT(8X ' YOU DID NOT ANSWER THE QUESTION, PLEASE TRY AGAIN',//)

GOTO 300

C

C IF NEW INDEXED NAMES ARE TO BE ENTERED, READ THEM INTO IN(30).

C70 WRITE(6,100)

100 F0RMAT(8X'ENTER INDEXED NAMES',/

1 8X'EG. PALLET CUTTER STACK')

READ(6, 102)(IN(J) ,J=1 ,30)

102 FORMAT(30 ( A1 )

)

120 J=0

K = 0 C

C ADVANCE THROUGH THE STRING IN(30), NOTING THE POSITION IN THE

C STRING WHERE EACH INDEXED NAME BEGINS AND ENDS AND STORING

C THESE POSITION VALUES IN FL(6). ALSO, STORE THE

C FIRST LETTER OF EACH INDEXED NAME IN NAME(3).

C20 J=J+1

IF( J.GT. 30)G0T0 40

IF(IN(J) .EQ.8224)G0T0 20

K = K+1

FL(K)=J

D=(K+1)/2

NAME(D)=IN( J)

15 J=J+1

IF( J.GT.30)G0T0 40

IF(IN(J) .NE.8224)G0T0 15

K = K + 1 FL(K)=J-1

GOTO 20

40 RETURN

END

DII-28

DII-30

I
C SUBROUTINE : LOCPT

CC ARGUMENTS : D = FLAG USED TO INDICATE IF A LOCATION NAME

C IS TO BE ENTERED FROM THE TERMINAL (D=0),

C OR IF THE LOCATION NAME IS COMING FROM

C THE CALLING PROGRAM (D=1)>

CC CALLED BY : AVOID LIMIT LINE NWTAB

C

C CALLS SUBROUTINES : C

C INPUT DATA : CS(50) = THE CHARACTER STRING THAT CONTAINS THE

C ASCII FORMAT OF THE LOCATION NAME.

C PN = THE POSITION IN THE ABOVE CHARACTER

C STRING OF THE FIRST LETTER OF THE

C LOCATION NAME.

C IN(30) = THE CHARACTER STRING THAT CONTAINS THE

C ENGLISH NAMES (IN ASCII FORMAT) OF THE

C INDEXED LOCATION POINTS.

C NAME(3) = THE FIRST LETTER OF THE INDEXED

C NAMES.

C NPT(3,20) = THE MATRIX THAT CONTAINS THE LOCATION

C TABLE POINTERS FOR ALL OF THE

C INDEXED POSITIONS.

C

C OUTPUT DATA : BN = THE LOCATION TABLE POINTER DECODED FROM

C THE ASCII FORMAT ENGLISH NAME OF THE

C LOCATION POINT.

C EN = THE LOCATION TABLE POINTER FOR THE

C LAST INDEXED POSITION IF A SEQUENCE OF

C INDEXED LOCATIONS HAS BEEN ENTERED.

C

C FUNCTION: TO DECODE THE ENGLISH NAME OF A LOCATION INTO ITS

C CORRECT LOCATION TABLE POINTER. IF A SEQUENCE OF

C INDEXED LOCATIONS IS TO BE DECODED, THIS ROUTINE

C RETURNS THE LOCATION TABLE POINTERS OF

C THE FIRST AND LAST LOCATIONS IN THE SEQUENCE. THIS

C ROUTINE CAN ALSO BE USED TO DECODE AN ASCII FORMAT

C NUMBER FROM 1-100 INTO ITS INTEGER VALUES.

C SUBROUTINE LOCPT(D)

IMPLICIT INTEGER (B-Z)

COMMON/IND/IN(30) ,FL(6) ,NAME(3) ,NPT(3,20)

COMMON/PTN/BN, EN

COMMON/CMD/CS{50) , PN , PLN

BN = 0 EN = 0

C

C TEST FLAG TO SEE IF LOCATION NAME TO BE ENTERED THROUGH

C TERMINAL.

IF(D. EQ. 1 )GOTO 75

C

i C ACCEPT LOCATION NAME FROM TERMINAL.

C500 WRITE(6,300)

300 F0RMAT(2X'ENTER TRAJECTORY NAME')

DII-31
READ(6,200)(CS(J) ,J=1 ,50)

200 FORMAT(50(A1 )

)

J = 0 10 J=J+1

IF(CS(J) .EQ.8224)G0T0 10

PN = J 75 J=PN

C

C TEST TO SEE IF FIRST CHARACTER IS A LETTER OR A NUMBER. IF A

C LETTER BRANCH TO TEST IF INDEXED NAME.

C ONE=CS( J)

IF(0NE.GT.8256)G0T0 50

C

C DECODE ASCII FORMAT NUMBERS INTO TWO DIGIT INTEGERS.

C TW0=CS( J+1

)

BN= ( 10»(ONE-82'40)+TWO-82 40)+90

J = J + 2 GOTO 41

50 J=J+1

IF(J.GT.50)G0T0 60

IF(CS(J) .NE.8224)G0T0 35

37 J=J+1

C

C IF LETTERED NAME DOES NOT HAVE ANY INDEXING NUMBERS, BRANCH TO

C STATEMENT 60.

C IF(J.GT.50)G0T0 60

IF(CS( J) .EQ. 8224)G0T0 37

IF(CS(J) .NE.8232)G0T0 60

35 IF(CS(J) .NE.8232)G0T0 50

C

C IF A OPEN PARENS IS DETECTED (INDICATING THE INDEXING NUMBERS),

C THEN DECODE THE INDEXED NAME USED.

C DO 20 K=1,3

20 IFCONE . EQ. NAME(K) )G0T0 7 100 WRITE(6,101)

101 F0RMAT(2X'INDEXED NAME ERROR')

GOTO 40

7 J=J+1

IF( J.LT.51 )GOTO 33

102 WRITE(6,103)

103 F0RMAT(2X'INC0RRECT NAME')

GOTO 40

CC DECODE THE INDEXING NUMBER USED WITH THE INDEXED NAME.

C33 IF(CS(J) .EQ.8224)G0T0 7 0NE = CS( J ) TW0=CS( J+1

)

F=(0NE-82 40)»10+TW0-82 40

BN=NPT{K,F)

J = J+2

80 IF(CS(J) .EQ.8237)G0T0 82

DII-32
IF(CS(J) .NE.8233)GOTO 100

EN=BN

GOTO 41

C

C IF A SEQUENCE OF INDEXING NUMBERS FOR A PARTICULAR INDEXED

C NAME IS GIVEN, DECODE THE LAST INDEXING NUMBER IN THE

C SEQUENCE.

C82 J=J+3

ONE=CS( J-2)

TWO=CS( J-1

)

F= (ONE-82 40) » 10+TWO-8240

C

C LOOK UP THE LOCATION TABLE POINTER FOR THE

C LAST INDEXING NUMBER FOR THE SPECIFIED INDEXED NAME. EG. FOR

C THE INDEXED NAME AND SEQUENCE NUMBERS - BOX(13-17) - THE

C INDEXED NAME IS 'BOX', THE FIRST INDEXING NUMBER IS '13', THE

C LAST INDEXING NUMBER IS '1?'. IF 'BOX' WAS THE SECOND

C INDEXED NAME STORED IN 'IN(30)' THEN THE FIRST LOCATION TABLE

C POINTER WOULD BE FOUND AT 'NPT(2,13)', AND THE LAST LOCATION TABLE

C POINTER WOULD BE FOUND AT 'NPT(2,17)'.

C EN=NPT(K,F)

GOTO 41

C

C DECODE THE NON-INDEXED LETTERED NAME INTO THE CORRECT LOCATION

C TABLE POINTER.

C60 BN=0NE-8196

EN = BN

J = J-1

41 PN=J

IF(BN.LE.0)GOTO 102

IF(BN.GT. 190)G0T0 102

40 RETURN

END

DII-33
UJQ- co^ _

a:ocD

3:i-CE otoi—

LUCDO-

—I '^1-0,. a:

DII-34
SUBROUTINE : LINE

ARGUMENTS

CALLED BY : INSERT LOOP

CALLS SUBR'OUTINES

INPUT DATA : CS(50)

PLN

BN

NEXT LOCPT

OUTPUT DATA : PR0G(PLN,6)

THE CHARACTER STRING THAT CONTAINS THE

ELEMENTAL MOVE ('GOTO' STATEMENT) COMMAND,

THE NUMBER OF THE LINE IN THE PROGRAM

MODULE THAT THE ABOVE STATEMENT WILL BE

CODED INTO.

THE INTEGER REPRESENTATION OF AN ASCII

CODED NUMBER IN THE CHARACTER STRING

CS(50), USED TO GENERATE LOCATION

TABLE POINTERS, VELOCITY VALUES, AND

CHANNELS FOR INTERLOCK (SEND AND WAIT)

SIGNALS [FROM 'LOCPT'].

= THE NEW LINE OF THE PROGRAM MODULE

THAT THE ELEMENTAL MOVE HAS BEEN

CODED INTO.

FUNCTION;

C

C

C

C25

125

C

CC

C

CC

C

C100

101

TAKES THE ENGLISH TEXT ELEMENTAL MOVE STATEMENT

TYPED INTO THE TERMINAL, CODES IT INTO THE CORRECT

POINTERS AND FLAGS, AND STORES THEM IN THEIR PROPER

POSITIONS IN THE SPECIFIED LINE IN THE

PROGRAM MODULE.

SUBROUTINE LINE

IMPLICIT INTEGER(B-Z)

COMMON/PMOD/PROG( 105 ,6) , ENDP , BRN CH ( 6 ) ,M

COMMON/PTN/BN, EN

C0MM0N/CMD/CS(50) ,PN,PLN

COMMON /BED/BEG, FIN , DIF , ERR

ZERO ALL OF THE VALUES IN LINE PLN, SET THE VELOCITY INDICATOR

EQUAL TO 50 CM/SEC AS A DEFAULT VALUE.

DO 125 J=2,6

PROG(PLN, J)=0

PROG(PLN , 3) =50

ADVANCE THROUGH THE CHARACTER STRING UNTIL THE DESTINATION NAME

IS REACHED(BY A CALL TO 'NEXT').

CALL NEXT

IF(PN.LT.51 )GOTO 41

THIS IS THE ERROR MESSAGE PRINTED IF ANY MISTAKE IN FORMAT

IS DETECTED.

WRITE(6 , 101

)

F0RMAT(8X'ERR0R IN »»» GOTO »»«STATEMENT '

)

GOTO 42

DII-35
C THE ASCII REPRESENTATION OF THE DESTINATION IS DECODED

C INTO THE CORRECT LOCATION TABLE POINTER BY A CALL TO 'LOCPT'.

C THIS POINTER IS THEN STORED IN THE PROGRAM MODULE.

CMl CALL LOCPT(I)

PROG ( PLN , 2) =BN

BBN=BN

EEN=EN

C

C ADVANCE THROUGH THE ASCII CHARACTER STRING UNTIL A FUNCTION

C COMMAND IS FOUND (BY A CALL TO 'NEXT').

C15 CALL NEXT

IF(PN.GT.50)GOTO 42

C

C THIS IS A SERIES OF TESTS ON THE ASCII FORM OF THE FIRST LETTER OF THE

C FUNCTION COMMAND TO DETERMINE IF IT IS A G ( GRASP ), R ( RELEASE )

,

C P(PROXIMITY),D(DETECT) ,B( BALANCE) ,U(UNSTACK) ,T(TOUCH) , C L(LINE) ,E(EDGE)

.

C

C

C 'GRASP'

C IF(CS(PN) .NE.8263)GOTO 60

PR0G(PLN,6)=1

GOTO 80

CC 'RELEASE'

C60 IF(CS(PN) .NE.8274)G0T0 61

PROG(PLN , 6) =2

80 PN=PN+1

C

C 'PROXIMITY'

C IF(CS(PN) .NE.8272)GOTO 81

82 PROG(PLN , 6) =PROG(PLN , 6)+2

81 CALL NEXT

IF(PN.GT.50)G0T0 42

C

C 'PROXIMITY'

C IF(CS(PN) .NE.8272)G0T0 85

GOTO 82

CC 'DETECT'

C

61 IF(CS(PN) .NE.8260)GOTO 62

PROG(PLN,6)=5

GOTO 81

C

C 'BALANCE'

C62 IF(CS(PN) .NE.8258)GOTO 63

PROG(PLN,6)=6

GOTO 81

DIl-36
c

C 'UNSTACK'

C63 IF(CS(PN) .NE.8277)GOTO 6H

PROG(PLN , 6) =7

GOTO 81

C

C 'TOUCH'

C64 IF(CS(PN) .NE.8276)GOTO 65

PROG(PLN, 6)=8

GOTO 81

C

C 'LINE'

C65 IF(CS(PN) .NE.8268)GOTO 66

PROG(PLN, 6)=9

GOTO 81

C

C 'EDGE'

C66 IF(CS(PN) .NE.8261 )GOTO 85

PR0G(PLN,6)=10

CALL NEXT

CALL LIMIT

PR0G(PLN,5)=BEG

GOTO 81

C

C TEST FOR A 'VELOCITY' COMMAND, IF THERE IS ONE THEN DECODE THE

C ASCII FORM OF THE SPECIFIED VELOCITY (CM/SEC) BY A

C CALL TO 'LOCPT'. MULTIPLY THIS INTEGER NUMBER BY 10 AND STORE

C AT POSITION 3 IN THE LINE IN THE PROGRAM MODULE.

C85 IF( CS(PN) .NE. 8278)GOTO 90

CALL NEXT

IF(PN.GT.50)GOTO 100

CALL LOCPT( 1

)

PROG(PLN,3)=(BN-90)

CALL NEXT

IF(PN.GT.50)G0T0 12

CC TEST FOR A 'SEND' COMMAND. IF THERE IS ONE, DECODE

C THE ASCII FORM OF THE OUTPUT CHANNEL NUMBER THAT IS TO

C BE SET HIGH, MULTIPLY THIS INTEGER BY 100 AND STORE

C IN POSITION 4 OF THE SPECIFIED LINE OF THE PROGRAM

C MODULE.

C90 IF(CS(PN) .NE.8275)G0T0 91

CALL NEXT

IF(PN.GT.50)GOTO 100

CALL LOCPT(I)

PROG(PLN, 4)=(BN-90)«100

CALL NEXT

IF(PN.GT.50)G0TO 42

C

C TEST FOR A 'WAIT' COMMAND. IF THERE IS ONE, STORE THE NUMBER

DII-37
C OF THE INPUT CHANNEL THAT MUST GO HIGH FOR THE PROGRAM

C TO CONTINUE.

C91 IF(CS(PN) .NE.8279)GOTO 100

CALL NEXT

IF(PN .GT.50)G0T0 100

CALL L0CPT( 1

)

PROGCPLN , n) =PROG(PLN , 4)+BN-90

CALL NEXT

IF(PN.LT.51 )GOTO 100

42 BNrBBN

EN=EEN

40 RETURN

END

DII-38

a:

ui :UJUD

;f; f-zz: 0002

:o_iQ;a:

CO

LU

>-

_JU.cc:"

a: uj^ >xi—

O'-.U. O

UJU. W_ QO—

uji— a:!±:"2'

iarcr

DII-40
C SUBROUTINE : PTNAME

C

C ARGUMENTS : NU = A LOCATION TABLE POINTER SPECIFIED BY

C THE CALLING PROGRAM.

C

C CALLED BY : PLINE

C

C CALLS SUBROUTINES : CC --- INPUT DATA : FL(6)

C

C

C NAME(3)

C

C NPT(3,20)

C

C

C OUTPUT DATA : NM(15)

CC

C

C

C

C FUNCTION: DECODES THE LOCATION TABLE POINTER 'NU' INTO THE

C CORRESPONDING ASCII FORMATED NAME.

C SUBROUTINE PTNAME(NU)

IMPLICIT INTEGER(B-Z)

COMMON/NAM/NM( 1 5

)

COMMON/IND/IN(30) ,FL(6) ,NAME(3) ,NPT(3,20)

C

C FILL THE CHARACTER STRING NM(15) WITH BLANKS.

C DO 10 J = 1 , 15

10 NM(J)=822U

C

C TEST IF POINTER 'NU' IS A SINGLE LETTERED NAME OR

C AN INDEXED LOCATION NAME. IF INDEXED NAME THEN BRANCH TO

C STATEMENT NUMBER 60.

C IF(NU.LT.61 )GOTO 60

C

C DECODE THE LETTERED NAME LOCATION TABLE POINTER INTO ITS

C APPROPRIATE ASCII FORMAT LETTER. STORE THIS VALUE IN T«E STRING

C NM(30) AND RETURN TO THE CALLING PROGRAM.

C NM( 1 )=NU+8196

GOTO 40

C

C SEPARATE OUT THE TEN'S DIGIT AND THE UNIT'S DIGIT FROM

C THE POINTER 'NU' FOR THE INDEXED NAME.

C60 DO 31 K=1,7

TEN=K-1

UNIT=NU-10«TEN

IF(UNIT.LT. 10)GOTO 80

= THE LOCATIONS IN THE CHARACTER STRING

IN(30) OF THE FIRST AND LAST LETTER

OF THE INDEXED LOCATION NAMES.

= THE FIRST LETTERS OF THE INDEXED

LOCATION NAMES.

= THE ARRAY OF VALUES FOR THE POINTERS

FOR THE INDEXED LOCATION NAMES.

= THE CHARACTER STRING THAT CONTAINS

THE SEQUENCE OF LETTERS (IN ASCII

FORMAT) THAT MAKES UP THE NAME THAT

CORRESPONDS TO THE LOCATION TABLE

POINTER 'NU'.

DII-41
31 CONTINUE

C

C THIS SECTION DECODES THE LOCATIONS IN THE STRING IN(30) OF THE

C FIRST AND LAST LETTER OF THE INDEXED NAME SPECIFED BY THE

C POINTER 'NU'. THEN STORES THE LETTERS OF THIS INDEXED NAME

C IN THE STRING NM( 1 5)

.

C80 J=2»( (NU-1 )/20)+1

BE=FL(J)

EN=FL( J+1

)

DO 82 LJ=1 , 15

NN=BE+LJ-1

IF(NN.GT.EN)GOTO 83

82 NM(LJ)=IN(NN)

CC AN OPEN PARENS IS NOW INSERTED IN THE STRING NM(15) AFTER THE

C INDEXED NAME.

C

83 NM(LJ)=8232

C

C THE CORRECT INDEX NUMBER IS DECODED AND STORED IN NM(15).

C

NTEN=( (TEN+1 )»2)/( J+1 )-1

IF(TEN.NE. ( J+1 ) )GOTO 85

NTEN=2

85 NM(LJ+1 )=NTEN+8240

NM(LJ+2)=UNIT+8240

C

C A CLOSE PARENS (IE THE ASCII FORMAT VALUE FOR A CLOSE PARENS) IS

C PLACED AFTER THE INDEX NUMBER IN NM(15) AND CONTROL RETURNS TO

C THE CALLING PROGRAM.

C

NM(LJ+3) =8233

40 RETURN

END

DII-42

DII-44
SUBROUTINE : PLINE

ARGUMENTS : LN = THE NUMBER OF THE PARTICULAR LINE

IN THE PROGRAM MODULE TO BE

PRINTED OUT. - CALLED BY : INSERT DELETE PRINT SAMPLE - CALLS SUBROUTINES : PTNAME - INPUT DATA

OUTPUT DATA

PROG(105,6) = THE PROGRAM MODULE.

NM(15) = THE ASCII FORMATTED

CHARACTER STRING THAT CONTAINS THE

NAME OF THE LOCATION 'NU' [FROM 'PTNAME']

: THE DECODED LINE FROM THE PROGRAM MODULE PRINTED

OUT ON THE. TERMINAL.

FUNCTION

200

C

C

C

C10

DECODES A SPECIFIED LINE FROM THE PROGRAM MODULE

INTO THE ENGLISH LANGUAGE CHARACTER STRING ORIGINALLY

USED TO ENTER IT, AND DISPLAYS IT ON A TERMINAL.

SUBROUTINE PLINE(LN)

IMPLICIT INTEGER(B-Z)

COMMON /PMOD /PROG (105, 6) , ENDP , BRNCH ( 6 ) ,M

COMMON/NAM/NM( 1 5

)

C0MM0N/CMD/CS(50) ,PN,PLN

DIMENSION FN(6 , 1 1

)

DATA FN/6»' 1

» 2 'GR ' ,

' AS

,

' P

' ,3*' t

> 3 'RE' , 'LE' ,

• AS' , 'E ' 4 'GR','AS' , 'P •

, 'PR' ,''0X','' ',

5 'RE'.'LE' ,

' AS' , 'E •

, ' PR ' , ' OX' , 6 'DE'.'TE' ,

' CT ' ,3»' t

> 7 'BA' , 'LA' ,

' NC •

, 'E '

» » 8 'UN •

, ' ST,

• AC '

, 'K '

1 ^ » 9 'TO' , 'UC , 'H • ,3»' •

1 1 'LI • , 'NE' ,4»' t

>

1 'ED' , 'GE' ,4»' •/

IF THE FIRST LINE OF THE PROGRAM MODULE IS TO BE PRINTED OUT,

THEN DECODE THE STARTING LOCATION BY A CALL TO 'PTNAME'

WHICH RETURNS THE ASCII FORMATTED NAME IN THE STRING 'NM(15)'

THIS NAME IS THEN PRINTED OUT WITH THE WORD 'START'.

IF(LN.NE. 1 )GOTO 10

NU=PROG( 1, 1

)

IF(NU.EQ.0)GOTO HO

CALL PTNAME(NU)

WRITE (6, 200) (NM( J) , J=1 , 15)

FORMAT (6X, 'START ',1X,15(A1))

THE DESTINATION POINTER 'NU' IS DECODED BY 'PTNAME' AND RETURNED

IN 'NM(15)'.

NU=PR0G(LN,2)

Dll-45
IF(NU.EQ.O)GOTO 40

CALL PTNAME(NU)

CC THE FLAG 'F' IS CALCULATED FROM THE FUNCTION FLAG OF THE

C OF THE PROGRAM LINE AND IS USED TO SPECIFY THE PROPER LINE

C OF THE MATRIX 'FN(6,11)' IN ORDER TO PRINT OUT THAT FUNCTION

C NAME.

C F=PROG(LN, 6)+1

C

C IF AN OUTPUT SIGNAL IS TO BE SENT OUT ON A CHANNEL, THEN THE

C CHANNEL NUMBER IS DECODED AND STORED IN 'SEN'.

C SEN=PROG(LN,4)/100

C

C IF THE PROGRAM IS TO WAIT FOR A SIGNAL ON AN INPUT CHANNEL, THEN

C THE INPUT CHANNEL NUMBER IS DECODED AND STORED IN ' WA '

.

C WA=PROG(LN, 4)-SEN»100

C

C THE VELOCITY VALUE IN CM/SEC IS DECODED AND STORED IN 'VEL'.

C

c VEL=PROG(LN, 3)

C

C THE DECODED PROGRAM LINE IS NOW PRINTED OUT ON THE TERMINAL.

C IF(PR0G(LN,5) .EQ.OGOTO 140

LE=PROG(LN, 5)

WRITE(6 , 300)LN, (NM(L),L=1,15),(FN{E,F),E=1,6) , LE , VEL , SEN , WA

300 FORMAT( ' + '15, • GOTO ' , 1 5 ( A 1 )

, 1 X , 6 ( A2 )

, T34 , 12 , T4 1

,

' VELOC IT Y

(

' , 1 3

,

1 ') SEND(',I2,') WAIT( ' ,12, • )

' ) GOTO 40

140 WRITE (6 , 301 )LN, (NM(L) ,L=1 , 15) , (FN(E,F) ,E=1 , 6) , VEL, SEN,WA

301 FORMATC • + • ,15, ' GOTO • , 1 5 ( A 1 )

, 1 X , 6 ( A2 )

,

' VELOC IT Y

(

'

, 1 3

,

1 •) SEND(',I2,') WAIT( ' ,12, ' )

' ) 40 RETURN

END

%

DII-46
3

I
DII-48
C SUBROUTINE : NEXT

C

C ARGUMENTS : C

C CALLED BY : PRINT INSERT DELETE LINE

CC CALLS SUBROUTINES : C

C INPUT DATA : CS(50) = THE CHARACTER STRING THAT CONTAINS THE

C ASCII CODED INPUT INFORMATION.

C PN = THE INDICATOR DESIGNATING THE PRESENT

C POSITION IN THE CHARACTER STRING

C CS(50).

CC OUTPUT DATA : PN = THE POSITION INDICATOR IN THE STRING

C CS(50) THAT INDICATES THE BEGINNING OF

C THE NEXT PIECE OF INFORMATION.

C

C FUNCTION: ADVANCES THE POSITION INDICATOR 'PN' UNTIL REACHES

C THE BEGINNING OF THE NEXT PIECE OF INFORMATION IN THE

C CHARACTER STRING CS(50).

C SUBROUTINE NEXT

IMPLICIT INTEGER(B-Z)

COMMON /CMD/CS( 50) , PN , PLN

GOTO 21

48 PN=PN+1

21 IF(PN . GT . 50)G0T0 40

CC THIS (PN) POSITION IN THE CHARACTER STRING IS CHECKED TO SEE IF

C IT IS A BLANK, AN OPEN PARENS, OR A CLOSE PARENS.

C IF( CS( PN ) . EQ. 8224 )GOTO 41

IF(CS(PN) .EQ.8232)GOTO 50

IF(CS(PN) .EQ.8233)GOTO 50

CC IF THE PRESENT CHARACTER IS NONE OF THE ABOVE THEN CYCLE

C BACK TO STATEMENT 48, STEP TO THE NEXT CHARACTER

C IN THE STRING AND TEST AGAIN.

C GOTO 48

C

C ONCE A BLANK IS DETECTED, BRANCH TO THIS SECTION OF CODE AND

C CONTINUE STEPPING THROUGH THE CHARACTER STRING TESTING

C FOR BLANKS, OPEN PARENS, OR CLOSE PARENS.

C41 PN=PN+1

IF(PN.GT.50)GOTO 40

IF(CS(PN) .EQ.8224)GOTO 41

IF(CS(PN) .EQ. 8232)GOTO 50

C

C IF A CHARACTER IS DETECTED THAT IS NOT A BLANK, OPEN PARENS,

C OR A CLOSE PARENS THEN IT IS ASSUMED THAT THIS IS THE

C BEGINNING OF THE NEXT PIECE OF INFORMATION AND RETURNS

C TO THE CALLING PROGRAM.

C

DII-49
IF(CS(PN) .NE.8233)GOTO 40

EITHER A OPEN OR CLOSE PARENS IS DETECTED THEN ADVANCE TO THE

NEXT CHARACTER AND TEST FOR A BLANK.

PN=PN+1

IF(PN .GT . 50)G0T0 40

A BLANK IS FOUND, CONTINUE STEPPING THROUGH THE STRING

UNTIL THE FIRST NON-BLANK CHARACTER IS DETECTED THEN RETURN

TO THE CALLING PROGRAM.

IF(CS(PN) .EQ.8224)G0T0 50

RETURN

END

DII-50
I
, QLU

OCT * 2

DII-52
c SUBROUTINE • LIMIT

c

c ARGUMENTS : c

c CALLED BY : INSERT DELETE PRINT

c

c

c CALLS SUBROUTINES : LOCPT NEXT

c INPUT DATA : CS(50) — THE CHARACTER STRING THAT CONTAINS A

c NUMBER OR A RANGE OF NUMBERS TO BE

c DECODED FROM ASCII INTO INTEGER FORM.

c PN - THE POSITION IN THE STRING CS(50) OF

c THE FIRST LINE NUMBER TO BE DECODED.

c BN THE INTEGER VALUE OF THE LINE NUMBER

c DECODE FROM THE ASCII FORMAT BY A CALL

c

c TO THE SUBROUTINE 'LOCPT'.

c OUTPUT DATA : BEG = THE VARIABLE IN THE COMMON BLOCK THAT

c RETURNS THE INTEGER VALUE OF THE FIRST

c PROGRAM MODULE LINE NUMBER SPECIFIED

c IN THE EDITOR COMMAND.

c FIN = THE VARIABLE IN THE COMMON BLOCK THAT

c RETURNS THE INTEGER VALUE OF ^HE LAST

c PROGRAM MODULE LINE NUMBER SPECIFIED

c IN THE EDITOR COMMAND.

c DIF = THE DIFFERENCE (THE NUMBER OF LINES)

c BETWEEN 'BEG' AND 'FIN'.

c

c FUNCTION: TO RETURN THE BEGINNING AND ENDING PROGRAM MODULE LINE

c NUMBERS THAT THE PARTICULAR EDITOR COMMAND SPECIFIES

c

c FOR ITS OPERATION

SUBROUTINE LIMIT

IMPLICIT INTEGER(B-Z)

COMMON/PTN/BN, EN

COMMON /BED /BEG, FIN , DIF, ERR

C0MM0N/CMD/CS(50) ,PN,PLN

ERR = 0

C

C TRANSLATE THE ASCII FORMAT BEGINNING LINE NUMBER INTO AN

C INTEGER FORMAT AND STORE IN 'BEG' BY A CALL TO

C 'LOCPT'.

C CALL LOCPTC 1

)

BEG=BN-90

IF(BEG. LE . 0)GOTO 100

IF(BEG.GT. lOOGOTO 100

C

C TEST FOR A DASH(-), CLOSE PARENSO), OR A BLANK( ) AND BRANCH

C TO THE APPROPRIATE STATEMENT.

C IF(CS(PN) .EQ.8237)GOTO 30

IF(CS(PN) .EQ.8233)GOTO 45

IF(CS(PN) .EQ.8224)G0T0 45

100 WRITE(6,101)

101 F0RMAT(8X'F0RMAT ERROR IN PROGRAM MODULE LINE NUMBER')

DII-53
ERR= 1 GOTO 40

C

C IF A CLOSE PARENS WAS DETECTED THEN THIS INDICATES ONLY ONE LINE

C WAS SPECIFIED THEREFORE SET 'FIN' EQUAL TO 'BEG' . C45 FIN=BEG

GOTO 60

C

C IF A DASH WAS DETECTED, THIS INDICATES THAT AN ENDING

C NUMBER IS ALSO SPECIFIED THEREFORE ADVANCE TO THE NEXT NUMBER

C IN THE STRING CSC50) (BY A CALL TO 'NEXT') AND TRANSLATE IT INTO

C THE INTEGER FORMAT AND STORE IN 'FIN' (BY A CALL TO 'LOCPT')

C30 PN=PN+1

IF(CS(PN) .NE.8224)G0T0 31

CALL NEXT

31 CALL LOCPT(I)

FIN=BN-90

C

C TEST TO MAKE CERTAIN THAT THE LINE NUMBERS ARE WITHIN THE

C ALLOWABLE RANGE.

C60 IF(FIN.LE.0)GOTO 100

IFCFIN.GT. 100)GOTO 100

C

C CALCULATE THE NUMBER OF LINES SPECIFIED AND STORE IN THE

C COMMON VARIABLE 'DIF'.

C DIF=FIN-BEG+1

40 RETURN

END

DII-54
I
DIl-56
C SUBROUTINE : ADJUST

CC ARGUMENTS : C

C CALLED BY : INSERT DELETE AVOID

C

C CALLS SUBROUTINES : C

C INPUT DATA : PROG(105,6) = THE PRESENT PROGRAM MODULE.

C

C OUTPUT DATA : PROG(105,6) = THE VERIFIED PROGRAM MODULE.

CC FUNCTION: MAKES CERTAIN THAT FOR EVERY LINE IN THE PROGRAM

C MODULE, THE PRESENT LOCATION POINTER IS THE SAME

C AS THE DESTINATION POINTER OF THE PREVIOUS LINE

C THEREBY ASSURING CONTINUITY IN THE COMMANDED

C POINT-TO-POINT MOTION.

C

SUBROUTINE ADJUST

IMPLICIT INTEGER(B-Z)

COMMON/PMOD/PROG ( 1 05 , 6) , ENDP , BRNCH ( 6 ) ,M

C

C LOOP THROUGH THE PROGRAM MODULE, TESTING TO DETERMINE IF

C EACH PROGRAM LINE HAS A DESTINATION POINTER. IF IT

C DOES, THEN SET THAT LINE'S PRESENT LOCATION POINTER EQUAL

C TO THE PREVIOUS LINE'S DESTINATION POINTER. IF THE PROGRAM

C LINE DOES NOT HAVE A DESTINATION POINTER, THEN ZERO ITS

C PRESENT LOCATION POINTER AND BRANCH TO STATEMENT 207.

C200 DO 201 M=2 , 100

IF(PR0G(M,2) .NE.0)GOTO 201

PROGCM, 1 )=0

GOTO 207

201 PROG(M, 1)=PR0G(M-1 ,2)

C

C THIS LOOP ZEROS OUT THE REMAINDER OF THE PROGRAM MODULE TO ELIMINATE

C ANY EXTRANEOUS DATA.

C207 DO 208 RM=M, 100

DO 208 MM= 1,

6

208 PROG(RM,MM) =0

40 RETURN

END

DII-57

Location Module

(Module #3)

LOCTAB - Requests data to completely specify a location point.

ARRAY - Requests data to specify the dimensions of

the array of points to be entered.

ARRLOC - Computes the coordinate values to specify

all of the locations in the array.

JOY - Uses input values from joystick to control

robot's motions.

POS - Calls in and scales the present joint

position values.

DIII-2

DIII-10

DI 11-14

DIII-18

DIII-24

DIII-1
DIII-2
C SUBROUTINE : LOCTAB

CC ARGUMENTS : C

C CALLED BY : SAMPLE

C

C CALLS SUBROUTINES : ARMIN INDEX LOCPT ARRAY ARRLOC JOY POS

C COOR

C

C INPUT DATA : INBUF(28) = WHEN THIS SWITCH IS UP, CAUSES THE

C PROGRAM TO JUMP BACK TO THE PREVIOUS

C ' DATA ENTRY REQUEST.

C INBUF(29) = WHEN THIS SWITCH IS DOWN, CAUSES THE

C CONTROL TO RETURN TO THE CALLING

C PROGRAM. . C INBUF(32) = WHEN THIS SWITCH IS UP, THE ARM IS

C UNDER JOYSTICK CONTROL.

C INBUF(34) = WHEN THIS SWITCH IS UP, THE LOCATION IS

C TO BE ENTERED AS AN •X,Y,Z' COORDINATE

C POSITION.

CC OUTPUT DATA : LTAB(90,^,7) = THE UPDATED LOCATION TABLE MODULE.

C

C FUNCTION: USES A QUESTION AND ANSWER SYSTEM TO OBTAIN THE

C NECESSARY INFORMATION FOR THE DESCRIPTION OF A

C LOCATION AND THE CONSTRUCTION OF THE LOCATION TABLE

C MODULE.

C SUBROUTINE LOCTAB

IMPLICIT INTEGER(B-R)

IMPLICIT INTEGER(T-Z)

COMMON/ARMBUF/INBUF(64) ,OUTBUF(64)

COMMON/LMOD/LTAB( 90 ,4,7) ,PRES,DEST

C0MM0N/0UT/JP0S(8)

COMMON /ARR/B1 ,E1,J1,J2,AD1(3) ,AD2(3) ,DA

C0MM0N/IND/IN(30) ,FL(6) ,NAME(3) ,NPT(3,20)

COMMON/PTN/BN, EN

C0MM0N/C0RT/AC(6 )

, AXEP , AYEP , AZEP

EQUIVALENCE ( EABORT ,INBUF( 28) ) E QUI VALENCE (NRE,INBUF( 29) ) , ( JSTK , INBUF ( 32 ) ) , ( XYZ , INBUF ( 3 4 )

)

CALL ARMIN

CC MONITOR SWITCH 29 ('NRE') TO DETERMINE WHEN CONTROL SHOULD BE

C RETURNED TO CALLING PROGRAM.

C500 IF(NRE .GT .-2000)G0T0 40

C

C INITIALIZE THE FLAG 'DA' AT ZERO. USED TO INDICATE WHEN AN ARRAY

C OF LOCATION POINTS ARE TO BE STORED.

r\^ DA = 0

CC THE FOLLOWING CODE READS THE PRESENT INDEXED NAMES OUT OF THE

C LOCATION TABLE MODULE, AND BY A CALL TO 'INDEX', PRINTS

C THESE INDEXED NAMES ON THE TERMINAL AND PROVIDES THE OPERATOR

C THE OPPORTUNITY TO CHANGE THESE NAMES.

DIII-3
M = 0 DO 104 J=89,90

DO 104 K= 1,

4

DO 104 L= 1 ,

7

M=M+1

IF(M. EQ. 31 )GOTO 98

104 IN(M)=LTAB( J,K,L)

98 CALL INDEX(O)

C

C THE FOLLOWING CODE PRINTS OUT THE FUNCTIONS OF THE VARIOUS CONTROL

C SWITCHES AND ASKS THE OPERATOR IF A SINGLE LOCATION OR AN ARRAY

C OF LOCATIONS IS TO BE ENTERED.

C303 WRITE(6,300)

300 F0RMAT(8X' SW 32 UP FOR JOYSTICK CONTROL',/

1 8X' SW 34 UP FOR XYZ ENTRY',/

1 8X' SW 32 & 34 DOWN FOR MANUAL ENTRY',//)

WRITE(6,201

)

201 F0RMAT(8X ' »»»TYPE +1»«» TO RECORD AN ARRAY OF INDEXED LOCATIONS',/

1 8X'»«»TYPE [CR]»«« TO RECORD LOCATIONS ONE AT A TIME')

READ(6 , 202)DA

202 F0RMAT(I3)

CALL ARMIN

IF(NRE.GT.-2000)GOTO 40

IF(EABORT.LT.-2000)GOTO 500

C

C THE OPERATORS RESPONSE IS TESTED AND IF AN ARRAY OF POINTS IS

C TO BE ENTERED, THEN THE SUBROUTINE 'ARRAY' IS CALLED.

C IF(DA. EQ. 0)GOTO 275

CALL ARRAY

C

C THE LOCATION TABLE POINTER FOR THE FIRST LOCATION TO BE ENTERED

C IS STORED IN THE VARIABLE 'BN' ('B1' IS THE FIRST LOCATION OF

C THE ARRAY DECODED AND RETURNED FROM THE CALL TO 'ARRAY').

C BN=B1

GOTO 321

C

C IF ONLY A SINGLE LOCATION IS TO BE ENTERED, THEN THE VALUE OF ITS

C LOCATION TABLE POINTER IS DECODED BY A CALL TO 'LOCPT',

C RETURNED IN THE VARIABLE 'BN' AND STORED IN BOTH

C 'B1' AND 'El', THE VARIABLES THAT ARE THE FIRST AND LAST

C LOCATIONS TO BE ENTERED.

C275 CALL LOCPT(O)

B1=BN

E 1 =BN

C

C THE VALUE OF THE SWITCH FOR JOYSTICK CONTROL IS CHECKED AND IF UP,

C THE SUBROUTINE 'JOY' IS CALLED TO TRANSFER CONTROL TO THE

C JOYSTICK BOX.

C321 IF( JSTK.GT.-2000)GOTO 400

CALL JOY

DIII-4
GOTO 401

C

C THE VALUE OF THE SWITCH FOR 'X,Y,Z« ENTRY IS CHECKED AND IF UP, THEN

C THE 'X.Y.Z' COORDINATE VALUES FOR THE LOCATION ARE

C REQUESTED.

C400 IF(XYZ.GT.-2000)GOTO 401

765 WRlTE(6,304)

304 F0RMAT(8X'ENTER X,Y,Z VALUES (DECIMAL CENTIMETERS)',/

1 8X'0F THE LOCATION TO BE RECORDED')

READ(6,305)A1X,A1Y,A1Z

305 F0RMAT(3 (F10 . 3)

)

CALL ARMIN

IF(EAB0RT.LT.-2000)G0T0 303

C

C THE OPERATOR IS NOW ASKED IF THESE COORDINATES ARE ABSOLUTE

C VALUES OR DELTA OFFSETS FROM THE PREVIOUS VALUES FOR

C THE PRESENT LOCATION OR FROM ANOTHER PREVIOUSLY ENTERED

C LOCATION.

C

766 WRITE(6,306)

306 F0RMAT(8X ' «»«TYPE +1»»»IF THESE ARE ABSOLUTE VALUES',/

1 8X'»»»TYPE [CR]«»»IF OFFSETTING PRESENT LOCATION BY THESE AMOUNTS',/

1 8X'»»»TYPE .1«»»IF THESE ARE OFFSETS FROM ANOTHER LOCATION')

READ(6,307)RAD

307 F0RMAT(I3)

CALL ARMIN

IF(EABORT.LT.-2000)GOTO 765

DDrRAD

IF(RAD.NE.-1 )GOTO 176

CC IF OFFSET FROM ANOTHER PREVIOUSLY RECORDED LOCATION, HERE, THAT

C LOCATION NAME IS ENTERED.

C WRITE(6 , 308)

308 F0RMAT(8X ' WHAT IS THE OTHER LOCATION YOU ARE OFFSETTING FROM ?')

RAD=0

.

MNN=BN

C

C THIS OTHER LOCATION NAME IS IDENTIFIED BY A CALL TO 'LOCPT',

C AND THE VALUE OF ITS LOCATION TABLE POINTER RETURNED IN THE

C COMMON VARIABLE 'BN'.

C CALL LOCPT(O)

C

C THE VALUE OF THE JOINT POSITIONS FOR THIS OTHER POINT ('BN') ARE

C STORED IN THE COMMON VARIABLE 'AC(1-6)' TO BE USED BY THE

C COORDINATE TRANSFORMATION ROUTINE 'COOR' AS THE REFERENCE POINT

C FROM WHICH TO OFFSET THE NEW LOCATION BY THE DELTA COORDINATE

C VALUES ( 'A1X' , 'A1Y' , 'A1Z' )

.

C176 DO 402 JX=1 ,

6

402 AC( JX)=LTAB(BN, 1

, JX)

CALL COOR(A1X,A1Y,A1Z,RAD,0)

IF(DD.NE.-1 )GOTO 479

BN=MNN

DIII-5
GOTO H79

C

C THE PRESENT VALUES OF THE JOINT

C POSITION INDICATORS ARE ENTERED AS THE JOINT POSITION VALUES

C FOR THIS DESIGNATED LOCATION BY A CALL TO 'POS' TO READ IN

C THESE VALUES.

C401 CALL POS

IF(EABORT.LT.-2000)GOTO 500

C

C THE PRESENT JOINT POSITION INDICATOR VALUES FROM THE CALL TO 'POS'

C ARE ENTERED IN THE LOCATION TABLE FOR THIS

C DESIGNATED LOCATION.

C479 DO 415 JX=1,6

AC( JX)=JPOS( JX)

415 LTAB(BN, 1

, JX)=JPOS( JX)

IF(DA.NE. 1 )GOTO 71

1

DA = 0 CALL ARRLOC

C

C IF THE START OF THE APPROACH PATH IS TO BE A DELTA X,Y,Z AWAY

C FROM THE END POINT, THOSE DELTA VALUES ARE ENTERED HERE.

C711 WRITE(6,521)

521 F0RMAT(8X«ENTER DELTA X,Y,Z (DECIMAL CM) TO START OF APPROACH PATH')

READ(6 , 522)ADX, ADY, ADZ

522 FORMATO (F7 . 3) ) CALL ARMIN

IF(EABORT.LT.-2000)GOTO 303

HX=ADX»100

HY=ADY*100

HZ = ADZ» 1 GO

CC THE OPERATOR NOW SUPPLIES THE INFORMATION AS TO WHETHER THE HAND COMES

C TO A STOP AT THIS LOCATION (FX=1) OR IT DOES NOT STOP

C HERE (FX=0).

C630 WRITE(6,109)

109 F0RMAT(8X ' »»»TYPE +1«»«IF HAND WILL STOP AT THIS LOCATION',/

1 8X ' »»»TYPE[CR]»»«IF HAND DOES NOT STOP HERE')

READ(6 , 1 10)FX

110 F0RMAT(I3)

CALL ARMIN

IF(NRE .GT . -2000 )GOTO 40

IF(EABORT .LT. -2000 )GOTO 303

DO 620 J=B1,E1

LTAB( J, 2,7)=FX

LTAB(J,3,4)=HX

LTAB( J, 3,5)=HY

620 LTAB(J,3,6)=HZ

C

C THE OPERATOR IS GIVEN THE OPTION OF DISPLAYING THE LOCATION

C TABLE ON THE TERMINAL OR ENTERING A NEW LOCATION IN THE

C TABLE.

C

DIII-6
WRITE(5 , 111)

111 F0RMAT(8X»»»»TYPE [CR]»»» FOR ANOTHER POINT',/

1 8X'»»»TYPE TO DISPLAY TABLE THEN RETURN TO BEGINNING')

READ(6 , 1 12)HH

112 FORMATdS)

CALL ARMIN

IF(EABORT.LT.-2000)GOTO 303

M = 0 C

C THESE NEXT LOOPS SCAN THE LOCATION TABLE AND IF A DELTA OFFSET IS

C GIVEN TO THE START OF AN APPROACH PATH, THEN THE

C JOINT POSITION VALUES OF THE START OF THIS APPROACH PATH

C IS CALCULATED BY A CALL TO 'COOR' AND THESE VALUES

C ENTERED IN THE TABLE (

' LT AB ( J , i<

, 1 -6 )

' ) . IF THERE IS NO

C APPROACH PATH, THEN THE JOINT POSITION VALUES OF

C THE END POINT ARE ALSO STORED IN ' LTAB ( J , 4 , 1 -6 ) . C THE NAMES OF THE INDEXED LOCATIONS ARE STORED IN THE LAST TWO

C POSITONS IN THE LOCATION TABLE.

C DO 850 J=1,86

IF(LTAB( J, 1 , 1 ) .EQ. OGOTO 850

AX=LTAB(J, 3,4)7100.

AY = LTAB( J , 3 , 5 ) /I 00 . AZ=LTAB(J, 3,6)/100.

DO 830 JB = 1 , 6 830 AC( JB)=LTAB( J, 1 , JB)

CALL COOR (0 ., 0 ., 0 . , 0 , 1 ) LTAB( J, 2, 1 )=AXEP»100

LTAB(J,2,2)=AYEP*100

LTAB( J,2, 3)=AZEP»100

IF(AX.NE.O. )GOTO 2500

IF(AY.NE.O. )GOTO 2500

IF( AZ.NE.O. )GOTO 2500

DO 2400 KBL= 1

,

6

2400 LTAB(J,4,KBL)=LTAB( J, 1 ,KBL) > GOTO 850

2500 CALL COOR(AX,AY,AZ,0,0)

DO 820 JA=1,6

820 LTAB(J,4, JA)=JPOS(JA)

850 CONTINUE

CC HERE THE NAMES OF THE INDEXED LOCATIONS ARE STORED IN THE LAST

C TWO POSITIONS OF THE TABLE.

C772 DO 704 J=89,90

DO 704 K=1 , 4 DO 704 L = 1 , 7 M = M+1

IF(M. EQ. 31 )GOTO 798

704 LTAB( J,K,L)=IN(M)

C

C IF 'HH' EQUAL TO ZERO (IE. A CARRIAGE RETURN WAS ENTERED), THEN

C RETURN TO BEGINNING OF PROGRAM TO ENTER ANOTHER LOCATION.

C IF 'HH'=1, THEN PRINT THE LOCATION TABLE OUT ON THE

C TERMINAL.

C

1 piII-7
798 IF(NRE.GT.-2000)GOTO 40

IFCHH.NE. 1 )GOTO 303

DO 120 KL=1 , 90

CALL ARMIN

IF(EABORT.LT.-2000)GOTO 500

DO 120 NK= 1

,

4

WRITE(6 , 120 1 )KL, ( LTAB ( KL , NK , J )

, J=1 ,7)

40

1201

120

F0RMAT(8 (2X , I6)

)

CONTINUE

GOTO 500

RETURN

END

Dlll-d

DIII-10
C SUBROUTINE : ARRAY

C

C ARGUMENTS : CC CALLED BY : LOCTAB

C

C CALLS SUBROUTINES : C

C INPUT DATA : PA(30) = THE CHARACTER STRING FROM THE TERMINAL

C THAT CONTAINS THE INDEXED NAME AND THE

C - STARTING AND ENDING INDEXED LOCATION

C VALUE FOR THIS PARTICULAR ARRAY [FROM

C THE TERMINAL].

C J1,J2 r THE DIMENSIONS OF THE ARRAY [FROM THE

C TERMINAL].

C AD1(3) = THE DELTA 'X,Y,Z' DISTANCES (IN

C CENTIMETERS) BETWEEN THE LOCATION

C POINTS ALONG THE FIRST DIMENSION ('J1')

C OF THE ARRAY [FROM THE TERMINAL].

C AD2(3) = THE DELTA 'X,Y,Z' DISTANCES BETWEEN THE

C LOCATION POINTS ALONG THE SECOND

C DIMENSION OF THE ARRAY [FROM THE

C TERMINAL].

C

C OUTPUT DATA : J1,J2 = AS DESCRIBED ABOVE.

C ADU3) = AS DESCRIBED ABOVE.

C AD2(3) = AS DESCRIBED ABOVE.

C B1 = THE LOCATION TABLE POINTER FOR THE

C FIRST INDEXED LOCATION SPECIFIED

C IN THE ARRAY.

C El ' = THE LOCATION TABLE POINTER FOR THE

C LAST INDEXED LOCATION SPECIFIED IN

C THE ARRAY.

C DA = A'FLAG SET EQUAL TO ONE TO INDICATE

C THAT AN ARRAY OF LOCATION POINTS

C HAS BEEN ENTERED.

C

C FUNCTION: TO READ IN FROM THE TERMINAL THE NECCESARY INFORMATION

C TO CONSTRUCT THE CORRECT LOCATION TABLE VALUES FOR THE

C ARRAY OF LOCATIONS SPECIFIED. THE ACTUAL ENTERING

C OF THE VALUES INTO THE TABLE WILL BE DONE BY THE

C SUBROUTINE 'ARRPT'.

C SUBROUTINE ARRAY

IMPLICIT INTEGER(B-Z)

COMMON/ARMBUF/INBUF(64) ,0UTBUF{6il)

COMMON /ARR/B1 ,E1,J1,J2,AD1(3),AD2(3),DA

COMM0N/IND/IN(30) ,FL(6) ,NAME(3) ,NPT(3,20)

E QUI VALENCE(E ABORT , INBUF( 28) ) DIMENSION PA(30)

CC READ IN FROM THE TERMINAL, THE INDEXED NAME AND THE RANGE OF

C LOCATIONS, THE DIMENSIONS OF THE ARRAY, AND THE DELTA COORDINATE

C OFFSETS ALONG EACH DIMENSION.

C500 WRITE(6,200)

DIII-11
200 F0RMAT(2X 'ENTER LOCATION NAME AND INDEX NUMBERS',/

1 2X'EG. PALLET(0 1 -20 )

'

)

READ (6 ,201)(PA(J),J=1,30)"

201 F0RMAT(30(A1 )

)

WRITE(6,202)

202 F0RMAT(2X 'ENTER ARRAY DIMENSIONS',/

1 2X' (EG. 4,5 )

'

)

READ(6 ,203) J1 , J2

203 F0RMAT(2 (13)

)

WRITE(6,20M)

204 F0RMAT(2X 'ENTER X,Y,Z DELTAS (DECIMAL CM) BETWEEN ',/

1 2X' ARRAY POSITIONS ALONG FIRST DIMENSION.')

READ(6 , 205 ) ( AD1 ( J ) ,J=1 ,3)

205 F0RMAT(3 (FY . 3)

)

WRITE(6,214)

214 F0RMAT(2X ' ENTER X,Y,Z DELTAS (DECIMAL CM) BETWEEN ',/

1 2X' ARRAY POSITIONS ALONG SECOND DIMENSION.')

READ(6 ,215) ( AD2( J) , J= 1 , 3)

215 F0RMAT(3 (FY . 3)

)

C

C DECODE THE NAME AND THE SPECIFIED INDICES INTO THE CORRECT LOCATION

C TABLE POINTERS.

C

J = 0 10 J=J+1

IF( J.EQ.31 )G0T0 40

IF(PA(J) .EQ.8224)G0T0 10

DO 20 K= 1 ,

3

20 IF(PA( J) .EQ.NAME(K) )G0T0 Y WRITE(6,120)PA(J) ,NAME(1 ) , NAME (2) ,NAME(3)

120 F0RMAT(4(A1 )

)

WRITE(6, 100)

100 F0RMAT(2X'CANN0T FIND THIS NAME,',/

1 2X'«»»TYPE +1»»» TO RE-ENTER NAME',/

1 2X'»»» TYPE [CR]»»« TO ABORT.')

READ(6 , 206)BOR

206 F0RMAT(I3)

IF(B0R . EQ. 1 )G0T0 500

GOTO 40

C

C DETERMINE WHAT THE FIRST SPECIFIED INDEX VALUE IS.

C

7 J=J+1

IF( J . EQ. 31 )G0T0 40

IF(PA( J) .NE.8232)G0T0 Y 8 J=J+1

IF(J.EQ. 31 )G0T0 40

IF(PA(J) .EQ.8224)G0T0 8 0NE=PA(J)

TWO=PA( J+1

)

F= ( 10«(0NE-8240) )+(TWO-8240)

J = J + 1 IF( J.EQ. 31 )G0T0 40

C

C DETERMINE WHAT THE LAST SPECIFIED INDEX VALUE IS.

C

DIII-12
17 J=J+1

IF( J . EQ. 31 )GOTO 40

IF(PA( J) .EQ.8224)G0T0 1?

IF(PA(J) .EQ.8237)G0T0 17

ONE=PA( J)

TWO=PA( J+1

)

L= (10»(0NE-82 40) ) + (TW0-82 40)

C

C DECODE THE FIRST AND LAST INDEX NUMBERS INTO THEIR CORRESPONDING

C LOCATION TABLE POINTERS AND STORE IN THE COMMON VARIABLES 'BT AND

C 'El • . C B1=NPT(K,F)

E1=NPT(K,L)

DA = 1 40 RETURN

END

DIII-13
DIII-14
C SUBROUTINE : ARRLOC

C

C ARGUMENTS : C

C CALLED BY : LOCTAB

C

C CALLS SUBROUTINES : COOR

C

C INPUT DATA : J1,J2 = THE DIMENSIONS OF THE ARRAY.

C AD1(3) = THE DELTA 'X,Y,Z' DISTANCES (IN

C CENTIMETERS) BETWEEN THE LOCATION

C POINTS ALONG THE FIRST DIMENSION ('J1')

C OF THE ARRAY.

C AD2(3) = THE DELTA 'X,Y,Z' DISTANCES BETWEEN THE

C LOCATION POINTS ALONG THE SECOND

C DIMENSION OF THE ARRAY.

C B1 = THE LOCATION TABLE POINTER FOR THE

C FIRST INDEXED LOCATION SPECIFIED

C IN THE ARRAY.

C El = THE LOCATION TABLE POINTER FOR THE

C LAST INDEXED LOCATION SPECIFIED IN

C THE ARRAY.

C DA = A FLAG SET EQUAL TO ONE TO INDICATE

C THAT AN ARRAY OF LOCATION POINTS

C HAS BEEN ENTERED.

C

C OUTPUT DATA : JP0S(1-6) = THE JOINT POSITION VALUES FOR EACH OF

C THE LOCATIONS IN THE SPECIFIED ARRAY.

C AC(-|-6) = THE JOINT POSITION VALUES THAT

C CHARACTERIZE THE REFERENCE POINT USED

C BY THE COORDINATE TRANSFORMATION

C ROUTINE.

C

C FUNCTION: TO GENERATE AND STORE IN THE LOCATION TABLE, ALL OF THE

C JOINT POSITION VALUES FOR THE LOCATIONS OF THE SPECIFIED

C ARRAY.

C SUBROUTINE ARRLOC

IMPLICIT INTEGER(B-Z)

C0MM0N/ARMBUF/INBUF(6i|) ,0UTBUF(64)

COMMON/LMOD/LTAB(90 , 4 , 7 ), PRES , DEST

C0MM0N/0UT/JP0S(8

)

COMMON / COR T/AC( 6) , AXE P , A YE P , AZ EP

COMMON /ARR/B1 ,E1,J1,J2,AD1(3) ,AD2(3) ,DA

EQUIVALENCE(EAB0RT,INBUF(28)

)

DIMENSION AY2(3) ,AXY(3)

C

C STORE THE LOCATION TABLE POINTER FOR THE FIRST INDEXED LOCATION

C IN THE VARIABLE 'PAA'.

C PPA=B1

C

C PLACE THE JOINT POSITION VALUES OF THIS FIRST INDEXED LOCATION IN THE

C COMMON VARIABLE 'ACd-S)' WHICH WILL BE USED AS A REFERENCE POINT

C BY THE COORDINATE TRANSFORMATION ROUTINE 'COOR'.

C

DIII-15
DO 15 J=1 , 6 15 AC(J)=LTAB(B1 , 1

, J)

C

C THESE LOOPS GENERATE THE CORRECT JOINT POSITION VALUES FOR ALL OF THE

C LOCATIONS IN THE ARRAY. THIS IS DONE BY SPECIFYING THE

C DELTA COORDINATE OFFSETS FOR EACH LOCATION IN THE ARRAY FROM

C THE FIRST SPECIFIED REFERENCE LOCATION ('AC(1-6)') AND OBTAINING

C FROM THE COORDINATE TRANSFORMATION ROUTINE THE CORRESPONDING JOINT

C POSITION VALUES, THEN STORING THESE VALUES IN THE CORRECT

C PLACE IN THE LOCATION TABLE.

C DO 110 J22= 1

, J2

JS=J22-1

DO 1 1 7 J4= 1 ,

3

117 AY2( J4)=JS»AD2( J4)

DO 1 10 J1 1 = 1 , J1

JR=J1 1-1

DO 120 JA=1,3

120 AXY( JA)=JR«AD1 ( JA)+AY2( JA)

CALL C00R(AXY(1 ) ,AXY(2) ,AXY(3) ,0,0)

CC HERE THE JOINT POSITION VALUES ARE STORED IN THE LOCATION

C TABLE UNDER THE CORRECT POINTER VALUE FOR THIS PARTICULAR NAME

C AND INDEX NUMBER.

C DO 115 K=1 ,6

115 LTAB(PPA, 1 ,K)=JPOS(K)

110 PPA=PPA+1

RETURN

END

DIII-16

DIII-18
C SUBROUTINE : JOY

C

C ARGUMENTS : C

C CALLED BY : LOCTAB

C

C CALLS SUBROUTINES : ARMIN COOR SERVO POS

C

C INPUT DATA : JP0S(1-6) = THE VALUE OF ALL OF THE JOINT POSITION

C INDICATORS AT THE BEGINNING OF THIS

C ROUTINE.

C INBUF(26) = THE CHANNEL THAT CONTROLS THE VELOCITY

C OF THE ARM WHILE UNDER JOYSTICK CONTROL.

C INBUF(27) = THE CHANNEL THAT CONTROLS THE 'GRASP* AND

C 'RELEASE'.

C INBUF(31) = THE CHANNEL THAT CALLS FOR A PRINT OUT ON

C THE TERMINAL OF THE PRESENT 'X,Y,Z'

C VALUES.

C INBUF(32) = THE CHANNEL USED TO RETURN CONTROL TO THE

C CALLING PROGRAM ('LOCTAB').

C INBUF(37) = THE CHANNEL THAT CONTROLS THE WRIST

C ROLL.

C INBUF(38) = THE CHANNEL THAT CONTROLS THE i^RIST

C FLEX.

C INBUF(39) = THE CHANNEL THAT CONTROLS THE HAND

C ROLL.

C INBUF(UO) = THE CHANNEL THAT CONTROLS THE 'X' MOTION

C OF THE HAND.

C INBUF(41) = THE CHANNEL THAT CONTROLS THE 'Y' MOTION

C OF THE HAND.

C INBUF(42) = THE CHANNEL THAT CONTROLS THE 'Z' MOTION

C OF THE HAND.

CC OUTPUT DATA : AC(1-6) = THE STARTING JOINT POSITION VALUES

C THAT ARE USED AS A REFERENCE POINT

C FOR THE COORDINATE TRANSFORMATION

C CALCULATIONS.

C JP0S(1-8) = THE JOINT POSITION VALUES THAT ARE

C THE COMMANDED OUTPUT VALUES TO THE

C SERVOS THAT WILL CAUSE THE HAND

C TO MOVE IN THE DIRECTION INDICATED

C BY THE JOYSTICK.

C

C FUNCTION: READS IN THE VALUES OF THE JOYSTICK POTENTIOMETERS,

C CALCULATES THE PROPER JOINT POSITION VALUES

C TO ACCOMPLISH THE COMMANDED MOVES AND OUTPUTS THESE

C JOINT POSITION VALUES TO THE SERVOS.

C SUBROUTINE JOY

IMPLICIT INTEGER (D-R)

COMMON/ARMBUF/INBUF(64) ,OUTBUF(64)

C0MM0N/0UT/JP0S(8)

COMMON /CORT/AC( 6) , AXEP , AYEP , AZEP

EQUIVALENCE (EABORT,INBUF( 28)

)

COMMON/XYZO/AOI , A05 , A06

DIMENSION KIN(5) ,KBN(5) ,KMD(3)

DIII-19
DATA KIN/37, 38, 39,27,42/

C

C THE FLAG 'PTC IS SET EQUAL TO ZERO. THIS FLAG IS USED IN THE GRASP

C AND RELEASE FUNCTIONS TO CONTROL THE SETTING OF THE HAND BRAKE.

C PTC = 0 C

C THE DELTA COORDINATE OFFSETS ( • AX ' ,' AY '

, ' AZ ' ) AND THE DELTA

C ROTATIONAL OFFSETS ( • A04 ' ,

' A05 ' , ' A06 ' ) FOR THE WRIST AND HAND

C ARE INITIALIZED TO ZERO.

C AX = 0.

AY = 0.

AZ = 0 . A04=0.

A05 = 0 . A06=0

.

C

C THE PRESENT JOINT POSITION VALUES ARE READ IN AND STORED IN THE

C COMMON VARIABLE 'AC(1-6)' TO BE USED AS THE REFEREJICE

C POINT FOR ALL OF THE COORDINATE TRANSFORMATION CALCULATIONS

C TO BE DONE BY ' COOR •

. C CALL POS

DO 525 J J= 1 , 6 525 AC( JJ)=JPOS( JJ)

C

C THE MESSAGE THAT TELLS WHICH SWITCHES CONTROL WHICH FUNCTIONS

C IS SENT TO THE TERMINAL.

C WRITE(6 , 526)

526 F0RMAT(3X'T0 RETURN TO THE TEACH TRAJECTORY MODE,',/

1 3X'FLIP SWITCH 32 TO THE DOWN POSITION.'/

1 3X'FLIP SW 31 UP TO DISPLAY X Y Z VALUES.')

CC THE VALUES OF ALL OF THE INPUTS FROM THE JOYSTICK BOX ARE

C READ IN.

C20 CALL ARMIN

C

C THE VARIABLE 'RT' CONTAINS THE VALUE OF THE SWITCH USED TO

C RETURN TO THE CALLING PROGRAM ('LOCTAB').

C RT=-INBUF(32)

IF(RT-2000)40,527,527

C

C THE INPUT VALUES FOR THE 'X' AND 'Y' MOTION ARE STORED IN THE

C VARIABLES 'IX' AND 'lY'.

C527 IX=-INBUF(40)

IY=-INBUF( 4 1

)

C

C THE THREE ROTATIONAL ANGLES OF THE HAND-WRIST, THE FINGER CLOSING,

C AND THE 'Z' MOTION INDICATOR ARE DECODED INTO THE NUMBER '2'

C FOR POSITIVE MOTION, OR THE NUMBER '-2' FOR NEGATIVE MOTION,

C OR THE NUMBER '0' FOR NO MOTION. THESE CODED NUMBERS FOR

DIII-20
C EACH OF THESE FIVE MOTIONS ARE STORED IN THE VARIABLE 'KBN(1-5)'.

C

DO ICQ KL=1 ,

5

FLL=KIN(KL)

ICQ KBN(KL)= ( (INBUF(FLL)+l850)/1000)*(-2)

C

C THE VALUE OF THE POTENTIOMETER ON CHANNEL 26 IS READ IN AND USED AS

C A CONTROL FOR THE SIZE OF THE COMMANDED MOVE FOR EACH OUTPUT,

C THEREBY CONTROLING THE VELOCITY. THIS CONTROL VALUE

C IS STORED IN THE VARIABLE 'SPD'.

C SPD=( (-INBUF(26)-2150)/10)+1

C

C A VELOCITY FACTOR FOR EACH OF THE THREE ROTATIONAL ANGLES OF THE

C HAND-WRIST IS GENERATED BY MULTIPYING THE VELOCITY CONTROL

C VALUE ('SPD') BY THE PARTICULAR JOINT'S MOTION INDICATOR

C NUMBER ( 'KBN( 1 -3 )

' )

.

C

DO 200 J= 1 , 3 200 KMD(J)=KBN( J)»SPD

C

C EACH OF THE THREE ANGLES IS OFFSET BY ITS CORRESPONDING VELOCITY

C FACTOR ( 'KMD(1-3) ' ) . C

A04 = A04+KMD( 1 ) A05=A05+KMD(2)

A06=A06+KMD(3

)

C

C THE FINGER MOTION INDICATOR IS CHECKED TO DETERMINE IFTHE

C FINGERS ARE TO BE OPENED OR CLOSED. IF NEITHER,

C THEN BRANCH TO STATEMENT 300.

C IF(KBN(4) .EQ.OGOTO 300

CC THE FLAG 'PTC IS SET EQUAL TO ONE, AND WILL BE USED TO SET THE BRAKE

C ON THE FINGERS AFTER THIS FINGER MOTION IS FINISHED.

C

IF(PTC . EQ. 1 )GOTO 75

PTC=1

C

C THE CORRECT VALUE IS SET IN 'JP0S(8)' TO RELEASE THE BRAKE ON THE

C FINGERS.

C JPOS(8)=32767

CALL SERVO(O)

75 IF(KBN(4) .EQ.2)G0T0 301

CC HERE, THE COMMANDED JOINT POSITION VALUE TO THE FINGERS IS

C INCREMENTED FOR EACH OUTPUT TO CAUSE THE FINGERS TO OPEN.

C IF( JPOS(7)-30 0 00)305, 310 ,310

305 JPOS (7 ) =JPOS (7 )+200

GOTO 310

C

C HERE, THE COMMANDED JOINT POSITION VALUE TO THE FINGERS IS

C DECREMENTED FOR EACH OUTPUT TO CAUSE THE FINGERS TO CLOSE.

DIII-ZL
c301 IF( JP0S(7 )-3500)310 , 308 , 308

308 JPOS(7)=JP0S(7)-200

310 CALL SERVO(O)

GOTO 20

C

C THE BRAKE FLAG IS CHECKED TO SEE IF IT IS SET ('FTC'rl). IF IT IS,

C THEN THE PROPER NUMBER IS PLACED IN 'JP0S(8) TO CAUSE THE

C BRAKE ON THE FINGERS TO BE SET. THE FLAG IS ALSO RESET TO

C ZERO.

C300 IF(PTC.EQ.0)GOTO 720

PTC = 0 JP0S(8) =32703

CALL SERVO(O)

C

C A FLOATING POINT VARIABLE 'ASP' IS DEFINED AS THE VELOCITY FACTOR

C 'SPD' MULTIPLIED BY 1/300 OF A CENTIMETER. THE NUMBER OF

C THESE DELTA INCREMENTS TO BE MOVED IN THE 'X', 'Y', AND 'Z'

C DIRECTIONS IS DETERMINED BY MULTIPLYING THIS DELTA STEP BY

C A DECODED VALUE FROM THE INPUT CHANNEL FOR EACH COORDINATE AXIS.

C720 ASP=SPD/300.

MX= (IX-1 070 ) /75

BX=MX»ASP

AX=AX+BX

MY=(1070-IY)/75

BY=MY»ASP

AY=AY+BY

BZ=KBN(5)»ASP

AZ=AZ+BZ

C

C ONCE THE DELTA MOTIONS IN EACH OF THE THREE COORDINATE AXES HAS BEEN

C CALCULATED, THEN THESE DELTA 'X', 'Y', 'Z' VALUES ARE GIVEN TO THE

C COORDINATE TRANSFORMATION ROUTINE SO THAT THE JOINT POSITIONS

C FOR THESE MOTIONS CAN BE CALCULATED.

C CALL COOR(AX,AY,AZ,0,0)

C

C THESE CALCULATED JOINT POSITION VALUES ARE THEN OUTPUTED TO

C THE SERVOS.

C CALL SERVO(O)

CC IF SWITCH 31 IS SET THEN THE ABSOLUTE 'X', 'Y', 'Z' COORDINATE

C POSITIONS ARE PRINTED OUT ON THE TERMINAL.

C OHH=INBUF( 3 1

)

IF(OHH.GT.-2000)GOTO 20

WRITE(6 , 591)AXEP, AYEP, AZEP

591 FORMAT ( 2X 'X= ' ,F10.2,/,2X'Y=',F10.2,/,2X'Z=',F10.2)

425 CALL ARMIN

OHH=INBUF( 3 1

)

IF(OHH.LT.-2000)GOTO 425

GOTO 20

40 RETURN

END

DIII-22

DIII-,'24
- SUBROUTINE : POS

- ARGUMENTS :

- CALLED BY : EMOVE LOCTAB JOY

- CALLS SUBROUTINES : ARMIN

- INPUT DATA : INBUF ( 5 , 8 , 1 1

, 1 3 , 1 5 , 1 7 ) = THE PRESENT VALUES OF THE

SIX JOINT POSITION INDICATORS.

- OUTPUT DATA : JP0S(1-6) = THE SCALED VALUES OF THE SIX JOINT

POSITON INDICATORS AS CALCULATED IN

THIS SUBROUTINE.

- FUNCTION: SCALES THE JOINT POSITION INDICATOR VALUES INTO

A RANGE OF 0 TO 32767.

SUBROUTINE POS

IMPLICIT INTEGER(A-Z)

COMMON /ARMBUF/INBUF( 64) ,OUTBUF(64)

C0MM0N/0UT/JP0S(8

)

A CALL TO THE SUBROUTINE 'ARMIN' IS MADE TO READ IN THE VALUES

OF THE JOINT POSITION INDICATORS.

CALL ARMIN

THE FOLLOWING TWO 'DO LOOPS' ARE USED TO

GENERATE THE CORRECT INPUT CHANNEL NUMBERS

(5,8,11,13,15,17,19) FOR THE SEVEN JOINT

POTENTIOMETERS.

JPOSd )=INBUF(5)»2+16383

JP0S(2)=INBUF(8)»2+163 83

K = 9 DO 51 1=3,6

K = K + 2 JP0S(I)=INBUF(K)»2+16383

CONTINUE

JPOSd ) = (JP0S(1 )»1 . )-22.

JP0S(2)= (JP0S(2)» .9879) +210

.

JPOS(3)=(JPOS(3)*1 .001 ) -30.

JP0S(4)=(JP0S(4)»1 .Ol65)-283.

JP0S(5)= (JP0S(5)« .990 4)+H5

.

JP0S(6) = ( JP0S(6 )» . 997 ) + 15

0UTBUF(7 )= (0UTBUF(7 )*1 . 0227) -40 0.

RETURN

END

Din-25
NBS-IMA (REV. 7-73)

IIU.b.c np'DT Ut.t-'l, r\(jru r"i^wMivi. r\KJi KA BIBLIOGRAPHIC DATA

SHEET

1. PUBLICATION OR REPORT NO. NBS SP 500-232. Gov't Accession

No.

3. Recipient's Accession No.

4. TITLE AND SUBTITLE

COMPUTER SCIENCE & TECHNOLOGY:

AN ARCHITECTURE FOR A ROBOT HIERARCHICAL CONTROL SYSTEM

5. Publication Date December 1977

6. Performing Organization Code

7. AUTHOR(S) Anthony J. Barbara 8. Performing Organ. Report No.

9. PERFORMING ORGANIZATION NAME AND ADDRESS

NATIONAL BUREAU OF STANDARDS

DEPARTMENT OF COMMERCE

WASHINGTON, D.C. 20234

10. Project/Task/Work Unit No.

11. Contract/Grant No.

12. Sponsoring Organization Name and Complete Address (Street, City, State, ZIP)

Same as item 9.

13. Type of Report & Period Covered

14. Sponsoring Agency Code

15. SUPPLEMENTARY NOTES

Library of Congress Catalog Card Number: 77-17960

16. ABSTRACT (A 200-word or /ess (actual summary of most signilicant information. If document includes a significant

bibliography or literature survey, mention it here.)

Complex automation systems, such as industrial robots, require a computerbased control system for the effective utilization of this advanced technology.

This report describes such a control system developed at the National Bureau of

Standards. The approach has been to partition the control system into a hierarchy

of different functional levels. This has proven to be a powerful technique in

obtaining sensor-controlled robot behavior at a minimum cost of programming

time and computer size. Further, this partitioning has greatly simplified

the implementation of additional functions and sensors. This report discusses

the control system, its implementation and use, and provides a documented

listing of all of the control programs.

17. KEY WORDS (six to twelve entries; alphabetical order; capitalize only the first letter of the first key word unless a proper name; separated by semicolons

)

Adaptive; automation; computer; control; goal -oriented;

hierarchical control; robot; sensors

18. AVAILABILITY Unlimited

1 For Official Distribution. Do Not Release to NTIS

1 y Order From Sup. of Doc, U.S. Government Printing Office Washington. D.C. 20402. SD Cat. No. Cli .^1 0- '^00-9'^

\

! Order From National Technical Information Service (NTIS)

Springfield, Virginia 22151

19. SECURITY CLASS

(THIS REPORT)

UNCLASSIFIED

21. NO. OF PAGES

227

20. SECURITY CLASS

(THIS PAGE)

UNCLASSIFIED

22. Price $4.25

USCOMM-OC ^9042-P74

<r U. S. GOVERNMENT PRINTING OFFICE :1 977--261 -238/1*24
ANNOUNCEMENT OF NEW PUBLICATIONS ON

COMPUTER SCIENCE & TECHNOLOGY

Superintendent of Documents,

Government Printing Office,

Washington, D. C. 20402

Dear Sir:

Please add my name to the announcement list of new publications to be issued in the series: National Bureau of Standards Special Publication 500-.

Name ; Company

Address

City State Zip Code

(Notification key N-503)

NBS TECHNICAL PUBLICATIONS

! PERIODICALS

1 JOURNAL OF RESEARCH—The Journal of Research

of the National Bureau of Standards reports NBS research and development in those disciplines of the physical and

engineering sciences in which the Bureau is active. These

include physics, chemistry, engineering, mathematics, and

computer sciences. Papers cover a broad range of subjects, with major emphasis on measurement methodology, and

the basic technology underlying standardization. Also in- cluded from time to time are survey articles on topics closely

related to the Bureau's technical and scientific programs. As

a special service to subscribers each issue contains complete

, citations to all recent NBS publications in NBS and non- NBS media. Issued six times a year. Annual subscription: domestic $17.00; foreign $21.25. Single copy, $3.00 domestic;

$3.75 foreign. Note: The Journal was formerly published in two sections: Section A "Physics and Chemistry" and Section B "Mathematical Sciences." DIMENSIONS/NBS (formerly Technical News Bulletin)—

This monthly magazine is published to inform scientists,

, engineers, businessmen, industry, teachers, students, and

j consumers of the latest advances in science and technology,

with primary emphasis on the work at NBS. The magazine

highlights and reviews such issues as energy research, fire

i protection, building technology, metric conversion, pollution

abatement, health and safety, and consumer product per- formance. In addition, it reports the results of Bureau pro- grams in measurement standards and techniques, properties

of matter and materials, engineering standards and services, instrumentation, and automatic data processing.

. Annual subscription: Domestic, $12.50; Foreign $15.65.

NONPERIODICALS

I Monographs—Major contributions to the technical liter-

! ature on various subjects related to the Bureau's scientific

I and technical activities.

I Handbooks—Recommended codes of engineering and industrial practice (including safety codes) developed in cooperation with interested industries, professional organizations,

' and regulatory bodies.

I Special Publications—Include proceedings of conferences

' sponsored by NBS, NBS annual reports, and other special

publications appropriate to this grouping such as wall charts,

j pocket cards, and bibliographies.

Applied Matiiematics Scries—Mathematical tables, manI uals, and studies of special interest to physicists, engineers,

j chemists, biologists, mathematicians, computer programmers,

I and others engaged in scientific and technical work,

'j National Standard Reference Data Series—Provides quanti-

' tative data on the physical and chemical properties of

! materials, compiled from the world's literature and critically evaluated. Developed under a world-wide program co- ordinated by NBS. Program under authority of National

Standard Data Act (Public Law 90-396).

1

j The following current-awareness and literature-survey bibli- ographies are issued periodically by the Bureau:

Cryogenic Data Center Current Awareness Service. A litera- ture survey issued biweekly. Annual subscription: Domes-

, tic, $25.00; Foreign, $30.00.

i| Liquified Natural Gas. A literature survey issued quarterly.

Annual subscription: $20.00.

NOTE: At present the principal publication outlet for these data is the Journal of Physical and Chemical Reference Data (JPCRD) published quarterly for NBS by the American Chemical Society (ACS) and the American Institute of

Physics (AIP). Subscriptions, reprints, and supplements

available from ACS, 1155 Sixteenth St. N.W., Wash., D.C.

20056.

Building Science Scries—Disseminates technical information developed at the Bureau on building materials, components,

systems, and whole structures. The series presents research

results, test methods, and performance criteria related to the

structural and environmental functions and the durability and safety characteristics of building elements and systems. Technical Notes—Studies or reports which are complete in themselves but restrictive in their treatment of a subject.

Analogous to monographs but not so comprehensive in scope or definitive in treatment of the subject area. Often

serve as a vehicle for final reports of work performed at NBS under the sponsorship of other government agencies.

Voluntary Product Standards—Developed under procedures

published by the Department of Commerce in Part 10, Title 15, of the Code of Federal Regulations. The purpose

of the standards is to establish nationally recognized require- ments for products, and to provide all concerned interests with a basis for common understanding of the characteristics of the products. NBS administers this program as a supple- ment to the activities of the private sector standardizing

organizations.

Consumer Information Series—Practical information, based on NBS research and experience, covering areas of interest

to the consumer. Easily understandable language and

illustrations provide useful background knowledge for shopping in today's technological marketplace.

Order above NBS publications from: Superintendent of Documents, Government Printing Office, Washington, D.C.

20402. Order following NBS publications—NBSIR's and FIPS from

the National Technical Information Services, Springfield, Va. 22161.

Federal Information Processing Standards Publications (FIPS PUB)—Publications in this series collectively constitute the Federal Information Processing Standards Register.

Register serves as the official source of information in the Federal Government regarding standards issued by NBS

pursuant to the Federal Property and Administrative Services Act of 1949 as amended. Public Law 89-306 (79 Stat. 1127), and as implemented by Executive Order 11717

(38 FR 12315, dated May 11, 1973) and Part 6 of Title 15 CFR (Code of Federal Regulations).

NBS Interagency Reports (NBSIR)—A special series of

interim or final reports on work performed by NBS for outside sponsors (both government and non-government).

In general, initial distribution is handled by the sponsor;

public distribution is by the National Technical Information

.Services (Springfield, Va. 22161) in paper copy or microfiche form.

Superconducting Devices and Materials. A literature survey

issued quarterly. Annual subscription: $30.00. Send subscription orders and remittances for the preceding bibliographic

services to National Bureau of Standards, Cryogenic Data

Center (275.02) Boulder, Colorado 80302.

BIBLIOGRAPHIC SUBSCRIPTION SERVICES
U.S. DEPARTMENT OF COMMERCE

National Bureau of Standards

Washington. D.C. 20234

OFFICIAL BUSINESS

Penalty for Private Use. S300

POSTAGE AND FEES PAID

U.S. DEPARTMENT OF COMMERCE COM-21S

SPECIAL FOURTH-CLASS RATE

BOOK