\

Supervisory Control AlgorithmsFor Telerobotic Space Structure Assembly

by

DAVID ELLIOTT ANDERSON

B.S., Massachusetts Institute of Technology(1986)

Submitted in partial fulfillment ofthe requirements for the degree of

MASTER OF SCIENCEIN AERONAUTICS AND ASTRONAUTICS

at the

MASSACHUSETTS INSTITUTE OF TECHNOLOGYMay 6, 1988

© Massachusetts Institute of Technology 1988

Signature of Author

Certified by

Accepted by

Departmt of Aeronautics and AstronauticsMay 6, 1988

Professor David L. AkinThesis Sunervisor

Professor Harold Y. WachmanChairman, Departmental Graduate Committee

MAY 4 lubd

L J S R M I E S. ;

"!9 - ._

"' ; ' . i ·- ? ltsF, 1 a I:

. ; - - - -- -4.,~~~~~~~~~~~

-

SUPERVISORY CONTROL ALGORITHMSFOR TELEROBOTIC SPACE STRUCTURE ASSEMBLY

by

DAVID ELLIOTT ANDERSON

Submitted to the Department of Aeronautics and Astronauticson May 6, 1988 in partial fulfillment of the requirements for

the degree of Master of Science

ABSTRACT

A method for applying a task primitive supervisory control capability to a structuralassembly teleoperator is presented and compared with manual control modes. The task ofstructural assembly is often repetitive. Several of the basic steps in an assembly can becompleted by the computer faster with more accuracy and repeatability, lowering theworkload on operator and increasing productivity. These results are demonstrated byexample using the Beam Assembly Teleoperator.

The primary application of the Beam Assembly Teleoperator is the assembly oftetrahedral structures constructed of six beams and four nodes. The beam manipulation andjoint assembly portions of the assembly are augmented by the task primitive supervisorycontrol algorithm, Triad. Manual and supervisory modes are compared by repeated jointassembly and by full structure assembly.

Use of Triad proved to be significantly faster in repeated joint assembly, yielding timesfor a n'ive subject using Traid equal or better than an experienced operator under manualcontrol.

For full EASE structural assembly, Triad produced an estimated full assembly time20% faster than the previously established manual time, despite increased times for theportions of assembly not under Triad control. The times for the portions assembly underTriad control were decreased 43% from the corresponding manual times.

Thesis Supervisor: Prof. David L. AkinTitle: Rockwell Assistant Professor of Aeronautics and Astronautics

ACKNOWLEDGEMENTS

For the correct combination of guidance and freedom, and for a unique research

environment, I sincerely thank Professor Dave Akin.

Thanks also to Herb Viggh and John Spofford for bringing BAT into functionality

and tolerating my questions about circuitry and 'Spofford Assembly Language'. Thanks to

the BAT UROP'ers, Lisa, Jud, and Beth, for their skill with vacuum grease and devotion

to WIF tests. Courage, Jud. BAT can function. When in doubt, read chapter 4.

For keeping me going through Multivariable, thanks to Sue and Rob. I am sure that

if I put as much time into a marriage as we did into that class, it has got to work out.

For a truly eventful working environment, thanks to office mates Janice, Craig,

Mary, Vicky, Wendy, Tony, and to Russ, even if he did make me sit in the corner for the

last four months.

Thanks to my roommates, Chris, Win, Dana, and Jean+l, for good times, good

food, a comfortable home and lots of cran-raspberry juice.

Thanks especially to my family out west in Oregon for never letting me forget that

there is a world outside of Boston with good people, moderate weather, and decent

pavement.

This research made possible by NASA's Office of Aeronautics and Space

Technology under NASA Grant NAGW-21.

Table of Contents1. INTRODUCTION .............................................................................. 6

1.1 Teleoperation .................. ......... ..................... 61.2 The addition of supervisory control ................................................ 71.3 EASE and the SSL loop group . ....................................................1.4 The vehicle, BAT ................................................. 101.5 The control station, ICS ................................................ 131.6 Triad . ..... ........ ............................................................151.7 Scope of thesis ....................................................................... 16

2. UPGRADED CONTROL ALGORITHMS 5DOF ARM .................................. 172.1 Previous control system ............................................................. 17

2.1.1 Chain of command to move the arm .................................... 172.1.2 The role of BATSOFT ................................................... 182.1.3 The role of the joint cards ................................................ 182.1.4 Problems with the previous control system ........................... 19

2.2 Digital PID control ................................................. 202.2.1 Digital approximations of PID control .......... .................. 202.2.2 Tuning ............................................ .21

2.3 Application to BAT ............. ......... .. ......... ............ 222.3.1 Direct application ......................................................... 222.3.2 Addition of a separate integrator ........................................ 23

3. TRIAD, TASK PRIMATIVE SUPERVISORY CONTROL .............................. 243.1 Theory ............... ................................. 24

3.1.1 Supervisory Control .................................................... 243.1.2 Trajectory control and path planning ................................... 24

3.2 Method ................................................ 253.2.1 BAT's Limitations ................................................ 253.2.2 Application to BAT . ............................................... 263.2.3 Triad trajectory development ................... 2......... .........28

3.3 Application to the EASE structure assembly ...................................... 293.3.1 Breakdown of EASE joint assembly . .................................. 293.3.2 Assembling a joint manually ............................................ 303.3.3 Assembling a joint with Triad ........................................... 30

4. MANUAL vs. TRIAD ASSEMBLY of the EASE JOINT . ...............................314.1 Manual assembly using P-type control law ....................................... 314.2 Manual assembly using PD-type control law . .................................... 314.3 Manual assembly using PID-type control law ....................................344.4 Triad assembly ................................................ 35

4.4.1 The procedure ............................................................. 354.4.2 Results ..................................................................... 36

4.5 Comparing manual and Triad assemblies . .................................... 374.5.1 Overall time ................................................................ 374.5.2 Joint completion time . ....................................................384.5.3 Tasks covered by Triad .................................................. 39

4

5. FULL EASE STRUCTURE ASSEMBLY ............... .................................. 405.1 Results of NB-67 .................................................................... 40

5.1.1 Test setup and procedure ................................................ 405.1.2 Times for assembly and proportions . ...................... .......415.1.3 Comments on Fatigue . . ..................................................42

5.2 Results of NB-67C ...................................................... 425.2.1 Test setup and procedure ................................................ 425.2.2 Mechanical setbacks ..................................................... 445.2.3 Estimation of Triad assembly times and proportions ................. 44

5.3 Comparison of NB-67 and NB-67C data ......................................... 455.3.1 Direct comparison of assembly times .................................. 455.3.2 Weighted comparison . ................................................... 46

5.4 Limitations of the present configuration ........................................... 476. CONCLUSIONS AND RECOMMENDATIONS .......................................... 48

6.1 Conclusions ........ .. ......... ......... .......................486.2 The future of BAT ............................................................... 49REFERENCES . ..................................................... 52

APPENDIX I. BAT mobility using RMS ..................................................... 53APPENDIX II. BATSOFT ...................................................... 55APPENDIX III. JCS code ...................................................... 119APPENDIX IV. Triad trajectories .............................................................. 132APPENDIX V. EASE structure assembly schedule ......................................... 150

5

1. INTRODUCTION

1.1 Teleoperation

In hazardous environments such as the containment vessel of a nuclear reactor, the high

pressure depths of the ocean, or radiation-intense regions of space, it is dangerous or

impossible to have a human on site. In these situations, the human must be kept remote

from the site while a machine completes the task. One way to do this, with existing

technology, is teleoperation, which involves an on-site machine controlled by a human in

some safe remote location. A teleoperator enacts the human's commands, scaling them and

enhancing them. From a safe distance, the human can use the enhancements of the system

to move an object larger than he can move or make fine movements on a smaller scale.

In the case of the Beam Assembly Teleoperator (BAT) developed by the M.I.T. Space

Systems Lab (SSL), the mobile machine has propellers to give the teleoperative base 6

degrees of freedom (DOF), a dextrous, anthropomorphic arm for manipulation, a claw for

grappling and a claw for carrying. BAT operates in the neutrally buoyant environment of a

swimming pool, controlled by an operator in the Integrated Control Station (ICS) on the

surface.

The ultimate in teleoperation is called telepresence: "at the work site, the manipulators

have the dexterity to allow the operator to perform normal human functions" and "at the

control station, the operator receives sufficient quantity and quality of sensory feedback to

provide a feeling of actual presence at the work site."[1] Telepresence allows the operator

to use all the control strategies ingrained in the neural network of his brain from the long

years of childhood and adulthood that allow him to use his body to interact with the

environment and apply them to the remote work site. The control station and the

teleoperator are "transparent" in the control scheme. This would be a tremendous savings

in training time. Instead of the operator training to associate this joystick with that

manipulator joint and this toggle with that thruster, he simply uses the training he has on

6

the use of his own body to associate closing his fingers with grasping an object.

Unfortunately, no truly telepresent system has yet been developed.

1.2 The addition of supervisory control

If a teleoperator that could transmit all the agility of a human could be built, it would

equal human on-site productivity. But such a teleoperator must still be controlled manually

through each step of the task at hand, limited by that same agility, and by the motivation

and fatigue of the operator. One way to increase productivity is to hand off some portions

of the task to a computer control system on the vehicle. The computer receives commands

from the operator, and, by means of sensors and actuators, implements them. The operator

monitors the system, intermittently adjusting or supplying new commands. This is called

supervisory control.

Supervisory control is the middle ground between completely manual teleoperation and

completely autonomous robotics. Depending on the sophistication of the system, the

operator hands off some level of control, such as the major task level, task component

level, or task primitive level. A major task is one requiring the orchestration of all of the

system components. The computer must have the ability to plan, diagnose, and react to its

environment. Operator commands to a major task level supervisory controller would be on

the order of "diagnose and repair satellite telemetry module." Similarly, at the task

component level, a controller may also require the use of all system components, but does

not require planning or diagnoses. The computer implements a series of preplanned

manipulations. An example of an operator command to a task component level supervisory

controller is "detach and remove satellite telemetry module." At a more basic level, a task

primitive involves a single system component. The computer implements a single

manipulation such as "move freed module from point A to point B."

7

Supervisory control allows for quicker task completion, quicker automatic reflexes to

disturbances and other unanticipated events, greater accuracy and repeatability, and,

perhaps most importantly, increased duration [2]. Another opportunity afforded by

supervisory control is multi-tasking. While the human operator may only be able to operate

one remote machine manually, he may be able to supervise several, stepping in as required

to solve problems the lower level computer had trouble with. The practicality of multi-

tasking is discussed in [7].

1.3 EASE and the SSL loop group

The Experimental Assembly of Structures in EVA, or EASE, was a NASA sponsored

Shuttle flight experiment to study the capabilities and limitations of humans in EVA (Extra-

Vehicular Activity). Productivity of humans in space was considered key in determining

the economic viability of any space system requiring assembly. Suited subjects assembled

the EASE structure in neutral buoyancy at the NASA Marshall Space Flight Center Neutral

Buoyancy Simulator (MSFC-NBS) and in EVA on STS flight 61-B in November and

December 1985. Data returned from this experiment has already had some impact on space

station design. Detailed results are found in [3].

A secondary objective of the EASE experiment was to verify neutral buoyancy as a

simulation of sustained zero gravity. Preliminary studies concluded that neutral buoyancy

was an accurate simulation for task involving manipulation of masses sufficiently large that

inertia dominated water drag. Damping due to water drag was expected to facilitate

alignment and body positioning in neutral buoyancy but not expected to affect task

completion time. Times for full assembly in EVA were found to be 18% faster than those

in neutral buoyancy. EVA and neutral buoyancy assembly repetitions showed similar

learning and fatigue curves, though the onset of fatigue appeared earlier in neutral

buoyancy. The overall conclusion was that, for the construction of the EASE structure,

8

there is a good correlation between working in space and simulating it under water in

neutrally buoyant conditions.

The EASE structure is a tetrahedron with six beams and three nodes assembled in the

space shuttle payload bay (see figure 1.1). One of the nodes, or clusters, is fixed to the

payload bay. The clusters and beams are connected by the EASE joint (see figure 1.2).

The cluster has a mushroom end which fits into a receptacle on the end of the beam. A

sleeve on the beam end is then slid over the joint to hold it in place. The sleeve is held in

place by a spring loaded button. The capture rocker shown in figure 1.2 was not used in

the human assembly version of the EASE joint.

Figure 1.1 The EASE tetrahedron

The EASE tetrahedron was designed to be generic in nature rather than an operational

prototype. The structure was designed to be challenging to assemble in order to assess

learning, productivity and fatigue. As such, the allowable misalignment of the two ends to

be joined is less than ± 2-3 degrees for a successful joint completion.

The first teleoperator developed by the Space Systems Laboratory, the Beam Assembly

Teleoperator or BAT, was designed to assemble the EASE structure to compare

teleoperated assembly with human assembly. Developmental testing for BAT is performed

at M.I.T.'s Alumni Swimming Pool. Full scale testing has been conducted at MSFC-

9

NBS, which is a cylindrical tank of water 23m across and 12m deep. Across the bottom of

the tank is a mock-up of the space shuttle payload bay.

Mushroom

capture rocker End

Cluster

Figure 1.2 The EASE joint, unassembled and assembled

The EASE structure used by BAT is much the same as that used by human subjects in

pressure suits with the addition of a capture latch, or rocker, to hold the mushroom in place

while the sleeve is slid. BAT does not use a pair of "harpoons" used by the suited subjects

to tether beams and clusters during assembly.

1.4 The vehicle, BAT

BAT is a neutral buoyancy simulation of a space based teleoperator. BAT's base is maneuverable in 6D

use of 8 ducted propellers (see figure 1.3). Unlike vehicles designed specifically for underwater use, ca

attention is paid in neutral buoyancy to placing the center of mass of the vehicle coincident with the cente

buoyancy, so that BAT has no preferred attitude. This attitude freedom provides a

10

Bean

Stereo Cameras

Ducted Propeller-'TJ T ilt and Pan Unit

BellyCameras

Beam Carrier Claw

Handle

Side View Front View

Figure 1.3 BAT

11

simulation of the rotational dynamics of a space vehicle, allowing BAT to 'fly'

underwater.

BAT has one dextrous 5DOF arm which is proportionally similar to a human arm (see

figure 1.4). The arm consists of a 2DOF differential wrist, a 1DOF elbow and a 2DOF

shoulder. The motors for the upper three joints are held close to the upper frame origin to

keep down inertia. Since the EASE joint has an axis of symmetry, the wrist did not need to

have a degree of freedom in pitch. Each joint is also equipped with an angular position

encoder.

[) Shoulder Pitch) Elbow

rict,) Shoulder Yaw Side View Roll

I p V IVV

Figure 1.4 BAT's right arm

BAT has two other arms. The left arm is used for grappling, and includes a stowage

handle for the right arm claw. The stowage handle is used to hold the right arm in place by

its claw when the arm joint motors are not powered. The beam carrier arm is used to

remove a beam from the beam rack, hold it back while BAT is flying, and then present it

for assembly. The claws for all three arms as well as the extension and retraction of the

beam carrier are controlled with pneumatic air cylinders.

There are two sets of stereo cameras on BAT for operator feedback. The cameras are

mounted in pairs to allow the operator to have depth perception. One stereo pair is

mounted on BAT's belly in the center of the vehicle. This pair has wide angle lenses and is

12

4-

used for flying. The other pair has a more narrow field of view and is mounted on a tilt

and pan base. The tilt and pan allows the camera to be commanded in 2DOF. The motion

of the tilt and pan is normally slaved to the motion of the operator's head to allow him to

look around. The magnification and aiming ability makes the tilt and pan cameras

applicable to assembly tasks.

The motors, encoders, batteries and circuitry on BAT are enclosed either in water tight

boxes or in waterproof potting. Most boxes are pressurized so that any leaks that occur will

leak air out rather than water in. The battery boxes are not pressurized. Since batteries vent

hydrogen as they are discharged, pressurization of the battery boxes would create a danger

of ignition if hydrogen from the battery box were allowed to mix with air. The main battery

box is purged with nitrogen before and after use.

1.5 The control station, ICS

ICS, the Integrated Control System, is the operator station for controlling BAT. (see figure

1.5) The operator interface consists of a master arm used to control BAT's dextrous right

arm, a head controller used to control the tilt and pan cameras, two 3DOF joysticks, one

translation and one rotation, used to control BAT's propulsion system, six monitors

showing computer graphics and whatever external video might be available, four switches

mounted on the master arm, four switches mounted on the rotational joystick, four foot

switches, four panel switches, six pneumatic enable switches, one large switch to change

between stereo camera pairs, and a keyboard, used mainly in system initialization.

13

tors showing BAT video

AT vid

Figure 1.5 The Integrated Control Station

14

5

Figure 1.6 Photograph of the Integrated Control Station

The interface is monitored by an IBM PC running the program BATSOFT. BATSOFT

reads the switches and the potentiometers on the arm and head controller, creates the

graphic display, beeps the appropriate beeps, and gives commands to the Joint Control

System (JCS), which controls the dextrous right arm, the pneumatics, the tilt and pan, and

the propulsion system thrusters. The JCS contains four cards each driven by a Z80

microcomputer that calculates joint duty cycles, one Z80-based card that calculates thruster

duty cycles, and a transceiver card which multiplexes all of the motor command signals

together along with signals to control the state of the pneumatics, and sends it down the

communications down link to BAT. The transceiver card also reads the up link from BAT

containing mainly joint encoder positions for the joint cards and for BATSOFT.

Since the operator has primarily visual feedback with little oral and no tactile feedback,

BAT and ICS are certainly not an example of telepresence. The operator interface,

however, has been shown to be functional with the construction of the EASE structure

detailed in [5] and chapter 5.

1.6 Triad

Triad is a set of task primitive supervisory control algorithms which use learned or

established trajectories to move the dexterous arm through the motions of joint assembly.

Triad reads in a command line from a file composed of angular positions for each joint plus

commands to control the gains in the arm control system, the accuracy at which the

trajectories are to be followed, and the state of the various pneumatics.

Triad temporarily replaces the master arm manual control. Triad works entirely in joint

space, as the master arm does, providing the JCS with command set points for each joint of

the arm.

15

Since the tilt and pan cameras are only for operator feedback and have no use to Triad,

they remain in the control of the operator while Triad is running. In this way, the operator

can monitor Triad.

1.7 Scope of thesis

Using TRIAD in addition to the already functional manual control system, the

advantages of the addition of supervisory control are analyzed. Chapter 2 details upgrades

made to the arm control system that allowed Triad to function. In chapter 3, the

specifications for Triad and its application to BAT are listed. In chapter 4, the EASE joint is

assembled repeatedly under manual and supervisory control situations for both experienced

and naive operators to discover the differences in productivity and its components in

training and fatigue. In chapter 5, assembly of the full EASE tetrahedron is demonstrated

and compared with a previous manual assembly.

16

Figure 1.7 BAT assembles the mini-EASE tetrahedron in the M.I.T. Alumni Pool

16A

Figure 1.8 The Integrated Control Station

2. UPGRADED CONTROL ALGORITHMS 5DOF ARM

2.1 Previous control system

The prime objective for BAT was to

assemble the EASE tetrahedron. This

objective was accomplished in January 1987

at NASA Marshall Space Flight Center's

Neutral Buoyancy Simulator during test

series NB-67. The control of the 5DOF right

arm was accomplished with a simple

proportional or P-type control algorithm.

2.1.1 Chain of command to move the arn

The actuation of the right arm is

accomplished by permanent magnet DC

motors fed with a pulse width modulation

(PWM) duty cycle. Gains for the P-type

control system are input into BATSOFT

which transmits them to the joint cards in the

JCS via a dual port ram (DPR). Using these

gains, a duty cycle is calculated for each joint

by the joint cards and then multiplexed into a

serial line for communication to BAT. On

BAT, the serial data is demultiplexed and

transmitted to power modules. The power

modules contain H-bridge darlington

transistors which reproduce the signal duty Figure 2.1 ICS-BAT Chain of Command

17

cycle at a current level capable of running the motors.

Each joint is also equipped with an optical incremental shaft angle encoder. Circuitry

on BAT integrates the encoder quadratures and transmits the angular position of each joint

back up the communications link to the JCS. In the JCS, the encoder positions are made

available to the joint cards and to BATSOFT. Conversion of encoder value to joint angle in

degrees is detailed in [4].

2.1.2 The role of BATSOFT

As mentioned above, the man-machine interface is monitored by an IBM PC running

the C program, BATSOFT. BATSOFT reads the various potentiometers, switches and

keys on ICS, and relays commands to the JCS. Code for BATSOFT can be found in

Appendix II.

In the running of the right arm, BATSOFT sets the gains and provides command set

points. The gains are set by the operator by loading prepared gain files or by adjusting

each joint separately. The gains are then transmitted to the joint cards, via the DPR. When

the operator turns on the master arm, BATSOFT reads the potentiometers at each joint of

the master, adjusts them to produce a command set point for each joint, and transmits each

set point to the appropriate joint card.

The tilt and pan is controlled in much the same way, with either the potentiometers on

the head controller or those on a separate joystick being read for command set points.

As long as the master arm is connected, BATSOFT transmits a new position set point

for each joint once per main loop. The frequency of the BATSOFT main loop is 8Hz.

2.1.3 The role of the joint cards

The frequency of the joint card main loop is 98Hz, an order of magnitude faster than

the BATSOFT main loop. There are four joint cards resident in the JCS. Each calculates

the duty cycle for two of BAT's motors. There are eight motors total, five in the right arm,

two in the tilt and pan, and one for the cooling pump. The code for the joint cards is listed

in Appendix III.

18

For the P-type control system, the joint cards read in the encoder position of a joint,

subtract it from the command set point, and multiply by the constant gain supplied by

BATSOFT. The result is then clipped to maximum duty cycle, if necessary, and sent to

BAT in PWM form.

2.1.4 Problems with the previous control system

The P-type control law does not provide good tracking. Overcoming friction in the

joints requires error integration. The P-type control law allows the arm to become stuck

short of the mark, since its duty cycle diminishes as the actual joint position approaches the

command set point. The operator is forced to account for this by overshooting a desired

end point with the master arm and then coming back to stop the arm at the desired location.

This is especially difficult for fine movements, in which the operator must use a large

starting error to get the arm moving and come back just far enough to stop it again at the

desired end point. The amount of leading required is variable with load and desired arm

speed.

This problem is addressed here with the addition of integrated error and differentiated

error to the control algorithm. The resulting controller is PID-type or proportional-integral-

derivative type. The derivative term excites response in the applied torque due to quick

changes in commanded speed. The integral term builds up applied torque in the presence

of an error to get rid of steady state error and to overcome friction.

Another problem with the control system which is not addressed here is the frequency

of the duty cycle used in the pulse width modulation. PWM is a method to approximate a

DC voltage using a digital two-state signal. The premise of PWM theory is that the load

has an electrical time constant sufficiently longer than the period of the PWM so that it sees

the current it would otherwise receive from the corresponding DC voltage. Here, the PWM

modulation has a frequency of 1KHz, but the motor response also about 1KHz. So,

instead of the steady current draw expected by PWM, the motors on BAT draw current in

19

spikes in phase with the PWM duty cycle. The primary result of this problem is that there

is no linear relationship between duty cycle and resultant motor torque.

A third problem with the existing control system is noise in the encoder signals. The

wiring from the encoders in the present construction is laid along side the motor power

cables for some distance. The highly oscillatory nature of the power current tends to

induce noise in the adjacent, unshielded encoder cable. This is especially prevalent in the

wrist, where an occasional twitch can be seen. The noise, together with nonlinearities

produced by water currents, water drag, friction, stiction, and backlash makes a rather

fuzzy picture of arm dynamics.

2.2 Digital PID control

2.2.1 Digital approximations of PID control

An advantage of a PID controller is that, when correctly tuned, it can give good system

response without a resident model of the system dynamics. A standard continuous time

PID-controller looks like:

1 TSU(s) = K( 1 + T + /N )E(s)TI S 1+ Ts/N

where U(s) and E(s) are Laplace transforms of control and error, K is the proportional

gain, TI is the integral time constant, TD is the derivative time constant, and N is the

number of sampling periods per rise time to a step input.

In order to implement a PID algorithm in the digital domain, the analog control law

must be approximated. An extended discussion of digital approximations is found in [8].

There are three basic methods of approximation:

20

s _ Z 1 (Euler Method)h

s = z- 1 (Backwards Difference)zh

s = h(z - 1) (Tustin Approximation)h(z+1)

where s is the Laplace operator, z is the forward shift operator such that Z=esh,

and z x X = X , where Xn is the value of X at t = h x n, and h is the sampling period

of the digital system.

According to [8], the most common way to approximate a PID controller is to make an

Euler approximation of the integral part and a backwards difference approximation of the

derivative part:

u(kh) + h TD_ z-1u(lh) = K(1+Ti(z-) + h+TD/N x z- TD/(Nh+T ) )e(kh)

where k is the sample number. For the purposes of tuning, the equation was streamlined to:

u(kh) = KD(+ h 1 1 T z-i )e(kh) T z- 1 + z +where TD = TI, TDD = TxN, and = eTNfrd.

2.2.2 Tuning

With a short sampling period, a digital PID controller behaves much like an analog

PID. Several tuning methods have been developed for the analog PID. Assuming a

sufficiently short sampling period, theses methods can be directly applied to the digital

PID. The method used here is the transient-response method developed in [9].

The transient-response method models the system as a first order system with

transportation lag. The step response to such a system is shown in figure 2.1.

21

-1

:3{oL

o0rZ

L *LI rIU

Figure 2.1 Open Loop Step Response of a System [8]

The lag and slope of the response can be used as a first approximation of PID parameters,

using the following equations:

1.2KD= RL TID= 2L TDD=0.5L

Using these parameters, the controller can be tuned by trial and error for best response.

2.3 Application to BAT

2.3.1 Direct application

The first problem encountered is that listed in section 2.1.4, that of the failure of PWM

for BAT. Instead of the smooth step response curve needed for the tuning, BAT's actual

response is a series of spikes. The way chosen around this is to examine each of the spikes

as a separate step response. The resulting values of N,the number of sampling periods per

rise time, R, the torque response slope, and L, the torque response lag, are not guaranteed

to provide the best starting point but were reasonably close.

Measuring the current output of each motor, which is proportional to output torque,

values were found for R and L to construct a first iteration PID system for BAT's right

arm. This was then implemented into the code running the joint cards. This code is listed

in Appendix III.

The control law is arrived at as follows. Beginning with the equation above, solve for

Uk, the control value for the present sampling period:

22

U(kh) a b(z- l)E(kh) =S( z-1)+ (z+c) )

where a = h/TID, b = TDD/h , c = y, and S is the number of encoder pulses per joint angle

degree. U here is the actual duty cycle sent to the arm, and E is the error between

commanded and actual encoder positions.

UE ( l+(c- 1)z'l-cz 2)=KS (l+b)+KS(c+a- 1-2b)zl+KS(ac-c+b)z 2

Uk= ( 1-c)Uk cUk _+KS( l+b)Ek+ KS(a+c- 1-2b)Ek_ tKS(ac-c+b)Ek_ 2

The coefficients of each of the control and error terms is calculated in BATSOFT and input

to the JCS as a separate set of constants for each joint:

U.= (M1 U,_M TJ -, xE PE +GEk-i l~ 2 k-2 k-1 k-2) k

where the portion in the brackets can be precalculated at each step and added to the multiple

of encoder error as soon as it is read.

Tuning of this algorithm failed to produce adequate tracking. Essentially, the integrator

did not function correctly due to the slow sampling period. The use of a Tustin

approximation instead of an Euler did not improve the response.

2.3.2 Addition of a separate integrator

A simple integrator was then added to the control law to force an integration of error to

help zero out residual errors. The integrator simply sums a multiple of the error. The sum

is then clipped, if necessary. The multiplier for each joint was determined by tuning for

fastest response. The clipping level was determined from the duty cycle necessary to

overcome starting friction. The integrator sum for each joint is reset every time the error

passes through zero to avoid integrator windup.

This addition improved the tracking sufficiently to allow the use of Triad. Sets of

parameters for each joint were then established for manual and Triad use.

23

3. TRIAD, TASK PRIMATIVE SUPERVISORY CONTROL

3.1 Theory

3.1.1 Supervisory Control

In the process of supervisory control, the operator issues commands to a computer

which in turn closes its own loop with the vehicle being controlled. The computer

produces a integrated status display to the operator to show response to these commands

and the operator steps in when necessary to change commands or make adjustments.

Sheridan, in [2], goes into great detail concerning the roles of the computer and

operator under supervisory control. He lists five major functions of the operator. The first

is to plan the task to be done, decide a strategy for its completion including the appropriate

roles of the operator and computer, and consider the initial conditions. The second is to

teach the computer how to do the task, decide how the computer is to deal with unexpected

occurrences, decide how far the computer is to go between commands, and give the

command to begin. The third function is to monitor the computer, make minor adjustments

when necessary, and diagnose failures. Fourth, the operator intervenes and takes manual

control when the computer fails, in order to reconsider the plan and how it was taught to

the computer. Last, the operator is to learn about the system in order to understand it and

make best use of its characteristics.

3.1.2 Trajectory control and path planning

The object of planning trajectories is to move between points in an optimal way,

primarily with regard to time. The path is constrained by its end points and obstacles that

must be avoided. The trajectory, the time versus position sequence required to follow the

path, is limited by joint torque maximums and allowable joint angles.

The output of a trajectory planning algorithm is a set of joint positions to be fed into the

arm control system. The set points are typically required at a rate of 60Hz to 200Hz,

depending on the control system, in order to provide a smooth arm motion as shown in [6].

24

The calculation of the set points may be accomplished by several methods. A set of

intermediary or 'via' points can be added along the desired path to insure that the arm

misses the obstacles in its path. The required arm velocities and accelerations at each point

then become constraints to the path. The path can be calculated as an Nth order polynomial

accounting for each of the constraints. But for even slightly complicated trajectories, the

polynomial is of high enough order that checking for torque limit satisfaction becomes quite

difficult. This type of trajectory is not optimal; due to the number of roots in the

polynomial, it is likely to overshoot and wander.

A way around this problem is the use of knot points. Knot points are placed at the via

points and where mathematically convenient. Given the arm state at each of the knot points,

separate cubics are calculated to govern the motion from each knot point to the next.

A point of contention among the robotic community is whether these calculations

should be conducted in cartesian space or joint space. A cartesian space trajectory is more

easily visualized by the user of a system. As noted in Appendix I, it is difficult for a

human operator to make a mental Jacobian transformation to move joints to go straight in a

cartesian direction. But cartesian position and orientation are not enough for a trajectory

planner. The inverse kinematics also require answers to questions such as whether the

elbow should be up or down. The trajectory planner working in cartesian space has

knowledge of end effector position but does not know the joint angle required to place the

end effector. For this reason, it can assure obstacle avoidance for the end effector, but not

necessarily for the upper portions of the arm.

3.2 Method

3.2.1 BAT's Limitations

Without major hardware changes, the application of supervisory control to BAT is

limited. The propulsion system for the base is operated on a loop closed only through the

visual system of the cameras and the human operator. Without any position sensing, BAT

25

has no way of knowing where it is inertially at any given time. BAT does know the state

of its pneumatics and the encoder position of the right arm. With this information,

BATSOFT could close a loop with the JCS to control the position of the right arm and the

state of the pneumatics.

As noted in chapter 2, BAT has no exact control of torque. Joint velocity and

acceleration are also not available. These limitations make a generalized trajectory planner

impractical. But since the task primitives of the EASE joint assembly were repetitive,

specialized trajectories could be developed for each portion.

3.2.2 Application to BAT

As stated in the introduction, Triad temporarily replaces the master arm control. Triad

works entirely in joint space, as the master arm does, providing the JCS with command set

points for the arm.

BATSOFT, with all its analog to digital conversions reading the potentiometers of the

master arm, head controller and joysticks, has a main loop period of 8Hz. Triad, without

the burden of the full human interface, can run much faster, approximately 1KHz. The

JCS runs at 98Hz. This allows Triad to monitor each sampling period of the JCS.

Triad operates using a file of saved joint positions and commands. A command file can

be generated during main loop manual control in Triad's learn mode. Triad saves the actual

arm position to file once every main loop. The file can then be "played back" using triad's

point-to-point mode, or modified off-line to smooth out the motion and get rid of errors.

Files can be played back singly or concatenated together for one long trajectory. A stack of

files can be set up for playback via a menu or a button software configured to load in a set

of trajectories.

Triad's learn mode saves the actual arm position, as opposed to the command position,

for two reasons. As will be shown in section 4.1, the operator often provides a higher

order control system to pick up where the JCS leaves off. The commanded position may

contain lead and dither not meant to appear in the actual arm position. The second reason is

26

that the Triad arm gains are higher than manual gains, and response to a command given

using manual gains is different than that using Triad gains. Triad gains are tuned to

provide the fastest possible tracking.

Manual gains are limited to allow for smooth arm motion under the control of the main

loop of BATSOFT. The manual gains are sufficient to provide tracking of a single

command set point in approximately one main loop of BATSOFT. If the tracking were

significantly faster, the arm would arrive at the set point before a new set point could be

supplied by BATSOFT, and the arm would slow toward a stop. The result would be

discontinuous slave arm velocity in response to a constant master arm velocity.

After Triad has issued a set point to the JCS, it monitors joint positions and waits for

the actual joint space position of the arm to come within a target region of the commanded

position. The size of the target region is set by the command file being read, but is usually

kept small near a target and large during a free move through open space. Triad also has

control over the JCS control laws for each joint. If the arm takes too long to track to a new

set point, Traid loads in higher control gains for each joint to get the arm going again.

Friction is a large factor for BAT's right arm due to the fact that the arm is operated

immersed in water, and is therefore difficult to lubricate. Once the actual position reaches

inside the target region, Triad reads in a new command from the file, passes on any

pneumatic changes in the command line, and gives the JCS a new set point.

Once the file or set of files is complete, Triad loads manual control gains to the arm if it

is still in action, or loads zero gains to the arm if it was stowed, and returns control to the

human interface. If the operator wishes to bail out of Triad in the event of an error or a

change in plans, he can pull a finger switch. Triad responds immediately by loading in

manual control gains, changing the master arm offsets so that the present master arm

position is aligned with the arm position, and returning to main loop manual control.

27

Since the tilt and pan cameras are only for operator feedback and have no use to Triad,

they remain in the control of the operator while Triad is running. In this way, the operator

can monitor Triad's progress.

3.2.3 Triad trajectory development

Each trajectory developed for Triad started with points in joint space. Starting and

ending points were taken as well as via points clear on approach to grappling positions, and

clear of any obstacles. A path was then plotted between these points using linear joint

interpolation, or a straight lines in joint space. Spacing between set points was determined

by the tuning detailed in chapter 2. Commands were then added to the Triad file to change

pneumatic states at the appropriate set points.

Commands to change arm gains were added. Three sets of gains are available: Triad 1,

2 and 3. Triad 1, the default, was used in guarded situations on approach to a grappling

point. Triad 2, with slightly higher gains, was used in free portions of the trajectory when

the arm was swinging across free space. Triad 3, with high gains, was used for arm

movements with a load in the right claw.

In situations where a particularly large torque was required, such as getting a load

started moving, a single set point was modified and moved beyond the desired position.

The actual desired position remained within the target region of the modified set point.

This would supply the large error to the joint cards to provide the torque required for the

job without waiting for the integrator to build up. The load would then respond to the jolt,

allowing the arm to move toward the desired set point. Once the actual position was within

the target region of the modified set point, by design quite close to the desired set point,

Triad would go on to the next set point. The next set point could then be back on the

nominal path.

28

3.3 Application to the EASE structure assembly

3.3.1 Breakdown of EASE joint assembly

The assembly of the EASE joint used in the comparison of manual and supervisory

control assembly is the configuration with BAT docked with its left claw to a cluster. A

beam in the beam carrier is attached to the cluster. The assembly, called a beam joint

assembly, consists of five steps:

1. Grab the beam. With a beam already in the retracted beam carrier claw, the beam is

swung forward. The right arm is powered up and the right claw open. The right arm

moves clear of the stow handle mounted on the side of the left arm, and moves clear of the

left arm. The right arm waits for the beam carrier to extend and settle the beam to a stop

and then moves up to close the right claw on the beam end. The beam carrier claw is then

opened and the beam carrier swung back.

2. Make the joint. With the beam held in the right claw, the arm maneuvers the beam to

place the cluster mushroom end into the receptacle on the beam end. The wrist must then

be twisted to apply sufficient force to set the rocker in the beam end so that it holds the

mushroom end in place.

3. Grab the sleeve. With the joint just completed, the right claw opens on the beam

end, the arm backs off of the beam end, approaches the sleeve, and grabs the sleeve with

the right claw.

4. Slide the sleeve. With the right claw closed on the sleeve, the right arm applies

sufficient force to slide the sleeve over the cluster, holding the mushroom end in place.

The sleeve is slid until the spring loaded button pops up to hold the sleeve in place.

5. Stow the right arm. With the right claw on the slid sleeve, the claw opens. The

right arm backs the claw off of the sleeve and clear of the left arm. The arm approaches the

stow handle so that the right claw can be closed on the stow handle. The gains to the right

arm are then zeroed.

29

3.3.2 Assembling a joint manually

Manual assembly of the joint involves actuating switches to change pneumatic states.

The arm movements are commanded with the master arm. Each time the master arm is

brought into use, the operator hits a software switch to set the present master arm position

equal to the present slave arm position. This process is called 'connecting' the master arm.

When the master arm position becomes uncomfortable for the operator, he may disconnect

the master arm and reconnect it in a more comfortable position. The arm is powered up and

down with a button push. The operator uses the tilt and pan cameras slaved to the head

controller to view the scene.

3.3.3 Assembling a joint with Triad

For each of the basic parts of the EASE joint assembly there is a Triad file. All five

steps are reduced to button pushes with the exception of setting the rocker on the

mushroom end. In the manual assembly, getting the rocker to set on the mushroom end

requires wiggling the beam end until the rocker receives enough force to set. Due to

problems in commanding torques, as discussed in section 3.2.1, together with inexact

positioning of the cluster in the left claw, Triad only goes so far in making the joint as to

place the beam end close to the mushroom end for easier manual completion.

For the purposes of assembling the EASE joint with Triad, the Triad trajectories are

grouped. The operator hits a switch actuating GRABBEAM and JOINT trajectories, and

watches the right arm grab the beam and move it close to the cluster. The operator

manually gets the rocker to set, and hits the switch again. Triad runs GRABSLV and

SLIDESLV. Once the operator sees the button pop on the sleeve, he hits the switch a third

time. Triad runs STOW. During the process, if a Triad trajectory fails, the operator

completes the failed task primitive manually, and restarts Triad at the beginning of the next

task primitive.

30

4. MANUAL vs. TRIAD ASSEMBLY of the EASE JOINT

4.1 Manual assembly using P-type control law

In January of 1987 during test NB-67, BAT successfully completed the EASE structure

at MSFC-NBS using manual control and a proportional (P-type) control law on the arm.

The P-type control law forced the operator to use such tricks as supplying command set

points beyond the desired location in order to overcome friction. Since assembly of the

EASE joint is repetitive, the operator could learn the necessary master arm motions to

assemble the joint.

During the testing, full size beams approximately 4m long were used. In the swimming

pool facilities available at M.I.T., full size beams are difficult to accommodate, so mini-

beams, approximately 1.5m long, are used instead. The mini-beams have the same beam

ends as the full beams. The difference in joint assembly is contained in the setting of the

rocker, where BAT must move the full beam. In this instance, the inertia of the full beams

made them more difficult to maneuver.

4.2 Manual assembly using PD-type control law

For the purposes of comparing manual and Triad joint assembly, each operator

completed several joints in succession. BAT was mounted to a frame at the bottom of the

pool. Before each run, a diver would place a beam in the retracted beam carrier. The

operator would then power up the arm, extend the beam carrier, and run through the five

steps of assembly detailed in chapter three.

Data taking consisted of video of the operator's eye view through the BAT cameras,

and a record of commanded and actual arm position and duty cycle. As soon as the data

from one run was stored, the operator would start the next assembly. Data storage took

approximately one minute per run.

There were three operators used in the testing. The experienced operator already had

more than 25 hours on the arm as the primary operator during development and the NB-67

31

testing. His experience was exclusively with the P-type manual control system. The less

experienced operator had only two hours of arm time at the start of the testing, but had

more knowledge of the new control system and of Triad. The third operator was naive,

having no operational experience with the arm, and only basic training in its use.

The first testing was performed using a PD control system before the integrator detailed

in chapter 2 was implemented. Assembly times are listed in table 4.1.

Subject:Experienced PD-Control ManualRun #:Grab BeamJointGrab SleeveSlide SleeveStowTotal Time

seconds1 2 3 4 5 6 Average:

15.0 10.0 15.0 13.0 11.0 9.0 12.249.07.08.0

17.096.0

29.09.05.0

28.081.0

30.05.05.0

17.072.0

19.07.03.0

10.052.0

25.09.04.0

16.065.0

Subject: Less Experienced PD-Control Manual

Run #: 1 2 3 4 5Grab Beam 19.0 84.0 39.0 23.0 30.0Joint 37.0 96.0 50.0 7.0 30.0Grab Sleeve 10.0 6.0 11.0 7.0 5.0Slide Sleeve 10.0 26.0 46.0 5.0 5.0Stow 20.0 26.0 10.0 15.0 13.0TotalTime 96.0 238.0 156.0 57.0 83.0

Subject: Niive PD-Control ManualRun #: 1 2 3 4 Average:Grab Beam 215.0 150.0 133.0 116.0 153.5Joint 568.0 90.0 107.0 57.0 205.5Grab Sleeve 18.0 40.0 32.0 24.0 28.5Slide Sleeve 29.0 27.0 40.0 25.0 30.3Stow 48.0 54.0 16.0 21.0 34.8Total Time 878.0 361.0328.0243.0 452.5

28.07.04.09.0

57.0

30.07.34.8

16.270.5

seconds

6 Average:14.0 34.813.0 38.817.0 9.38.0 16.7

14.0 16.366.0 116.0

Table 4.1 Manual PD assembly times

The experienced operator had the shortest times, exhibiting learning through the first

four runs. Run four for the experienced operator was the fastest manual assembly time

encountered in the entire test. Since the addition of a differentiator to the control law does

not significantly decrease steady state error, the experienced operator should continue to

benefit from his experience with the P-type controller. This is quite evident in the graph of

32

command versus arm position. Figure 4.1 shows the shoulder pitch trace as an example.

All the way through the trajectory, the operator's command leads position, overshoots to

keep the arm moving, then comes back to stop the arm motion. The actual arm position is

quite smooth, and as was already noted, quite fast. The commanded position is much more

oscillatory, especially near the end as the arm approaches the stow handle. In this way, the

operator has added his own lead to the system to compensate for the inherent lag in the PD

controller.Experienced, PD Manual, Run 4Shoulder Pitch Trajectory

UUU

-10000

0

4 -15000

-20000

sh position

slp ommadA

0 10 20 30 40

Time (seconds)

Figure 4.1 Example of Operator Addition of Higher Order Control

Traces from the less experienced and naive operators do not show this kind of

oscillation. The commanded position contains the same sort of oscillation, the larger

command to move the arm, followed by a pull back. But in these cases, the pull back is not

as well timed. The pull back is apparently actuated by the operator's view of the arm

overshooting the desired position. This leads to actual arm motion that is much more

oscillatory, and thereby slower.

33

The less experienced operator shows a much more erratic series of assembly times.

This is caused by a series of operator errors such as missing the mushroom with the beam

end and trouble aligning the claw with the sleeve while sliding it. Here the largest variation

is in the completion of the joint. This task requires the application of the correct force to set

the rocker, and the arm is not set up for force control. The result is that the operator must

know the correct master arm motions to apply forces in the correct directions. Without any

force feedback, this can be difficult.

The naive operator has times much higher than the other two operators. His times do

show a learning curve in overall time, though not consistently in each component of the

assembly. Due to operator fatigue, the naive operator only completed four runs before

having to stop.

4.3 Manual assembly using PID-type control law

During a separate test, the experienced and the less experienced operators repeated the

testing using PID arm control. The integrator added the feature of zero steady state error,

balancing out the lag in the system. Assembly times are listed in table 4.2.

Subject:Experienced PID-Control ManualRun #: 1 2 3 4Grab Beam 24.0 20.0 22.0 19.0Joint 60.0 37.0 44.0 104.0Grab Sleeve 12.0 11.0 13.0 18.0Slide Sleeve 19.0 24.0 8.0 10.0Stow 21.0 29.0 19.0 32.0TotalTime 136.0121.0 106.0 183.0 1

Subject: Less Experienced PID-Control

Run #: 1 2 3 4Grab Beam 22.0 18.0 19.0 22.0Joint 86.0 24.0 53.0 24.0Grab Sleeve 12.0 9.0 7.0 11.0Slide Sleeve 15.0 30.0 5.0 4.0Stow 20.0 16.0 17.0 12.0TotalTime 155.0 97.0101.0 73.0

seconds5 6 Average:

17.0 16.0 19.746.0 66.0 59.59.0 8.0 11.8

16.0 6.0 13.819.0 18.0 23.007.0 114.0 127.8

Manual so

536.014.011.04.0

16.081.0

627.08.0

10.03.0

19.067.0

Average:24.034.810.010.216.795.7

Table 4.2 Manual PD assembly times

34

econds

The experienced operator shows an 80% increase in average assembly time. There is

no clear learning curve. It is possible that the experienced operator had to unlearn the

portion of the control law he was used to providing to the P-type controller.

The less experienced operator shows a 20% improvement in average assembly time.

This is the expected result, considering that the arm response is faster and more accurate

here than under PD control.

4.4 Triad assembly

4.4.1 The procedure

The testing was repeated using Triad to replace most of the manual portions of the

assembly. The function of the operator through the Triad portions of the assembly was as

an observer. On one occasion when Triad failed to grab the beam, the operator stepped in

to complete the task manually.

With a beam ready in the beam carrier and the right arm on the stow handle, the

operator began by pushing a button. Triad would extend the beam carrier, grab the beam

with the right arm, release the beam from the beam carrier and move it close to the

mushroom end on the cluster. The operator then moved the beam manually to insert the

connector and set the rocker on the mushroom end. The operator next pushed the same

button to get Triad to grab and slide the sleeve over the mushroom end. Verifying the

sleeve to be slid, the operator would push the button a third time to get Triad to stow the

arm.

The assembly was completed six times in succession. Operator's eye view was video

taped and a trace of the arm command and positions was recorded. This led to the same

one minute delay between runs for data storage.

35

4.4.2 Results

The same less experienced and naive subjects were used for testing. The experienced

operator was not available for this portion. The times for the Triad runs are listed in table

4.3.

Subject:Run #:Grab BeamJointGrab SleeveSlide SleeveStowTotal Time

Subject:Run #:Grab BeamJointGrab SleeveSlide SleeveStowTotal Time

Less Experienced1 27.4 7.6

31.1 27.96.6 5.51.7 5.56.1 7.7

52.8 54.1

Niive Triad1 28.4 7.6

97.6 88.88.3 7.61.6 1.46.5 7.9

Triad37.7

27.95.11.58.3

50.4

326.1

120.56.12.07.8

122.4 113.3 162.5

47.5

47.93.07.69.3

75.4

48.3

31.83.83.56.2

58.2

17.15.9

11.27.6

49.9

58.6

50.45.61.76.6

seconds6 Average:8.0 7.7

82.6 39.15.4 5.21.4 4.87.3 7.7

104.7 64.6

seconds6 Average:8.1 11.2

28.4 69.66.9 6.41.6 2.06.5 6.9

53.7 72.8 51.5 96.0

Table 4.3 Triad assembly times

The less experienced operator was one third faster than his manual PID average; half of

the runs equal or exceed the experienced operator's record manual assembly time of 52

seconds. The times for the portions of the assembly competed by Triad are consistent.

During runs 4 and 5, Triad failed to slide the sleeve all the way. The operator completed

the task with little loss in time. The only real variation comes from the manual portion of

the assembly, the completion of the joint with the operator showing fatigue in the final run.

The naive operator shows an 80% decrease in average assembly time, including one

time faster than the record manual assembly. Again the variability is contained mainly in

the manual joint portion. Grabbeam failed in run 3. The operator recovered without a great

loss in time.

36

Overall Average Assembly Times

452

11b 1 I 128

65

' Less Exp Less Exp. Niw 'PID-Type PID-Type PID-Type

:lanual Triad TriadSubject/ Run Type

Figure 4.2 Overall Assembly Times

4.5 Comparing manual and Triad assemblies

4.5.1 Overall time

Table 4.4 shows the average assembly times of all the test runs. As stated earlier, the

Triad times show a distinct improvement for the operator, comparable to the manual times

of the next more experienced operator. Figure 4.2 shows the same information in graphical

form.

SubjectControl

Grab BeamJointGrab SleeveSlide SleeveStowTotal Time

Experienced L. Exper.Manual PD Manual PD

12.2 34.830.0 38.87.3 9.34.8 16.7

16.2 16.370.5 116.0

Table 4.4 Overall Assembly Times

37

500 -

450 -

400 -

350 -

o : 300 -0·D 250 -

v]

200 -

150 -

100 -

50 -

El Stow

IZ Slide Sleeve

[ Grab Sleeve

U Joint

1 Grab Beam

70

0 .1 1.1 IExpert

PD -Type

96

Less Exp.PD-TypeManual

N'ivePD-TypeManual

ExpertPID-Type

Manual

NaiveManual PD153.5205.528.530.334.8

452.5

ExperiencedManual PID

19.759.511.813.823.0

127.8

L. Exper.Manual PID

24.034.810.010.216.7

95.7

L. Exper.Triad

7.739.15.24.87.7

64.6

NaiveTriad11.269.66.42.06.9

96.0

BA~

K

i

rz A \q R-\J

.I

I-

w I I i I X !! ,

4.5.2 Joint completion time

As stated above, since Triad does not replace the entire joint-making portion of the

assembly, the times for this portion of the assembly remain variable for the Triad runs. The

task is still the same. The operator must move the master arm to build up a command

position error on the slave arm to apply force on the beam sufficient to set the rocker. The

amount of the force is not always the same due to the differences between rockers, the

position of the right arm grip on the beam end, and the position of the cluster in the left

arm.

For the less experienced operator, the joint completion portion is not significantly faster

than for manual completion. But for the naive operator, the time for joint completion is

much improved, two thirds faster than his manual time. This is perhaps due to the lowering

of workload on the operator. The naive operator can now concentrate on learning and

performing the joint completion. The lower workload and the faster completion times also

lead to lower fatigue levels for the naive operator allowing for better performance.Average Assembly Times without Joint

250 -

200 -

,)

o0la>

100-

50 -

0

40

77

247 1

UX

L1 Grab Beam

68 6161

25 26Ong

+ + l l l

Expert Less Exp. Nive Expert Less Exp. Less Exp. Na'ivePD-Type PD-Type PD-Type PID-Type PID-Type PID-Type PID-TypeManual Manual Manrual Manual Manual Triad Triad

Subject/ Run Type

Figure 4.3 Overall assembly times showing Triad replaced portions only

38

[] Slide Sleeve

U Grab Sleeve

__J__,, ~ ~ - . -

4.5.3 Tasks covered by Triad

Figure 4.3 shows the overall assembly times only for the portions of the assembly

replaced by Triad. There are two major points here.

· First, the decrease in assembly times are concentrated in these four portions of the

assembly, so the differenced are more pronounced.

* Second, the Triad times for the less experienced and naive operators is nearly the same,

verifying that Triad assembly does not depend on experience.

39

5. FULL EASE STRUCTURE ASSEMBLY

5.1 Results of NB-67

5.1.1 Test setup and procedure

In January of 1987, during test NB-67, BAT successfully completed the EASE

structure at MSFC-NBS using manual control and a P-type control law on the arm. This

was the first full scale demonstration of teleoperated structural assembly in zero gravity.

The intent of this demonstration was to generate data to be used in trade offs between using

teleoperators and humans in space. The testing is detailed in [5].

Figure 5.1 NB-67 Structural assembly setup

The MSFC-NBS consists of a cylindrical tank of water 12m deep and 23m across. A

mockup of the space shuttle payload bay across the bottom of the tank was used as a base

for BAT's EASE structure assembly. The EASE base cluster was attached to the side of

the payload bay. Since the shuttle bay doors were not included in the mockup, BAT could

approach the base cluster from outboard and below. Beam and cluster supplies were

40

simulated by a rod attached to the other side of the mock up. Docking to this rod, BAT

would receive a beam or cluster from a support diver.

ICS was located along a walkway outside the tank. The operator had no direct view of

BAT. Together with the video from the cameras onboard BAT, ICS was also equipped

with video from cameras on the bulkhead of the payload bay and on the side of the tank.

These external views were very useful for planning longer flying trajectories and for

operator orientation.

Due to limitations on battery life and pressurization and pneumatic air supplies, BAT

was able to operate for one hour at a time, insufficient for a full assembly of the EASE

structure. Instead, the steps toward the completion were done separately and repeatedly

over several runs. Video tape data from these runs were then used to determine a time for

full structure assembly.

5.1.2 Times for assembly and proportions

Assembly of the Ease structure was divided into four major steps, each repeated three

times. Each of the major steps were then divided into three or four tasks and each task

divided into up to 16 primitive tasks. The full break down is listed in appendix V. The

major tasks are

1. Attach a cluster to a beam in the beam rack.

2. Attach an upright beam to the base cluster.

3. Attach a cross beam to a top cluster.

4. Complete a triangle joint. Attach the free end of a cross beam to the appropriate top

cluster. Structural constraints make the beam manipulation more difficult in this task.

Each major task is made up of four primitive task types. These are flying and docking,

beam retrieval, video switching, and joint assembly.

Using these divisions, the first full assembly time for the EASE structure was found to

be 89 minutes, slightly longer than the 71 minutes for the first neutral buoyancy assembly

41

by a pressure suited subject. The first BAT assembly was divided into 36% flying and

docking, 4% beam retrieval, 4% video switching, and 56% joint assembly.

5.1.3 Comments on Fatigue

The high percentage of joint assembly time prompted the development of Triad with the

intent of lowering the workload of the operator. One hour seemed to be the limit on

continuous teleoperation before the operator began to fatigue significantly. With this figure

it seems improbable that if BAT itself were capable of carrying sufficient consumables for

the hour and a half necessary for a end to end manual assembly, that the operator would be

capable of completing all 12 major tasks in one sitting. Decreased workload is another

driving force behind the study of automation of whatever portion of the assembly is

possible.

5.2 Results of NB-67C

5.2.1 Test setup and procedure

In January of 1988, during test NB-67C, BAT went back to MSFC-NBS for the

purpose of assembling the EASE structure with the addition of supervisory control.

This time the setup was changed to attempt a more realistic configuration. The

configuration used during STS flight 61B held the beams in a vertical rack with the clusters

placed on top. Since BAT requires more elbow room, and to avoid getting tangled up in

the payload bay, the beams and cluster were held in horizontal racks at the level of the top

of the payload bay. The base cluster was placed on the same pallet at the center of the

payload bay. See figure 5.2.

This set up is a more accurate approximation of an actual flight configuration. The

extensive use of divers in the NB-67 set up was eliminated.

Due to mechanical problems, it was not possible to complete the entire EASE structure

at MSFC-NBS. Repeated times for each task were recorded. Times for many of the tasks

involving Triad use were verified at the M.I.T. Alumni Swimming Pool.

42

Figure 5.2 NB-67C Structural assembly setup

The procedure again contained four major tasks as follows:

1. Attach a cluster to a beam in the beam rack. This involves docking to one of the

grappling points on the cluster rack with the left claw, retrieving a cluster with the right

arm, flying to the next available beam in the beam rack, docking and attaching the cluster to

the beam end.

2. Attach an upright beam to the base cluster. Once the clusters are attached to the first

three beams, the first two beams are attached to the base cluster as uprights. This involves

flying to the beam rack, docking to the next available grappling point, retrieving a beam

from the rack with the beam carrier, flying over to the base cluster, docking, and attaching

the beam to the base cluster.

3. Attach a cross beam to a top cluster. Once two riser beams are attached, the clusters

on their free ends are available. Between these, a cross beam is attached. This involves the

same procedure as in major task 2, except that the beam is attached to a top cluster instead

of the base cluster.

43

4. Complete a triangle. This involves completing the second connection to a cross

beam and immediately follows attaching the first connection to the cross beam. BAT

translates along the cross beam, docks to the free end of the beam and maneuvers it close to

the cluster. The operator then uses the right arm to grab the top cluster and bring it in to

make the joint.

5.2.2 Mechanical setbacks

The PID control system, in the process of tightening up the response of the arm, also

increases the wear. The point to point arm control system is very discontinuous in

acceleration. As each new command comes in, the duty cycle changes instantly, changing

the torque on the motor and giving a jerk to the ann. In comparison, the P-type motions

were more smooth due to the slow and linear change in duty cycle with respect to command

position error.

As a result, during the testing at MSFC-NBS, several of the arm components

malfunctioned, bringing testing to a sudden halt. During the course of NB-67C, one of the

wrist encoders failed due to moisture, the elbow bearings occasionally froze, the shoulder

pitch motor coil melted, and a brush on the shoulder yaw motor failed. All of these

problems were repairable. If, indeed, they had occurred simultaneously, they could have

been dealt with without loss of too much testing time. They were not simultaneous,

however. The more the arm was run using the high PID gains of Triad, the more

breakdowns occurred. Only after three weeks of overhaul back at M.I.T. was the arm

completely functional again.

5.2.3 Estimation of Triad assembly times and proportions

Using the task component times developed during NB-67C and subsequent pool tests

at M.I.T., an assembly time of 72 minutes was established. Considering that the operator

did not have extensive experience with the full EASE structure, the assembly can be

considered a first assembly and is comparable to the naive pressure suited subject time of

44

71 minutes. The time spent on assembly was broken down to 51% flying and docking,

5% beam retrieval, 5% video switching, and 39% joint assembly.

5.3 Comparison of NB-67 and NB-67C data

5.3.1 Direct comparison of assembly times

There are two major differences between the assemblies in NB-67 and NB-67C. One

is the difference in setup, leading to flying times that are not directly comparable. The other

difference is, of course, the joint assembly (manual or Triad). These tasks are exactly the

same in result, and can thereby be compared directly. Table 5.1 shows the direct

comparison of NB-67 assembly times and those of NB-67C.

EASE Assembly BreakdownManualTriad Major Task (times in seconds)NB-67 NB-67C Task Component393 177 1. Attach a Cluster to a Beam in the Beam Rack97 46 A.Fly to cluster rack (from free beam end)62 31 B. Get cluster from cluster rack90 46 C. Fly to beam end

144 54 D. Attach cluster to beam522 472 2. Attach an Upright Beam to the Base Cluster56 103 A. Fly to beam rack (from cluster rack, base cluster, top cluster)41 38 B. Load a beam in the beam carrier

109 180 C. Fly to the base cluster316 151 D. Assembly beam to base cluster442 433 3. Attach a Cross Beam to a Top Cluster46 148 A. Fly to beam rack (from base cluster or top cluster)34 37 B. Load beam in beam carrier

116 110 C. Fly to proper top cluster246 138 D.Assemble beam to top cluster427 364 4. Complete a Triangle Joint

91 81 A. Fly to the free end of the beam32 18 B. Fly beam end close to cluster

304 265 C. Assembly joint53524338 Total Assembly Time

89.2 72.3 minutes

Breakdown (times in minutes)31.9 36.6 1. Fly and Dock3.8 3.7 2. Beam Retrieval (from rack)3.5 3.4 3. Video Switching

50.2 28.5 4. Joint AssemblyTable 5.1 Direct comparison of assembly times

45

The flying times differ by nearly five minutes overall. The significance of this will be

discussed in the next section. The beam manipulation and video switching times were

nearly the same. The big difference, as one might expect, is in joint assembly, where the

Triad time shows a 43% reduction from the manual time. Again, the Triad assembly still

includes the manual setting of the rocker, the most time consuming portion of the

assembly. This shows a significant advantage to supervisory control, even in this primitive

application.

5.3.2 Weighted comparison

The setup for NB-67 was very different from that of NB-67C. This leads to different

proportions in flying times. The flying times during NB-67 do not have a defined

beginning point, only a defined end point. When getting the beam, the operator would

dock to the rod shown in figure 5. 1, or receive the beam from a diver at an undefined

location. The position of the base cluster was not constant, changing position depending

on the other testing being performed at the time. In NB-67C, the process is exactly the

same each time, starting at the same beam rack grappling point or the same top cluster.

This is a more realistic simulation of an orbital assembly.

For the purpose of better comparing the overall times, table 5.2 shows the NB-67 joint

assembly with NB-67C flying times. It is important to note that the operators during both

test doubled or tripled their operation experience during the test. It is easy to accept that an

operator with more experience free flying open loop could easily lower these times.

46

AmalgamationManual Triad Major Task (times in seconds)NB-67C NB-67C Task Component298 177 1. Attach a Cluster to a Beam in the Beam Rack46 46 A.Fly to cluster rack (from free beam end)62 31 B. Get cluster from cluster rack46 46 C. Fly to beam end

144 54 D. Attach cluster to beam637 472 2. Attach an Upright Beam to the Base Cluster103 103 A. Fly to beam rack (from cluster rack, base cluster, top cluster)

38 38 B. Load a beam in the beam carrier180 180 C. Fly to the base cluster316 151 D. Assembly beam to base cluster541 433 3. Attach a Cross Beam to a Top Cluster148 148 A. Fly to beam rack (from base cluster or top cluster)37 37 B. Load beam in beam carrier

110 110 C. Fly to proper top cluster246 138 D.Assemble beam to top cluster403 364 4. Complete a Triangle Joint

81 81 A. Fly to the free end of the beam18 18 B. Fly beam end close to cluster

304 265 C. Assembly joint56374338 Total Assembly Time

94.0 72.3 minutesBreakdown (times in minutes)

36.6 36.6 1. Fly and Dock3.8 3.8 2. Beam Retrieval (from rack)3.5 3.5 3. Video Switching

50.2 28.5 4. Joint Assembly

Table 5.2 Comparison of manual and Triad assembly times with NB-67C flying

From Table 5.2, it is clear that the addition of Triad alone has lowered the estimated

total assembly time by 23%, and the joint assembly time specifically by 43%.

5.4 Limitations of the present configuration

Triad, by comparison with other supervisory control methods, is an automation of only

a limited portion of the assembly process. The present hardware and sensor configuration

of BAT limits what can be done. These limits will be discussed in detail in chapter 6.

47

6. CONCLUSIONS AND RECOMMENDATIONS

6.1 Conclusions

Teleoperation is needed for task completion in dangerous environments. Supervisory

control extends the abilities of manual control for better repeatability, speed and duration.

A method was presented for applying a task primitive supervisory control to the Beam

Assembly Teleoperator, and compared with manual control modes. The supervisory

routines, known as Triad, allowed the operator to relinquish control of BAT's dextrous

arm to the computer for several portions of the joint assembly.

Manual and Triad enhanced control were compared by repeated assembly of the EASE

joint by three operators of varied experience, and by assembly of the full EASE structure.

Various types of manual control were also used. A PD-type manual control system proved

best for the operator experienced in using a P-type control system. Command inputs by the

experienced operator were shown to contain a higher order control strategy to optimize the

performance of the PD control. A PID-type manual control system reduced assembly times

for the less experienced operator by 20% due largely to improved tracking. The

experienced operator's times, however increased by 80% from the PD- to the PID-type

system, attributable mainly to unlearning of previous control techniques.

The addition of Triad produced a decrease of 80% average assembly time for the naive

operator and 30% for the less experienced operator. Half of the less experienced operator's

times under Triad equaled or exceeded the experienced operator's fastest manual time. For

the portions of assembly controlled by Triad, average assembly times were nearly the same

for the less experienced and the naive operators, showing an independence with respect to

training.

For full EASE structural assembly, Triad produced an estimated full assembly time of

72 minutes, 20% faster than the previously established manual time of 98 minutes, despite

48

increased times for flying and docking. The times for the portions assembly under Triad

control were decreased 43% from the corresponding manual times.

6.2 The future of BAT

The purpose of BAT as set out in Eric Shain's thesis [10] is to extend the Space

Systems Lab's work in the area of human assembly of simulated space structures in the

direction of man-machine systems. BAT was designed specifically for assembly of the

EASE structure. Productivity results could then be directly compared with those produced

by manual assembly. Shain envisioned a more dextrous arm, stereo vision systems, and

innovative control schemes in the future.

One area yet to be explored is that of coordinated control of the body of BAT and its

arm together. The need to coordinate is inherent in a zero gravity environment. In

manipulation of some object of finite mass, the base of the teleoperator will also move,

causing errors. John Spofford in [11] and Craig Carignan in [12] deal with this problem in

detail. In the case of BAT, the manipulator arm is missing a degree of freedom in rotation.

Coordinated control would allow for the use of the propulsion base to make up for this lost

ability.

Presently, BAT gets around the problem by docking to some object defined as fixed

and manipulating objects in a reference framed based on that object. In space, there are

instances in which this will be unacceptable. One example is that of completing a triangle

in the construction of an EASE structure. In order to complete a triangle, the operator

docked to the top beam and flew it over to upright beam. On arrival, he maneuvered the

base of BAT to align the cluster and the beam, in some cases actually moving the entire

EASE structure. Only after the beam and cluster had been properly aligned and the cluster

brought into the range of the right arm, could the operator power up the arm and complete

the task.

49

Herb Viggh, in [5], tried several methods of combining control of the propulsion base

and the arm at the same time, a total of eleven degrees of freedom. Using the head

controller for rotation, he controlled three DOF with his head, one hand on the translation

joystick, and the other in the master arm controlling the right arm. In another mode, he

used a finger switch to use the master arm alternately to control the right arm and the

rotational DOF of the propulsion base. With enough repetition these modes might have

become natural to the operator, but the biggest barrier was that the propulsion system and

the right arm positioning were independent. Using coordinated control, the operator need

only be concerned with the position of end effector and allow the control systems to figure

out what combination of propellers and servos are required to get it there. The barrier

between BAT and coordinated control is a lack of sensor capability. The propulsion

system currently runs open loop, without feedback. The Three Dimensional Attitude

Positioning System, 3DAPS, under development by the SSL, shows promise for

providing absolute positioning information.

Improvement of the right arm control involves modeling. Due to water drag, water

currents, motor damping, backlash, encoder noise and non-linear duty cycle to torque ratio,

modeling would be quite difficult. Implementation of a model based control system would

be facilitated by joint velocity feedback and determination of joint torque.

Another direction is that of compliance control. With the addition of force sensors to

the wrist of the right arm, elements of force control could be added. This would be

especially advantageous for joint completion. A force controller could slide the beam end

and mushroom together and control the force necessary to set the capture rocker.

The addition of proximity sensors in each of the claws would increase the information

available to a program such as Triad. Many of the errors possible with Triad could be

avoided if the system knew whether the right claw was approaching a target or whether the

beam carrier actually held a beam to be retrieved.

50

Considering these possible directions of research, the preceding chapters should be

considered a report on the progress of the Beam Assembly Teleoperator, not its

completion.

51

REFERENCES[1] Akin,D.L., Minsky,M.L.,Theil,E.D.,Kurtzman,C.R.Space Applications of

Automation, Robotics and Machine Intelligence (ARIMIS) - Phase II, NASA CR 3734,1983.

[2] Sheridan,T.B.,Supervisory Control of Remote Manipulators, Vehicles, andDynamic Processes: Experiments in Command and Display Aiding. Cambridge: Man-Machine Systems Laboratory Report, 1983.

[3] Akin, D.L., et al., EASE Final Report, in preparation.

[4]Spofford, J.R., Beam Assembly Teleoperator: Coordinates and Kinematics. MITSpace Systems Laboratory Report #6-87, 1987.

[5] Viggh, H.E.M, Artificial Intelligence Applications in Teleoperated RoboticAssembly of the EASE Space Structure, M.S. Thesis, M.I.T., 1987.

[6] Brady, M. "Trajectory Planning", Robot Motion: Planning and Control,Cambridge: MIT Press, 1982, pp.2 2 1 -2 4 3 .

[7] Marra, A., Human Operator Performance in Dynamic Multi-Task Environments,M.S. Thesis, M.I.T., 1988.

[8] Astrom, K.J., and Wittenmark, B, Computer Controlled Systems, Theory andDesign, Prentice-Hall, 1984.

[9] Ziegler, J.G., and Nichols, N.B., "Optimal Settings for Automatic Controllers,"Trans. ASME, v64, pp759-768, 1942.

[10] Shain, E.B., "Design and Control of a Beam Assembly Teleoperator ", M.S.Thesis, M.I.T., 1983.

[11] Spofford, J.R., "Coordinated Control of a Free-Flying Teleoperator ",PhDThesis, M.I.T., 1988.

[12] Carignan, C.R., "Control Strategies for Manipulating Payloads in Weightlessnesswith a Free-Flying Robot ", ScD Thesis, M.I.T., 1987.

52

APPENDIX I. BAT mobility using RMS

In January of this year, during test NB-67C, an opportunity arose to use the neutral

buoyancy version of the shuttle Remote Manipulator System (RMS) for BAT's mobility.

The back side of BAT was attached to the RMS end effector and BAT's thrusters were

disabled.

During the short test, the RMS operator maneuvered BAT to dock to a cross beam of a

completed EASE tetrahedron, then translate along the beam and dock to the other end of the

same beam. The first docking took 19 minutes. The translation and second docking took 7

minutes. These are compared with average free flying times of 1.5 minutes for each

maneuver.

The operating conditions were difficult. The RMS operator at the MSFC-NBS is

stationed on the second level on the outside of the tank with only a view through a porthole

to work with. Distortion through the glass and a working distance of lOm made depth

perception difficult. The ICS operator, with the assistance of BAT's belly and tilt and pan

stereo cameras, gave directions to the RMS operator in vehicle coordinates. The operator

then figured out which joints to move to produce the cartesian movement. The mechanical

condition of the RMS was also not perfect. Potentiometers controlling several of the joints

of the RMS were not steady, making station keeping non-trivial.

Still, the demonstration was successful. The RMS positioned BAT sufficiently for

docking.

There are advantages to using an arm for base mobility. Since the RMS is mounted to a

sufficiently large mass, station keeping and reaction forces during manipulation are not as

important. BAT can swing a beam without the beam and BAT swinging toward each

other. A by-product is that BAT need not dock to any other fixed target in order to interact

53

with it. This does assume an arm that provides rigid positioning, not completely true for

the RMS.

This particular demonstration warrants repetition under better conditions. First, the

RMS controls at MSFC-NBS are two 3DOF hand controllers, with each DOF directly

controlling one joint of the RMS in rate. Using a resolved rate joystick control has been

shown to be much more productivel in manipulation. The key is to make the control of the

RMS as transparent as possible. If the operator is working from the vehicle cameras, the

control system should take input in vehicle coordinates. Second, the RMS operator should

have a good view of the scene, such as shuttle bay bulkhead cameras, RMS cameras, and

vehicle cameras, perhaps under the control of the vehicle operator. A stereo image is more

helpful than a single image2. The sum of this is that the operator of the teleoperator should

not feel the difference between free flight and RMS positioning, except for the advantages

in the increased station keeping inertia afforded by the anchoring of the RMS.

1Poulton, E.C., Tracking Skill and Manual Control, New York: Academic Press, 1974.

2 Cole, R.E., Pepper, R.L., and Pinz, B.E., The Influence of Head Movement Parallax on Perceptual

Performance Under Direct and TV-Displayed Conditions. San Diego NOSC Technical Report #678, May,

1981.

54

APPENDIX II. BATSOFT/ - ---------------------------- --/* The following is a listing of the PC portion of the BAT software.** It is a bit jumbled due to the number of authors, but rve tried to** strearmlirl it as much as possible. Nonfunctional subroutine have** been commented out, but are keep as a record. TRIADl.c and** TRIAD2c, found at the end, are entirely my own. DEA 3/88 *//* --------------------- ----- - - ---------

/* -- BATSOFT.c -- Main Programs for BAT operation. *//* Requires support files as follows: *//* SUPPORT1, SUPPORT2, SUPPORT3, SUPPORT4, SUPPORTS, DATERS, PCIO** Triadl, andTriad2 *//* Beam Assembly Teleoperator Controller

version 6.4October 1987

(revisions: Finger Switch access to Triad)John Spofford, Tom Waschura, Tony Manganiello, Herbert Viggh

& David E. AndersonSpace Systems Laboratory

Massachusetts Institute of Technology */

#include "DAT.H" / defines data logging */#include "PORTNUMSH" /* defines io ports & commands */#defineRABKDZ 8 /* deadzone for backup arm pots */#define RASTDZ 24 /* dz on stick control */#define SWITCH_TC 12 /* 13 sec lockout at 9 samples/sec */#define ACTCOLS 17 /* display actuator columns in screen */

/* -------------------------------------------------------------

int testing; /* flag for development */mainO

extm int testing;char tempc;

scrdr0;initialize_bat();calibrate_bat();screen();run bat();shutdown_bat();

/* --------------------------------------

/* initialize_bat -- set up io ports, set initial values,** */

initialize_batO

inti;

scr_rowcol(23,0);printf("Initializing\n");batports();

outport(P4C,0); /initialize video switching - inactive since v5.4*/outport(POC,OxB8); /* cs high, rnaster&puma stb hi *1outport(POA,0);outport(P1B,0);outport(P1C,OXC0);/* 11000000 spare pue high default /outport(P2C,0xlF); /* 00011111 JM-STB high, all pnuematics default */outport(P3A,0);outport(P3B,0);outport(P3C,O0);outport(POX,C6HI); /* reset JCS */outport(P2X,C7HI); /* reset propulsion controller */for (i = 0; i < 20; i++); /* pulse width */outport(POX,C6LO);outport(P2X,C7LO);for (i= 0; i< 36; i++)

synchronizeO; /* let JCS check memory */

printf("Loading JCS software\n");if (load_control_system0)

printf('n%cError loading control system software\n",7);shutdown_bat();exit(0);

start_logO; /* init data recording buffers */

/*----------------------- ----- SI--------------/* calibrate_bat --** */

/* triad preset variables */int funmelspeed;char triad_data;/* ------------------------ /

55

int stickoff[6];int rbkoff[5],masoff[5]; /* backup & master offsets */float rbkmul[5],masmul[5]; /* backup & master multipliers *//*This has been added to the current batsoft for the purposeof slaving thbe tilt and pan unit to the head controller.The head controller has five pots from which canpitch. yaw,androll can be calculated. The mux assigrments are:

pot name pot label(x,y) pot#

top yaw 0,5 1top pitch 0,6 2roll 10,0 3botpitch 11,0 4botyaw 12.0 5

tilt = cpitch = bot pitch - top pitchpan = cyaw = bot yaw + top yawroll = roll HV 2/6/85 */

intpotlabelx[5] = 0 0 101112 };int potlabely[5] = { 5 6 0 0 0 };int headoff[5]Ahpot[5];float cpit-mult = 1.0;float cyawmult = 1.0;int cpitchcyaw;int wra_off,wrb_offelb_off;float p_mult,rmult,ymult; /* arm flying */float ppit_mult,pyaw_mult,prol_mult; /* head flying */float joymult[6];int pheadoff[5];int ppitch,pyaw,proll;

calibrate_ba)

static int maxindex[5] = ( 9, 6, 5, , 2 };static int min_index[5] = { 7, 8, 4,1, 3 };static int count_index[101 = { 6, 6, 7, 7, 5, 5, 4, 3, 4, 3 };extem int stickoff[6];extem int rbkoff[5]masofflS5]; /* backup & master offsets */extem float rbkmul[5],masmul[5]; / backup & master multps */int i, dummy, word, fp, count, tdat;,char tempc;float tmpf;float batmax[5], bat min[5];int batmp[l10];extem intmaxjcs_val[8], minjcsyval[8];extem char imvt2[8], icnvt[8];extem float align gain[8];extem int alignoffset[8];extem int headoff[5];extem float opitmult'cyaw_mult;float gain[13];extem float p_mult,rmnult,y_mult; /* arm flying */extem float ppitmult,pyawmult,prol_mult; /* head flying */externm int wra_off,wrb_offelb_off;extem int pheadoff[5],h_pot[5];extem int furmelsped;extem char triad_data;

funnel_speed = 0x400;triaddata =FALSE;

scrrowcol(23,0);scr_drlO;printf("Calibrating, return joysticks to center.');for (i = 0; i <= 32000; i++)

dummy = 1 *2*3*4*5;dummy=1 * 2* 3 *4 *5;dummy=1 *2*3*4*5;

stickoff[O] = convert(0,8) & OxFFF;stickoff[l] = convert(0,10) & OxFFF;stickoff[2] = convert(0,9) & OxFFF;stickoff[3] = convert(6,0) & OxFFF;stickoff[4] = convert(7,0) & OxFFF;stickoff[5] = convert(8,0) & OxFFFP;for (i = 0; i <= 5; i++)

stickoffli] = 1024 - stickoff[i];

scr_setmode(0);if ((fp = open("BAT_SET.ARM",0)) != -1)

for (count = 0; count < 10; count++)

while((tempc = getc(fp)) !=':');fscanf(fp,"%fin' ,&tempf);battemp[count] =tempf;

for (count = 1; count < 3; count++)

while ((tempc = getc(fp)) != ':');fscanf(fp,"%fW',&tmpf);

56

maxjcsval[count] = tempfwhile ((tempc = getc(fp)) != ':');fscanf(fp,"%fn`",&trmpfO;min jcs_val[countl = tempf;

for (count = 0; count < 13; count++)

while ((tempc = getc(fp)) !=':');fscanf(p, "%f\n",&tempfO;gain[count] = tempf;

for (count = 0; count < 6; count++){while ((tempc = getc(fp)) = ':');fscanf(fp,"%fn",&tempf);joymultcount] =tempf;

close(fp);

else

scr_setmode(2);printf('n\n%c==> BAT armnn data not found <---n",7);exit(1);}

for (i=O; i<5; i++){align_gain[i+3] = gain[i];}

cpitmult = gain[5];cyawmnult = gain[6];pmult = gain[7];rmult = gain[8];ymult =gain[9];ppit_mult = gain[10];pyawmult = gain[ll];proLmult = gain[12];for (i = 3; i <8; i++)

tdat = (convert(icnvt2[i],icnvt[i]) & OxFFF) -1024;tdat = adjust(tdati);batmax[i-3] = (float) batJenmp[max-index[i-3]];batmin[i-3] = (float) bat tenp[min_index[i-31];word = (int) ((batjnax[i-3]+bat_min[i-3])/(2.0*aligngain[i]));align_offseti] = word - tdat;maxjcsval[i] = (int) batmax[i-3];minj cs_val[i]= (int) batmin[i-3];

wra_off = convert(2,0) - 1024;wrb_off = convert(1,0) -1024;elb_off = convert(3,0) -1024;

read _had();for (i=O; i<=4; i++) /* liad controller offsets

{eadoffi] =h-pot[i];pheadoffi] = h_pot[i];

/* -------------------------------------/*run_bat-- */

r The following RAM is set aside for joint positioncontrol. The arrays are used as follows:

maxp .. maximum positive positionmin_p....... aximum negative positiondiv ormul..left or right shifts?pgain..n....umber of bits to shiftprangeerr..a range error occured on that joint

Note, all are indexed from 0 to 7 which correspond todifferent joints. The actual correspondence is:

0............left arm1............camera tilt2...........camera pan3............wrist a4............wrist b5............elbow6............shoulder yaw7............shoulder pitch

These are software definable as to the addresses andcommands in iaddr and icmnd.

T.E.W. April 24,1984 */

char *potname[8] ={"Left ArmN0", "Camera TiltNO", "Camera PanNO",

57

"Wrist A\0), "WristBVY, Elbow\0","Shoulder Yaw\D', 'Shoulder Pitch\0"};

......-------------.. Lcft,CarmT,CamP, WrA, WrB,Elbow, ShY, ShP)*/char icnvt2[8]

={ 0, 0, 0, 2, 1, 3, 4, 5);char icnvt[8]

={ 2, 5, 6, 0, 0, 0, 0, 0);intmrnaxp[8]

= { 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024};int minp[81

= { -1024, -1024, -1024, -1024, -1024, -1024, -1024, -1024);int div_or_ mul[8]

= { FALSE, FALSE, FALSE, FALSE, FALSE, FALSE, FALSE, FALSE);int pgain[8]

={ 0, 0, 0, 0, 0, 0, 0, 0);int prangeerr[8]

= { TRUE, TRUE, TRUE, TRUE, TRUE, TRUE, TRUE, TRUE);static char iaddr[8]

={ 3, 4, 4, 1, 1, 3, 2, 2};static char imnd[81

= { 0x90, 0x80, 0x90, 0x80, 0x90, 0x80, 0x90, 0x80);int maxjcs_val[8]

= {0x07FF,0x0 78 , x0600,OxO7A,0x0780,0x0660,0x0140};int minjcs_val[8]

= { xF800,OxF880,0xF80,0xF880,OxFA20,xFC00,0xF960,OxFB80};int alignRoffset[8]

= { 0, 0, 0,xFE90,0x0213,0x03DF,OxOD48,0xFC06};float aligngain[8]

= { 1.0, 1.0, 1.0, 3.00, 3.00, 1.75, -4.00, 1.25);int cardorder[8]

={ 6, 7, 8, 1, 2, 5, 4, 3);/*-------------{ Left, CamT, CamP, WrA, Wr B, Elbow, ShY, ShP)*//* dataset 4/13/84 JRS */int nnf[8] = {3, 4, 7, 6, 5, 0,1, 2);int slave_toggle; /* variables for slaverest()*/int currmaster[5],curr_h_pot[5];int set_triad;int Gain[9], alpha[9], beta[9], alg[9], pos[9], multl [9], mult2[9];int joint_pos[8], jointcom[8];char comr_disp;char mdat[30], mcom[30], madd[30];char data_flag, boost_flag; /*logical flags*/char leftclaw_state, masterconnected;char head_controller, pumaon, draw, right_dclaw;char head_ctrlconnected, slave_arm_on;char masterflying, head_ctrl_flying;char beam_carrier, beamC_daw;char sparel, spare2; /*spare pnuematics toggles*/

/* successive triad counters for forward and reverse assembly */int trifor, trirev;

DATATAG cnt[TAGSIZE] = ("ALAoop counter"};DATATAG thO[TAGSIZE] = {"BIFjoystisk tbr");DATATAG thl[TAGSIZE] = "CIFtrimmed thr"};DATATAG rawmfTAGSIZE] = {"DIEraw master"});DATATAG jcomTAGSIZ] = {"ElEjoint com"};DATATAG jpos[TAGSIZE] = ("FlEjoint pos");DATATAG zcnt[TAGSIZE] = "GIAz80 loop");DATATAG adty[TAGSIZE] = {"HEarmn duty");P--------- ------------------------- 4/

runbat(

extem char comnm_dip;extemn pos[9];extem char mdat[30],mcom[30],madd[30]; /* master transmit buffer */charpdat[10];extem char cswitch[16]; /* ICS control switches */char ij,ktempc;char comrow,pos_row,thr_old_row[6] ,old_corn_row[8] ,oldposrow[8],col;char row; /*display parameters */int dvec[8][ACTCOLS+1]; /* display filter */int thrraw[6], thrJtrim[6], temp; /* x,y,z,p,r,.y */extem int maxjcsval[8], minjcsval[8];extem char icnvt2[8],icnvt[8];extern int set_triad;float spw;

int mlast,plast;int proll,p_pitchpaw,px_yaw,pxp_y,pz;int tdat,actdat[8];int a_syaw,a_spit,a_elbo,a_wrol,a_wyaw,aJeft;extem int testing;

extem char dataflag; /* logical flags */extem char left_claw_state, master_connected;extem char headcontroller, puma-on, draw, rightLaw;extem char headctrl_conncted, slave_arm_on;extem char masteriflying, headctrl flying;extem char beam_carrier, beamcdaw;extem char keep[MAXGRPNUM]; /* select variables */int rawmaster[5];

/* puma transmitbuffer */

/* fast loop index, temp */

/* master,puma buffer ends */

/* temp & data */

58

int posm[8];exte int joint com[8], joint_pos[8];int thr_boost[6];

int tran um;char jump, /* flag for arm jump */extem int curr_master[5];extem int stickoff[6];extem int rbkoff[5],masoff[5]; /* backup & master offsets */extem float rbkmul[51,masrmul[5]; /* backup & master mults */int counter; /* mainloops */extem int slavetoggle; /*toggle to reset slave arm gains*/extem int potlabelx[5],potlably[5]; /* head controller muxes */extem int headoff[5]; / hc offsets */extem float cpit_multcyaw_mult /* he multipliers */int extem h_pot[5];int extem cpitch,cyaw;int oldheadofflS],old_hpot[5];int loopz80, armduty[5];extem int curr hpotl5];

exte int wra_off,wrb_off clb_off;it wra_raw,wrb_raw,elb_raw;int a_adelta_b,delta_c;extem float p_multr_mult,y_mult;extem float joy_mult[6];extem ppitch,proll,pyaw;/* successive triad counters for forward and reverse assembly */extem int tri for, trirev;

for (i = 0; i < 8; i++) * aet up display filters */

spw = ((float) (maxjcs_vali]-minjcs_val[i]))/((float) ACTCOLS);dvec[i[0] = maxjcs_val[il;dvec[i][ACTCOLS+11] = minjcs_val[i];for (j = 1;j <= ACTCOLS; j++)

dveci][j] =max jcsyal[i - (int)(((float)j)* spw);

trifor = 1;tri-rev = 1;comm_disp =FALSE;puma_on = FALSE;head_controller = TRUE;draw = TRUE;dataflag = FALSE;leftdclawstate =FALSE;boost_flag = FALSE;masterconnected = TRUE;headctrl connected = TRUE;slavearm_on =TRUE;masterJlying = FALSE;head_ctrl_flying = FALSE;rightdlaw =FALSE;slavetoggle = 0;beam_carrier = TRUE;beamclaw = FALSE;sparel = FALSE;spare2 = FALSE;jump = FALSE;set_triad = 0;

outport(X,C7HI); /* reset propulsion controller */for (i = 0; i < 20; i++); /* pulse width */

outport(P2X,C7LO);for (i = O;i < 6;i++)

pdat[i] = 128; /* zero each command */tranpuma(pdat,5); /* Prevent thruster start-up */

screen2();for (i = 1; i <= 4; i++)

startjc(i); /* start card #1,2,3,4 */for (i = O;i <= 5; i++)

throld-row[i] = 13;oldcomrow[i] = 13;old..posrow[i] = 13;

old_com.row[6] = 13;old_comrow[7] = 13;old_pos_row[6] = 13;old_posrow[7] = 13;

outport(P2X,COLO); /I ACTIVATE POWER RELAY */

/* avoid randonmss */

synchrncize0;for (counter = 0; TRUE; counter++) / as long as in sync...*/

statO;ff(dataflag)

indexlog();outport(Y2X,C6HI); / --- 1* TEST lTMING: WATCH P2C6 ** */

59

/* get in sync I/

/* --- set up loop paramters */mlast =-1; /* nada in buffers *plast = -1;

check_switchesO; /* updates offsets and flags */

/* convert appropriate BAT pots,massage & transmit to master card */

if(head_ctrLconneted&& !head ctrl flying){readjhead();for (i=0; i<5; i++)

curr_h_pot[i] =h_pot[i;headcamslavel0;trantpO;

else

read hLad();for (i=0; i<5; i++)

currh_pot[i] = hpot[i];

if(maslerconnected && !maser_flying)

if (slave_toggle = 1) slaverest(2); /*reset gains if powering up*/for (i = 3; i <= 7;i++)

{raw master[i-3] = (convert(icnvt2[i],icnvt[i]) & OxFFF) -1024;curr master[i-3] = adjust(rawmaster[i-3],i);pos[cardlorder[i]] = align(currmaster[i-3],i) & OxOFFF;

tran_armO; /* transmit commnand to arm*/

else

if (slavetoggle = 1)

tran_zero; /* zero out residual commands */slaverest(2);

for (i = 3; i <= 7;i++)

rawnaster[i-3] = (convert(icnvt2[i],icnvt[i]) & OxFFF) - 1024;currmaster[i-3] = adjust(rawmaster[i-3],i);

/* display command and arm position -DEA 11/86 */if(draw)

readjoint(jointcomrjoint-pos);if(setriad = 3) leanrecord();for (i=0; i<=7; i++)

if ((joint_com[i] & 0x8000) = 0) /*negative command */{/rright shift 12 bit number and sign extend */joint_com[i] = (joint_com[i] >> 4) 1 OxF000;

if(i = 3 I i = 611 i=4) /*elb, shy, ct counter intuitive*/joint_corn[i] = -jointcomli];

else

joint-comfi] =jointcom[i] >>4;if (i = 3 11 i 611 i==4) /*elb, shy, ct counter intuitive*/

jointLcom[i] = -jointcom[i];

if ((joint pos[i] & Ox8000) = 0) /*negative position*/

posm[i] = (jointpos[i] >> 4)1 OxF000;if (i = 3 11 i = 611 i=4) /*elb, shy, ct counter intuitive*/

posm[i]= -posm[i];

else

{posm[i] = joint-pos[i] >> 4;if(i = 3 11 i = 611 i=4) /*elb, shy, ct counter intuitive*/

posm[i] = -posm[il;

corn row =0;pos_row = 0;for (j=l;j <= ACTCOLS+1; j++)

if((jointcom[i] >= dvec[nnfli]]U]) && (jointcont[i] <= dvec[nnfli]][j-1]))comrow =j+l;

if((posm[i] >= dvec[nnf[i]]Uj]) && (posm[i] <= dvc[nnrf[i]][j-1]))posrow =j+l;

col = (i << 1) + 24;if(comrn_row != old_comrrow[i])

{/*cover up old command position */scr_rowcol(old_comrow[i],col); scrco(179);oldcorn_row[i] = comrow;

60

if ((posrow & OxlF) !=(old_pos row[i] & OxlF),col){/* cover up old arm position I/

scrrowcol(old_posrow[i],col); scr_co(179);old_posrow[i] = pos_row;

if (com_row = posjow){/* command and position coincide */scr_rowcol(com_row,col); scr_co(219);}

else{/* show new cormmand and arm positions I/scrrowcol(cornow,col); scrco(233);scrrowcol(posrow,col); scr_co(4);

/* --- convert appropriate PUMA pots,massage & ransmit to PUMA controller */

thrraw[O] = -(convert(7,O) - 1024 + stickoffi4])joy_mult[0]; /*X*/tbr_raw[l] =-(convert(8,0) - 1024 + stickoff[5])*joy_mult[l]; /*Y*/thrraw[2] = -(convert(6,0) - 1024 + stickofl3])*joy_mult[2]; /fZ*/

if(masterflying)

if(mastercormncted)

wraraw = (convert(2,0) - 1024 - wra_off);wrb_raw = (convert(l,O) - 1024 - wrb_off);elb_raw = (convert(3,0) - 1024 - elb_off);delta_a = wra_raw;deltab = wrbraw;deltac = delta-a - delta_b;thrraw[3] = -(lb_raw * pmult);thrraw[4] = (deltaa -(delta_c * 0.5)) * rmult;thrraw[5] = (delta_c* 0.5) ymult;

else

thrraw[3] = 0;thr-raw[4] = 0;tbhr_raw[5] = 0;

else if(head_ctrLflying)

if (head_ctrlconnectd)

read_hbead();headpumaslavelO;tbrraw[3] = ppitch;thr_raw[4] =-proll;tbrraw[S] = -pyaw;

else

thrraw[3] = 0;thrraw[4] = 0;thbrraw[5] = 0;

else

thr_raw[3] = -(caovert(0,9) - 1024 + stickoffl2])*joy_mult[3]; /*P*/thr_raw[4] = -(convert(0,10) - 1024 + stickoffll])*joy_mult[4];/*R*/tbrmrw[5] = (convert(0,8) -1024 + stickoff0])*joy_mult[5];/*Y*/

for (i=O; i<6; i++)

if (thr_raw[i] > 1024)thrraw[i] = 1024;

else if (thrraw[i] < -1024)thrraw[i =-1024;

if(boostflag)

calc_boost(iointcomjoi ntpos,thr_boost);for (i = 0; i <= 5; i++)

thrraw[i] += thrboost[i];

imtruthrrawtrtrimthrrawthr);for(i =0; i <= 5; i++)

temp = tbrtrim[i];temp = (temp > 127) ? (127): (temp); /* limit I/temp = (temp < -127) ? (-127): (tnmp);pdat[i] = (temp + 128);

61

if(pdat[i] = OxFF)pdat[i] = OxFE;

/* DISPLAY THRUST LEVELS */if(draw)

for (i = O;i <= 5; i++)

row = 17 -(pdat[i] >> 4);if (row != throld_rowi])

col =(i<< 1) + 1;scrrowcol(row,col);scr_co(219);scr_rowcol(throld_row[il,col);scr_co(179);throldrow[i] = row;

}

if(puma_on)tran_pura(pda45);

else{for (i=O; i<=5; i++)

pdat[i] = 128;tran_purna(pdat,5);

if(comnm_disp)comm_display();

outport(P2X,C6O); /* -- ** TEST TIMING: WATCH P2C6 ** */temnpc = scr_csts();if (tempc = 0)

synchronize; /* avoid sync loss from printing */else if (tcmpc = 9)

break; /* shutdown on a tab*/else

keyboard_cornmand(tmpc); /* process */syncbronize();

if(dataflag && keep[l])logint(cnt,counter);

if(data flag && keep[2])logint_a(thO,thr_raw);

if (dataflag && keep[3])logint_a(thl ,thr trim);

if(dataflag && kecp[4])logint_a(rawmnrawmaster);

if(dataflag && keep[5])logint_a(comnjointco );

if(dataflag && keep[6])logint_a(posjoint_pos);

if(dataflag && koep[8])

arm_duty[O] = pck(OxC,OxE880);annrmuty[l] =peek(OxD,OxE880);armduty[2] = pek(OxCOxE900);annrmduty[3] = peek(OxD,OxE900);armduty[4] = pck(OxC,OxE980);loginta(adty,arnnduty);

if(data flag && kecp[7])

loopz80 = peek(0xlO,OxE900)(peexk(Oxll,OxE900) << 8);logint(zcntloopz80);

scrrowcol(10,0);printf"('%LOST SYNCHRONIZATION\n"); /* go to shutdown */

/---------------------------------------/* shutdownbat --** *lshutdown_bat()

int i;

outport(P2C.7); / DEACTIVATE POWER RELAY and claws */

outport(POX,C6Hl); /* reset JCS */outport(P2X,C7HI); /* reset propulsion controller */for (i = 0; i < 20; i++); /* pulse width /outport(POX,C6LO);outport(P2X,C7LO);scr_setmode(2);}

62

/* ------------------------------------- - ------/* This function checks all of the switches and toggles theproper logic flags and updates offsets. - HV */

int old_master[5], oldoffset[5];int old_hpot[5], oldheadoff[5];int foldheadoff[5], fold_h_pot[5];char triadfn[10][15];

check_switches()

extem char data_flag, boostflag;extem char leftclaw_state, master_connected;extem char head_controller, puma_on, draw, right_claw;extem char head ctrl conmted, slave_arm_on;extem char masterflying, headctrflying;extem char beam_carrier, beam_claw;extem char sparel, spare2; /*spare pnuematics*/extem int align_offset[8];extem int curr_naster[5],curr_h_pot[5];extem int jointpos[8];exterm int old_master[5], oldoffset[5];extem int foldmaster[5], fold_offset[5];exter int wra_off, wrb_off, elb_off;extem int hpot[5], headofi[5], pheadoff[5];extem int old_headoflf5], old_bhpot[5];extem int set_triad;extern char triadf n[0][15]; /*list of file naxes for triad*/int i,dummy;extem char cswitch[8];extem int fold headoff[5],foldh_pot[5];extem int tri_for, tri_rev; /* for successive triad use */read_switches(; /* --- check ICS switches */

if (cswitch[0]) /-RED-*/

shutdownbat0;exit(2);

if (cswitch[1])/* start/stop recording data */

if(settriad=0)

dataflag = !dataflag;co(7);sceen2();

else if(setjriad=l)

co(7);poinLtopoint(;

else if(settriad>=2)

co(7);learnO;

if(cswitch[9]) /*-BLE-*/{ /* thruster toggle *pumaon = !puma_on;co(7);scren2();

/*--------------------------Removed for triad switching-------- *//I if(master flying)

if(cswitch[7]) * TURN OFF *

masterflying = FALSE;screen2();co();if (master_connected)

if(slave_armnn){slaverest(l); realign master arm with slave arm *slave_toggle =0; * keep slaverest from reloading gains*resetpidO;

elsefor (i=0; i<5; i++)

align_offset[i+3] = oldoffset[i] + oldmaster[i -currmaster[i];

else *STILL ON*

else

if(cswitch[7]) * TURN ON *

63

tasterjlying = TRUE;scrn2();co(7);wraoff = covert(2,0) - 1024;wrboff = ccnvert(1,0) - 1024;elboff = convert(3,0) - 1024;if (asterconnectd)

for (i=O; i<5; i++)

old_offset(i] = alignoffet[i+3];oldnaster[iJ =curr_rnaster[i];}

else *STILL OFF

if(headctrlflying)

if(cswitch[5]) * TURN OFF *

head_ctr_flying = FALSE;if (head_ctrl_conected)

for (i=0; i<5; i++)

hcadofflil=fold_hadofflil+currh_otfi]-foldhpot[i];

else

for(i=0; id; i++){old_headoff[il = foldhadoffi];oldh-pot[i] = foldhpot[i];

co(7);scren20;

else *STILL ON*

else

if(cswitch[5]) * TURN ON *{hadctrlflying = TRUE;read had;for (i=0; i<5; i++)

p_headofftil = hpot[i];if (head_ctrl_connected)

for (i=O; i<5; i++)

foldheadoffli] = headofi];fold3hpot[i] = currhpot[i];

else

for (i=O; i<5; i++)

foldheadoff[il = oldheadoff[i];fold_hpot[i] = old_h_pot[i];

co(7);screen20;

else * STILL OFF *

/*switching for easy triad acccess DEA*/if(cswitch[2]) /*initite trajectory cgrab */

strcpy(triadfi[0],"cgrab.trd");srcpy(triadfn[1 ],"0");pointtopoint();

if(cswitch[4]) /*take arm from stow to general area of clustcr */

strcpy(triadf[0],"ctri.trd");strcpy(triadfn[],"0");point_to_pointO;}

64

if(cswitch[5]) /*index finger push does successivc forward assembly*/

if(tri_for = 1){strcpy(triadfn[O],"grabbeantrd");strcpy(triadf[ 1l],"joint.trd");strcpy(triadf2n[21,0");point_to_point();scr_rowcol(23,16); puts("Grabslv ");

if(trifr = 2)

strcpy(triadfn[O] ,"grabslv.trd");strcpy(triadfn[l],"slideslv.trd");strcpy(triadfn[2],"0");point_to_point(;scr_rowcol(23,16); puts("Stow ");

if(trifor = 3)

srcpy(triadfn[O],stow.trd");strcpy(triadfn[l],"0");pointtopoint;_to_scrrowcol(23,16); puts("Grabbeam ");

trifor = trifor + 1;if(tri_for = 4) tri_for = 1;

if(cswitch[71)

if(trirev = 1){strcpy(triadfn[O] ,"cjoint.trd");strcpy(triadfn[l],O"');point_to_point();scrrowcol(23,25); puts("Rgrabslv ");

if(trirov = 2)

strcpy(triadfn[O] ,"rgrabslv.trd");strcpy(triadfn[ 1 ] "rsldslv.trd");strcpy(triadfn[2],O");pointtopoint();scr_rowcol(23,25); puts("Rstow ");

if(trirev =3)

strcpy(triadf[O] ,"rstow.trd");strcpy(triadn[l ],"O");pointto_point();scrrowcol(23,25); puts("Cjoint ");

trirev = trirev + 1;if(trirev = 4) trLrev = 1;

if (cswitch[3]) / incrernnt next up triads*/

trifor = trifor + 1;if(trifor = 4) trifor = 1;tri_ev =tri_rev + 1;if(trirev = 4) ti rv = 1;scren2(;}

/*8_--------------------------------------if(cswitch[11]) / toggle right claw */

{rightclaw = !rightclaw;/*------for default right claw --------if (rightclaw)

outport(P2X.C2LO);else

outport(P2X,C2-I);-------replace with pnue 1-----------*/if(rightclaw)

outport(PlX,C6LO);else

outport(PlX,C6HII);co(7);screen2();

if (cswitch[8]11 cswitch[l5) /*left claw toggle(was p2cl,now using plc7)*/

leftclaw_state = !leftclawstate;if (leftclawstate)

outport(PlX, C7LO);else

outport(PlX, C7HI);co(7);screen2();}

65

if(masterconnrmcted)

if(cswitch[6]) /* DISCONNECT i/

masterconnected = FALSE;scr2;co(7);if(!maserjflying)

for (i=0; id; i++)

old_offset[i] = align_offset[i+3];old_master[i = currmaster[i];

else /* still cormected /

else

{if(cswitch[6]) / CONNECT */

master_connectcd =TRUE;screen20;co(7);if(masterflying)

wraoff= convert(2,0) - 1024;wrb_off = cnvert(l,0) - 1024;elb_off= convert(3,0) - 1024;

if(!master_flying)

if(slave_armon)

slaverest(l);J*realign master with slave*/slavetoggle = Of;*keep slaverest from reloading gains */resetpid();

if(!slavearmon)for (i=O; id; i++)

align_offset[i+3] = old_offset[i] + old_master[i] - curr master[i];

else /* still disconnected I/{

if(cswitch[10]) /*slave toggle switch*/

if (slave_armeon)slaverest(0);

elseslaverest(l);

coC7);

if (hadctrlconrzctd)

if(cswitch[13]) /* DISCONNECT */

ead_ctrlconncted = FALSE;for (i=0; id; i++)

old headoffli] = headoff[i];co(7);screen2t;

else /* STILL CONNECTED */{for (i=0; id; i++)

old_h_pot[i] c ;urhpot[i;

else

if(cswitch[13]) /* CONNECT */

head ctrl conncted =TRUE;for (i=0; id; i++)

headoftli] = old_hcadofffi]+currhpot[i]-old_h_pot[i];readhead;for (i=0; id; i++)

pheadoffti] = hpot[i];co(7);scrcn2);

else /* STILL DISCONNECTED */

}

66

if(cswitch[12) /*ogW beam carir*/

beamcmrier = lbeamar r,

outport(P2X,C3HI); /xxld bean m carr/

else

outpo(P2XC3LO); /etract beam carrir*/

co(7);scmn20;

if(cswitch[14]) /togle beam carir claw (was also cswitch[3])*/

beamclaw = !beamdaw;if(beamclaw)

outpor(P2XC4LO); dclose bam carrier claw/

elm

outpot(PXC4H ); /openbeam carrier daw*/

co(7);scmen2;}

if (cswitch[]) *spara mmatics 1Ispael = Isparol;if(apamnl)

oatporPX,C6LO);

else

outpol(X,C6HI);

co(7);

if (cswith[) *spam pmnumaics 2*

spae2 = sp-a2;if(spam2)

aotportP1X.C7LO):

eLa

outporn(PlX,C7HI);

co(7);

if(cswih[])stowO;

*/

*acuate spare pnuematics 1'

/* End of Batsoft.c */

67

.1

/* SUPPORT1.C -- BAT support routines version 6.4** by John Spofford and Tom Waschura and David E. Anderson,** M.I.T. Space Systemrns Laboratory December 4,1987** revisions include Joint step input routine */#include "PORTNUMS.H"#include "DATH"#define READ CNT 21/* -------------------- *//* FEEDBACK.C -- interacts with the BAT/joint card/master card loop *feedback()

{int not_done, selcctioni;

not_done =TRUE;while (not_done)

selection = getsel2(; * light pen soon? *switch(selection)

{case 1:case 2.case 3:case 4:

shut_card_off(selection);work_card(selection);beak;

case 0:not_done = FALSE;breal

case 9:testroutine0;bseak;

defaultbeep4(25);scr_rowcol(l,1);printf("lnvalid Selection');pause(100);

shut_card_off(l);shut_card_off(2);shut_card_off(3);shut card off(4);

beep4(n)int n;{int i;

outport(0x61,0x43);for (i=0;i<n*25;i++);outport(0x61,0x40);retur(0);

paint_menu_3()

scr_setmode(0);scrdr0o;scrrowcol(2,0);printf(" M.I.T. SSL\n");printf(" Beam Assembly Teleoperatoh\n");printf(" Hierarchical Control Evaluation\nhnu');printf(" 1. Wrist A + B\n");printf(" 2. Shoulder Pitch + Yaw\n");printf(" 3. Elbow + Left \n");printf(" 4. Camera Tilt + Pan\n");printf(" 0. EXIT\n");printf(" 9. Debugging test routine");scr_rowcol(22,0);printf(" Enter Selection ");retum(0);

a {-int query;

paint menu3();scrrowcol(22,19);printf(" ");scrrowcol(22,19);scanf("%d,&query);

retum(query);

char read_buffer[40];

work_card(card_addr)int card_addr;

68

extern char read buffer[];int i, posx, posy;

scrcursoffO;paintnenu4();scrrowcol(3,0);switch(cardaddr)

case 1: printf(" Wrist A Wrist B); break;case 2: printf(" Shoulder Pitch Yaw"); break;case 3: printfl" Elbow Left Arm"); break;case 4: printf" Camera Tilt Pan"); break;default prinrf"ERROR");

while( !(scr_cstsO = 13)) * unless CR *

rqstmstr(card_addr);read_mast(READ_CNT);

* posx = read_buffer[0] + read_buffer[l *256;posy = readbuffer[2] + read_buffer[3]*256;scrrowcol(14,10);printf("%04x %04x"); *

scr_rowcol(14,0);for (i = 0;i <READCNT;i++)

printf(" %02x",rea_buffer[i]);while( scr_csts( = 32); * stop a while *

read_mast(cnt)int cnt;

extern char readbuffer[];char i, waitc;

for(i= 0; i < cnt; i++)

for (waitc = 0;!(inport(P4B) & 1);waitc++) * JM-RDY hi *if (waitc> 250)

printf"R");break; * timnout *

read bufferlil = inport(P4A);outport(P2X, CSHI); * JM-STB high *for (waitc = 0;inport(P4B) & 1;waitc++) * JM-RDY hi *

if (waitc > 200){printf"r");break; * tirneout *

outport(P2X, CSLO); * JM-STB low *

paint_nenu_40

scrcr0;scrrowcol(0,0);printfi"CARD:\n');printf(" Channl X Y \n");printf(" \n");printf(" n");printf(" Gain\n");printf(" Ref Posn");printf("Joint Pos\n");

shut_cardoff(card)int card;

unsigned jcseg;

jcseg = SEGOFF + Ox80 * card;poke('S',COMM_FLAG,j_seg);while (peek(JCSTATUSjcsog) != W')

scrcsts(); * no hangup *poke('G.,COMM_FLAGjc_seg);while (peek(JC_STATUSjc_seg) != 'C')

scrcsts(); * no hangup *

char tenp_ch;*-----------------*

testroutine(

char strobe, ready_var, jnd_data, buffer[150], i;extem char ternmpch;

69

scrcldr0;scr rowcol(0,0);printf("l= STB hi, 2= STB lo, 3= log, 4= conftnXa");printf"RDY STB DATAn');while(l)

{tempch = scr_csts();switch (temp_ch)

{case '1': strobe = 1;

outport(P2X,CSHI); * jm-stb hi *blealq

case '2: strobe = 0;outport(P2X,C5LO); * jm-stb lo *break;

case '3': * read 150 bytes *outport(P2X,C5LO);for (i = O;i < 150;i++)

buffer[i] = readjm(; * get data *scr-rowcol(5,0);for (i = O;i < 150;i++)

printf(" %02x",buffer[i]);while(!scrcstsO);

case '4': * cont. read *outport(P2X,C5LO);while(!scrcstsO)

#asm; ASM88 *

Modified READJM.C for speed

CSEG

mov dx,311h ,port 4Bin al,dx ;get datamov ahO ;char valueand ax,lcmp ax,0Ojnz a_12jmp all

a_12:mov dx,310h ;port4Ain al,dx ;get datamov temp_ch-al ;retum in tempchmov ax,l 1 ;C5 HImov dx,30Bh ;Port 2Xout dx,al ;put it out..

a_3:mov dx,311h ;Port 4Bin al,dx ;get daamov ah40 ;char valueand ax,lcmp ax,Ojz a14jmp aJ_3

a14:mov ax,l0 ;CSLOmov dx,30Bh ;Port 2Xout dx,al ;put it out..

case 13: retum;default break;

scr_rowcol(3,0);readyvar = (inport(P4B) & 1); * JM-RDY *jmd_data = inport(P4A); * JMData printf("%2x %2x %02x",ready var,strobejmd_data);

*-*------------

* rcadjrmc -- reads joint control system master card *a produces assembly code *readjm(

char tempc;

while(!(inport(P4B) & 1)); * wait for rdy *tempc = inport(P4A); * get data *outport(P2X,CSHI); jm-stb hi *while(inport(P4B) & 1); * wait for rdy low *outport(P2X,CSLO); * jm-stb lo *retum(tempc);

-------------------- */#defime CAMERAADDR 4#define WRIST ADDR 1#define SHOULDER ADDR 2

70

#define LEF_ARM_ADDR 3#define CAMERATILT 0#define CAMERAPAN 1#define WRIST_A 0#define WRIST B 1#define SHOULDERPITCH 0#defm SHOULDERYAW 1#define LEFIARM 1#define ELBOW 0/*1--------------------*char databuffer[411];static int rows[] = { 5, 6, 7, 8, 9 );static int pot[] = { 0, 1, 2, 3, 4 };char commandbuffer[41] = { OxE0, OxCO, 0xA0, 0x80, 0x01,

OxFO, OxDO, OxB0, 0x90, 0x03,0xEO, OxCO, 0xA0, 0x80, 0x01,OxFO, OxD0, OxB0, 0x90, 0x03,OxEO, OxCO, OxAO, 0x80, 0x01,0xF0, OxDO, OxB, 0x90, 0x03,OxE0, OxCO, OxAO, 0x80, 0x01,OxFO, OxDO, OXB, 0x90, 00 };

char addrbuffer[41] = ({ 4,4, 4, 4, 44,4,4,4,4,

1,1,1,1,1,1, 1, 1, 1, 1,2,2.2, 2,2,2,2,2.2,2,3,3,3,3,3,3, 3, 3,3,3,0);

char *algorithn[6] = { "P-typoe 0", "PID-typeY', "SVC-type\0","* OL. *NV","ERROR \0", "ERROR \0");

char cur_command_buffer[41], cur_databuffer[41];/* WRA WRB SHP SHY ELB PMP CT CP*/float scale[9] = 0.0, 50.272, 50.272, 67.28, 50.272, 63.104, 50.272, 50.272, 50.272};int Gain[9], alpha[9], beta[9], multl[9], mult2[9], alg[9], pos[9], mult3[9];/---------------------setupjoints(t

intnot_done,i;char selection;char fame[15];notdone =TRUE;while (not_done)

selection = getselectionO; /* light pen? */switch(selection)

case '1':tunejoint(CAMERA_ ADDR, CAMERA TILT,7);bieak

case '2':tuncjoint(CAMERA_ADDR, CAMERA_PAN,8);beak;

case '3':tunejoint(WRISTADDR, WRISTA, 1);break;

case '4':tunejoint(WRIST_ADDR, WRISTB, 2);breakl

case '5':tunejoint(SHOULDER_ADDR, SHOULDER_PITCH, 3);break;

case '6':tunjoint(SHOULDER_ADDR, SHOULDER_YAW, 4);biak;

case '7':tunejoin(LEFT_ARM_ADDR, ELBOW, 5);break;

case '8':tuncjoint(LEFTARM_ADDR, LEFT_ARM, 6);

case '9':printf" Load filename [.BDT]: ");scanf("%s\n",fiamn);strcat(fname,".BDT");load_from file(fname);scrrowcol(21,0);prinft" Data Transmitted to BAT");pause(100);bzak

case '0':notdone = FALSE;

defaultbeep(25);scrrowcol(l,1);printft"Invalid Selcction, Dude! Try a number.");pause(200);

printf(" Write filenamc [.BDT]: ");scanf("%s\n",fname);

71

strcat(fiame,".BDT");writ_out_data(fname);

/* -------------------.beep(n)

intn;

int i;

outport(Ox61,0x43);for (i=O;in*100;i++);outport(Ox61,0x40);retra(O);

/* --------------------/paint_mrnenu_l0

scrsetmode(O);scr dr();scr_rowcol(2,0);printf(" M.I.T. SSL\n");printf(" Beam Assembly Teleoperatornn");printf(" Joint Algorithm Tuning\n\n\n");printf(" 1. Camera Tiltn");prinff(" 2. Camera Pan\n");printf(" 3. Wrist An");printf(" 4. Wrist Bn");printf(" 5. Shoulder Pitch'n");printf(" 6. Shoulder Yaw'n");printf(" 7. Elbow \n");printf(" 8. Cooling Pump\n");printf(" 9. LOAD From File'n");printf(" 0. END JOINT TUNING");scrrowcol(22,0);print" Enter Selection: ");retum(O);

/*---------- --------getselectionO

char quer,

paintmenul();scr_rowcol(22,19);prirntf(" ");scr_rowcol(22,19);scanf("%c",&query);rtum(query);

/*______________...*/tunejoint(cardaddr, xory, joint)int cardaddr, xory, joint;

extem float scale[9];extem int Gain[9], alpha[9], beta[9], multl[9], mult2[9], alg[9], pos[9], mult3[9];float gainf,alphaf, betaf, multlf, rmult2f;float K, A, B, C;int i, done, transmit;int tem,char query, direct, inputt, dh, dl, conversion;int jseg, joff;inputt = FALSE;transmit = FALSE;conversion = FALSE;jsegm = OxE800 + card_addrOx80;joff = x_ory * 2;paintmenu_2);scr_rowcol(0,19);switch(joint)

case 1: printf("Wrist A");bkral

case 2: printf("Wrist B");break;

case 3: printf("Shoulder Pitch");break;

case 4: printf("Shoulder Yaw");break;

case 5: prinf("Elbow");break;

case 6: printf("Cooling Pump");break;

case 7: printf("Camera Tilt");break;

case 8: printf("Camera Pan");break;

default: printf("ERROR");

poke('l,TOZ80,jsegm);dl = peek((joff+0x84)jsegm); dh = peek(joff+0x85)jsegm);temp = dl + (dh <<8);scrrowcol(5,21);

72

printf("%4x ",ternmp);dl = peek((joff+Ox88)jsegmn);temp = dl + (dh<< 8);scrrowcol(6,21);printf("%4x ",ternp);dl = pek((joff+Ox8C)jsegm);temp = dl + (dh << 8);scrrowcol(7,21);printf("%4x ",temp);dl = peek(joff+ Ox80jsegm);temp = (dl >> 4) + (dh << 4);scrrowcol(8,21);printf("%d ",(signext(temp)));temp = peek(((joff/2)+0x90)jsegm);scrrowcol(9,21);printf("%s",algorithm[temp]);dl = peek(joff+0x9Cjsegm);temp = dl + (dh << 8);scr_rowcol(10,21);printf("%4x ",temp);dl = peek(joff+OxAOjsegm);ternmp = dl + (dh << 8);scr_rowcol(11,21);printT"%4x ",temp);dl = peek(joff+OxA4jsegm);temp = dl + (dh << 8);scrrowcol(12,21);printf("%4x ,ternmp);poke('O',TOZ80 jsegm);scrrowcol(13,20);printf("Transmit OFF');scrrowcol(13,00);printf"type I to input");scrrowcol(14,20);printf("Conversions C: OFF");domne = FALSE;scr_rowcol(14,0);printf"Input Varables");scr_rowcol(15,0);printf(" [000->FFFI: ");while (done = FALSE)

{if(inputt)

iif(conversin)

else

dh = peek(joff+Ox89)jscgm);

dh = peek((joff+Ox8D)jsegm);

dh = peek((joff+0x81)jsegm);

dh = peek(joff+0x9D)jscgm);

dh = peek((joff+OxAl)jsegm);

dh = peek((joff+0xA5)jsegm);

scr rowcol(17,20);printf("Set K: ");scanf("%f",&K);scrrowcol(18,20);print("Set A: ");scanf( %f,&A);scrrowcol(19,20);printf("St B: ");scanf("%f',&B);scr_rowcol(20,20);printf("Set C: ");scanf("%f',&C);scrrowcol(21 ,20);printf("Position: ");scanf("%d",&pos[joint]);scr_rowcol(22,20);printf("Int Sat(hex): ");scanf("%4x",&mult2[joint]);scrrowcol(23,20);printf("Int Mult(hx): ");scanf("9%4x",&mult3[joint]);multlf= C*65536.0;if (multlf >= 65536.0) printf("multl overflow");multl oint] = (int) multlf,gainf=scaleUjoint]*K*(B + 1.0);if(gainf >= 65536.0) printf("gain overflow");Gainljoint] = (int) gainf;alphaf = scaleUoint]*K*(A - C - 1.0 -2.0 * B);if(alphaf <= -65536.0) printf("alpha overflow");alphaljoint] = -((int) alphaf);betaf = scale[joint]*K*(B -A*C + C);if (betaf >= 65536.0) printf("beta overflow");beta[ioint] =(int) betaf;

scr_rowcol(16,0);printf("Set G: ");scanf("%4x",& Ga int jo ]);scrrowcol(17,0);printf("Set Alpha: ");scanf("%94x",&alpha[joint);scrrowcol(18,0);printf("Set Beta: ");scanf("%4x,&bcta[joint]);scrrowcol(19,0);

73

printf("Position: );scanf("%d ",&pos[joint]);scrrowcol(20,0);prmh SetMultl: ");scanf("%4x",&multl[joint]);scrrowcol(21 ,0);printf("Set Mult2 ");scanf(%4x",&mult2ljoim);scrrowcol(22,0);prinff("Set Mult3: );scanf("%4x",&mult3[joint]);

scr_rowcol(5,30);printf("%4x ",Gain[jointl);scr_rowcol(6,30);printf("%4x ",alpha[joint]);scr_rowcol(7,30);printf("%4x ,betajoint]);scrrowcol(8,30);printf("%d ",p

os [joint]

);

scr rowcol(10,30);pin"%.4x ",multl[loint]);scrjowcol(11,30);printf("%4x ",ult2[joint);scrrowcol(12,30);printf("%4x ",mlt3[joint]);scr rowcol(9,30);printf(%s",algorithm[alg[joint]]);inputt = FALSE;

/* put raten onto screen *1while(peek(FROMZ80jsegm) = 'U);poke('R',FROMZ80jsegm);temp = peek(OxO4+joffjsegm) l(peek(OxO5+joffjscgm)<<8);scrowcol(13,35);printf("%4x",temp);pokc('O'.FROMZ80jsegm);query = scrCsts(;switch(query)

case 0: brea;case T':case Y:

scr rowcol(13,30);if(transmit)

transmit = FALSE;print("OFFP");

else

trasmit =TRUE;printf("ON ");

bskcase '0':case '1:case '2:case '3':

algojoint] = query - '0';scrrowcol(9,30);printf("%s",algorithm[alg[joint]]);brealk;

case 'i':case T

inputt = TRUE;if(conversion)

scr rowcol(17,20);printf( ");scrrowcol(18,20);prinrf(" );scrrowcol(19,20);printf(" );scrrowcol(20,20);printf(" ");scr-rowcol(21 ,20);printf(" ");scrrowcol(22,20);prinlf(" ");scrrowcol(23,20);printf(" ");

else

scrrowcol(16,0);prinff(" \a");printf(" ");

printf(" n");printf(" n");prinff(" aN);

74

printf(" . );

breakcase 'c':case 'C:

scr_rowcol(14,37);if(conversion)

conversion = FALSE;printf("OFF");scr_rowcol(15,20);printf(" );scr_rowcol(16,20);printrf(" ");scrrowcol(14,0);printf("Input Varables");scr_rowcol(15,0);printf(" 000->FFF]: );

else{conversion = TRUE;print("ON ");scrrowcol(15,20);printf"enter PID parameters");scrrowcol(16,20);printf(" [00.000 -99.999] ");scr_rowcol(14,0);printf(" ");scrrowcol(1S,0);printf(" ");}

brea*;case 13: done =TRUE;

beak;default becp(25);

scr_rowcol(23,0);printf("Invalid Key ");pause(200);scr_rowcol(23,0);printf(" ");

if(transmit)

randirect(jsegm, joff, joint);

/*--------------------*/paint_menu_2()

scrdrO;scrrowcol(0,0);printf("Joint Tuning for.n\n");printf(" Current Newn");printf(" Param eter Value Value\n\");printf(" GainS");print(" Alphaa");printf(" Beta\n");printf(" Position\n");printf" Algorithmnn");printf(" Multla");printf(" MultZan");printf(" Mult3");

/* --------------------*/loadfom_ file(fame)char finare[15];

int ij,fn, seg;extem int Gain[9], alpha[9], beta[9], pos[9], alg[91], multl[9], mult2[9], mult3[9];

if((fn = open(fname,0))= -1)

scr rowcol(22,0);beep(25);printf("ERROR in loading");pause(250);

else

for(i=l; i<=8; i++)

Gain[i] =getw(fn);alpha[i = getw(fn);beta[i] = getw(fn);pos[i] = getw(fn);alg[i] = getw(fn);multl[i] = getw(fn);mult2[i] =getw(fn);mult3[i] = getw(fn);}

75

dlos(fn);seg =OxE800;for(i = 1;i<=7;i=i+2)

scg = seg+0x80;while(peek(TOZs0,seg) = 'R');poke(U',TOZ80,seg);/* put parameters to dual port ram jcl*/poke(Gain[i] & OxFF,Ox84,seg); poke(Gain[i] >>8,0x85,seg);poke(Gain[i+l] & OxFF,Ox86,seg); poke(Gain[i+l] >>8,0x87,seg);poke(alpha[i]& OxFFP,Ox88,seg); poke(alpha[i]>>8,0x89seg);poke(alpha[i+1]& OxFF,Ox8A,seg); poke(alpha[i+l]>>8,0x8B,seg);poke(beta[il & OxFF,Ox8C,seg); poke(beta[i] >>8,0x8D.seg);poke(beta[i+l] & OxFF,Ox8E,seg); poke(beta[i+] >>8,0x8F,seg);/* no position input for anything but pump i/if(i=5)

poke((pos[i+ll << 4) & OxFF,Ox82,seg);poke((pos[i+l] >> 4) & OxFF, 0x83.seg);

poke(alg[i] & OxFFOx90,seg);poke(alg[i+l] & OxFF,0x91,.seg);poke(multl[i] &OxFF,Ox9C,seg); poke(multl[i] >>8, Ox9D,seg);poke(multl[i+l] & OxFF,Ox9E,seg);pok(multl[i+l] >>8, Ox9F,seg);poke(mult2[i] & OxFF.POxAO.seg); poke(mult2[il >>8, OxAl.,seg);poke(mult2[i+l] & OxFF,OxA2,seg);poke(mult2[i+l] >>8, OxA3,seg);poke(mult3[i] & OxFF,OxA4,seg); poke(mult3[i] >>8, OxA5,seg);poke(mult3[i+l] & OxFF.OxA6,seg);poke(mult3[i+1] >>8, OxA7,seg);pokc('N',TOZ80,seg);

/* .--------------------pause(n)

int n;{inti j;for (i=O;i<n*100;i++)

j=j+l;

/*--------------------*writeoutdata(fname)char fnam[15];

int fn, i,j;extem int Gain[9], alpha[9], bcta[9], pos[9], alg[9], multl[9], mult2[9], mult3[9];

if((fn = creat(frame)) = -1)

scrrowcol(0,0);beep(25);printf(lUnable to Write File");pause(25

0);

else

for(i=l;i<c-=8;i++)

putw(Gain[il,fn);putw(alpha[i],fn);putw(beta[il,fn);putw(pos[i],fn);putw(alg[i],fn);putw(multl [i],fn);putw(mult2[i],fn);putw(mult3[i],fn);

close(fn);

/*n tse----------------------------t ----------tran-irect(osegmjoffjoint)intjsegm,joffjoint;

{extrm int Gain[9], alpha[91, beta[9], pos[91, multi [9], mult2[91, alg[9];while((peek(TOZ80jsegm))='R);poke('U',TOZ80jsegm);poke((multl[joint] & OxFF)joff+Ox9Cjsegm);poke(((multl joint] >> 8) & OxFF)joff+Ox9Djsegrn);poke((mult2ljoint] & OxFF)joff+OxAOjsegm);poke(((mult2joint] >> 8) & OxFF)joff+OxAljsegm);poke((mult3[joint] & OxFF)joff+OxA4,jsegn);poke(((mult3joint] > 8) & OxFF)joff+OxA5jsegm);poke((alpha[joint] & OxFF),joff+Ox88)jsegm);poke(((alphaUjoint] >> 8) & OxFF),(joff+Ox89)jsegm);poke((betafjoint] & OxFF),(off+Ox8C)jsegm);poke(((betaUjoint] > 8) & OxFF),(joff+Ox8D)jsegmn);poke(algjoint],((joff/2)+0x90)jsegm);poke((Gainljoint] & OxFF),Ooff+Ox84)jscgm);poke(((Gainfjoint] > 8) & OxFF),(joff+Ox85)jsegm);poke(((posjoint] <<4) & OxFF),(joff+Ox80)jsegm);poke(((posljoint] >> 4) & OxFF),(joff+Ox8l)jsegm);

76

poke ('N,TOZ8Ojsegm);

/*------- ----- --------------------- - --------------- /tran_zero() /*zero out all commands in the arm joint*//* revision 8/4/87 -- for the purposes ofjlt arm control algorithm,this must include a zeroing of the upper byte of mult2 to reset theintegrator summation */

/* zero out any residual commands jcl*/while(peek(FROMZ80,0xE880) = '); poke('R',FROMZ80,0xE880);while(peek(TOZ80,xE880) = R'); poke('U,TOZ80,xE880);pokc(peek(OxOO,OxE880),0x80,0xE880); poke(peek(OxOl,OxE880),0x81,OxE880);poke(peck(x2,xE880),0x82,0xE80); poke(pek(0x03,xE880),0x83.,OxE880);poke(O,OxAl,OxE880); poke(O,OxA3,OxE880);poke('N',TOZ80,0xE880);poke('O',FROMZ80,0xE880);/* zero out any residual commands jc2*/while(peek(FROMZ80.0xE900) -= J); poke('R',FROMZ80,0xE900);while(peek(TOZ80,0xE900) = R'); pok('U',TOZ80,xE900);poke(peek(OxOO,OxE9E 90 0); pok(peck(OxOl,OxE900),0x81,OxE900);poke(peek(x0O2,OxE900),Ox82,0xE900); poke(peek(0x03,OxE900),0x83,OxE900);poke(O,OxAl,OxE900); poke(0,OxA3,OxE900);poke('N,TOZ80,0xE900);poke('O',FROMZ80,0xE900);/* zero out any residual commands jc3*/while(peek(FROMZ80,0xE980) = UY); pokc('R'.FROMZ80,0xE980);while(peek(TOZ80,0xE980) ='R'); poke(U,TOZ80, 0xE980);poke(peek(OxOO,OxE980),0x80,xE980); poke(peek(OxOl,OxE980),0x81.OxE980);poke(O,OxAl.OxE980);poke('N,TOZ80,xE980);poke('O',FROMZ80,0xE980);

P------------ --------- -------------------

tranarm/) transmits to commands to the arm *//* due to pots, pos[i] is 12bits left justified but we reed to *//* poke 12 bits leftjustified into 16 bits*/

extemn int pos[9];while(peek(TOZ80,xE880) = R'); poke('U',TOZ80.xE880);poke((pos[l] << 4) & OxFF,0Ox80,xE880); poke((pos[ll] >>4) & OxFF, 0x81,xE880);poke((pos[2] << 4) & OxFP,0x82,OxE880); poke((pos[2] >>4) & OxFF, 0x83,OxE880);poke('N',TOZ80,0xE880);while(peek(TOZ80,xE900) = 'R'); poke('U',TOZ80,0xE900);poke((pos[31 << 4) & OxFFP,0x80,OxE900); poke((pos[3] >>4) & OxFF, x81,OxE900);poke((pos[4] << 4) & OxFF,0x82,OxE900); poke((pos[4] >>4) & OxFF, 0x83,OxE900);pokc('N,TOZ80,OxE900);while(peek(TOZ80,0xE980) = R'); poke('tJ.TOZ80.xE980)poke((pos[5] << 4) & OxFF,0x80,OxE980); pokc((pos[5] >>4) & OxFF, Ox81,xE980);poke('N,TOZ80,0xE980);

/*---- .. ------------------- ----------intposl[8];tran_ann() /* transmits to commands to the arm in XXXO form*/

/* pos[i] isl2 bits left justified into 16 bits */

extent intposl[8];while(pek(TOZ80,xE880) == 'R'); poke('U',TOZ80,xE880);poke(posl[O] & OxFP,0x80,xE880); pokc((posl[O] >>8) & OxFF, 0x81,OxE880);poke(posl[l] & OxFF,0x82,OxE880); poke((posl[l] >>8) & OxFF, 0x83,0xE880);poke('N,TOZ80,xE880);while(peek(TOZ80,0xE900) == 'RD; poke(lJ',TOZ80,0xE900);poke(posl[2] & OxFF,0x80,OxE900); poke((posl[2] >>8) & OxFF, 0x81,xE900); poke(posl[3] & OxFF,Ox82,0xE900); poke((posl[3] >>8) & OxFF, 0x83,OxE900);poke('N',TOZ80,0xE900);while(peek(TOZ80,xE980) == 'R'); poke(U',TOZ80,OxE980);poke(posl[4] & OxFP,0x80,xE980); poke((posl[4] >>8) & OxFF, 0x81,O0xE980); poke(''N,TOZ80,0xE980);

---------------------------------------------- ----

tran_tp() /* transmits to commands to the tilt and pan */

extem pos[9];while(peek(TOZ80,OxEAOO) = R'); poke('U',TOZ80,0xEAOO);poke((pos[7] << 4) & OxFF,0x80,xEAOO); poke((pos[7] >>4) & OxFF, 0x81,OxEAOO); poke((pos[8] <<4) & OxFF,0x82,OxEAOO);poke((pos[8] >>4) & OxFP, 0x83,OxEAOO);poke('N',TOZ80,0xEAOO);

signext(i)inti;{if((i & 0x800) != 0)

i=i I OxFOOO;

retum(i);

/* ..........--------------- - ...--------- ------------ /comm_display() /* this just displays what duty cyde is being sent -DEA */

int trmp,scr rowcol(16,24);printf("%2x",peek(OxC,OxE880));scr_rowcol(15.26);

77

printf("%2x",peek(xD,OxE880));scr_rowcol(16,28);printf("%2x",peek(OxC,OxE900));scrrowcol(l5,30);printf("%2x",peck(0xD,OxE900));scr_rowcol(16,32);printf("%2x",peek(OxC,OxE980));scr_rowcol(15,36);printl("%2x",peek(OxC,OxEA0O));scr_rowcol(16,38);print("%2x",peek(OxD,OxEAOO));

/*.------------...--------.----------..-----------

/* Joint Step is meant to allow the operator to apply a step command to a jointand record the results via the daters routines. I will use a more directmethod of addressing the joint cards than tranjcs_dpr. The dual port ram thatacts as the two way closet between the pc and the joint card z80's can beaccessed simply by peeking and poking the variables by which they communicate.No complex pointer/buffer system is required.

David E. Anderson, March 8,1987 *DATATAG onejnt[TAGSIZE] = "GlAone joint po" };

DATATAG oncom[TAGSIZE] = {"HIAone joint comn};DATATAG cntt[TAGSIE] = {"IIAstep counter"};joinstep(

extemrn char dataflag;exten float scale[9];extem int Gain[9], alpha[9], beta[9], pos[9], multli [9], mult2[9], alg[9];int joint;char testing, looking, transmit, dl, dh;char written, direct, inputt;int jsegm, counter, temp, jnt, algjc, joff;int posjc, templ;float tempf, multlf, mult2f, gainf, alphaf, betaf;float K, A, B, C;transmit = FALSE;testing = TRUE;looking = TRUE;direct = FALSE;inputt = FALSE;while(testing)

while(looking)( * paint a joint selection menu *paintmenul0;scrrowcol(5,0);printf(" PID Joint Step Algorithm ");scr_rowcol(15,7);printf("G. Daters Initialization\n");printf(" T. Calibrate Control Loop Time "n");

printf(" 0. End Joint Step ");scr_rowcol(22,19);scanf("%c",&jnt);painmenu0; paint the runtime menu *scr_rowcol(0,0);printf("Stcp Input For: ");looking = FALSE;joff= 0;jnt = toupper(jnt);switch(int)

{ * see which joint was selected *case '1':

printf("Camera Tilt");jsegm = OxEA00O;joint = 7;break

case '2:printf("Camera Pan");joff= 2;joint = 8;jsegm = OxEAOO;break

case '3':printf("Wrist A");joint = 1;jsegm = 0xE880;break

case '4':printf("Wrist B");joff= 2;joint = 2;jscgm = OxE880;beak

case '5':printf("Shoulder Pitch");jsegm = OxE900;joint = 3;break

case ':

printf("Shouldr Yaw");joff= 2;joint = 4;

78

jsegm = OxE900;break

case '7':prinff(Elbow");jsegm = OxE980;joint = 5;break

case 'G':setdata0;joint = 6looking = TRUE;break

case T:timeJoop0;looking = TRUE;break

case '0':testing = FALSE;break

default:looking = TRUE;boep(25);scr_rowcol(1,1);printf(" Invalid Selection. Try a number.");pause(200);

if(!looking){ paint the rest of the runtirme nenu *scrrowcol(10,12);printf("Runtim: Position:);scr_rowcol(ll,0);print" Direct inp: An");printf(" Start Step: Tln");printf(" Zero command: Zsn");printf(" Take input n");printf(" Change Joints: J\n");printf(" TakeData D");printf(" Exit: X")if(data_flag)

scr_rowcol(16,19);printf("SELECrED");

}

temp = sccstsO;tmp = toupper(tenp);switch(temp)

{ * check for commands *case 0:

breakcase 'X: * return to main loop *

testing = FALSE;break

case ': * quick power down ** zero out any residual commands jc3*while(peek(FROMZ8Ojsegm) = 'U'); poke(R',FROMZ8Ojsegm);whilc(peek(TOZ80jsegm) = 'R); poke('U',TOZ80jsegm);poke(peck(OxOl+joffjsegm) joff+Ox80jsegm); poke(peek(OxOl+joffjsegm),0x81+joffj segm);poke('N',TOZ80jsegm);poke('O',FROMZ8Ojsegm);break;

ease 'A':if (direct)

direct = FALSE;scr rowcol(11,19);printf(" ");

else

direct = TRUE;inputt = TRUE;scrjowcol(11,19);printff("ON);

breakcase ':

if(direct)

inputt -- TRUE;scr_rowcol(18,0);printf" \n");printf(" W);printf(" \n");printf(" \n);

breakcase 'D':* take data during run *

data_flag = !dataflag;scrrowcol(16,19);if(dataflag)

79

printf("SELECrED");else

printf( ");beaq

caseT : * go back to joint select *looking = TRUE;break

case '0': * set algorithm *alg[joint] = O;

case '1':

case '2:

case '3':

alg[joint] = 1;bra;lc

alg[joint] =2;breal

alg[joint] =3;breal;

case T: * set flag to start a run *trasmit = TRUE;break

defaultscr_rowcol(23,0);printf("Invalid Key");pause(200);scrrowcol(23,0);printAf(" );break

read the present dpr variables. Note that all but the algorithm are *12 bit numbers right justified in 16 bits *

dl = peek(off+0x84)jsegm);temp = dl + (dh << 8);scr_rowcol(5,21);printf("%4x ",temp);dl = peek((off+Ox88)jsegm);temp = dl + (dh << 8);scr_rowcol(6,21);printi"%4x ",temp);dl = peek((joff+0x8C)jsegm);temp = dl + (dh << 8);scrrowcol(7,21);printf"%4x ,terp);dl = peek(joffjsegm);posjc = (dl>>4) + (dh << 4);scr_rowcol(8,21);printf("%d ",(signext(p

osjc)));

temp = pek(((joff/2)+0x90)jsegm);scr rowcol(9,21);printf('%s",algorithm[temp);

* Now read pots to set input variables for run *if(direct)

if(inputt)

dh = peek((joff+0x85)jsegm);

dh = peek((joff+0x89)jsegm);

dh = peock((joff+Ox8D)jsegm);

dh = peek((joff+l)jsegm);

scr_rowcol(5,30); printf(" );scr_rowcol(6,30); printf(" ");scr_rowcol(7,30); printf(" ");scr rowcol(18,0);printf("Set K: ");scanf("%f',&K);scrrowcol(19,0);printf("sct A: );scanf("%f",&A);scr_rowcol(20,0);printf("Set B: ");scanf("%f.,&B);scr_rowcol(21 ,0);printf("SetC: ");scanf("%f",&C);scr_rowcol(22,0);printf("Position: ");scanf("%d",&pos[joint]);multlf = C*(float) 65536;multl[joint] = (int) nrultlf;mult2f = (C(float) 1) * (float) 65536;mult2joint] = (int) mult2f;gainf = scale[joint]*K*(B + (float) 1);Gain[joint] = (int) gainf;alphaf = scale[joint]*K(A + C -(float) 1 -(float) 2 * B);alpha[joint] = -((int) alphaf);betaf= scale[jointl*K(B + A*C -C);beta[joint] = (int) betaf;scrrowcol(5,30);printf("9%3x",Gain[joint);scr_rowcol(6,30);prir"%3x",alpha[joint]);scr_rowcol(7,30);printf("%3x",beta[joint]);scr_rowcol(8,30);printf("%d ,pos[jointl);

80

inputt =FALSE;

else

posUojont] = convert(0,3) - 1024;*command** and put them on the screen *

scrrowcol(5,29);printf(" %4x ,GainUoint]);scrrowcol(6,29);prinf(" 9b4x ",alphaUjoint]);scrrowcol(7,29);printf(" %4x ",beta[ointl);scr rowcol(8,29);signext(pos[joint]);printf" %d ",posljoint]);scr_rowcol(9,30);printf("%s",algorithm[algjoint]])temp = peek((joff/2)+OxCjsegm);scxrjowcol(15,36);printf"%3x",temp);temp = peek((joff/2)+0xEjsegm)&0xl;scrowcol(16,36);if(temp) printf("rv");else printf("for");temp = peek(joff+Ox8jsegm)l(peek(joff+Ox9jsegm) << 8);scr_rowcol(17,36);printf("%4x",tcmp);

algjoint] = 1;* tirme to start the run *

if(transmit)

sc_rowcol(12,20);printf("Transmitted*ANY KEY*");scrrowcol(13,30);printf("*TO EXIT*");logint(onecompos[joint]);scr_rowcol(15,28);printf("Duty:');scrjowcol(16,28);printi'"Dir:");scr_rowcol(17,28);printf("Ctrl:");scr_rowcol(18,24);printf("count");scrrowcol(l9,24);pritf"ratex");sctrowcol(20,24);printf"rat4y");

* input runtime variable to dual port ram, poking cornmand position last *tran_direct(jscgmjoffjoint);

*let it go until a key is hit *while(!scr_csts)

scrrowcol(10,30); * put the present position on the screen *posjc = (peek(joffjsegm) > 4)+(peek((joff+l)jsegm) << 4);printf("%od ",(signext(posjc)));temp = peek(off2)+OxCjsegm);scrjrowcol(15,36);printf("%3x",temp);temp = peek((joff2)+0xEjsegmn)&Oxlscrrowcol(16,36);if(temp) printf("rev");else printf("for");temp = peek(joff+Ox8jsegm)l(peek(joff+x9jscgm) << 8);scrrowcol(17,36);printf("%4x",tcmp);counter=peck((joff+Ox1 O)jsegm)+(peck((joff+Ox 1) l)jsegm)<<8);scrrowcol(18,32);printf("%d",countr);scrrowcol(19,32);* put raten onto screen *while(peek(FROMZ80jsgm) = 'U');pok('R',FROMZ80jsegm);temp = peek(x04jsegm)l(peek(0x05 jsegm)<<8);templ = peek(OxO6jsegm)l(pek(xO7 jscegm)<<8);poke('O'FROMZ80jsegrn);printf("%d ",temp);scrrowcol(20,32);printf"%d ",templ);****data logging***if(dataflag)

logint(cntt,counter); * keep the counter *logint(onecom,pos[joint]); * keep commanded position*logint(onejnt,posjc);* and the output arm position*

* zero out any residual corunands jc3*while(peek(FROMZ80jsegm) = 'U);poke('R',FROMZ80jsegrn);while(peek(TOZ80,jsegm) = 'R'); poke(',TOZ80jsegm);

81

poke(poek(Oxo0+joffjsegm) joff+Ox80jscgm);poke(peek(OxOl+joffjsegm),0x81+joffjsegm);

poke(0.0x88+joffjsegm); poke(0,x89+joffjsgm);pokc(0,0x84+joffjsegm); pokc(O0,0x85+joffjsegm);pok(0,0xSC+joffjscgm); poke(0.0x8D+joffjsgm);pokc(O,Ox9C+joffjscgm); poke(0,0x9D+joffjsegm);pokc(0,OxAO+joffjsegm); poke(0,0xAl+joffjsegm);poke('N',TOZ80,j segm);poke('O'FROMZ80jscgm);transmit =FALSE;scrrowcol(12,20);prtf(" );scrrowcol(13,30);printf ");scrrowcol(l5,28);printf(" ");scr_rowcol(16,28);printf(" ");

screon0;screen2);

timeloop(seg)int seg;

chartirm[9];long cntr, end;while(peek(FROMZ80,seg) = 'I);cntr = peek(0x10,seg) + (peck(Oxll,scg) << 8) + (pock(0x12,scg) << 16);times(tire);scrdclr;scr rowcol(5,5);printf("loop counter: %d",cntr,"n");printf("time: %s",ime,'Nn");end = catr + 100000;while(cntr < end)

cntr = peek(OxlO,seg) + (peek(Oxll,seg) << 8) + (peek(Ox12,seg) << 16);inmes(time);

scr rowcol(8,5);printf("loop counter: %d",cntr,"n");printf("tim: %s",tim,"n");printf(type any key to exit");while(!scrcstsO);}*/

82

/* SUPPORT2.C -- BAT support routines version 6.4** by John Spofford, Herbie Viggh, and David Anderson** M.I.T. Space Systems Laboratory*/#include "PORTNUMS.H"/*#define MASBUFLIM 100 * # of buffer checks before timeout *//*#define MASREDLIM 100 * # of ready checks before timeout *//* ------- 1-------------/* tran mast -- transmitbuffers to master control card tran_mast(mdata,mconmmaddr,mlast)char mdata[] mcomm[] maddr[];int mlast;

int retval, waite; * timeout variable *int i; * loop index *

puts("Using old tran_mast\n"); * use tranjcsdpr! *

retum(0);retval = 0;* master buffer full? if so, error *if (inport(P1A) & 0x40)

print("** MASTER BUFFER ERROR AT START **\n");*retval =-1; * -1 is start buffer full error *

* master has hung joint card? if so, error *if (inport(P1A) & 0x80)

printf("** MASTER HAS HUNG JOINT CARD **n");*retval = -2; * -2 is hung joint card error *

for (i = 0;i <= mlast;i++) * transmit to master *

/* master buffer full? try timeout *for (waite = 0;inport(P1A) & 0x40;waitc++)

if (waitc > MASBUFLIM)

printf("** MASTER BUFFER TIMEOUT ERROR **E");*retval = -3; * -3 is buffer full timeout error *break

* is master ready? try timeout *for (waite = 0;!(inport(P1A) & 0x20);waitc++)

if (waite > MASREDLIM)

printi(" * MASTER NOT READY FOR TRANSMIT * %");*retval = -4; * -4 is ready timeout error *break

if(mcomm[i] -0) * dont pass a zero command *

retval = -5;continue;

* output to master *outport(P3Amdata[i]); * data *outport(P3B,mcomm[i]) command *

outport(P3C,maddr[i]); * address (low 3) *outport(POX,CSLO); * strobe lo *outport(POX,CSHI); * and hi *

} *end of transmit to master *

outport(POX,CSHI); * leave it high *retun(retval); * all is okay *

*-__________________ #1

/* tranpuma -- transmit buffers to puma control card */#define PUMLIM 23#define PUMSYNC OxFF/* -------------------- /tran_puma(pdat,plast)char pdat[l;int plast;

int waite; /* timeout variable */int i; /* loop index */

/* is puma ready? try timeout *1for (waite = 0;!(inport(P1A) & Ox02);waitc++)

if(waitc >PUMLIM)

printf"'* PUMANOT READY FORTRANSMIT *\n");*/retum(-2); /* -2 is ready timeout error */

/* send PUMA sync code /outport(PlB,PUMSYNC); /* send sync */outport(POX,C3LO); /* strobe lo *1outpott(POX,C3HI); /* and hi */

83

for (i = O;i <= plast;i++) /* transmit to puma */

/* is puma ready? try timeout */for (waitc = O,!(inport(P1A) & OxO2);waitc++)

if(waitc > PUMLIM){printi'"** PUMA NOT READY FOR TRANSMIT *%a");*/retun(-2); /* -2 is ready timeout error I/

/* output to puma */outport(P1B,pdat[i]); /* data */outport(POX,C3LO); /* strobe lo */outport(POX,C3HI); /* and hi */} /* end of transmit to puma */

return(0); all is okay */}

/*.-------------------*/

/* adctest.c -- convert analog inputs using AD364 *//* convert -- returns signed integer (-2048..2047)** 9999 onerror */convert(ais_ch, muxch)int aisch, muxch; /* the two input multiplexers */

int highS, low4, value;

if ((ais_ch > 15 11 aisch < 0) II (mux_ch > 15 muxch < 0))retun(9999); / error return big num */

outport(POA, (aisch << 4) 1 mux_ch); /* ais high, mux low */

/* start conversion (need 15us delay after setting muxes) */outport(POX,C1LO); /* AO select 12 bit conversion /outport(POX,C2LO); /* R/C set to convert */outport(POX,COHI); /* CE trigger conversion */outport(POX,COLO); /* CE drop trigger */outport(POX,C2HI); /* R/C set for read */

/* wait for end-of-conversion */while (inport(Pl A) & 1); /* STATUS */

/* read data */outport(POX,COHI); /* set CE for read */high8 = inport(POB);/* AO already low */outport(POX,C1HI); /* AO select low 4 bits of data */low4 = inport(POB) & OxFO, /* in upper 4 bits */outport(POXCOLO); /* reset CE low */

/* massage & return */value = ((high8 << 4) 1 (low4 >> 4)); /* adjust to form a number */return(value-2048); /* return centered at zero */

/* --------------------rqstmstr(cardnum)char cardnum;

intretval, waitc;* timeout variable *

retval = 0;* master buffer full? if so, error *if (inport(P1A) & 0x40)

* printf("* MASTER BUFFER ERROR AT START *'");*retval = -1; * -1 is start buffer full error *

* master has hung joint card? if so, error *if (inport(P1A) & Ox80)

* printf("** MASTER HAS HUNG JOINT CARD **\n");*retval = -2; * -2 is hung joint card error *

* is master ready? try timeout *for (waite = 0;!(inport(P1A) & Ox20);waitc++)

if (waite > MASREDLIM)

* printf("* MASTER NOT READY FORTRANSMIT **\n");*retval = -4; * -4 is ready timeout error *break;

* output to master *outport(P3A,0); * data *outport(P3B,0); * command outport(P3C,cardnum); * address (low 3) *outport(POX,CSLO); *strobe lo outport(POX,CSHI); * and hi ** end of transmit to master *retum(retval); * all is okay *}

84

---------------------- /

/* syn c -- waits for time interrupt and then returns.** it returns -1 ift was a time fault error */

#define TLOFF Ox6C /* offset of PC BIOS timer_low counter */#define TLSEG 0x40 /* segment of PC BIOS timer_low counter *//* --------------------/synchronize()

static char last_low;char peek;

if (pcek(T_LOFF,TLSEG) = last_low) /* look for error */

lastlow = peek(T_L_OFF,T_LSEG); /* set to new */return(TRUE); /* crror if flag already set /

while (peek(TL_OFFT_LSEG) = last_low); /* wait for new count */

last_low = peek(T_L_OFFT_L_SEG); / set itto new */retumr(FALSE); /* back to work */

/* --------------------/int addr, data;/*------------- ---- */irport(a)inta;

extern int addr, data;

addr = a;#asm;ASM88 /*

MOV DX, WORD addr_IN AL,DX ;get dataMOV AH, 0 ;char valueMOV WORD data, AX

;*/

#retum(data);

/* ---------------- /--outport(a,d)int a,d;

extern int addr, data;

addr = a;data = d;

#asm; ASM88/*

MOV DX, WORD addr_MOV AX, WORD dataOUT DX AL ;output char

#/*--------------------*1unsigned offsetl6, segmentl6;char byte8;/* --------------------/char peek(o,s)unsigned o,s;

extem unsigned offsetl6, segmentl6;extem char byte8;

offsetl6 = o;segmnentl6 = s;

#asm; ASM88 /*

MOV DI, WORD offsetl6MOV ES, WORD segmentl6_MOV AL, ES:[DI] ; kMOV byte8 AL ;return it

;*/

return(bytc8);

/* --------------------/poke(c,o.s)char c;unsigned o,s;

extem unsigned offsetl6, segmentl 6;extern char byte8;

offsetl6 = o;segmentl6 = s;byte8 = c;

#asm

85

AL, byte8_DI, WORD offsetl 6ES, WORD segmentl6_;poke

·---- *

/*clip & adjust position values*/

if((tdat & 0x800) != 0) /*sign extend*/

tdat = tdat I OxF00;

if (tdat > 1024)retum(1024);

else if (tdat < -1024)return(-1024);

elseretum(tdat);

/* offset and scale from master to real arm */align(dater ix)int dater, ix;

int ixy;extem int align offset[8];extent float aligngain[8];extem int maxjcsval[8, minjcsval[8];extem int masoff5]; /* master offset */extem float masnul[5]; /* master mult */

dater = aligngain[ix] * (dater + lignoffset[ix]);if (dater > maxjcsvalix])

retum(maxjcsval[ix]);else if (dater <minsjcs_val[ix])

retum(minjcsval [ix]);else

returm(dater);

/* read control station switches -return via extemal cswitch[i]TRUE = switch activated. comps for swapped leads on blue */

/* ------ */char cswitch[16];char old8, old9, oldlO, oldll;char initflag = TRUE;read_switches()

char iixnmask;int temnpc;extem char cswitch[16];extem char old8, oldlO, oldl 1, old9;extem char initflag;

I 0(F) 1 (T) Location0 on off1 on off2 off on3 on off4 on off5 on off6 on off7 on off8 on off9 an off10 on off11 on off12 on off13 on off14 on off15 on off

Modified from direct i/o port reading of switches to scaning through a 16 to1 mux. -10/86 DEA

*//*

/*5

as of 10/28Function (run bat)

panel: red shutdown & exitpanel: white slave togglepanel: blue toggle PUMA enablepanel: black master flyingann: out/pull(lB) conect headamr out/push(lA) toggle left clawarm: i/pull(2B) master connectarm: in/push(2A) toggle right clawfoot: far left Beam carrier togglefoot near left Beam carrier clawfoot: near right head flying togglefoot: far righthand contright spare pnuematics 1hand contmiddle spare pnuematics 2hand contlefthand conetrigger

tempc = inpart(P4C); ***direct i/o port code***for (i = 0,i < 8;i++)

cswitch[i] = !((tempc >> i) & 1);if (cswitch[2]) cswitch[2] = 0;

else cswitch[2] = 1;returm(tempe); */

serial port reader*/

tempc = 0;for (i=0;i<16;i++)/Iscan through all 16 switches*/

/* switch address, i, is sent to P3C3-6. Note that P3CO-2 areinitialized to 0, but are used by the feedback() function.*/

outport(P3C,((i<<3) & 0x78)); /*now send addresses with strobe lowered */x = (inporlt(PlA)>>2)&0xOl;

86

; ASM88 /*MOVMOVMOVSTOSB

;*/

}

Padjust(tdat, i)inttdat, i;

------------ --------- -- ----- - ----- ---- -------

if (i=2)x = !x;

/*read the switch -output is at P1A2 so shift by 2 bits and mask tocut out garbage. Note that output is inversion of switch state.When switch i is grounded, output, x, is high. Switch 2 leads arebackwards, so we invert x there.*/

/* installed 11/86 due to high beam foot switches, which are not impulseswitches like the rest -DEA*/

if(initflag)

if (i8) old8 = x;if(i---9) old9 = x;if (i=10) oldlO = x;if(i=11)

oldll =x;initflag = FALSE;

if (i=9)

if (x != old9)

old9 =x;x=1;

else

if (i=10)

if(x != oldlO)

else

if(i=-ll)

if(x !=oldll)

old9 = x;x =0;

oldlO = x;x=l;I

oldIl = x;

oldll =x;x=l;

else

oldll =x;x=O;

if (x != old8)

old8 =x;x=1;

else

cswitch[i] = (x & OxOl);tempc I= (x & OxOl)<<i;

old8 = x;x = 0;

/*set appropriate cswitch /

retum(tenpc);

/* ..------------------------------------------- -//* This function reads the five pots on the head controller. -HV */

readhead()

int i tdat;extern int potlabelx[5], potlabely[5];extem inthpot[5];

for (i=O; i<=4; i++)

tdat = (convert(potlabelx[i],potlabely[i]) & OxFFF) - 1024;h.pot[i] = tdat,if(hpot[i] > 1024)

h_pot[i] = 1024;

87

if(i==8)

else if(h_pot[i] <-1024)hpot[i] = -1024;

/*--------------------------- -------------------------------- //* This function calculates jes commands for the tilt an pan from thehead controller pots. -HV revised DEA */

head_canmslavel()

int i, tcpitch, t_cyaw;extem int hpot[5], headoff[5];extern int cpitch, cyaw, pos[9];extem float cpitmult, cyaw_mult;extem int maxjcs_val[8], minjcs_val[8];

for (i=0O; i<5; i++)hpot[ij = hpot[i] -headoff[i];

t_cpitch = hpot[3] -hpot[l];tcyaw = h_pot[4] + h_pot[];cpitch = tcpitch * cpitmult;cyaw = tcyaw * cyaw_mult;

if(cpitch > maxjcs_val[ 1])cpitch==maxjcsval[l];else if (cpitch < minjcs_val[l])

cpitch =minjcs_val[l];if(cyaw > maxjcs_val[2])

cyaw = maxjcs_val[2];else if(cyaw < minjcsval[2])

cyaw = minjcs_val[2];pos[7] = (cpitch) & OxFFPF;pos[8] = cyaw & OxFFF;

/* -------------------------------------/* This function calculates the PUMA commands for flying BAT rotationsfrom the head controller. -HV */

headpuma_slavelO

int i, t_ppitch, t_pyaw, t_proll;extem hpot[5], p_headoff[l5];extem float ppitmult, pyawmult, prol-mult;extem int ppitch, pyaw, proll;

for (i=0; i<S; i++)hpot[i] = hpot[i] -p_headoff[i];

tppitch = hpot[3] -h_pot[l];tpyaw = h_pot[4] + hpot[0];tproll = hpot[2];

ppitch = tppitch * ppitmult;pyaw = tpyaw * pyaw_mnult;prol = tproll * prol_nult;

/* ------------------------------------- */

88

/* SUPPORT3.C -- BAT support routines version 6A** by John Spofford, M.I.T. Space Systems Laboratory** revisions by DavidE. Anderson*/#include "PORTNUMS.H"#include "DAT.H"#define PI 3.1415926536#define PI2 1.5707963268#define R2D 180.0/PI#define D2R PI/180.0/*-------------------- */char jcs_codc[2048];unsigned load_addr;unsigned total;char jcstat[8] = {FALSEALSEALSEFALEALSEF SEA LSESEFALSE};DATATAGjc2pc[TAGSIZE] = {"KIBJC2pos corn"};DATATAG jc2pm[TAGSIZE] = { "MIBC2 pos meas" };/* -------------------- */load_control_system()

int i, j, k;extem char jc_stat[];

for (i = 1; i <= 4; i++)

if(load_dulram(i)){printf("\n%c Dual-port RAM loading failed (JC-%d)\n",7,i);for (j = 0; j <= 10; j++)

dawdle(' '); /* time to read message */

elsejc_stat[i] = TRUE; /* card is OK */

retum(FALSE);

/* -------------.--..*/load-jcs_buffer(fp)int fp;

extemrn char jcscode[2048];extem unsigned loadaddr;extem unsigned total;int i, byte, count, namecnt, code_ptr;char lastblock;

rl6(fp);nament = r8(fp)*256 + r8(fp);for (i = 0;i < namecnt;i++)

r8(fp);r8(fp);if (r8(fp) != OxAO)

printf("nNot a Z80-type file.\n");closeU(fp);return(TRUE);

lastblock = FALSE;code..ptr = 0;if ((count = r16(fp)) < 257)

lastblock = TRUE;count -= 5;total = count;rl6(fp);load addr = rl6(fp);for (i = 0;i < count;i++)

jcscode[code_ptr++] =r8(fp);if(!last_block)

while (Ilastblock)

rl6(fp);if ((count = rl 6(fp)) < 257)

last_block = TRUE;count -= 5;total += countr16(fp);r16(fp);for (i = 0i < count;i++)

jcs_code[code_ptr++] = r8(fp);

if (code-ptr != total)printf('ncodcptr != total"); /* just checking */

if((byte = codeptr % 16) != 0)for (i = 0; i < (16-byte); i++)

jcos_code[code[od_ptr++] = 0; /* even 16-byte blocks */total = codeptrclose(fp);retum(FALSE);

/* .*--------------------

89

r8(fp)int fp;

int byte;

if ((byte = getc(fp)) !=-1)retun(byte);

printf('n%cError reading file\n",7);close(fp);exit(O);}

/* _....--...--.. ...*/rl6(fp)int fp;

retum(r8(fp) + r8(fp)*256);

/* ================================lload_ dualram(jc)intjc;

int i, fp, rrc;char *JC_file;unsigned jcseg, u;exte char jcscode[2048];extem unsigned load_addr;extemn unsigned total;

JC_file = "J1.OBJ";

if ((fp = open(JCfile,O)) = -1)

printf('\n--> joint code [J1.OBJ] not foundn");rtumr(IRUE);

if (loadjcs_buffe(fp))return(TRUE);

if (total > Ox7FC)

printf('\nCode > 7FC bytesn");return(TRUE);

jc_seg = SEG_OFF + 0x80 * jc;printf'"\nwaiting for JC ready...");errc = 0;while (peek(FROMZ80,jcseg) != 'r') /* wait Z80 ready */

if (peek(FROMZ8Ojc_seg) ='m')

printf('WnJC-%d indicates bad mnmorn",jc);return(TRUE);

dawdle('.); / check for BREAK */if (++erc > 10)

return(TRUE);

prinff('\nCopying into JCS menoryn");jmove(totaljcscode,showds(,0jceg); /* use canned move */for (u = 0; u < total; u++) /* test for errors */

errc = 0;for (i = 0; i < 3; i++) /* three trys */

if (pek(ujcseg) != jcs_code[u)errc++;

if (errc > 1) /* rtry if too many errs*/

poke(cs-code[u],ujcseg);if(peek(ujcseg) !=jcs_code[uJ)

/* give up */printr("*** Error at %04xn",u);retumn(TRUE);

poke('c,TOZ80jc_seg); /* code loaded flag */print"waiting for code running flag...");errc = 0;while (peek(FROMZ80jc_seg) !='f) /* wait code running flag */

{dawdle('.'); /* check for BREAK */if (++errc > 5)

returnCI(RUE);

retumm(FALSE);

daweh)============= ===== ===*/dawdle(ch)

90

char ah;

putchar(dh);synchronize();synchronize);synchronize);synchronizeO;synchroniz();

static char tran slate[14]2] = * (old corn, ew offs} X,Y *{{0x8, 0x80}, {0x90, 0x82} * reference D *{0OxEO, 0x84), {0xFO, 0x86) * gainD *{0xCO, 0x88}, {0xDO, 0x8A} * alphaD *{0OxAO, x8C), {xBO, 0x8E} * beta D *{0x01,0x0), x90} {0x03,0x91) *algorithm*{0x04, 0x92), {0x05, 0x94} *pr-dither{0x06, 0x96), (0x07, 0x98}};* post-dither *

tranjcs_dpr(d,c,alast)char d[], c[], a[];int last;

int i, n, jc, dat;char corn, dh, dl, jc2_hit;unsignedjc_seg, jc._offs;extem char jstat[l;intjc2com[2];extem char keep[], data flag;

jc2_hit = FALSE;for (n = 0; n <= last; n++)

{ * for each entry *jc =a[n];if (!jcstatcj])

continue; * if JC is inactive, pass on *jc_seg = SEG_OFF +jc * 0x80;corn = c[n] & 0xFO;for (i = 0; i <= 7; i++)

{ * scan thru data cormmands*if (cor = tran-slate[i][0])

break;

if(i 7){ *ahit*

jcoffs = (unsigned) tran slate[i][1];dh = d[n];dl = (c[n] << 4) & 0xF0;dat = + (dh << 8);if(i =1)

jc2com[i] = dat;jc2 hit = TRUE;}

while (peek(lOZ80jc_seg) = 'R'); * wait for JC read *pokeC('UTOZ80jc.seg); * set Update flag *poke(dl jc_offs++ jc_seg);poke(dhjc_offsjc_seg);poke('N',TOZ80j_cseg); * set New data flag *

else{ * chck renmaining *

corn = c[n] & OxOF;for (i = 8; i <= 13; i++)

{ * scan thru other cormnands *if (com = translate[i][0])

break

if (i <= 13){ *ahit*jcoffs = (unsigned) tran_slat[i][l];while (peek(TOZ80jcseg) = 'R'); * wait for JC read *poke('U',TOZ80jc_seg); * set Update flag *poke(d[nljc_offsjc_seg);poke('N',TOZ80jcseg); * set New data flag *

} * otherwise ignore command *

* if (dataflag && jc2hit && keep[l 1])logint_a(jc2pcjc2co);

1*--------------------*1startjc(n)intn;

unsigned j__seg;int count, i;char gg;

if (!jc stat[n])

91

return(TRUE); /* cant start a dead JC */jc_seg = SEG_OFF + 0x80 * n;for (i = 0x80; i < COMM_FLAG; i++)

poke(0,ijc_seg); p clear command area */for (count = 0; peek(JC_STATUSc_seg) !='W'; count++)

{ /* JC should set Waiting flag */synchronize(;if (count > 36) /* 2 sec limit */

shutdownbat(;printf"%c'nJC-%d not ready for starn",7,n);exit(2);

poke('G',COMM_FLAGjc_seg); / Go flag to JC */if (peek(COMM_ FLAGjcs-eg) !='G)

poke('G'.COMMFLAGjc_seg); /* try again */gg = peek(COMMFLAGjseg);

for (count = 0; peek(JCSTATUSjc_seg) != 'C; count++)/* check for Controlling flag */

synchronize();if(count > 18) /* 1 sec limit */

shutdown_bat();printf("%cnJC-d did not respond to start\n",7,n);dawdle(gg); dawdle(gg); dawdle(gg); dawdle(gg);exit(2);

log-joints(

exten char keep[];int jc2treas[2];

* read back joint card *jc2meas[0] = peek(0,0xE900) + (peek(1,0xE900) << 8);jc2meas[1] = peek(2,0xE900) + (peek(3,OxE900) << 8);if(keep[13])

loginta(jc2pmjc2eas);

monitorjc(

int sel;

while (TRUE)

sel = monjcxenuO;switch (sel)

case 1:

case 3:case 4:

watchjc(sel);

case 0:retumC(TRUE);

default:co(7); /* beep */break

/# =========================== ===== /watchjc(n)intn;

unsignedjcseg;char d, dl, dh, *oneb, *twob;long ld;

oneb = "%02x";twob = "%02x%02x";scrcdlrO;scrjrowcol(0,0);puts(" - X- -Y-\n");*O*/puts(" n");/*l */puts(" COMMANDED POSITION XXXX XXXX\n");1*2*/puts(" INDICATED POSITION XXXX XXXX\n-);/*3*/puts(" INDICATED RATE XXXX XXXX\n")J*4*/puts(" \n");/*5*/puts(" GAIN XXXX XXXX\n");/*6*/puts(" n");/*7*/puts(" n");/*8*/puts(" CALCULATED CONTROL XXXX XXXX\n");/*9*/puts(" DUTY CYCLE XX XX\n")'*10*/

92

putsC" DIRECTION CCC CCC\n")ll11*/puts(" LOOP COUNTER XXXXXX n");/*12*/puts(" n");/*13*/puts(" n");/*14*/puts(" n");/*15*/puts('cn Iit any key to exit");

scr_rowcol(O,2);printf("JC-%d",n);jc.seg = SEGOFF + Ox80 n;while (!scr_cstsO)

/* cn pos *dl = peek(0x80jc_seg);

scr_rowcol(02,25);dl = peek(0x82jcseg);

scrrowcol(02,33);/* ind pos */dl =peek(OxOOjcseg);

scrrowcol(03,25);dl = peek(0x02jc_seg);

scr_rowcol(03,33);/* ind rat */d = pek(xO4jc_seg);

scrjrowcol(04,25);dl = peek(Ox06jc_seg);

scrrowcol(04,33);/* gain*/dl =pek(Ox84jcseg);

scrrowcol(06,25);dl = peek(Ox86jc_seg);

scr rowcol(06,33);/* calc ctr */dl =peek(OxO8jcseg);

scr rowcol(09,25);dl =peek(OxOAjc_seg);

scrrowcol(09,33);/* duty cy */d = peck(Ox0Cjc.seg);

scrrowcol(10,27);d = peek(0x0Djc_scg);

scrrowcol(10,35);/* direct */d = peek(OxOEjcseg);

if(d) puts(d = peek(OxOFjcseg);

if(d) puts(/* loop t */scr-rowcol(12,23);d = peek(0xl2jc.seg);d = peek(Oxl ljcseg);d = peek(OxlOjc_seg);}

printprintprit

dh = peek(Ox81 jcseg);printf(twob,dhdl);

dh = peck(0x83 jc_seg);printf(twob,dh,dl);

dh = peek(OxOl jcseg);printf(twob,dhdl);

dh = peek(0x03jc_seg);printf(twob,dh,dl);

dh = peeki0x(O05(OxOjcseg);printf(twob,dh,dl);

dh = peek(OxO7cseg);printf(twob,dh,dl);

dh = peek(Ox85 jcseg);printf(twob,dh,dl);

dh = peek(Ox87 jcseg);printf(twob,dhdl);

dh = peek(0x09jc_seg);printf(twob,dh,dl);

dh = peek(OxOB jcseg);printf(twob,dh,dl);

printf(oneb,d);

printf(oneb,d);

scrrowcol(11,26);("REV"); else putsCFOR");

scrrowcol(11,34);:"REV"); else puts("FOR");

f(oneb,d);tf(oneb,d);foneb,d);

/*= . .. ........... ========........... '/monjc menuO

int query,

scrlrO;scr_rowcol(2,0);printf(" MIT SSL BAT/ICS\n\n");printf(" Monitor Joint Cardn\n");printf(" 1. WRIST A & B\n");printf(" 2. SHOULDER P & Yn");printf(" 3. ELBOW & LEFT ARMn");printf(" 4. CAMERA T & P'n");printf(" . EXl\n\n");printf(" EnterSelection: ");scanf("%dn",.&query);retum(query);

readjoints(comn,post)/* reads positions and conmmands for all power modules. Note that outputis a 12 bit number left shifted into a 16 bit number. */int com[8], post[8];

char d, dh;

/* command pos */dl = peek(0x80,0xE880);com[O] = dl + (dh << 8);dl = peek(0x82,0xE880);comrn[l] = dl + (dh << 8);dl = peek(0x80,0xE900);com[2] = dl + (dh << 8);dl =peek(0x82,0xE900);com[3] = dl + (dh << 8);dl = pek(0x80,0xE980);com[4] = dl + (dh << 8);dl = peek(Ox82,0xE980);

/* wristA*/

/* wrist B */

/* sh pitch *

/* sh yaw */

/* elbow */

dh = peek(0x81 ,OxE880);

dh = peek(0x83,xE880);

dh = peek(Ox81,OxE900);

dh = peek(0x83,0xE900);

dh = peek(0x81 ,0xE980);

dh = peek(Ox83,0xE980);

93

com[5] = dl + (dh << 8);dl = poek(Ox80,xEAOO);com[6] = dl + (dh < 8);dl = peek(Ox82,0xEAOO);cornm[7] = dl + (dh << 8);

/* indicated pos */dl =peek(OxOO,OxE880);post[O] = dl + (dh << 8);dl = poek(OxO2,0xE880);post[1] = dl + (dh << 8);dl = peek(OxOO,OxE900);post[2] = dl + (dh « 8);dl = peek(OxO2,OxE900);post[3] = dl + (dh << 8);dl = peek(OxO0,OxE980);post[4] = dl + (dh << 8);dl = pek(OxO2,0xE890);post[5] = dl + (dh << 8);dl = peek(OxOO,OxEAOO);post[6] = dl + (dh << 8);dl = pe(Ox020xEAOO);post[7] = dl + (dh << 8);

/* cooling pump */dh = peek(Ox81,OxEAOO);

/* camnra tilt (pitch) */dh = peek(0x83,OxEAOO);

/* camera pan (yaw) */

dh = peek(OxO1,OxE880);/* wrist A */

dh = peek(0x03,OxE880);/* wrist B */

dh = peek(OxO1,OxE900);/* sh pitch */

dh = peek(0x03,OxE900);/* sh yaw */

dh = peek(0x01,OxE980);/* elbow */

dh = peek(0x03,OxE890);/* cooling pump */

dh = peek(Ox01,OxEAOO);/* camera tilt (pitch) */

dh = poeek(0x03,0OxEAOO);/* camera pan (yaw) */

display_arm()

int allpos[8], armpos[5], dummy[8], i, v;float gripx, gripy, grip_z, angles[5];exten char potnamell;

scrdrc0;scrrowcol(O,O);puts(" ENCODER\n");*O*/puts(" HEX DEC JOINl\n");/*1*/for (i =3; i< 8; i++)

scr_rowcol(i.,18);puts(potnam[i]);scr rowcol(i + 7,5);printf("%c%c",233,i - 3 + 1');

scrrowcol(19.4);puts("Hit any key to exit");scrrowcol(9,10);puts("ANGLE (DEG)");

while (!scr_cstsO)

readjoints(dunmy,allpos);armpos[0] = allpos[0];armpos[l] = allpos[ll];armpos[2] = allpos[4];armpos[3] = allpos[3];armpos[4] = allpos[2];enc2tht(arrnmpos,angles);batfor(angles,&grip_x,&grip_y,&gripz);for (i = 0; i <5; i++)

scr_rowcol(3+i,0);v = annrmp[i];printf(" %04x %6d",v,v);scrrowcol(10+i,10);printf("%6.2f',angles[i] * R2D);

scrrowcol(16,4);printf("X = %5.lf Y = %5.lf Z = %5.l",gripx,grip_y,grip.z);

_ _ _ _ _}*

94

/* SUPPORT4.C-- BAT support routines version 6.4** by John Spofford and David E. Anderson, M.I.T. Space Systems Laboratory*

#include "PORTNUMSH"1* --------------- -------- ---- *adjustpots()

int number, newval, i, temp, fn, iii;char tempc, done, done2;extm int div_or_ mul[8], p_gain[8], maxp[8], min_p[8];extem char icnvt[8], icnvt2[8];extem char *potnam[8];extem float aligngain[8];extem int alignoffset[8];

done = FALSE;while(!done)

{scr_dr(;scrrowcol(3,0);printf(" ICS Pot Adjustments'n\n" );for (i = 0;i < 8;i++)

printf(" %ld %s\n",i+l,potname[i]);printf(" S Save to file\n");printf(" L Load from file\n");printf("[CR Exitn");printf(\n\nEntrr Selection: ");tempc = scr_ci0;switch(tempc)

case 13:done = TRUE;bSak

case 'S':case 's':

if((fn= creat("ICSDAT")) = -1)

beep(25);printg("UNABLE TO WRITE FILEn");pause(250);

elsefor (i = 0;i < S;i++)

putw(div_ormul[il,fn);putw(p_gain[i],fn);putw(max_p[i],fn);putw(minp[i],fn);putc(icnvt[i],fn);putc(icnvt2[i],fn);

close(fi);brea

case 1':case 1':

if ((fn = open("ICSDAT",0)) = -1)

beep(25);printf("UNABLE TO OPEN FLE\n");pause(250);

elsefor (i = O;i < 8;i++)

{divormul[i] = getw(fn);pgain[i] = getw(fr);maxp[i] = getw(fn);min_p[i] = getw(fn);ivtli] = getc(fn);icnvt2[i] = getc(fn);

close(fn);break,

case '1':case ':case '3':case 4':case '5':case '6':case '7':case '8':

iii = tempc -'1';scrdrO;scrrowcol(1 ,0);printf("Joint: %s\n",potname[iii]);printf(" Current Settingsa");prind(n"n);printf(" -Maximum Position: %5dn",maxp[iii]);printf("2--MinimumPosition: %Scn",min_p[iii]);printf("3-Pot Shifts: %Sdn",p_gain[ii]);printf"4--Shifl Direction ");

95

if(divor_ml[iii])printf("RIGHT");

elseprintf("LETFW");

printf"CnWn");printf("5--Pot convert(");printf("%2d, % 2d)\n",icnvt2[iii],icnvt[iii]);printf("[CR--Exit");done2 = FALSE;while(!done2)

scr_rowcol(18,0);printf("Enter Selection: ");scr rowcol(18,18);while((tempc = scrcstsO) 0)

scrrowcol(15,10);temp = (convert(icnvt2[iii],icnvt[iii]) & OxFFF;printf("Adjusted pot:%5dadjust(temp - 1024, iii));

scrjowcol(18,17);printf(%c",tempc);sc_rowcol(21.0);printf(" ");

switch(tempc)

case 13:done2 = TRUE;biak

case '1':max_p[iii] = getnw_val(4,31);b-lq

case '2':min_p[iii] = getLnewval(5,31);beak;

case '3':pgain[iii] = getnew_val(6,31);beak

case 4':getnew_val(21,16);div_ormul[iii] = !divormul[iii];scrowcol(7,31);if(div_or_mul[iii])

printf("LEF ");else

printf("RIGHT");bmea

case '5':temp = get_new_val(10,31);newval = getnew_val(10,31);scrrowcol(21,0);print(" );scrowcol(10,31);printf(4(%2d,%2d) ",tempnewval);icnvt2[iii] = temp;icnvt[iii] = wval;bseak

case '6':scrrowcol(21 ,0);printf("Enter offset value (int)");scanf("%dn",&alignoffset[iii]);scrjowcol(22,0);printf("off: 9%4d, ",alignoffset[iii]);printf("gain: %8.5f',aligngain[iii]);

case '7':scrrowcol(21 ,0);printf(Enter gain value (float)");scanf( "% fn,&align_gain[iii]);scrrowcol(22,0);printf("off: %4d, ",align_offset[iii]);printf("gain: %8.5f',align_gain[iii]);break

defaultscrrowcol(22,0);printf("%clnvalid Resp.,Please Reenter",7);pause(100);scrjowcol(22,0);printf(" ");bacak

b;ealdefault

scrrowcol(22,0);printi"%clnvalid Entry, Please Re-Enter".7);pause(100);scrrowcol(22,0);printf(" ");scrrowcol(15,17);printf(" );scrrowcol(15,17);

96

brcak;

get new val(worJoc)int wor, loc;

intret_val;

scr_rowcol(21,0);printf("Enter New Value");scr_rowcol(worJoc);printf(" ");scrjrowcol(worjoc);scanf("%d",&rtval);retumrn(rt_val);

/* stat --- check status *//* JC_STATUS: C = controlling, W = waiting */statO

char statword;char jc;unsignedjseg;ext char jcstat[];

/* is PUMA ready? */statword = inport(P1A);scr_rowcol(6,21);if (statword & 0x02)

scrco(2);else

scr co(25);

for (jc = 0; jc <7; jc++)if (icstatjc])

/* ifjc loaded */jseg = SEGOFF +jc * Ox80;statword = peek(JCSTATUS jseg);scr_rowcol(5,16 + jc);scr co(statword);

/* ------------------------------------------ */* batports.c -- initialize PC ports for BAT** *

/* batports.c - sets the PPI ports for BAT control */baports()

setport(0,0,l,0); /* OA:out OB:in OC:out */setport(l,1,0,0); /* 1A:in lB:out C:out */setport(2,1,1,0); /* 2A:in 2B:in 2C:out */setport(3,0,0,0); /* 3A:in/out 3B:out 3C:out */setport(4l,1,,0); /* 4A:in 4B:in 4C:out */

/* -------------------- -----------------..---/* setport.c -- sets a 8255s ports to input or output mode */setport(portnum,a,b,c)int portnuma,b,c;

char cw; /* mode control word */int addr, /* port address */

if (portnum < 0 11 portnum > 4)return (-1); /* on anerror /

if(a--0) /*port A*/cw = Ox80;

elsecw = Ox90;

if(b !=0) /* port B '/cw =cw +2;

if(c != 0) /* port C*/cw = cw + 9;

addr = 0x303 + (portnum << 2); /* addr range is 300..313 hex */outport(addr,cw); /* control ports are 303,307,...,313 */return (0);

/* -------------- -------------------------

/* screen.c -- sets up BAT video background. 1/8/84 jrs */screen()

sacr_setmode(0);scrsetupO;sc_cursoffO;

puts("");printf(" THRUSTERS %cfffffff%c ff ACTUATORS ff ",218,191);/* 0 */

97

puts(" BsATSOFT2 ");/* 1 */puts(" 2 2 2 2 2 2 2 V 6.4 2 2 2 2 2 22 2 ");/* 2 */printf(" 2> 2 2 2 %cfffffffc 2 2 2 2 2 ",192,217);/* 3 */puts(" 2 2 2 2 2 2 > 2 > 2 2 2 > > "):/* 4 */puts(" 2 2 2 2 2 JC: .... 2 2 2 2 ");/* 5 */puts("> 2 2 > 2 > PUMA 2 5 5 5 5 5 5 2 ")/* 6 */puts(" 2 2 - 5 _5 2 5 5 2 2 2 5 ");/* 7 */puts(" S 2> 2S 2 5 555 -2 5 ");/* 8 */puts ( " 22sS2oos s DRAW ... 2 > 2 > 2 > > 2 ");/* 9 */puts (" Ooos 5ooo HEAD ... +>+>+>+>+>+ +>+") :/* 10 */puts(" 2 2 52 2 5 MAST ... 5 55 2 5 2 55 ");/* 11 */puts(" 2 2 2> > 2 SLAVE ... -5 2 5 5 5 52 "):;/* 12 */puts(" - 5 5 52 FLYING: > > > 5 5 2 5 5 ");/* 13 */puts("- 2 2 55 ...... 2 2 2 2 ");/*14*/puts(" 2 2 2 2 RCLW ..... 2 22 2 2 2 "):/* 15 */puts(" -> 5 52 LCLW ..... 2 2 2 5 5 5 ");/* 16 */puts(" 2 2 2 2 BMCR ..... 2 2 " );/* 17 */puts(" BCLW ..... 2 5 5 5 2 555 ");/* 18 */printf(" X Y Z c %c %c ",232,233,234) ;:/*19*/puts(" W W S S E C C ");/* 20 */puts(" A B P Y L T P "):/* 21 */printf(" command:%c actual:%c ",233,4);/* 22 */puts(" ");/* 23 */puts(" MIT SSL Beam Assembly Teleoperator ");/* 24 */

/* --------------------------------------screen2(

extem char draw,puma_on,head_controller, dataflag;extem char boost_fag,head_c'l _cccmd;extem char master_cometed,slave_am_onmatr_flying;extem char head_ctrl_fyinrigh_claw.left_claw_ste;extem char beam_carrier, beamdaw;extem int set triad;extem int tri_for, trirev;/* for successive triad use */scrrowcol(21,3);puts("thruster");

scr_rowcol(09,19);if (draw) puts("ON ");else puts("OFF");

scr_rowcol(1,4);if(puma_on) puts("ON");else puts("COFF);

scrrowcol(08,15);if(data_lag) puts("DATA");else if(settriad = 0) pus(" ");

scr_rowcol(07,15);if (boostflag) puts("BOOST");els iset triad O) puts(" ");

scrrowcol(10,19);if (h ad_ctrl_comcted) puts("ON ");else puts("OFF");

scrrowcol(11,19);if (master_concted) puts("ON ");else puts("OFF");

scrrowcol(12,19);if (slave_arm_ on) puts("ON");else puts("OFF");

scrrowcol(14,15);if (master_ying) puts("MASTER");elseif(head ctrlfying) puts("HEAD ");else puts("STICK ");

scr_rowcol(l5,18);if (rightclaw) puts("CLOSE");else puts("OPEN ");

scrrowcol(16,18);if (left_claw stae) puts("CLOSE");else puts("OPEN ");

scr_rowcol(17,18);ifbeamncarrier) puts("BACK ");pcounterintuitive for Huntsville*/else puts("OUT ");

scr rowcol(18,18);if(beamclaw) puts("CLOSE");els puts("OPEN ");

scrrowcol(23,0);puts(" TRIAD Up Next: ");if(trifor = 1) puts("CGrabbeam ");if(trifor = 2) puts("Grabslv ");iftri_for = 3) pus("Stow ");iftrirev = 1) pus("Cjoint ");if(trirev = 2) puts("Rgrabslv ");

98

if(tri_rv = 3) puts("Rstow ");

strange()

int val2, vail;char i, signc;extern int align-offset[5];exten float aligngain[5];extem char icnvt[8], icnvt2[8];extem char *potnae[81];

scrcO);scr_cursoff);scrrowcol(,0);puts(" BAT Master Ann MonitoX\nn");puts(" Joint pot adjust align\\n"");/* .0123456789a123456789b123456789c123456789..line*/for (i = 0;i < 8;i++)

printf"%s\n %6.3f %5dn",ponamx[i],align gain[i,align_offset[i]);while (scr_csts( != 13)

for (i = O;i < 8;i++)

va12 = adjust((convert(icnvt2[i],icnv[i]) & OxOFF) - 1024, i);vail = align(val2i) & OxOFFF;signc ='-';if(val2 < 0)

val2 = -va12;else

signc =scrrowcol(i*2 + 5,13);print"(%2d,%2d)%c%04x%03x",icnvt2[i],icnvt[i],signc,val2,val 1);

/* ..------- ------....-.-..-..-.................keyboardconuand(tempc)char tempc;

extenm char ccamn_disp;extem char data_flag; /* logical flags */extem char masterflying, slave_arnnon;extem char left_claw_state, masterconoectcd;extcm char headcontroller, pumna_ n. draw;extem char beacarrier, beamclaw;extem char sparel, spare2; *spare pnuematics 1 and 2*/extemint pgain[8];exten int set_triad;extcm char icnvt[8], icnvt2[8];int i;tePnpc = toupper(ternpc);switch (tempc)

case 'C: r change camera control drive*/print %c",7); /*beep*/scrrowcol(14,15);if(haad_controller)

headcontroller = FALSE;icnvt1] = 9;icnvt[2] = 10;p_gain[ll] = 0;p_gain[21 = 0;puts("STICK");

else

head4controller = TRUE;icnvt[1] = 5;icnvt[2] = 6;p_gain[l] =0;pgain[2] =0;put(HEAD ");

break;case ¶': /* transmit to puma */

printf('%c",7);scr_rowcol(1.4);pumaon = !puma_on;if(pumaon)

putsCON ");else

puts("OFF");bmak;

case )': /*start or stop datalogging */dataflag = !dataflag;scseen20;break

case'E': /* draw to screen */printf(%c",7);draw = !draw;scrrowcol(9,19);

99

if(draw)puts("ON ");

elseputs("OFF"):

breakcase 'R': reset cs /

outport(POX,C6HI); /* reset JCS */outport(P2X,C7HI); / reset propulsion controller */for (i = 0; i < 20; i++); /t pulse width /outport(POX,C6LO);outport(P2X,C7LO);for (i = 0; i < 36; i++)

synchronize0; /* letJCS check memory for (i = 1; i <= 4; i++)

startjc(i); /* start card #1,2,3.4 */break

case T: /* tune joints I/setupjointsO;screcn;screen2(;break;

/* excess fat removedcase T: * adjust right arm *

adjustpots();screen();screen20;break

case 'B': * test feedback from BAT *feedback;scn0;screen2();break

case 'X':stange0;screenO;screen2(;break

cas 'L':leftclaw state = left clawjstatc; /* toggle */if (ftdawstate)

outport(P2X.C1LO); /* dose dclaw */else

outport(P2X,C1HI); open daw */co(7);scren2();break

case 'U':jointstepO;screen(;screen20;break;*/

case '?':print_help();scren(scren2);beak

case W': /* supervisory control calls setup menu for TRIAD*/triadO;screen(scrcn2();break

case 'F:setrimO;screenO;scmon2(;break

case 'G':setdataO;screenO;scren20;break;

case 'J:monitorjc);

screen();break

case 'A':displayarm();scrn0;,scren20;break

case 'Z':seLtboost(;scr2n0;

beakcase 'M':

beam_carrier = !beamcarrir;if(beam carrier)

outport(P2X,C3HI); /pextcnd beam carrier*/)

100

else

outport(PX,C3LO); /*etract beam carrier*/}

co(7);screen2();b-ak

case 'N':beam_claw = !beamclaw;if(beamclaw)

outport(PX,C4LO); /*close beam carrier claw*/

else

outport(P2X,C4HI); /*open beam carrier claw*/

co(7);scrcn2();beak

case 'S':if(slave anrmon)

slaverest(O);else

slaverest(1);break

case '0':sparel = !sparel; /*toggle spare pnuatics 1 iif(sparel)

outport(PlX,C6LO);

else{outport(P1X,C6HI);

co(7);break

case 'Y':commdisp = !cormdisp;break

case 'Q':spa2 = !spare2; /*toggle spare pnuematics 2*/if(spare2)

outport(P1X,C7LO);

else

outport(P1X,C7HI);

co(7);break

case 'V:if(settriad=l)

Ico(7);pointto_point0;

else if(settriad>=2)

coC7);leanmO;

beakcase 'K':

load_from_filc("zcro.bdt ");break

case '1':load_from_file("frcdpid.bdt ");beak

case '2':load_fom file("wrst.bdt ");break;

case '3':load_from_filc("cltst.bdt ");beak

case '4':load_from_file( " shptstbdt ");beak

case '5':load_from_file("shytst.bdt ");breaks

case '6':load_fromffle("tptst.bdt ");break

case '7':load_from_file("triadl.bdt ");break

case '8':loadfromfile("triad2.bdt );break

101

case '9':

case '0':

loadfromfileCtriad3.bdt );

load_from_file("fred.bdt ");bmak

scrclr();scrowcol(0,0);puts(" BAT / ICS Keyboard Assignments (Main)puts" (6.4)puts("A: display arm pos 2S: slave rest/resetputs("B: 2T: set joint cntrlputs("C: cam head/stick 2U:puts("D: toggle data log 2V: TRIAD activateputs("E: dis/enable draw W: TRIAD setupputs("F: thrust trim 2X: monitor masterputs("G: record data -Y: show commandsputs ("H: > Z: boost arm/thrustputs("I: > ?: print this listputs("J: monitor joint cd21: load fredpidputs("K: kill jc duty cyc>2: load wrtstputs("L: use left claw 23: load eltstputs("M: deploy beam cr 24: load shptstputs("N: use beam cr claw25: load shytstputs("O: use spare pnue 16: load tptstputs("P: dis/enable PUMA 27: load triadlputs("Q: use spare pnue 28: load triad2puts("R: reset JCS i tab: shutdown/exitputs(" ");puts(" Type CR) to retum tomain loop ");while (scrcsts( = 13);

"I ;

'1);"I ;

''I;"I ;11);.1);

11);1.);11);.1);11);11);11);11);

/*Slaverest is a function designed to cutout excess use of the rightarm motors. slaverest can also be used to change the master arm offsetsfor smooth power-up.

(0) Power down slave arm. Gains to zero.(1) Prepare to power up. Set offsets to reconcile master and slave.(2) Offsets have been digested. Reset gains.

Dave Anderson 5/20/87 */slaverest(state)int state;

extem int align_offsct[8], currmastr[5], slavetoggle;extem char mastercormected, masterflying, slave_arm_on;extem float aligngain[8];int com[8], posr[8], i, fn, temp[5];int ord[5] = (0, 1, 4, 3, 2);switch (state)

{case O:

case 1:

case 2.

default

slave_arm_on = FALSE;screan20;/* save and zero out slave armnn gains */write_out_data("slavercst.BDT ");load_fromfile("slavezero.BDT ");break;

slavearmon =TRUE;screen2);read.joints(com,posr);for (i=O;i<;i++)

if ((posr[ord[i]] & 0x8000) = 0)posr[ord[i]] = (posr[ord[ill >> 4) 1 OxF000;

elseposr[ord[i]] = posr[ord[i]] >> 4;

alignoffset(i+3] = posr[ord[i]]/align_gain[i+3] - cutrmaster[i];

slave_toggle = 1;break

/* load gains to arm. */slave-toggle = 0;load_from fileC(slaverest.BDT ");

scr_rowcol(12,19);puts("ERg");brealS

resetpid()

int emp;/* reset integrators */while(peek(TOZ80,0xE900)= 'R'); poke(U',TOZ,0,OxE900);poke(0,OxAl,0xE900);poke(0,OxA3,0xE900);

102

print_elp()

}}

}1.·-- ·

poke('N',TOZ80,OxE900);while(peek(TOZ80,0xE880)= 'R'); polk(U'.TOZ80,OxE880);poke(O,OxA1.OxE880);poke(,OxA3,0x880);poke('lN.TOZ80,0xE880);while(peek(TOZ800xE980)= 'R'); poke(UJ',TOZ80,OxE980);poke(O,OxAl,OxE980);poke('N',TOZ80,OxE980);/* once the joint cards have read the zero (set toz80 to 'O')*//* replace original nmultl value */temp = peek(OxO,OxE80); /* read counter on jcl */while(teomp = peek(Oxl,OxE880)); poke('tU.TOZ80,0xE900);poke(1,OxA1,OxE900);poke(l,OxA3,OxE900);poke('N',TOZ80,xE900);poke(U'lTOZ80,xE880);poke(1,OxAl,OxE880);poke(1,OxA3,OxE880);poke('N',TOZ80,OxE880);pokc('IJ,TOZ80,xE980);poke(l,OxAl1.OxE980);poke('N'TOZ80,0xE980);

/---..--........

103

l--========-====I- = ===========*/* SUPPORT5.c -- fakes SUPERVIS.c & MOVE.c 10/23/85 */

#include "PORTNUMSH"#define FACTOR 0.002 /* for pot trim *#define PDEDZON 80 /* for pot trim */#include <mathfh>#define PI 3.1415926536#define PI2 1.5707963268#define R2D 180.0/PI#define D2R PI/180.0' ============================.*/

supervis({co(7); /* beep */

float a[6][6] = /* adjustment matrix */{ 0.67,0.0,0.0,0.0,0.0,0.0 }, { 0.0,0.67,0.0,0.0,0.0.0 },{ 0.0,0.0,0.33,0.0,0.0,0.0 }, { 0.00.0,0.0,.0.67,0.0,0.0 },{ 0.0,0.0,0.0,0.0,0.67,0.0 }, { 0.0,0.0,0.0,0.0,0.0,0.33 } };

trimthrust(j,x) /* adjust thruster commands */intj[], x[];

inti,k;float in[6], out;

for (i = 0; i < 6; i++)in[i] = (float) j[i];

for (i = 0, i < 6; i++){out = 0.0;for(k=0;k<6;k++)

out += a[i][k]*in[k];x[i] = (int) out;

/*======= .======.=============*/set_trim() /* set thruster trim matrix */

int sel, i,j;floatv;char pdat[7];

for (i = O;i < 6;i++)pdat[i] = 128;

tranpuma(pdat,5);while (TRUE)

/* zero ach command *//* Prevent thruster start-up */

load_trim(;break

savetrim0;break

puts(r\nnX=0, Y=l, Z=2, P=3, R=4, Y=S\n");puts(" I is thruster output\n");puts(" J is joystick inputn");puts("Enter I, J: ");scanf("%d,%d'n"',&i&j);printf("A[I[J] is %6.2fn",a[i]lj]);puts("Enter new A[I[J]: ");scanf("%fn",&v);if((i >=0) &&(i < 6) &&(j >=0) &&(j <6))

a[i][j] =v;else

co(7); /* beep I/break

puts(\n\nX=0, Y=I, Z=2, P=3, R=4, Y=5\n");puts(" Iisthruster outputs(" I is tr otpt");puts(" J is joystick inpun");puts(C\i");for (i = 0; i < 6; i++)

printf(" %d ",i);puts(n");for (i = 0; i < 6; i++)

printf("%d: ",i);for (j = 0; j < 6; j++)

printf("%5.2f ",a[i][j]);printr',n");

puts('nHit any key...");gtchaO);

104

sel = trim_selectO;switch (sel)

case 1:

case 2:

case 3:

case 4:

case 5:

case 6:

cass 0

break

puts('.nSELECT THRUST AXIS TO TRIM");puts("\nX=0, Y=1, Z=--2, P=3, R=4, Y=5: ");scanf("%dn",&i);if((i >= 0) && (i < 6))

joystick(i);else

co(7); /* beep */break

showthrust();break

retumCTRUE);

defaultco(7); /* beep */break

/* -------------------/joystick(ax)intax;

int thr_ctrl, thrin[6], throut[6], i, temp;extem int stickoff[6];float pot[5];char pdat[7];

scrdrO);scrrowcol(15,5);puts("Hit any key to exit...");

for (i = 0; i < 6; i++)thr_in[i] = 0; /* clear *

scrrowcol(2,0);switch (ax)

case 0: / X*Iprintf("- Y - I- Z - I- P - I- R - I- Y -I");break

casel 1: / Y *I/printf("l- X - I- Z - I- P -I I- R -I I- Y -I");break

case 2 p Z */pritf("l- X -II- Y -II- P-I I- R - II- Y -I");break

case 3: P*P*/printf"- X -I - Y -I I-Z -I I- R-l I- Y -I");break

case4: /* R */printf("l- X -I -Y-I I- Z-I -P-I I- Y -I");break

cases 5: /* Y *I/

default:

printf("l- X - I Y -- Z -I -P-I I- R -I");break

co(7); rbeep */retum(FALSE);break

scr_rowcol(8,0);printif"THRUSTERS ARE ENABLED");

while (!scrcsts())

synchronize0; /* not too fast*/scr_rowcol(4,0);for (i =0; i < 5; i++)

temp = convert(0,i) - 1024;if(temp<0) /*ldeadzone *l

if (temp > -PDEDZON)temp = 0;

elsetemp = temp + PDEDZON;

elseif (temp < PDEDZON)

temp = 0;else

temp = temp -PDEDZON;pot[i] = (float) temp * FACTOR;printf("%7.2f ",pot[i]);

switch (ax)

case 0: Px*/thr_in[O] = -(convert(7,0) - 1024 + stickoff[4]);a[ll]ax] = pot[0];

105

}

a[2][ax] = pot[l];a[3][]ax = pot[2];a[4][ax] = pot[3];a[5][ax] = pot[4];bmak

casel: /* Y*/thrin[l] = -(convert(8,0) - 1024 + stickoff[5]);a[0][ax] = pot[0];a[2][ax] = pot[l];a[3][ax] = pot[2];a[4][ax] = pot[3];a[Sl[ax] =pot[4];break

case2 /* Z*/thr_in[2 = -(convert(6,0) -1024 + stickoff[3]);a[0][ax] =pot[0];a[ll[ax] =potll];a[3][ax] = pot[2];a[41[ax] = pot[3];a[5][ax] = pot[4]bmeak

case3: /P */thr_in[3] = -(convert(0,9) -1024 + stickoff[2]);a[0[ax] = pot[0];a[][ax] = pot[1];a[2][ax] = pot[2];a[4][ax] =pot[

3];

a[5][ax] = pot[4];break

case 4: /* R */tbrin[4 = -(convert(0,10)- 1024 + stickoffI1]);a[Ol[ax] = pot[];a[ll[ax] =pot[l];a[2][ax] = pot[2];a[3][ax] = pot[3];a[5][ax] = pot[4];b-Iq

case 5: /*Y*/thrin[5] = (convert(0,8) -1024 + stickofflO]);a[0O[ax] = pot[0];a[ll][ax] =pot[l];a[2][ax] =pot[2];a[3][ax] = pot[3];a[4][ax] = pot[4];bleak;

tri_thrust(thr_in,throut);for (i = 0; i <= 5; i++)

temp = thr_out[i];temp = (temp > 127) ? (127): (temp); /* limit */temp = (temp < -127) ? (-127): (temp);pdat[i] = (temp + 128);if(pdat[i] = OxFF)

pdat[i] = OxFE;

tran_puma(pdat,5);

for (i = O;i < 6;i++)pdat[i] = 128; /* zero each command /

tran_puma(pdat,5); /* Prevent thruster start-up */

/* --------------- ----showhrust()

int thr_in[6], throut[6], i;extem int stickoffi6];

scrdrO;scr_rowcol(3,15);puts("RAW TRIM");scrrowcol(15,5);puts("Hit any key to exit...");scrrowcol(13,5);puts("Need PPF80 < TRIM < 0080");while (!scr_cstsO)

thr_in[] = -(convcrt(7,0) -1024 + stickoff[4]); /*X*/thr_in[] = -(convert(8,0) -1024 + atickoff[5]); /IY*/thr_in[2] = -(convert(6,0) -1024 + stickoff[3]); /*Z*/thr_in[3] = -(convert(O09) -1024 + stickoff[2]); /*P*/thr_in[4] = -(onvert(0,10) - 1024 + stickoff[1]); /*R*/thr_in[5] = (convert(0,8) -1024 + stickoff[O]); /*Y*/trim_tnust(thr_in,thr_out);for (i = 0; i < 6; i++)

{scrrowcol(5 + i,15);printf("%04x %04x",thr_in[i],throut[i]);

106

-------------------- /

trimmnenu()

scrdr();scr rowcol(2,0);printf(" M.I.T. SSLan");prinf(" Beam Assembly Teleperator\n\n");printf(" Thruster Adjustmentn\n");printfI" 1. LOAD FROM FILEn ");printf(" 2. SAVE TO FILEn");printf(" 3. SET VALUE IN MATRIXn");printf(" 4. DISPLAY MATRIXDn");printf(" 5. TRIM JOYSTICKS w/POTS\n");printf(" 6. SHOW JOYSTICK TRIMn");printf(" O. EXITn");scrrowcol(22,0);printf(" EnterSelection ");retun(0);

/*--------------------*/trimselect()

int query;

trinmrnu ;scr_rowcol(22,19);prin" ");scrrowcol(22,19);scanf("%d",&query);retun(query);}

/*--------------------*/loadtrim()

int i, k, fchar fnare[20];

print" Load filenam [.TDT: ");scan("%gn",fane);strcat(fam,".TDT");if ((fn = open(fnam-,0)) =-1)

scrrowcol(22,0);/* beep(25); */printf("ERROR in loading");/* pause(250); */

else

for (i = 0; i < 6; i++)

for (k = 0;k< 6;k++)fscanf(ffn,"%f ",&a[i][k]);

fscanf(fnn");

close(fin);

returnm();

/* --------------------*/save_trim()

int fn, i k;char fnaen[201;

printf(" Write filename [.TDT]: ");scanf("%s\n,fiamc);strcat(finani,".TDT");if ((fn creat(fnarnm)) = -1)

{scrrowcol(0,0);/* beep(25); */printf("Unable to Write File");/* pause(250); */

else

for (i = 0; i < 6; i++)

for (k = 0; k <6; k++)fprintf(fn,"%97.f ",a[il[k]);

fprintf(fn'n");}

close(fn);

calc_boost(jc,jp,tb)int jc[5], jp[5], tb[6];

107

float comtht[5], postht[5], xcyczc, xp,yp,zp;int zero;exiem float boost_x, boosty, boost_z;

zero = 0;enc2tht(jc,comtht);bat_for(comtht,&xc,&yc,&zc)enc2tht(jp,postht);bat_for(postht,&xp,&yp,&zp);tb[0] = (int) (boostjx * (xp -xc));tb[l] = (int) (boost y * (yp -yc));tb[2] = (int) (boostz * (zp- zc));tb[3] = zero;tb[41 = zero;tb[5] =zero;}/* =========================.===

batfor(tx,y,z)float t[5];float *x, *y, *z;

float cl, sl, c2, s2, c3, s3, c23, s23, c4, s4;extem float 11,12,13;

cl = eos(t[0]);sl = sin(t[01);c2 = cos(t[l]);s2 = sin(t[1]);c3 = cos(t[2]);s3 = sin(t[21);c4 = cos(t[3]);s4 = sin(t[3]);c23 = c2 * c3 -s2 s3;s23 = s2 * c3 + c2 s3;

·x =ll*cl*c2+ 12*cl*c23 + 13*(cl*c23*c4 -sl*s4);y =11'slc2 + 12*sl*c23 + 13*(sl*c23*c4 + cls4);z = 11 s2 + 12*s23 + 13*s23*c4;

/* ======================= *1float 11 = 16.0;float 12 = 14.1;floatl3= 3.5;

int shyoff= 1692;int shpoff= -3760;intelboff= -12656;intwraoff= 2992;int wrboff= -3925;float shy2thl = 6.41e-5;float shp2th2 = 4.80e-5;:float elb2th3 = -6.00e-5;float wry2th4 = 4.80e-5;float wrr2th5 = 5.15e-5;

enc2tht(enc,tht)int enc[5]; /* wra,wrb,elb,shy,shp */float tht[5]; /* shy,shpelb,wry,wrr */

Iint i, wra, wrb;

wra = enc[0] + wraoff;wrb =enc[l] + wrboff;tht[0] = shy2thl * (float) (enc[3] + shyoff);tht[l] = shp2th2 * (float) (enc[4] + shpoff);tht[2] = elb2tb3 * (float) (enc[2] + elboff);tht[3] = wry2th4 * (float) (wra - wrb);tht[4] = wrr2th5 * (float) (wra + wrb);

float boostx, boost_y, boostz;setboost

extem float boostx, boost_y, boost_z;char ch;float val;

scrclr0;scrrjowcol(5,0);printf(" CURRENT BOOST GAINS: X %5.1fn",boostx);printf(" Y %5.1 in"boost_y);printf(" Z %5.lfn",boostz);printf('n ENTER NEW BOOST GAIN LIKE THISn");printf(" x ff.f[CR] x = {X,Y,Z)\n ?");scanf("%c %fn",&ch,&val);ch = toupper(ch);switch (ch)

case 'X:

108

boost_x = val;

case 'Y:boost-y = val;by*

case 'Z':boost_z = val;byk

defmaltcoC7);beak

109

/* DATERS.C -Data logging andtiming functionsFunctions:

t/ statlog() logchar()PL index_log() logcharaOt" logint( logfoat()/* logintaO logfloata

/"t ~ dumplog()/jpL John Spofford June, 1985, Jan 1986

/* revised July 1987 DEA V#include "DATH" /* data defs */#include "PORTNUMS.HW#define INSIZE 18000#define CHSIZE 8#define FLSIZE 2#define GRPSIZE 6000

char l_index[TAGSIZE] = {"@IAlndx "};char keep[MAXGRPNUM]; /* init to FALSE */

static char *data_nanm[MAXGRPNUM;static char grp_size[MAXGRPNUM];static char data_type[MAXGRPNUM];static char logflag[MAXGRPNUM; ];/* init to FALSE */

static int recdcnt;int intstor[INSIZE];static int *impnt;float floamstor[FLSIZE];static float *floatpnt;char charstor[CHSIZE];static char *charpnt;char grpstor[GRPSIZE];static char *grppnt;

setdata0 /* set data logging /

int sel;

while (TRUE)

sel = datarenu();switch (sel)

case 1:startlog0;hrea,

case 2savedataO;

case 3:select_kp0;break

case 4:durmplog();while (!scrcsts()); /* wait for kcb-ea

casc 5:debuglog();while (!scr_cstsO()); /* wait for kebmak

case 0:retum(TRUE);beak

defaultcoC7); /* beep */

/*====== ==========--== '===*/selectkeep()

int sel;

while (TRUE)

sl =keep_mjnu();if((sel < 0) 11 (sel > MAXGRPNUM))

co(7);continue;

if(sel = 0)retum(TRUE);

keep[sel]= !keep[sel];

dr=================================datamenu()

1/

~1a'~1'

5/

a'1~~~~~~~~~~a'~~~~~~~~~'a'~~~~~~~~~':rsion 6.2 */

trsion 6.2 */

=*1

110

Iint query;

scrdrO;scr_rowcol(2,0);printf(" M.I.T. SSL\n");printf(" Beam Assembly Teleoperator\n);printf(" Data Recording Function\n");printf(" 1. INITIALIZE DATA STORAGESn");printf(" 2. SAVE DATA TO FILEn");printf(" 3. SELECT VARIABLES TO RECORD\n");printf(" 4. DISPLAY DATA LOG ON SCREEN\n");printf(" 5. debug data log\n");printf(" 0. EXrln\n");printf(" Enter Selection ");scanf("%dsn,&query);retumrn(quory);

keepmenu()

int query;char kp[MAXGRPNUM];

for (query = 0; query < MAXGRPNUM; query++)if(keepfquery])

kp[query] = ';else

kp[query]= '-';scr_dr();scrrowcol(2,0);printf(" MIT SSL BAT'n");printi(" Data Recording FunctinnaXn");printf(" Variable Selection\n);pritf("[%c] 1 LOOP COUNTER [%c]11 JC MEAS POS n",kp[ l],kp[l 11);printf"[%c] 2JOYSTICKS [%c]12JC MEAS RATE \n"p[2]kp[12]);printf([%c] 3 THRUST COM [%c]13 JCCOM POS 'n",kp[3],kp[13]);printf([%c] 4 MASTER ARM [%c114 \n",kp[4,kp[14]);printf("[%c] 5 JOINT COM [%c]15 \n",kp[5],kp[151);printf("[%c] 6JOINTPOS [%c]16 \n"Jcp[6],kp[163);printf"[%c] 7 Z80LOOP [%c]17 \n",kp7].kp[17?);printf("[%c] 8 ARM DUTY CYC [%c]18 \n"k8]p[8]p[18]);printf("[%c] 9 [%c]19 \n",kp[9],kp[191);printf("[%c]10 \n\n",kp[10]);printf(" 0 -EXlTN\n");printi" Enter Selection ");scanf("%d'n",&query);return(query);

/*--------------------*/save_data()

char fname[20];int i, j, siz, fn;int grplogcnt;char ans, grp, frmt[20];int *indmnpt = intstor;float *fldpt = floatstor;char *chdpt = charstor;

printf(" Write filenanme [DAT]: ");scanf"%sn",fiamm);strcat(fnamn,".DAT");if((f = creat(fnamn)) = -1)

scrrowcol(0,0);beep(25);printf("Unable to Write File");pause(250);

else

fprintf( fn,'"%s\"'n" ,fname);for (grp = 0; grp < MAXGRPNUM; grp++)

if(ogflag[grp])

fprintf(fn,'n\"%d %s ",grp,datajname[grp]);fprintf(fn," %c %d"",data_type[grpl,grpsize[grp]);

fprintf(fn'- -);grplogcnt = grppnt -grpstor; /* # entries in group store */for (i = 0; i < grplogcnt; i++)

grp = grpstor[i];siz = grp_size[grp];fprintf(fn,"\n%d:",grp);switch (data_type[grp])

caseFLDAT: /* float store */for (j =0;j < siz;j++)

fprintf(fn," %e",*fldmpt++);breal

111

case INDAT: /* int store /for (j = 0; j < siz; j++)

fprintf( fn," %d",*indmpt++);

case CHDAT: /* char store */for ( = ; j < siz; j++)

fprintf(fn," %d",*chdmpt++);beak;

defaultprintf("datatype[%d] =%d ERROR",grp,data_type[grp]);

}

close(fn);

/* ===...============================--=start-log(

iti;

intpnt = intstor;floatpnt = floatstor;charprt = charstor;grppnt = grpstor,for (i = 0; i < MAXGRPNUM; i++)

logflag[i] = FALSE;recdcnt = 0;

indxlog()

logint(lindx,++recdcnt);

logint(tag,var)int var;char tag[];

char grp;exem char data_flag;

grp = tag[0] - '@';if (!log_lag[grp])

logflag[grp] = TRUE;datatyp[grp] =tag[Ill;grp_size[grp] = tag[2] - '@';datanae[grp] = &tag[3];

if (tag[1] != INMDAT)printf("\n** Wrong type to logint '***a);

if ((GRPSIZE - 1) >= (grppnt - grpstor))if ((INSIZE -1) >= (ipnt - intstor))

*grppnt++ = grp;*intpnt++ = var;

else

else

scr_rowcol(24,0); scrcrl0;printf("**a INT STORAGE OVERFLOW **);data_flag = FALSE;screen2();

scrrowcol(24,0); scr_clrl);printf("*** GRP STORAGE OVERFLOW **");data-flag = FALSE;scren2(0;I

…/- -=-= = -/

loginta(tag,var)int var[];char tag[];

int i, dim;char grp;extem char dataflag;

grp = tag[] - '';if (!logflag[grp])

logflag[grp] = TRUE;datatype[grp] = tag[1];grp_size[grp] = tag[2] - '';data name[grp] = &tag[3];

112

}

if(tag[ll] =INDAT)printf('n*** Wrong type to logint_a ***n");

dim = tag[2] - '@';if ((GRPSIZE- 1) >= (grppnt - grpstor))

if ((INSIZE -dim) >= (intpnt - intstor))

*grppnt++ = grp;for (i = 0; i < dim; i++)

*intplt++ =var[i];

else{scr_rowcol(24,0); scrdrlrO;prinf("*** INT STORAGE OVERFLOW ***");dataflag = FALSE;scen2();

else

scrrowcol(24,0); scrdcrl0;printf("*** GRP STORAGE OVERFLOW ***");dataflag = FALSE;screen2(0;

/* .============================ = .===logchar(tag,var)char var,char tag[];

char grp;extem dar data_flag;

grp = tag[0] - '@';if (logflag[grp])

logflag[grp] =TRUE;datatype[grp] = tag[l];grp_size[grp] = tag[2] - '@';dataarsm[grp] = &tag[3];

if(tag[l] != CHDAT)printf("an*** Wrong type to logchar ***\n");

if ((GRPSIE - 1) >= (grppnt - grpstor))if ((CHSIZE - 1) >= (charpnt -charstor))

*grppnt++ = grp;*charapt++ = var;

else{scrrowcol(24,0); scrdrlO;print"*** CHAR STORAGE OVERFLOW ***");dataflag = FALSE;scren2(;

else

scrowcol(24,0); scrdrl0;printf("*** GRP STORAGE OVERFLOW ***");data_flag = FALSE;screon2();

logchar_a(tag,var)char var[];char tag[];

int i, dim;char grp;extem char data-flag;

grp = tag0] - '@';if (!logflag[grp])

log_flag[grp] = TRUE;datatype[grp] = tag[1];grpsize[grp] = tag[2] - '@';datarnergrp] = &tag[3];

if(tag[l] != CHDAT)printf(n*** Wrong type to logchar ***");

din = tag[2] -'@';if ((GRPSIZE - 1) >= (grppnt - grpstor))

if ((CHSIZE -dim) >= (charpnt -charstor))

*grppnt++ = grp;for (i = 0; i < dim; i++)

113

*charpnt++ = var[i];

elsc{scrrowcol(24,0); scrdclrl0;printf("*** CHAR STORAGE OVERFLOW ***");dataiflag = FALSE;

cid ~s 2(scclse

scr_rowcol(24,0); scrclrO;printif"*** GRP STORAGE OVERFLOW ***");dataflag = FALSE;screen20;

logfloat(tag,var)float var;char tag[];

char grp;extem char data-flag;

grp = tag[0] - '@';if(!log_flag[grpl)

logflag[grp] =TRUE;datatype[grp] = tag[l];grp_sizc[grp] = tag[2] - '@';datalaon[grp] = &tag[3];

if(tag[1] != FLDAT)printf('n*** Wrong type to logfloat **\n");

if ((GRPSIZE - 1) >= (grppnt - grpstor))if ((FLSIZE -1) >= (floatpnt - floatstor))

*grppnt++ = grp;*floatpnrt++ = var;

else

scrrowcol(24,0); scrdrl0;printf"*** FLOAT STORAGE OVERFLOW ***");dataflag = FALSE;screen();

else

scrrowcol(24,0); scr_clrl0;printf("*** GRP STORAGE OVERFLOW ***");dataflag = FALSE;scren2(;

logfloat_a(tag,var)float var[];char tag[];

{int i, dim;char grp;cxtem char dataflag;

grp = tag[0] - '@';if(!logflag[grp])

logflag[grp] = TRUE;datatypc[grp] = tag[1];grpsize[grp = tag[2] - '@';data_na[grp] = &tag[3];

if(tag[l] != FLDAT)printf(%n*** Wrong type to logfloat **n");

dim = tag[2] - '@';if ((GRPSIZE - 1) >= (grppnt -grpstor))

if ((FLSIZE -dim) >= (floatpnt -floatstor))

*grppnt++ = grp;for (i = 0; i < dim; i++)

*floatpnt++ = var[i];

else

scr rowcol(24,0); scrdrlO;printf("*** FLOAT STORAGE OVERFLOW ***");data_flag = FALSE;screen20;

114

else

scr rowcol(24,0); scr_dl();printf*** GRP STORAGE OVERFLOW ***");dataflag = FALSE;screoan2(;

/* ============-=__=======/bailout(errstr)char *enstr;

printf(errstr);exit(2);

dupJog()

inti, j, siz;int grplogcnt;char ans, grp, fmt[20];int *indmpt = intstor.float *fldmpt = foatstor;char *chdmrpt = charstor;

printf('nDisplay data log verbose? (Y/N): ")ans = getchar();if (ars = 'y')

ans = 'Y';printf(nHit any key to stop\n");grplogcnt = grppnt - grpstor; /* # entries in group store */for (i =0; i < grplogcnt; i++)

{grp = grpstor[i];siz = grp_size[grp];j=O; /* copy format string *if (ans 'Y')

printf("'n%s: ",data

_nam

c [grp]);

elseprintf("n9td:",grp);

switch (data type[grp]){case FIDAT: /* float store */

for (j = 0; j < siz; j++)pritf(" %e",*fldmpt++);

brealgcase INDAT: /* int store */

for (j = 0; j < siz; j++)printf(" %d',*indmpt++);

breakcase CHDAT: /* char store */

for (j = 0;j < siz; j++)printf( %d",*chdmpt++);

breakdefault

printf("datatype[d %d] = %d ERROR",grp,data_type[grp]);

if(scrcsts())break;

printf"Dump complet\n");

/* ==============================-.==debuglog()

int indpth, fldpth, chdpth, grpdpth, i;

printf('CnDEBUG: recdcnt = %d",recdcnt);indpth = inpnt - intstor;fldpth = floatpnt -floatstor;chdpth = chalpnt - charstor;grpdpth = grppnt -grpstor,

printi"\nGRP: depth = %dsn",grpdpth);printf('\nCHR: depth = %&n",chdpth);printf('nlNT: depth = %dn",indpth);printf('nFLT: depth = %dn",fldpth);puts('\n--- GROUP -----\n");for (i = 0;i < gpdpth;i++)

printf("%d ,grpstor[i]);puts('Nn--- CHAR -----\n");for (i = 0;i < chdpth;i++)

printf("%d',charstor[i]);puts('\n- INT ----\n");for (i = 0;i < indpth;i++)

printf("%d ",intstor[i

]);

puts(\n--- FLOAT -----n");for (i = O;i < fldpth;i++)

printf("%e ,floatstor[i]);

115

116

l-====== - - - - - - -== E= === ==

/* TRIAD1.C -- Support Routine for Supervisory Control version 63** for the Beam Assembly Teleoperator October 1987** by David E. Anderson MIT Space Systems Laboratory**---........1...-........ --..-/* Compiler: Desmet C** - ---------- */

/* Triadl contains the Triad access menu for setting up files to be played** back or files to be learned

#include "PORTNUMS.H"#include "DAT.H"/*Triad selection menu. *triacd()

char notdone, query;char fname[15];extem int funelspeed;extem char triadfn[10][15], triad_data;extem char data_flag; /* world data flag */extem int settriad;int i, f, j;not_done = TRUE;settriad = 0; /* default Triad inactive */funnelspeed = 0x100; /*default funnel speed. *//* (Funnel() moves the arm from where it is to where the file to be played ** back starts out )*/scr_clr); print out menu */scr_rowcol(O,);printf("%cc%cc%c%c%cc%c%c%cc",201,205,205,205,205,205,205,205,205,205,187);printi" %c %c\n",186,186);printf(" %c TRIAD %c\n",186,186);printf("%c%c%c%c%c%c%c%c% c%c%c%c%c%c%c%cc%c% c%c%c%c%c%c%c%c%c%c%ccc",201 ,205,205,205,205,205,205,205,205,205,205,205,205,188,200,205,205

205,205,205,205,205,205,205,205,205,20505,205,187);printf(" %c %c",186,186);printf(" %c SUPERVISORY CONTROL ALGORITHM %c",186,186);printf(" %c %c",186,186);printf("%c%c% c%c%c%c%c%c%c%cc%cc%cc c%c%c%c%c%c%c%c%c% cc%c%cc% c%c%c%c%c%cc%cn",200,205,205.205.205,205,205,205,205,

205,205,205,205,205,205,205,205,205.205,205,205,205,205,205,205,205,205,205,205,205,205,205,205,205,205,205,205,205,188);

printf(" 1. Select File(s) for Playbaclin");printf(" 2. Set Up to Lear\n");printf(" 3. Full EASE Constructionmn");printf(" 0. Exit/TumoffTriad");while(not_dom) /* begin menu reading loop */

srrowcol(15,5);scanf("%c",&query); /* look for menu request */switch(query)

case '1': /* set up a file for play back */scrrowcol(15,0);printf("Set funnel speed [in hex]: ");scanf("%x",&funnel_speed); /* look for funnel speed */scrrowcol(15,26);printf(" %4x ",funne_speed); /* confirm funnel speed */scrrowcol(16,5);printf("Data? [y/n]: "); /* ask for data flag */triad_data = getchar);if(triaddata = 'y') triad_data = TRUE;else triad_data = FALSE;scr_rowcol(l7,5);printf("SELECT FILE [.TRDI:");for(i=0;notdone;i++)

{ / playback filename procurmnt loop */scr_rowcol(18+i,0);printf(" ->");gets(triadfitil);strcat(triadfn[i],".TRD");if(triadfn[i][0] '0)

strcpy(triadfn[i], "0");not done = FALSE;

scrrowcol(l8+i,5);printf("%s".triadfn[i]);

set_riad = 1; / activate Triad Playback mode */break;

case '2: /* enter learn mode set up */Ieamrn_stup();notdone = FALSE;bnak;

case '3': /* set up for full forward assembly */strcpy(triadfr[0], "grabbeamtrd");strcpy(triadfn[l ], "joint.trd");strcpy(triadfn[2], "grabslv.trd");strcpy(triadfn[3], "slideslv.trd");strcpy(triadfn[4], "stow.trd");strcpy(triadfn[5], "0");set_triad = 1; /* activate Triad Playback mode */notdone = FALSE;break;

117

case : /* exit */not_done = FALSE;break;

default: /* unrccognized command */scrrowcol(15,5);printf("MUDPIE command, try again");pause(250);scr_rowcol(15,5);printf(" ");b-*l

** Leam_set_up() saves a filenamn to save a learrnd trajectory to.* --------------- -----------------------

char fnamcr[15];leamsetup(

extern int settriad;extem char learning;extem char fnamer[15];sct_triad = 2; /* activate Triad ready to learn mode */scr_rowcol(15,5);printf("Set storage file [.TRD] -> ");gcts(fnamer);strcat(fnamer,'.TRD ");}

1*------------------------** Learnam() is called by a button push. Lamr starts and stops the process of saving actual ann positions.~ ......................................................................./

int frtt;leam0

extem int fntt;extem int settriad;extem char finaer[15];extemjointpos[81;ifsetriad=2) /* iffTriad in ready to learn mode active */

if((fnt = creat(famer)) = -1) /* make a new trajectory file */

scr rowcol(19,24);printf("Carnnot learn");beep(25);pause(150);scrrowcol(19,24);printf(" ");

else

set_triad = 3; /* activate Triad learn mode so save trajectory */scr_rowcol(19,24);print("Learing");

else /* if Triad leaving loam mode */{ /* save another command with EOF cod */

fprintf(fntt,"%4x %4x %4x %4 %4x Hn\n",joinLpos[O], jointpos[l, jointpos[2], joint_pos[3], joint_pos[4]);close(fntt); /* dose trajectory file */set_triad = 2; /* set to Triad ready to leam mode */scrrowcol(19,24);printf(" "); /* tun off learning sign */

P*-------------------------------------------------

** Lamrecord() is called once per Batsoft main loop. It saves the actual arm joint ** positions.

leam-record

extm int fntt;extem int jointLpos[8];fprinftf(fntt,"'4x %4x %4x % 4x %4x An", joint-pos[O], jointpos[1], joint_pos[2], jointpos[3]jointpos[4]);}

118

APPENDIX II. JCS code

; Z80 code for the JCS joint cards. Each Z80 controls two jointspage 60,132title BAT JOINT CONTROL CARD (w/DPR) Jlu.asm 8/4/87page 60,132* **** * ******** *********

Downloaded code for JC cardsuses dual-port RAM (DPR) for commands & data

;*********************************************by John Spofford, Tom Waschura, & David E. AndersonMIT Space Systems Laboratory

includeincludeinclude

zg0.macz80_math.macjoint.mac

;--------------------------------------MACROS START

macro b, reg, labelbit b, regjpc nz, labelendm

;--------------------------------------MACROS END

;---Set up all I/O Port Definitions

pio2a_datapio2a_cntlpio2b_datapio2b_cntlctc0ctclctc2ctc3pio3a_datapio3 a_cntlpio3b_datapio3b_cntl

;---Set up I/O

equ 4equ 5equ 6equ 7equ 8equ 9equ 10equ 11equ 12equ 13equ 14equ 15

related constants

bitmodebi_direc_modeinput_modeoutputmodeall_inputmaskall_output_maskinteruptctlno_interuptctlctc_vectorctc0_ctlctc 1 and2_ctltc_followsmin tc sizewaitcount

equequequequequequequequequ

equ OD7Hequequequequ

OCFH08FH04FHOOFHOFFH000H087H

007H010H ;%(LOW ctc_vectoraddr)

057H057H8OOAH ;for 10 ms loop should be 10

;---Set up Initial Value Constants for RAM Variables

dataram_lodataram_histack_topvar_page_size

equequequequ

0800hOBFFhdataram_hi + 0O100h

JPBIT

119

equequequequ

1000h17FDh1800h1FFFh

equ 1FFEhequ 1FFFhequ 1813hequ 1814hequ 189Ah

;see JRS labbook

;to PC;from PC

;---Define RAM

alpha_beta-ngain_nmultl_mult2_mult3_sigma_sign_nreferentheta_rcontro.

TCndelta_roldde]oolddtempitemplDstart_salgoritold_theoldcooold_ccdirecti(bits_2:n_abortinterrul

Variables (location, size and usage)

n

_n_n

n_n

ce_n

ln

ita_nelta_n

nignalhm_n

ta_nntrol_nontrol_nn_n

tptcnt

control algorithm coefficientcontrol algorithm coefficientcontrol algorithm coefficient

control algorithm coefficientintegrator on/off and integrator saturationintegrator multiplierintegration sum

present sign of error, used to reset sigma_nreference position indicated by IBM PCindicated position read from decoderscontrol signal outputted from controlalgorithm. (Note: not to be confusedwith TC_n)CTC time constanterror difference (reference - theta)OLD error differenceOld OLD error diference8 bit temporary RAM variable16 bit temporary RAM variablestart up FLAGcontrol algorithm selectionOLD indicated positionOLD control value

Old OLD control valuemotor direction indicatormulti-use port output byte

abort flagscount of CTC interrupt services

PID CONTROLLER:The PID compensator(control)/(error) = k1 + a/(z-1) + b(z-l)/(z+c)}+sigmawhere k is gain, a is (fake) integrator multiplier, b is differential mult.In batsoft, these are combined shifted to form a 0.16 fixed point binaryand sent to the dpr in the form of:

gain_nalpha_nbeta_nmultl_nmult2_n

mult3_nsigma_nsign_n

from_pc_vars

k(b+l) multiplier of error-k(a-l-c-2b) neg of mult of old errork(c-ac+b) mult of oold error

c mult of old controlupper byte: l=integrator on, 0=integrator off

lower byte: integrator saturationfraction multiple of each loop delta to add to sigmaintegrated sum. Upper byte added to control

keeps sign of delta. resets sigma when delta changes sign

START OF RAM DEFINITIONorg dataram_loequ $ ;order of variables is important!

120

coderam_locoderam_hidualram_lodualram_hi

to8088from8088wcjc_statuscomm_flag

==== == - -=========

reference_x, 2reference_y, 2gainx, 2

gain_y, 2 ;86alpha_x, 2alphay, 2beta_x, 2betay, 2algorithm_x, 1

algorithmy, 1predither_x, 2predither_y, 2postdither_x, 2 ;96post_dither_y, 2 ;98nothing, 2multl_x, 2multl_y, 2mult2_x, 2mult2_y, 2mult3_x, 2mult3_y, 2

$ ;order oftheta_x, 2thetay, 2rate_x, 2 ;ratey, 2calc_ctrl_x, 2calc_ctrl_y, 2 ;TC_x, 1TC_y, 1direction_x, 1 ;direction_y, 1 ;loop_counter, 3;

;80;82;84

;88;8A;8C;8E;90;91;92;94

;9A;9C;9E;AO;A2;A4;A6

variables is important!0002040608OA

;OC;OD

DEOF10

controlx, 2control_y, 2deltax, 2delta_.y, 2last_data_x, 2last_data_y, 2old_delta_x, 2old_delta_y, 2oold_delta_x, 2oold_delta_y, 2oold_control_x, 2oold_control_y, 2old_last_data_x, 2old_lastdata_y, 2vel_gain_x, 2

velgain_y, 2pos_gainx, 2pos_gainy, 2old_theta_x, 2oldthetay, 2old_control_x, 2

old_control_y, 2bits_2a, 1y_abort, 1x_abort, 1interruptcnt, 1tempO, 1temp_l, 1temp16_0, 2

defundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefun

to_pc_vars equdefundefundefundefundefundefundefundefundefundefundefun

defundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefundefun

121

defun templ6_1, 2defun status_2b, 1defun sigma x, 2defun sigma_y, 2defun sign_x, 1defun sign_y, 1IF $ GT dataramhi%out DATA RAM OVER-RUN!ENDIF

END OF RAM DEFINITION

-;__c__--- _== = -- ___ =----= ___-,-~==== -START OF Z80 CODEorg coderam_lojp beginroutine$org %(coderam_lo + 010H)

;---Place the Interupt VECTOR TABLE here, then jump to NMI service

ctc_vector_addr equ $dw ctcservicedw 00dw 00dw 00

$org %(coderam_lo + 30h)

;<<<<<<<<<<<<<<<<<<<<BEGIN MAIN CODE>>>>>>>>>>>>>>>>>>>>>>

;---Initialize all PIO's and the CTC

begin_routine equ $lri a, 'f' ;1/2/86 jrssta (to8088) ;free the PC

initpio %pio2a_cntl, %bitmode, %alloutputmask, %nointerupt_ctlinitpio %pio2b_cntl, %output_mode, 0, %nointerupt ctlinitpio %pio3a_cntl, %bitmode, %alLinputmask, %no_interupt_ctlinitpio %pio3b_cntl, %bitmode, %allinput_mask, %no_interuptctlinit_ctc %ctcO, %ctcO_ctl, ctc_vector, 0initctc %ctcl, %ctcland2_ctl, 0, 25init_ctc %ctc2, %ctcland2_ctl, 0, 25

;---Initialize CPU parameters

lrpi hl, stack_topIsphllri a, %(HIGH ctc_vector_addr)ldiaim2ei

;---Initialize both motor flip-flops OFF

lri a, 00000110B ;first set F-Flopsout pio2a_datalri a, 11100000Bout pio2a_datasta (bits_2a)lri a, tc_follows ;then reset them viaout ctcl ;the timers timing outout ctc2

122

lri a, 1out ctclout ctc2

;---Assuming the PC will not write to Joints until after initializing;---Initialize RAM Variables with their inital values

irpi hl, 0sthl (alpha_x)sthl (alpha_y)sthl (beta_x)sthl (beta_y)sthl (gain_x)sthl (gain y)sthl (reference_x)sthl (reference_y)sthl (theta_x)sthl (theta_y)sthl (ratex)sthl (ratey)sthl (controlx)sthl (controly)sthl (delta_x)sthl (delta_y)sthl (lastdata_x)sthl (lastdata_y)sthl (old_delta_x)sthl (old_delta_y)sthl (old_lastdata_x)sthl (old_lastdata_y)sthl (oldtheta_x)sthl (old_theta_y)sthl (old_control_x)sthl (old_controly)sthl (postdither_x)sthl (postdither_y)sthl (pre_dither_x)sthl (pre_dither_y)sthl (multl_x)sthl (multl_y)sthl (mult2_x)sthl (mult2_y)sthl (mult3_x)sthl (mult3_y)sthl (sigma_x)sthl (sigmay)sthl (loop_counter)sthl (oold_control_x)sthl (oold_control_y)sthl (oold_delta_x)sthl (oold_delta_y)

lri a, waitcountsta (wc)lri a, O0sta (TCx)sta (TCy)sta (direction_x)sta (directiony)sta (x_abort)sta (y_abort)sta (interruptcnt)sta (algorithm_x)sta (algorithm_y)

123

sta (sign_x)sta (sign_y)sta %(loop_counter+2)

;---Now that all is initialized & the motors;---for the start up code from the IBM PC.

lristaIdaandioutequ $Idacpijpc

a, 'W'(jc_status)(bits_2a)10111111Bpio2a_data

are OFF, sit and wait

;ready to go flag

;b6=0 --> not started

(comm_flag)'G' ;look for start flagnz, sit_and_wait

;---And we're off. Now we'll work on motor x first. Read the x;---position, and get a control_x based on the selected algorithm

equ $Ida (comm_flag)cpi 'S'jpc z, begin_routinelri a, 'C'sta (jc_status)

READ_THETA 0

;look for shutoff flag;reset;'controlling' flag

;O selects channel x

VMOV theta_x, templ6_0VNEG templ6_0VADD templ 6_0, reference_x, delta_x

;---Now check the algorithm type and dispatch accordingly

Idacpijpc

algorithm_x0nz, pd_algorithm_x

;---PROPORTIONAL CONTROL ALGORITHM

MULTIPLY gain_x, delta_x, control_xjp drain_x

;---PID CONTROL ALGORITHM

pd_algorithm_x equ $cpi 1jpc nz, SVC_algorithm_xMULTIPLY gain_x, delta_x, templ6_0VADD rate_x, templ6_0, control_x

jp drain_x

;---SVC CONTROL ALGORITHM

SVC_algorithm_xcpijpc

equ $2nz, OL_algorithm_x

MULTIPLY pos_gain_x, delta_x, control_xVMOV thetax, temp16_0

124

sit_and_wait

main_loop

VADD templ6_0, old_theta_x, templ6_0VADD control_x, templ6_0, controlxjp drain_x

;---OPEN LOOP CONTROLLER

OL_algorithm_x equ $MULTIPLY gainx, reference_x, controlx

;---Now filter the control value for saturation and direction

drain_x equ $VMOV control_x, calc_ctrl_x

filter x

; JOINT Y

READ_THETA 1

VMOV theta_y, templ6_0VNEG templ 6_0VADD templ6_0, reference_y, delta._y

;---Now check the algorithm type and dispatch accordingly

lda algorithm_ycpi 0jpc nz, pd_algorithmy

;--PROPORTIONAL CONTROL ALGORITHM

MULTIPLY gain.y, deltay, controlyjp drainy

;---PID CONTROL ALGORITHM

pd_algorithm_y equ $cpi 1jpc nz, SVC_algorithmyMULTIPLY gainy, delta_y, templ6_0VADD rate_y, templ6_0, control_y

jp drainy

;--SVC CONTROL ALGORITHM

SVC_algorithm_y equ $cpi 2jpc nz, OL_algorithm_y

MULTIPLY pos_gain_y, deltay, control_yVMOV theta_y, templ6_0VADD templ6_0, old_theta_y, templ6_0VADD control_y, templ6_0, control_yjp drain_y

;--OPEN LOOP CONTROLLER

OL_algorithm_y equ $MULTIPLY gain y, reference_y, controly

125

;---Now filter the control value for saturation and direction

drain._y equ $VMOV controly, calcctrl_yfilter y

;PRECALCULATE VALUES FOR NEXT LOOP if PID Controller used.

; this updates the state variables for the algorithms that; need them.

lda algorithm_xcpi 1jpc nz, try_y_l

; update x variablesVMOV old_delta_x, oold_delta_xVMOV delta_x, old_delta_xVMOV old_control_x, oold_control_xVMOV theta_x, old_theta_xVMOV controlx, old_control_x

; precalculate for PID controller for joint x

MULTIPLY multl_x, old_control_x, templ6_0MULTIPLY alpha_x, old_deltax, templ6_1VNEG templ6_1VADD templ6_0, templ6_1, templ6_1MULTIPLY beta_x, oold_deltax, templ6_0VADD templ6_0, templ6_1, rate_xMULTIPLY mult3_x, old._delta_x, templ6_0VADD templ6_0, sigma_x, sigma_x

; reset for the PID: mult2 is an on off switch

ldhl mult2_xbit 0,h ;if bit is set (z not set) then gojpc nz,xxxlri h,0lri 1,0sthl (sigma_x)

xxx equ $

; if the sign delta changes, reset sigma

ldhl old_delta_xbit 7, h ;check sign of errorjpc z, psign_x ;error is negativeIda sign_xbit 0, a ;if bit 0 not set, reset sigmajpc nz, norstxlri h, 0lri 1, 0sthl %(sigma_x)lri a, 0 ;delta now positive, change signsta (sign_x)jp norst_x

psign_x equ $Ida sign_x ;if bit 0 of a is set, reset sigmabit 0, ajpc z, norst_xlri h, 0

126

lri 1, 0sthl %(sigma_x)lri a, 1sta (sign_x)equ $norst_x

;clip sigma if necessaryIdrpidhlbitjpcldnegcpjpcldlriip

nbad_x equldlristrpldIriip

pos_x equldcpjpcldlriJP

pbad_x equldlristrpldri

good_x equstrpVADD

try_y_l

; update y variables

de, sigma_xmult2_x7, dz, posxa, 1

d ;if a>d, bad, carry not setnc, nbad_xe, dd, OFFhgood_x

d, ae, 0de, %(sigma_x)e, dd, OFFhgood_x$a, 1d ;if d> a, badcar, pbad_xe, dd, 0good_x$d, ae, 0de, %(sigma_x)e, dd, 0$de, %(templ6_0)templ6_0, rate_x, rate_x

equIdacpijpc

VMOVVMOVVMOVVMOVVMOV

carry set

$algorithm_y1nz, try_x_SVC

old_delta_y, oold_delta_ydelta_y, old_delta_yold_control_y, oold_control_ythetay, old_theta_ycontrol_y, old_control_y

; precalculate for PID controller for joint y

MULTIPLY multl_y, old_control y, templ6_0MULTIPLY alpha_y, old_delta_y, templ6_1VNEG templ6_1VADD templ6_0, templ6_1, templ6_lMULTIPLY beta_y, oold_deltay, templ 6_0VADD templ6_0, templ6_1, rate_yMULTIPLY mult3_y, old_delta_y, temp16_0VADD templ6_0, sigma_y, sigma_y

127

; reset for the PID: mult2

ldhl mult2_ybit O,hjpc nz,yyy

;if bit is set (z not set) then go

lri h,Olri 1,0sthl (sigmay)equ $

; if the sign delta changes, reset sigma

ldhlbitjpcIdabitjpclrilristhllristaJP

psign_yIdabitjpclrilristhllrista

norsty equ; clip sigma if necessary

ldrpldhlbitjpcldnegcpjpcldlriip

nbad_y equldlristrpldlriip

pos_y equldcpjpcldlriip

pbad_y equldlri

old_delta_y7, h ;check sign of errorz, psign_ysign_y0, anz, norst_yh, 01, 0%(sigma_y)a, 0(sign_y)norstyequ $sign_y ;if bit0, az, norstyh, 01, 0%(sigma_y)a, 1(sign_y)$

;error is negative

;if bit 0 not set, reset sigma

;delta now positive, change sign

0 of a is set, reset sigma

de, sigma_ymult2_y7, dz, pos ya, 1

d ;if a>d, bad, carry not setnc, nbad_ye, dd, OFFhgood_y$d, ae, 0de, %(sigma_y)e, dd, OFFhgood_y

a, 1d ;if d> a, badcar, pbad_ye, dd, 0good_y$d, ae, 0

carry set

128

YYY

is an on off switch

strpIdlriequstrpVADD

de, %(sigmay)e, dd, 0$de, %(templ6_0)temp160, ratey, ratey

; precalcualate for SFC controller

try_x_SVC equ $Ida algorithmxcpi 2jpc nz, tryySVC

equIdacpijpcnop

$algorithmy2nz, end_precalcs

endprecalcs equ $;-------------------------------------------------------; check for new info from PCcheck_new equ $

Ida (from8088)cpi 'N' ;new info flagjpc nz, dont readlri a, 'R' ;reading flagsta (from8088)irpi be, 28h ;setup for dirirpi hi, %(dualramo + 80h) ;from pc areairpi de, from_pcvarsidirlri a, 'O' ;info has been read flagsta (from8088)jp transfer

dont-read equ $cpi 'U' ;info being updated flag;do nothing, get it next time

; send data to PCtransfer

donetalking

equ $Idacpijpclristairpiirpiirpiidirlristaequ

(to8088)'R' ;currently beingz, done_talkinga, 'U' ;updating flag(to8088)bc, 13h ;setup for dirhi, to_pcvarsde, %(dualramo + 0)

read flag;next time..

;to pc area

a, 'N' ;new data available flag(to8088)$

;-------------------------------------------------------;now wait for interruptcount to get to wait_count. This will permit;a 10 ms calculation loop.

;--- set pio2-a7 zero while waitingIdares

(bits_2a)7, a ;b7=0 --> waiting

129

goody

;tryySVC

staout

;---here in wait loop, increase loopIrpilriaddmstrmlriincrpadcmstrmlriincrpadcmstrm

;---the looplri

wait_here equIdacpjpc

;---leaving loop, reset count and st;lristaIdasetstaJP

(bits_2a)pio2a_data

count:hl,(loop_counter)a, 1 ;increment value = 1

;first byteaa, 0 ;zero for carryhl

;second..aa, 0 ;zero for carryhl

;last bytea

b, wait_count

(interrupt_cnt)bnz, waithere

atus bit:a, 0(interrupt_cnt)(bits_2a)7, a(bits_2a)main_loop

;b7=1 --> done waiting

; This is the CTC interrupt service routine:

This routine is designed to have fast response between thepulse start signal and the timer start signals. The fastestthis can be done is in 34 cycles (@ 2 MHz = .017 ms). TheCTC counts every .004 ms, so the smallest count I can accuratelyrepresent is 5 (i.e. 4.25), hence if the tc_n is less than

; 5, it is ignored, & then it is decreased by 4.and if you understand that, congratulations!

equ $save_state

idaincsta

(interrupt_cnt)a(interrupt_cnt)

(tc_x)b, aa, 0bz, no_pulsetcO(x_abort)O, anz, no_pulse_tcOa, ctcland2_ctlctc 1(bits_2a)1, apio2a_data1, a

;check for small

;shut it down

;start pulse

130

ctc_service

daldlricpjpcIdabitjpclrioutIdasetoutres

·******%********************************

out pio2a_dataId a, bout ctcl

equ $ida (tc_y)Id b, alri a, O0cp bjpc z, no_pulse_tclIda (y_abort)bit 0, ajpc nz, no_pulse_tcllri a, ctcland2_ctlout ctc2Ida (bits_2a)set 2, aout pio2a_datares 2, aout pio2a_dataid a, bout ctc2

;shut 'em down

;start pulse

equ $

restore_stateeireti

; Multiply Subroutine (called by MULTIPLY macro)

multsub equ $Irpi hl, 0irpc x, <BC>irp numb, <7,6,5,4,3,2,1,0>

mugen x, numbendm

endmret

IF $ GT coderam_hi%OUT CODE EXCEEDS RAM LIMIT!ENDIF

end

131

no_pulse_tc0

no_pulse_tc 1

APPENDIX IV. Triad trajectories

Grabbeam.trd: 3/17/88Start at stow, extend beam carrier, grab the beamwith the right claw, retract beam carrier

28F4 5084 4F0 4BC0 DE8028F4 5084 432 4C76 DE3228F4 5084 374 4D2C DDE428F4 5084 2B6 4DE2 DD9628F4 5084 1F8 4E98 DD4828F4 5084 13A 4F4E DCFA28F4 5084 07C 5004 DCAC28F4 5084 FFBE 50BA DC5E28F4 5084 FF00 5170 DC1028F4 5084 FE42 5226 DBC228F4 5084 FD80 52E0 DB7428F4 5084 FC73 52E0 DB2628F4 5084 FB66 52E0 DAD828F4 5084 FA59 52E0 DA8A28F4 5084 F94C 52E0 DA3C28F4 5084 F83F 52E0 D9EE28F4 5084 F732 52E0 D9AO28F4 5084 F625 52E0 D95228F4 5084 F518 52E0 D90428F4 5084 F40B 52E0 D8B628F4 5084 F2FE 52E0 D86828F4 5084 FIF0 52E0 D8102A842C842E84308432843484368438843A843C843E84408442844484468448844A844C844E84508452845484568458845884588458845884

4E444C444A444844464444444244414441444144414441444144414441444144414441444144414441444144414441444144414441444144

FiFOFiFOFiF0F1F0FiFOF1F0F1F0F1F0FiFOFiFOF1F0FIF0F1F0FiF0F1F0F1F0F1F0F1F0FiFOFiF0F1F0F1F0FiF0FiFO

FiFOF1FOF1F0F1F0

50CD4EBA4CA74A9 4

4881466E445B424840353E223COF39FC37E935D633C33lB0

2F9D2D8A2B7729642751253E232B21181F051CF21ADF18CC

-- Open right claw-- Extend beam carrier

-- Right arm clear of stow handle

-- Right arm clear of left arm-- Load Triad2 gains

D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810

132

. E .

5884 4144 F1FO 16B9 D810 A -- Shy and wrist in position for grab5884 4144 F2F0 15CO D030 T -- Wait for beam carrier to settle5884 4144 F3B0 15CO D030 T5884 4144 F3BO 15CO D030 T5884 4144 F3B0 15C0 D030 N -- Load Triadl gains5884 4144 F46B 15C0 DOBC T5884 4144 F526 15CO D148 A5884 4144 F5E1 15CO D1D4 A5884 4144 F69C 15CO D260 A5884 4144 F757 15CO D2EC A5884 4144 F812 15C0 D378 A5884 4144 F8CD 15C0 D404 A5884 4144 F988 15C0 D490 A5884 4144 FA43 15CO D51C A5884 4144 FAFE 15C0 D5A8 A5884 4144 FBB9 15CO D634 A5884 4144 FC74 15CO D6C0 A5884 4144 FD2F 15C0 D74C A5884 4144 FDEA 15C0 D7D8 A5884 4144 FEA5 15CO D864 A5884 4144 FF60 15CO D8FO A5884 4144 1B 15CO D97C A5884 4144 D6 15CO DA08 A5884 4144 191 15C0 DA94 A5884 4144 24C 15C0 DB20 A5884 4144 307 15C0 DBAC A5884 4144 310 15C0 DBCO E -- Close right claw on beam5884 4144 310 15C0 DD40 F -- Open beam carrier claw5884 4144 310 15CO DD40 U -- Short pause while claw opens5884 4144 310 15CO DD40 L -- Retract beam carrier5884 4144 310 15C0 DD40 H -- End of File

133

Joint.trd: 3/17/88Start at grabbeam with beam.position beam for easy manualjoint assembly.

5584 4574 3B0 15C0 DD40 T -- Pause for beam to settle5584 4574 3D0 15C0 DD90 Q -- Load in heavy work gains5584 4574 3F0 15C0 DDEO A5584 4574 410 15C0 DE30 A5584 4574 430 15C0 DE80 A5584 4574 450 15C0 DEDO A5584 4574 470 15C0 DF20 A5584 4574 490 15C0 DF70 A5584 4574 4B0 15C0 DFCO A5584 4574 4D0 15C0 E010 A5584 4574 4F0 15C0 E060 A5584 4574 510 15C0 E100 A5584 4574 530 1620 E150 A5584 4574 550 16A0 E1AO A5584 4574 570 1720 ElFO A5584 4574 590 17A0 E240 A5584 4574 5BO 1820 E290 A5584 4574 5D0 18A0 E2EO A5584 4574 5F0 1920 E330 A5584 4574 610 19A0 E380 A5584 4574 638 1A20 E3D0 A5584 4574 660 1AAO E3EO T -- Pause for beam to settle5584 4574 660 1AAO E3EO H -- End of trajectory

134

Grabslv.trd: 3/17/88Start at just-made-joint, release beam,grab sleeve.

ACO 1AEO E430 DA16 1AEO E3A6 A96C 1AEO E31C A8C281876E6C461A5704C641C3722C821E174CA

1AE01AEO1AE01AE01AE01AE01AE01AEO1AE01AE01AEO1AE01AEO

--Open right claw

E292 AE208 AE17E AEOF4 AE06A ADFEO ADF56 ADECC ADE42 ADDB8 ADD2E ADCA4 ADC1A A

20 1AEO DB90 AFF76FECCFE22FD78FCCEFC24FB7AFACOFACOFACOFACOFACOFACOFACOFACOFACOFACOFACOFACOFACOFACOFACOFACOFACOFACOFACOFACOFACOFB5DFBFAFC97FD34FDD1FE6EFFOBFFA8

45

1AEO1AEO1AEO1AEO1AEO1AEO1AEO1AEO1BEOlC601CE01D601DEO1E601EEO1F601FEO206020E0216021EO0

226022E0236023EO0

246024E02500250025002500250025002500250025002500

DB06DA7CD9F2D968D8DED854D7CAD730D730D730D730D730D730D730D730D730D730D730D730D730D730D730D730D730D730D730D730D730D7CED86CD90AD9A8DA46DAE4DB82DC20DCBE

--Claw clear of beam, load higher gains

--Claw on approach to sleeve, load low gains

135

5534 44545534 44545534 44545534553455345534553455345534553455345534553455345534

4454445444544454445444544454445444544454445444544454

5534 4454553455345534553455345534553455345484528450844E844D844D844D844D844D844D844D844D844D844D844D844D844D844D844D844D844D844D844D844D844D844D844D844D844D84

4454445444544454445444544454445445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B445B4

4D84 45B4 E2 2500 DD5C A4D84 45B4 17F 2500 DDFA A4D84 45B4 21C 2500 DE98 A4D84 45B4 2B9 2500 DF36 A4D84 45B4 356 2500 DFD4 A4D84 45B4 3F3 2500 E072 A4D84 45B4 490 2500 Ell0 A4D84 45B4 52D 2500 E1AE A4D84 45B4 5CA 2500 E24C A4D84 45B4 667 2500 E2EA A4D84 45B4 704 2500 E388 A4D84 45B4 7A1 2500 E426 A4D84 45B4 83E 2500 E4C4 A4D84 45B4 8DB 2500 E562 A4D84 45B4 978 2500 E600 A4D84 45B4 A15 2500 E69E A4D84 45B4 AB2 2500 E73C A4D84 45B4 B4F 2500 E7DA A4D84 45B4 BEC 2500 E878 A4D84 45B4 C89 2500 E916 A4D84 45B4 D26 2500 E9B4 A4D84 45B4 DC3 2500 EA52 A4D84 45B4 E60 2500 EAFO A4D84 45B4 EFD 2500 EB8E A4D84 45B4 F9A 2500 EC2C A4D84 45B4 1050 2500 ECEO E --Close right claw4D84 45B4 1050 2500 ECEO H --End of file

136

Slideslv.trd: 3/17/88Start grabbing unslid sleeve. Slide it.

ECEO EECEO 7ECEO QED6C AEDF8 AEE84 AEF10 AEF9C AF028 AFOB4 AF140 AF1DO AF290 AF350 AF410 AF4DO AF590 AF650 AF710 AF7DO AF8FO AF8FO TF8FO NF8FO H

--Make sure claw is closed--Increase allowable position error--Load in heavy work gains

--Give Shy a kick and allow to settle--Reload normal gains--End of trajectory

137

4D844D844D844D044C844C044B844B044A844A0449844904481447144614451444144414441444144414441444144414

48144814481447D8479C4760472446E846AC4670463445F4467447744874497449744974497449744974497449744974

10501050105010A710FE115511AC1203125A12B11308136013FO1480151015A0163016C0175017E01850185018501850

250025002500256725CE2635269C2703276A27D1283828A02910298029FO2A602ADO2B402BBO2C202CCO32CO2CCO2CCO

-

Stow.trd: 3/17/88Start at slid sleeve and go to stow

1850 2CC0 F8FO D --Open right claw16EA 2CC0 F7AF A1584 2CC0 F66E A141E 2CC0 F52D A12B8 2CC0 F3EC A1152 2CC0 F2AB AFEC 2CC0 F16A AE86 2CC0 F029 AD20 2CC0 EEE8 ABBA 2CC0 EDA7 AA54 2CC0 EC66 A8EE 2CCO EB25 A788 2CC0 E9E4 A622 2CC0 E8A3 A4BC 2CC0 E762 A356 2CC0 E621 A1FO 2CC0 E4EO A8A 2CC0 E39F A

FF24 2CC0 E25E AFDBE 2CC0 EllD AFC58 2CC0 DFDC AFAF2 2CC0 DE9B AF98C 2CC0 DD5A AF826 2CC0 DC19 AF6C0 2CC0 DAD8 AF55A 2CC0 D997 A

4414 4974 F3F0 2CC0 D856 AF3FOF3FOF3FOF3FOF3FOF3F0F3FOF3FOF3F0F3FOF3FOF3FOF3FOF3FOF3FOF3F0F3FOF3FOF3F0F3FOF3FOF4A7F55EF615F6CCF783F83A

2CC0 D810

2F00 D8103100 D8103300 D8103500 D8103700 D8103900 D8103B00 D8103D00 D8103F00 D8104100 D8104300 D81044C4 D8104687 D810484A D8104AOD D8104BD0 D8104D93 D8104F56 D8105119 D81052E0 D81052E0 D81052E0 D81052E0 D81052E0 D81052E0 D81052E0 D810

25F4 5084 F8F1 52E0 D810 A

P --Claw is clear of beam, load higher gainsAAAAAAAAAAAAAAAAAAAN --On approach to stow handle, use low gainsAAAAAA

138

44144414441444144414441444144414441444144414441444144414441444144414441444144414441444144414441444144414

49744974497449744974497449744974497449744974497449744974497449744974497449744974497449744974497449744974

4414443C42A4410C3F743DDC3C443AAC3914377C35E4344C32B4311C2F842DEC2C542ABC292428FC27F426F425F425F425F425F425F4

49744A844B844C844D844E8C4F84508C5084508450845084508450845084508450845084508450845084508450845084508450845084

--Pause briefly on final approach to stow

--Call for slaverest at end of trajectory--Close right claw--End of trajectory

139

25F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F425F4

508450845084508450845084508450845084508450845084508450845084508450845084508450845084508450845084508450845084508450845084

F9A8FA5FFB16FBCDFC84FD3BFD80FDDAFE34FE8EFEE8FF42FF9CFFF6

50AA10415E1B821226C2C632037A3D442E4884F04F04F0

52E052E052E052E052E052E052E05280522051Co5160510050AO50404FE04F804F204EC04E604E004DAO4D404CEO4C804C204BCO4BCO4BC04BCO4BC0

D810 AD810 AD810 AD810 AD810 AD810 AD810 UD862 AD8B4 AD906 AD958 AD9AA AD9FC ADA4E ADA80 ADAF2 ADB44 ADB96 ADBE8 ADC3A ADC8C ADCDE ADD30 ADD82 ADDD4 ADE26 ADE80 ADE80 SDE80 EDE80 H

Cgrab.trd: 3/17/88Start at stow, grab cluster from rackand retract to running position, leave live.

4BC0 DE80 D --Open right claw4C76 DDB4 A4D2C DD4E A4DE2 DCE8 A4E98 DC82 A4F4E DC1C A5004 DBB6 A50BA DB50 A5170 DAEA A5226 DA80 A52E0 DAOO P --Clear of stow handle, load higher gains52E0 D966 A52E0 D8CC A52E0 D832 A52E0 D798 A52E0 D6FE A52E0 D664 A52E0 D5CA A52E0 D530 A52E0 D496 A52E0 D3FC A52E0 D360 A50CD D360 A4EBA D360 A4CA7 D360 A4A94 D360 A4881 D360 A466E D360 A445B D360 A4248 D360 A4035 D360 A3E22 D360 A3COF D360 A39FC D360 A37E9 D360 A35D6 D360 A33C3 D360 A31BO D360 A2F9D D360 A2D8A D360 A2B77 D360 A2964 D360 A2751 D360 A253E D360 A232B D360 A2118 D360 A1F05 D360 A1CF2 D360 A1ADF D360 A18CC D360 A16B9 D360 A14A6 D360 A1293 D360 A1080 D360 A

140

28F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F42AD42CD42ED430D432D434D436D438D43AD43CD43ED440D442D444D446D448D44AD44CD44ED450D452D454D456D458D45AD45CD45ED460D462D464D466D46894

50845084508450845084508450845084508450845084508450845084508450845084508450845084508450844E444C444A44484446444444425440543E543C543C543C543C543C543C543C543C543C543C543C543C543C543C543C543C543C543C543C543C543C543C543C54

4F04323742B61F813A07C

FFBEFF00FE42FD80FC73FB66FA59F94CF83FF732F625F518F40BF2FEF1F0F1F0F1F0F1F0FiF0F1F0F1F0F1F0F1F0FiF0F1F0F1F0F1F0F1F0F1F0F1F0F1F0FiF0F1F0F1F0F1F0F1F0F1F0F1F0F1F0F1F0FiF0F1F0F1F0F1F0F1F0FiF0F1F0

3C54 F1FO3C54 FIFO3C54 F1FO3C54 FiFO3C54 F2A63C54 F35C3C54 F4123C54 F4C83C54 F57C3C54 F6343C54 F6EA3C54 F7AO3C54 F8563C54 F9103C54 F9103C54 F9633C54 FA563C54 FAF93C54 FB9C3C54 FC3F3C54 FCE23C54 FD853C54 FE283C54 FECB3C54 FF6E3C54 113C54 B43C54 1573C54 1FA3C54 29D3C54 3403C54 3E33C54 4863C54 5303C54 5303C54 5303800 1038A2 103944 1039E6 103A88 103B2A FFF43BCC FF6E3C6E FEE83D10 FE623DB2 FDDC3E54 FD563EF6 FCDO3F98 FC4A403A FBC440DC FB3E4184 FABO4184 FABO

E6D D360 AC5A D360 AA47 D360 A834 D360 A740 D360 A740 D360 A740 D360 A740 D360 A740 D360 A740 D360 A740 D360 A740 D360 A740 D360 A740 D360 A740 D360 N --On approach to cluster, use lower gains740 D3D9 A740 D452 A740 D4CB A740 D544 A740 D5BD A740 D636 A740 D6AF A740 D728 A740 D7A1 A740 D81A A740 D893 A740 D90C A740 D985 A740 D9FE A740 DA77 A740 DAFO A740 DB69 A740 DBE2 A740 DC60 E --Close right claw740 DC60 Q --Increase gains to heavy740 DC60 7 --Increase allowable error6AO D170 A --Jerk the cluster off the rack6A0 D146 A6AO CFBO A6AO CE1A A6AO CC84 A717 CAEE A78E C958 A805 C7C2 A87C C62C A8F3 C496 A96A C300 A9E1 C16A AA58 BFD4 AACF BE3E AB46 BCAO AOBCO BCAO AOBCO BCAO H --End of file

141

68946894689468946894689468946894689468946894689468946894689468946894689468946894689468E46894689468946894689468946894689468946894689468946894689460005F3E5E7C5DBA5CF85C365B745AB259F0

592E586C57AA56E85626556454945494

Ctri.trd: 3/17/88Start at stow and move arm into the workspace

DE80 DDE32 ADDE4 ADD96 ADD48 ADCFA ADCAC ADC5E ADC10 ADBC2 ADB74 ADB26 ADAD8 ADA8A ADA32 AD9EE AD9AO AD952 AD904 AD8B6 AD868 AD810 PD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 AD810 A

--Open right claw

--Claw clear of stow, load higher gains

142

28F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F428F42AD42CD42ED430D432D434D436D438D43AD43CD43ED440D442D444D446D448D44AD44CD44ED450D452D4549454945494549454945494549454945494549454945494

50845084508450845084508450845084508450845084508450845084508450845084508450845084508450844E444C444A44484446444444425441844184418441844184418441844184418441844184418441844184418441844184418441844184418441844184418441844184

4F04323742B61F813A07C

FFBEFF00FE42FD80FC73FB66FA59F94CF83FF732F625F518F40BF2FEFIFOFIFOFiFOFIFOFIFOFiFOFiFOFIFOFiFOFiFOFiFOFIFOFIFOFIFOFiFOFIFOFiFOFiFOFIFOFiFOFiFOFIFOFIFOFIFOFiFOFIFOFiFOFIFOFiFOFIFOFiFOFIFOFIFOFIFO

4BCO4C764D2C4DE24E984F4E500450BA5170522652E052E052E052E052E052E052E052E052E052E052E052E050CD4EBA4CA74A944881466E445B424840353E223COF39FC37E935D633C331B02F9D2D8A2B7729642751253E232B21181F051CF21ADF18CC16B914A612931080E6D

5494 4184 FF05494 4184 FF0

C5A D810 ABC0 D810 H --End of trajectory

143

Cjoint.trd: 3/17/88Start at running position for carryinga cluster, and move cluster for easymanual joint assembly.

BCO BCAO TBCO BCAO TC67 BE4D PDOE BFFA ADB5 C1A7 AE5C C354 AF03 C501 AFAA C6AE A1051 C85B A10F8 CA08 A119F CBB5 A1246 CD62 A12ED CFOF A1394 DOBC A143B D269 A14E2 D416 A1589 D5C3 A1630 D770 A16D7 D91D A177E DACA A1825 DC77 A18CC DE24 A1973 DFD1 A1A1A E17E AlADO E340 TlADO E340 TlADO E340 H

--Pause for cluster to settle

--Load higher gains

--Pause to allow cluster to settle

--End of trajectory

144

549454945494549454945494549454945494549454945494549454945494549454945494549454945494549454945494549454945494

418441844184418441844184418441844184418441844184418441844184418441844184418441844184418441844184418441844184

FABOFABOFB64FC18FCCCFD80FE34FEE8FF9C

501041B826C3203D448853C5FO6A475880C8CO974A28AFOAFOAFO

Rgrabslv.trd: 3/17/88With claw on cluster just attached toexisting beam, grab sleeve on beam end.

1B20 E2C0 D --Open right claw1B20 E232 A1B20 E1A4 A1B20 E116 A1B20 E088 A1B20 DFFA A1B20 DF6C A1B20 DEDE A1B20 DE50 A1B20 DDC2 A1B20 DD34 A1B20 DCA6 A1B20 DC18 A1B20 DB8A A1B20 DAFC A1B20 DA6E A1B20 D9EO A1B20 D952 A1B20 D8C4 A1B20 D836 A1B20 D7AO P --Claw clear of cluster, load higher gains1B80 D7AO AlC00 D7A0 A1C80 D7A0 A1D00 D7AO A1D80 D7AO A1E00 D7AO A1E80 D7AO A1F00 D7AO A1F80 D7AO A2000 D7AO A2080 D7AO A2100 D7AO A2180 D7AO A2200 D7AO A2280 D7AO A2300 D7AO A2380 D7AO A2400 D7AO A2480 D7AO A2500 D7AO A2580 D7AO A2600 D7AO A2680 D7AO A2700 D7AO A2780 D7AO A2800 D7AO A2880 D7A0 A2900 D7AO A2980 D7A0 N --On approach to sleeve, load lower gains2980 D872 A2980 D944 A2980 DA16 A2980 DAE8 A

145

557455745574557455745574557455745574557455745574557455745574557455745574557455745574548453845284518450844F844E844D844C844B844A844984498449844984498449844984498449844984498449844984498449844984498449844984498449844984

44A444A444A444A444A444A444A444A444A444A444A444A444A444A444A444A444A444A444A444A444A4452445244524452445244524452445244524452445244524452445244524452445244524452445244524452445244524452445244524452445244524452445244524

8407936E663958C4DF4323852D822B17ED124

FF77FECAFE1DFD70FCC3FC16FB69FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FB30FBE8FCAOFD58FE10

4984 4524 FEC8 2980 DBBA A4984 4524 FF80 2980 DC8C A4984 4524 38 2980 DD5E A4984 4524 FO 2980 DE30 A4984 4524 1A8 2980 DF02 A4984 4524 260 2980 DFD4 A4984 4524 318 2980 EOA6 A4984 4524 3D0 2980 E178 A4984 4524 488 2980 E24A A4984 4524 540 2980 E31C A4984 4524 5F8 2980 E3EE A4984 4524 6B0 2980 E4CO A4984 4524 768 2980 E592 A4984 4524 820 2980 E664 A4984 4524 8D8 2980 E736 A4984 4524 990 2980 E808 A4984 4524 A48 2980 E8DA A4984 4524 BOO 2980 E9AC A4984 4524 BB8 2980 EA7E A4984 4524 C70 2980 EB50 A4984 4524 D28 2980 EC22 A4984 4524 DEO 2980 ECF4 A4984 4524 E98 2980 EDC6 A4984 4524 F50 2980 EE98 A4984 4524 1008 2980 EF6A A4984 4524 10CO 2980 F03C A4984 4524 1178 2980 FlOE A4984 4524 1240 2980 F1EO E --Close right claw4984 4524 1240 2980 F1EO H --End of trajectory

146

Rsldslv.trd: 3/17/88Slide the sleeve from existing beamonto a cluster being attached.

F1EO EF144 QFOA8 7FOOC AEF70 AEED4 AEE38 AED9C AEDOO AECAO AEC40 AEBEO AEB80 AEB20 AEACO AEA60 AEAOO AE9AO AE940 AE906 AE8CC AE892 AE858 AE81E AE7EO TE7EO H

--Close right claw--Load heavy gains--Increase allowable error size

--Jolt Shy and allow to settle--End of trajectory

147

498449664948492A490C48EE48D0

48B2489448F8495C49C04A244A884AEC4B504BB44C184C844CC14CFE4D3B4D784DB54DF44DF4

4524451A4510450644FC44F244E844DE44D444C444B444A444944484447444644454444444444434442444144404440444044404

124011EE119C114A10F810A610541002FBOF75F3AEFFEC4E89E4EE13DD8D9DD60D36DOCCE2CB8C8EC60C60

2980292028C0

2860280027A0274026E02680263425E8259C2550250424B8246C242023D42380233A22F422AE226822201E202220

Rstow.trd: 3/17/88While attaching a cluster to existing beam,leave slide sleeve and go to stow handle.

--Open right claw

--Clear of sleeve, load higher gains

148

4DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44DF44BF449F447F445F443F441F43FF43DF43BF439F437F435F433F431F42FF42DF42BF429F427F425F425F425F425F4

440444044404440444044404440444044404440444044404440444044404440444044404440444044404440444044404440444044404440444044404440444A445AC46A447AC48A449AC4AA44BAC4CA44DA44EA44FA450A450A450A450A450A450A450A450A450A450A450A4

C60BEBB76B01A8CA179A292D8B88437CE7596E466F5FA58551049B4263B13301AA24

FE9EFD18FB92FAOCF886F700F57AF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FOF3FO

222022202220222022202220222022202220222022202220222022202220222022202220222022202220222022202220222022202220222022202220222023CO2560270028AO2A402BEO2D802F2030C0

3260340035A0

374038E03A803C203DCO3F60410042A0

444045E04780

E7EOE77BE716E6B1E64CE5E7E582E51DE4B8E453E3EEE389E324E2BFE25AE1F5E190E12BEOC6E061DFFODF27DE5EDD95DCCCDC03DB3ADA71D9A8D8DFD810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810D810

25F4 50A4 F3F0 4920 D810 A25F4 50A4 F3FO 4AC0 D810 A25F4 50A4 F3FO 4C60 D810 A25F4 50A4 F3FO 4E00 D810 A25F4 50A4 F3FO 4FAO D810 A25F4 50A4 F3FO 5140 D810 A25F4 50A4 F3FO 52E0 D810 A25F4 5084 F3FO 52E0 D810 A25F4 5084 F4A7 52E0 D810 A25F4 5084 F55E 52E0 D810 A25F4 5084 F615 52E0 D810 A25F4 5084 F6CC 52E0 D810 A25F4 5084 F783 52E0 D810 A25F4 5084 F83A 52E0 D810 A25F4 5084 F8F1 52E0 D810 A25F4 5084 F9A8 52E0 D810 A25F4 5084 FA5F 52E0 D810 A25F4 5084 FB16 52E0 D810 A25F4 5084 FBCD 52E0 D810 A25F4 5084 FC84 52E0 D810 A25F4 5084 FD3B 52E0 D810 A25F4 5084 FD80 52E0 D810 A25F4 5084 FD80 52E0 D810 A25F4 5084 FD80 52E0 D810 N --On approach to stow, load lower gains25F4 5084 FD80 5280 D862 A25F4 5084 FD80 5220 D8B4 A25F4 5084 FD80 51C0 D906 A25F4 5084 FD80 5160 D958 A25F4 5084 FD80 5100 D9AA A25F4 5084 FD80 50A0 D9FC A25F4 5084 FD80 5040 DA4E A25F4 5084 FD80 4FEO DA80 A25F4 5084 FE12 4F80 DAF2 A25F4 5084 FEA4 4F20 DB44 A25F4 5084 FF36 4EC0 DB96 A25F4 5084 FFC8 4E60 DBE8 A25F4 5084 5A 4E00 DC3A A25F4 5084 EC 4DAO DC8C A25F4 5084 17E 4D40 DCDE A25F4 5084 210 4CE0 DD30 A25F4 5084 2A2 4C80 DD82 A25F4 5084 334 4C20 DDD4 A25F4 5084 3C6 4BC0 DE26 A25F4 5084 458 4BCO DE80 A25F4 5084 4F0 4BC0 DE80 S --Call for slaverest() at end of trajectory25F4 5084 4F0 4BC0 DE80 E --Close right claw25F4 5084 4F0 4BC0 DE80 H --End of trajectory

149

APPENDIX V. EASE structure assembly scheduleBAT EASE assembly progression0. Startup

A. Set up right arm1). Call for control power on2). Run right arm through it's zeros3). Enable Left Claw.4). Enable Right Claw.5). Enable Beam Carrier.6). Have right arm placed on the stow handle.7). Close right claw8). Slaverest the arm

B. Tilt and Pan setup.1). Run tilt and pan through it's zeros.2). Center head controller3). Disconnect head.

C. Enable Puma1). Call for main power on.2). Enable Puma3). Power relay on.4). Video switch to Belly

1. Attach the First Top Cluster to the first racked upright beam (Cla - Blb).A. Dock to cluster Rack position 1.

1). Manuever thrusters to place left claw on the first cluster rack dock location.2). Close left claw.

B. Retreive cluster C11). Video switch to Tilt and Pan (op.)2). Connect head controller (op.)3). Manuever head to view stow handle (op.)4). operators arm to master arm.5). Press Button to initate Triad trajectory "Cgrab" to reteive cluster6). Verify the cluster is free of the rack (arm will remain live)7). Disconnect master arm.8). video switch to Belly (op.)9). Open left Claw

C. Dock to free end of beam Blb1). Manuever thrusters to place left claw on beam end Blb to the left side of the2). Close left Claw.

D. Attach cluster to Beam end (Cla - Blb)1). Video switch to Tilt and Pan2). Connect head controller3). Operators arm to master arm4). Verify "Cjoint" showing on next up reverse trajectory5). Push inner finger switch to initiate "cjoint" trajectory.6). Manuever right arm with master arm to make the joint7). Push inner finger switch to initiate "Rgrabslv" and "Rsldslv" trajectories8). Verify button pop9). Push inner finger switch to initate "Rstow" trajectory10). Verify Stow.11). disconnect head12). Video switch to Belly13). Open left Claw.

2. Attach the Second Top Cluster to the second racked upright beam (C2a - B2b).A. Dock to cluster Rack position 2.

1). Manuever thrusters to place left claw on the second cluster rack dock locatio2). Close left claw.

B. Retreive cluster C21). Video switch to Tilt and Pan (op.)2). Connect head controller (op.)3). Manuever head to view stow handle (op.)4). operators arm to master arm.5). Press Button to initate Triad trajectory "Cgrab" to reteive cluster6). Verify the cluster is free of the rack (arm will remain live)

150

e disk.

n.

7). Disconnect master arm.8). video switch to Belly (op.)9). Open left Claw

C. Dock to free end of beam B2b1). Manuever thrusters to place left claw on beam end B2b to the left side of the disk.2). Close left Claw.

D. Attach cluster to Beam end (C2a - B2b)1). Video switch to Tilt and Pan2). Connect head controller3). Operators arm to master arm4). Verify "Cjoint" showing on next up reverse trajectory5). Push inner finger switch to initiate "cjoint" trajectory.6). Manuever right arm with master arm to make the joint7). Push inner finger switch to initiate "Rgrabslv" and "Rsldslv" trajectories8). Verify button pop9). Push inner finger switch to initate "Rstow" trajectory10). Verify Stow.11). disconnect head12). Video switch to Belly13). Open left Claw.

3. Attach the Third Top Cluster to the fourth rack upright beam (C3a - B4b).A. Dock to cluster Rack position 3.

1). Manuever thrusters to place left claw on the third cluster rack dock location.2). Close left claw.

B. Retreive cluster C31). Video switch to Tilt and Pan (op.)2). Connect head controller (op.)3). Manuever head to view stow handle (op.)4). operators arm to master arm.5). Press Button to initate Triad trajectory "Cgrab" to reteive cluster6). Verify the cluster is free of the rack (arm will remain live)7). Disconnect master arm.8). video switch to Belly (op.)

C. Dock to free end of beam B4b1). Manuever thrusters to place left claw on beam end B4b to the left side of the disk.2). Close left Claw.

D. Attach cluster to Beam end (C3a - B4b)1). Video switch to Tilt and Pan2). Connect head controller3). Operators arm to master arm4). Verify "Cjoint" showing on next up reverse trajectory5). Push inner finger switch to initiate "cjoint" trajectory.6). Manuever right arm with master arm to make the joint7). Push inner finger switch to initiate "Rgrabslv" and "Rsldslv" trajectories8). Verify button pop9). Push inner finger switch to initate "Rstow" trajectory10). Verify Stow.11). disconnect head12). Video switch to Belly13). Open left Claw.14). Open left Claw.

4. Attach the first upright beam to the base cluster (Bla - CBa).A. Dock to Beam Rack position 1.

1). Manuever thrusters to place left claw on the first beam rack dock location.2). Close left claw.

B. Retreive Beam B11). Extend Beam carrier2). Close Beam Carrier claw.3). Retract Beam Carrier4). Open left claw.

C. Dock to Base Cluster CBa.1). Manuever thrusters to place left claw on Base Cluster CBa next to black tape.2). Close left Claw.

D. Attach beam to base cluster (Bla - CBa)

151

1). Video switch to Tilt and Pan2). Connect head controller3). Manuever head to view stow handle (op.)4). Operators arm to master arm5). Verify "Grabbeam" showing on next up forward trajectory6). Push outer finger switch to initiate "Grabbeam" and "joint" trajectory.7). Manuever right arm with master arm to make the joint8). Push outer finger switch to initiate "grabslv" and "slidslv" trajectories9). Verify button pop10). Push inner finger switch to initate "stow" trajectory11). Verify Stow.12). disconnect head13). Video switch to Belly14). Open left Claw.

5. Attach the second upright beam to the base cluster (B2a - CBb).A. Dock to Beam Rack position 2.

1). Manuever thrusters to place left claw on the second beam rack dock location.2). Close left claw.

B. Retreive Beam B21). Extend Beam carrier2). Close Beam Carrier claw.3). Retract Beam Carrier4). Open left claw.

C. Dock to Base Cluster CBb.1). Manuever thrusters to place left claw on Base Cluster CBb next to black tape.2). Close left Claw.

D. Attach beam to base cluster (B2a - CBb)1). Video switch to Tilt and Pan2). Connect head controller3). Manuever head to view stow handle (op.)4). Operators arm to master arm5). Verify "Grabbeam" showing on next up forward trajectory6). Push outer finger switch to initiate "Grabbeam" and "joint" trajectory.7). Manuever right arm with master arm to make the joint8). Push outer finger switch to initiate "grabslv" and "slidslv" trajectories9). Verify button pop10). Push inner finger switch to initate "stow" trajectory11). Verify Stow.12). disconnect head13). Video switch to Belly14). Open left Claw.

6. Attach the first cross beam to the first top cluster (B3a - Clb).A. Dock to Beam Rack position 3.

1). Manuever thrusters to place left claw on the 3rd beam rack dock location.2). Close left claw.

B. Retreive Beam B31). Extend Beam carrier2). Close Beam Carrier claw.3). Retract Beam Carrier4). Open left claw.

C. Dock to top Cluster Clb.1). Manuever thrusters to place left claw on top Cluster Clb on the edge of the silver tubing.2). Close left Claw.

D. Attach beam to top cluster (B3a - Clb)1). Video switch to Tilt and Pan2). Connect head controller3). Manuever head to view stow handle (op.)4). Operators arm to master arm5). Verify "Grabbeam" showing on next up forward trajectory6). Push outer finger switch to initiate "Grabbeam" and "joint" trajectory.7). Manuever right arm with master arm to make the joint8). Push outer finger switch to initiate "grabslv" and "slidslv" trajectories9). Verify button pop10). Push inner finger switch to initate "stow" trajectory

152

11). Verify Stow.12). disconnect head13). Video switch to Belly14). Open left Claw.

7. Complete the triangle involving the first cross beam and the second top cluster (B3b - C2c).A. Dock to the free end of beam B3b.

1). Manuever thrusters to place left claw on the beam end B3b next to the disk.2). Close left Claw.

B. Fly the beam end up to cluster C2cC. Attach beam to top cluster ( B3b - C2c)

1). Video switch to Tilt and Pan2). Connect head controller3). Manuever head to view stow handle (op.)4). Operators arm to master arm5). Verify "Cjoint" showing on next up reverse trajectory6). Push inner finger switch to initiate "ctri" trajectory.7). manuever right arm with master arm to position right claw over beam end next to the disk8). Close right claw9). Manuever right arm with master arm to make the joint10). Push inner finger switch to initiate "Rgrabslv" and "Rsldslv" trajectories11). Verify button pop12). Push inner finger switch to initate "Rstow" trajectory13). Verify Stow.14). disconnect head15). Video switch to Belly16). Open left Claw.

8. Attach the third upright beam to the base cluster (B4a - CBc).A. Dock to Beam Rack position 4.

1). Manuever thrusters to place left claw on the 4th beam rack dock location.2). Close left claw.

B. Retreive Beam B41). Extend Beam carrier2). Close Beam Carrier claw.3). Retract Beam Carrier4). Open left claw.

C. Dock to Base Cluster CBc.1). Manuever thrusters to place left claw on Base Cluster CBc next to black tape.2). Close left Claw.

D. Attach beam to base cluster (B4a - CBc)1). Video switch to Tilt and Pan2). Connect head controller3). Manuever head to view stow handle (op.)4). Operators arm to master arm5). Verify "Grabbeam" showing on next up forward trajectory6). Push outer finger switch to initiate "Grabbeam" and "joint" trajectory.7). Manuever right arm with master arm to make the joint8). Push outer finger switch to initiate "grabslv" and "slidslv" trajectories9). Verify button pop10). Push inner finger switch to initate "stow" trajectory11). Verify Stow.12). disconnect head13). Video switch to Belly14). Open left Claw.

9. Attach the second cross beam to the second top cluster (BSa - C2b).A. Dock to Beam Rack position 5.

1). Manuever thrusters to place left claw on the 5th beam rack dock location.2). Close left claw.

B. Retreive Beam B51). Extend Beam carrier2). Close Beam Carrier claw.3). Retract Beam Carrier4). Open left claw.

C. Dock to top Cluster C2b.1). Manuever thrusters to place left claw on top Cluster C2b on the edge of the silver tubing.

153

2). Close left Claw.D. Attach beam to top cluster (B5a - C2b)

1). Video switch to Tilt and Pan2). Connect head controller3). Manuever head to view stow handle (op.)4). Operators arm to master arm5). Verify "Grabbeam" showing on next up forward trajectory6). Push outer finger switch to initiate "Grabbeam" and "joint" trajectory.7). Manuever right arm with master arm to make the joint8). Push outer finger switch to initiate "grabslv" and "slidslv" trajectories9). Verify button pop10). Push inner finger switch to initate "stow" trajectory11). Verify Stow.12). disconnect head13). Video switch to Belly14). Open left Claw.

10. Complete the triangle involving the second cross beam and the third top cluster (B5b - C3c).A. Dock to the free end of beam B5b.

1). Manuever thrusters to place left claw on the beam end B5b next to the disk.2). Close left Claw.

B. Fly the beam end up to cluster C3cC. Attach beam to top cluster ( B5b - C3c)

1). Video switch to Tilt and Pan2). Connect head controller3). Manuever head to view stow handle (op.)4). Operators arm to master arm5). Verify "Cjoint" showing on next up reverse trajectory6). Push inner finger switch to initiate "ctri" trajectory.7). manuever right arm with master arm to position right claw over beam end next to the disk8). Close right claw9). Manuever right arm with master arm to make the joint10). Push inner finger switch to initiate "Rgrabslv" and "Rsldslv" trajectories11). Verify button pop12). Push inner finger switch to initate "Rstow" trajectory13). Verify Stow.14). disconnect head15). Video switch to Belly16). Open left Claw.

11. Attach the third cross beam to the third top cluster (B6a - C3b).A. Dock to Beam Rack position 6.

1). Manuever thrusters to place left claw on the 6th beam rack dock location.2). Close left claw.

B. Retreive Beam B61). Extend Beam carrier2). Close Beam Carrier claw.3). Retract Beam Carrier4). Open left claw.

C. Dock to top Cluster C3b.1). Manuever thrusters to place left claw on top Cluster C3b on the edge of the silver tubing.2). Close left Claw.

D. Attach beam to top cluster (B6a - C3b)1). Video switch to Tilt and Pan2). Connect head controller3). Manuever head to view stow handle (op.)4). Operators arm to master arm5). Verify "Grabbeam" showing on next up forward trajectory6). Push outer finger switch to initiate "Grabbeam" and "joint" trajectory.7). Manuever right arm with master arm to make the joint8). Push outer finger switch to initiate "grabslv" and "slidslv" trajectories9). Verify button pop10). Push inner finger switch to initate "stow" trajectory11). Verify Stow.12). disconnect head13). Video switch to Belly

154

14). Open left Claw.12. Complete the triangle involving the third cross beam and the second top cluster (B6b - Clc).

A. Dock to the free end of beam B6b.1). Manuever thrusters to place left claw on the beam end B6b next to the disk.2). Close left Claw.

B. Fly the beam end up to cluster ClcC. Attach beam to top cluster ( B6b - Clc)

1). Video switch to Tilt and Pan2). Connect head controller3). Manuever head to view stow handle (op.)4). Operators arm to master arm5). Verify "Cjoint" showing on next up reverse trajectory6). Push inner finger switch to initiate "ctri" trajectory.7). manuever right arm with master arm to position right claw over beam end next to the disk8). Close right claw9). Manuever right arm with master arm to make the joint10). Push inner finger switch to initiate "Rgrabslv" and "Rsldslv" trajectories11). Verify button pop12). Push inner finger switch to initate "Rstow" trajectory13). Verify Stow.14). disconnect head15). Video switch to Belly16). Open left Claw.

13. Shut DownA. Power relay offB. Call for main power offC. Call for Control power offD. Disable PumaE. Disable beam carrierF. Disable left clawG. Disable right claw

155