perm filename IINIT.2[EAL,HE]2 blob
sn#706557 filedate 1983-04-19 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00005 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 {$NOMAIN Interpreter Initialization routines }
C00028 00003 { Externally defined routines: }
C00031 00004 (* Initialization and wrapup routines: calibrate, initArms & killArms *)
C00038 00005 (* Initialization routines: consDef, initWorld *)
C00044 ENDMK
C⊗;
{$NOMAIN Interpreter Initialization routines }
const
GARMDEV = 1; (* device numbers for ARM *)
GHANDDEV = 2;
RARMDEV = 3;
RHANDDEV = 4;
DRIVERDEV = 5;
VISEDEV = 6;
garmpower = 1; (* bit defs - used in response to initarmscmd *)
garmcal = 2;
ghandpower = 4;
ghandcal = (* 10B *) 8;
rarmpower = (* 20B *) 16;
rarmcal = (* 40B *) 32;
rhandpower = (* 100B *) 64;
rhandcal = (* 200B *) 128;
(* Constants from EDIT *)
maxLines = 28;
maxPPLines = 12;
maxBpts = 25;
maxTBpts = 20; (* max could be exceeded by huge case stmnt *)
listinglength = 2000; (* Length of Listingarray *)
(* Random type declarations for OMSI/SAIL compatibility *)
type
byte = 0..255; (* doesn't really belong here, but... *)
ascii = char;
atext = text;
{ Define all the pointer types here }
vectorp = ↑vector;
transp = ↑trans;
strngp = ↑strng;
eventp = ↑event;
framep = ↑frame;
statementp = ↑statement;
varidefp = ↑varidef;
nodep = ↑node;
pdbp = ↑pdb;
envheaderp = ↑envheader;
enventryp = ↑enventry;
environp = ↑environment;
cmoncbp = ↑cmoncb;
messagep = ↑message;
(* This one is used whenever a pointer is needed for which the *)
(* definition is missing from this file; naturally, all *)
(* pointers use the same space *)
dump = ↑integer;
token = array[1..4] of integer; {Uses same space as a token}
cursorp = array[1..4] of integer; {Ditto, for cursorp}
(* datatype definitions *)
datatypes = (pconstype, varitype, svaltype, vectype, rottype, transtype,
frametype, eventtype, strngtype, labeltype, proctype, arraytype,
reftype, valtype, cmontype, nulltype, undeftype,
dimensiontype, mactype, macargtype, freevartype);
scalar = real;
u = (used,free);
vector = record case u of
used: (refcnt: integer; val: array [1..3] of real);
free: (next: vectorp);
end;
trans = record case u of
used: (refcnt: integer; val: array [1..3,1..4] of real);
free: (next: transp);
end;
cstring = packed array [1..10] of ascii;
c4str = packed array [1..4] of ascii;
c5str = packed array [1..5] of ascii;
c20str = packed array [1..20] of ascii;
linestr = packed array [1..130] of ascii;
strng = record
next: strngp;
ch: cstring;
end;
event = record
next: eventp; (* all events are on one big list *)
count: integer;
waitlist: pdbp;
end;
frame = record
vari: varidefp; (* back pointer to variable name & info *)
calcs: nodep; (* affixment info *)
case ftype: boolean of (* frame = true, device = false *)
true: (valid: integer; val, fdepr: transp; dcntr: integer; dev: framep);
false: (mech: integer; case sdev: boolean of
true: (sdest: real); false: (tdest,appr,depr: transp));
(* sdev = true for scalar devices, false for frames *)
end;
(* statement definitions *)
stmntypes = (progtype, blocktype, coblocktype, endtype, coendtype,
fortype, iftype, whiletype, untiltype, casetype,
calltype, returntype,
printtype, prompttype, pausetype, aborttype, assigntype,
signaltype, waittype, enabletype, disabletype, cmtype,
affixtype, unfixtype,
movetype,jtmovetype,operatetype,opentype,closetype,centertype,
floattype, stoptype, retrytype,
requiretype, definetype, macrotype, commenttype, dimdeftype,
setbasetype, wristtype, saytype, declaretype, emptytype,
evaltype, armmagictype);
(* more??? *)
statement = packed record
next, last: statementp;
stlab: varidefp;
exprs: nodep; (* any expressions used by this statement *)
nlines: integer;
bpt,bad: boolean;
case stype: stmntypes of
armmagictype: (cmdnum,dev,iargs,oargs: nodep);
end;
(* auxiliary definitions: variable, etc. *)
varidef = packed record
next,dnext: varidefp;
name: dump;
level: 0..255; (* environment level *)
offset: 0..255; (* environment offset *)
dtype: varidefp; (* to hold the dimension info *)
tbits: 0..15; (* special type bits: array = 1, proc = 2, ref = 4 & ? *)
dbits: 0..15; (* for use by debugger/interpreter *)
case vtype: datatypes of
arraytype: (a: nodep);
proctype: (p: nodep);
labeltype,
cmontype: (s: statementp);
mactype: (mdef: statementp);
macargtype: (marg: dump);
pconstype: (c: nodep);
dimensiontype: (dim: nodep);
end;
(* definition of the ubiquitous NODE record *)
nodetypes = (exprnode, leafnode, listnode, clistnode, colistnode, forvalnode,
deprnode, viaptnode, apprnode, destnode, byptnode, durnode,
sfacnode, wobblenode, swtnode, nullingnode, wristnode, cwnode,
arrivalnode, departingnode,
ffnode, forcenode, stiffnode, gathernode, cmonnode, errornode,
calcnode, arraydefnode, bnddefnode, bndvalnode,
waitlistnode, procdefnode, tlistnode, dimnode, commentnode,
linearnode, elbownode, shouldernode, flipnode, wrtnode,
loadnode,velocitynode);
exprtypes = ( svalop, (* scalar operators *)
sltop, sleop, seqop, sgeop, sgtop, sneop, (* relations *)
notop, orop, xorop, andop, eqvop, (* logical *)
saddop, ssubop, smulop, sdivop, snegop, sabsop, (* scalar ops *)
sexpop, maxop, minop, intop, idivop, modop,
sqrtop, logop, expop, timeop, (* functions *)
sinop, cosop, tanop, asinop, acosop, atan2op, (* trig *)
vdotop, vmagnop, tmagnop,
vecop, (* vector operators *)
vmakeop, unitvop, vaddop, vsubop, crossvop, vnegop,
svmulop, vsmulop, vsdivop, tvmulop, wrtop,
tposop, taxisop,
transop, (* trans operators *)
tmakeop, torientop, ttmulop, tvaddop, tvsubop, tinvrtop,
vsaxwrop, constrop, ftofop, deproachop, fmakeop, vmkfrcop,
ioop, (* i/o operators *)
queryop, inscalarop,
specop, (* special operators *)
arefop, callop, grinchop, macroop, vmop, adcop, dacop, jointop,
badop,
addop, subop, negop, mulop, divop, absop); (* for parsing *)
leaftypes = pconstype..strngtype;
reltypes = sltop..sgtop;
forcetypes = (force,absforce,torque,abstorque,angvelocity);
node = record
next: nodep;
case ntype: nodetypes of
exprnode: (op: exprtypes; arg1, arg2, arg3: nodep; elength: integer);
leafnode: (case ltype: leaftypes of
varitype: (vari: varidefp; vid: dump);
pconstype: (cname: varidefp; pcval: nodep);
svaltype: (s: scalar; wid: integer);
vectype: (v: vectorp);
transtype: (t: transp);
strngtype: (length: integer; str: strngp) ); (* also used by commentnodes *)
listnode: (lval: nodep);
waitlistnode: (who: pdbp; when: integer);
procdefnode:(ptype: datatypes; level: 0..255;
pname, paramlist: varidefp; body: statementp);
dimnode: (time, distance, angle, dforce: integer);
end;
(* process descriptor blocks & environment record definitions *)
queuetypes = (nullqueue,nowrunning,runqueue,inputqueue,eventqueue,sleepqueue,
forcewait,devicewait,joinwait,proccall);
pdb = packed record
nextpdb,next: pdbp; (* for list of all/active pdb's *)
level: 0..255; (* lexical level *)
mode: 0..255; (* expression/statement/sub-statement *)
priority: 0..255; (* probably never greater than 3? *)
status: queuetypes; (* what are we doing *)
env: envheaderp;
spc: statementp; (* current statement *)
epc: nodep; (* current expression (if any) *)
sp: nodep; (* intermediate value stack *)
cm: cmoncbp; (* if we're a cmon point to our definition *)
mech: framep; (* current device being used *)
linenum: integer; (* used by editor/debugger *)
case procp: boolean of (* true if we're a procedure *)
true: (opdb: pdbp; (* pdb to restore when procedure exits *)
pdef: nodep); (* procedure definition node *)
false: (evt: eventp; (* event to signal when process goes away *)
sdef: statementp); (* first statement where process was defined *)
end;
envheader = packed record
parent: envheaderp;
env: array [0..4] of environp;
varcnt: 0..255; (* # of variables in use ??? *)
case procp: boolean of (* true if we're a procedure *)
true: (proc: nodep);
false:(block: statementp);
end;
enventry = record
case etype: datatypes of
svaltype: (s: scalar);
vectype: (v: vectorp);
transtype: (t: transp);
frametype: (f: framep);
eventtype: (evt: eventp);
strngtype: (length: integer; str: strngp);
cmontype: (c: cmoncbp);
proctype: (p: nodep; penv: envheaderp);
reftype: (r: enventryp);
arraytype: (a: envheaderp; bnds: nodep);
end;
environment = record
next: environp;
vals: array [0..9] of enventryp;
end;
cmoncb = record
running, enabled: boolean; (* cmon's status *)
cmon: statementp;
pdb: pdbp;
evt: eventp;
fbits: integer; (* bits for force sensing *)
oldcmon: cmoncbp; (* for debugger *)
end;
(* definition of AL-ARM messages *)
msgtypes = (initarmscmd,calibcmd,killarmscmd,wherecmd,
abortcmd,stopcmd,movehdrcmd,movesegcmd,
centercmd,operatecmd,movedonecmd,signalcmd,
setccmd,forcesigcmd,forceoffcmd,biasoncmd,biasoffcmd,setstiffcmd,
zerowristcmd,wristcmd,gathercmd,getgathercmd,readadccmd,writedaccmd,
errorcmd,floatcmd,setloadcmd,
armmagiccmd,realcmd,vectorcmd,transcmd);
errortypes = (noerror,noarmsol,timerr,durerr,toolong,featna,
unkmess,srvdead,adcdead,nozind,exjtfc,paslim,nopower,badpot,devbusy,
baddev,timout,panicb,nocart,cbound,badparm);
message = record
cmd: msgtypes;
ok: boolean;
case integer of
1: (dev, bits, n: integer;
(* (dev, bits, n, evt: integer; (* for arm code version *)
evt: eventp;
dur: real;
case integer of
1: (v1,v2,v3: real);
2: (sfac,wobble,pos: real);
3: (val,angle,mag: real);
4: (max,min: real);
5: (error: errortypes));
2: (fv1,fv2,fv3,mv1,mv2,mv3: real); (* may never use these... *)
3: (t: array [1..6] of real);
end;
interr = record
case integer of
0: (i: integer);
1: (err,foo: errortypes);
end;
listingarray = packed array [0..listinglength] of ascii;
(* global variables *)
var
(* from EDIT *)
listing: listingarray; (* first 150 chars are used by expression editor *)
(* next 40 by header & trailer lines *)
{*} cursorStack: array [1..15] of cursorp; {These are BIG records! }
(* lbuf: array [1..160] of ascii;
ppBuf: array [1..100] of ascii; *)
dum1: array[1..260] of ascii;
lines: array [1..maxLines] of dump;
ppLines: array [1..maxPPLines] of dump;
(* marks: array [1..20] of integer;
reswords: array [0..26] of reswordp;
idents: array [0..26] of identp;
macrostack: array [1..10] of tokenp;
curmacstack: array [1..10] of varidefp;
screenheight,dispHeight: integer;
ppBufp,oppBufp,ppOffset,ppSize,nmarks: integer;
lbufp,cursor,ocur,cursorLine,fieldnum,lineNum,findLine,pcLine: integer;
firstDline,topDline,botDline,firstLine,lastLine,curLine: integer;
freeLines,oldLines: linerecp;
*) dum2: array[1..117] of integer;
sysVars: varidefp;
dProg: statementp;
curBlock, newDeclarations, findStmnt: statementp;
macrodepth: integer;
filedepth, errCount, sCursor: integer;
curChar, maxChar, curFLine, curPage: integer;
nodim, distancedim, timedim, angledim,
forcedim, torquedim, veldim, angveldim: varidefp;
fvstiffdim, mvstiffdim: nodep;
pnode: nodep;
(* smartTerminal: boolean;
setUp,setExpr,setCursor,dontPrint,outFilep,collect,fParse,sParse,
eofError,endOfLine,backup,expandmacros,flushcomments,checkDims,
shownLine: boolean;
*) dum3: array[1..16] of boolean;
curtoken: token;
file1,file2,file3,file4,file5,outFile: atext;
bpts: array [1..maxBpts] of statementp; (* debugging crap *)
tbpts: array [1..maxTBpts] of statementp;
debugPdbs: array [0..10] of pdbp;
(* nbpts,ntbpts,debugLevel: integer;
eCurInt: pdbp;
STLevel: integer;
*) dum4: array[1..5] of integer;
singleThreadMode,tSingleThreadMode: boolean;
(* from INTERP *)
inputLine: array [1..20] of ascii;
talk: text; (* for using the speech synthesizer *)
curInt, activeInts, readQueue, allPdbs: pdbp;
sysEnv: envheaderp;
clkQueue: nodep;
allEvents: eventp;
etime: integer; (* used by eval *)
curtime: integer; (* who knows where this will get updated - an ast? *)
stime: integer; (* used for clock queue on 10 *)
msg: messagep; (* for AL-ARM interaction *)
inputp: integer; (* current offset into inputLine array above *)
resched, running, escapeI, iSingleThreadMode: boolean;
msgp: boolean; (* flag set if any messages pending *)
inputReady: boolean;
(* various constant pointers *)
xhat,yhat,zhat,nilvect: vectorp;
niltrans: transp;
gpark, rpark: transp; (* arm park positions *)
(* various device & variable pointers *)
speedfactor: enventryp;
garm: framep;
{ Externally defined routines: }
(* From ALLOC *)
function newEheader: envheaderp; external;
function newEnvironment: environp; external;
(* From IAUX1A *)
function getPdb: pdbp; external;
function getEntry (level, offset: byte): enventryp; external;
procedure makeVar(e: enventryp; vari: varidefp; tbits: integer);external;
function enterEntry (var i,j: integer; var env: environp;
envhdr: envheaderp; v: varidefp): enventryp; external;
procedure sendCmd; external;
procedure sendTrans(tr: transp); external;
(* From IAUX2A *)
procedure getReply(sendIt: boolean); external;
function whereArm (mech: integer): transp; external;
(* Arithmetic Routines *)
function vmake (a,b,c: scalar): vectorp; external;
function tmake (t: transp; v: vectorp): transp; external;
function vsaxwr(ax: vectorp; w: real): transp; external;
(* From RSXRUT *)
procedure initMsg(var buf: messagep; var flag: boolean); external;
(* From RSXMSG *)
function startArm: boolean; external;
(* From PP *)
procedure ppLine; external;
procedure ppOutNow; external;
procedure ppChar(ch: ascii); external;
procedure pp5(ch: c5str; length: integer); external;
procedure pp10(ch: cstring; length: integer); external;
procedure pp10L(ch: cstring; length: integer); external;
procedure pp20(ch: c20str; length: integer); external;
procedure pp20L(ch: c20str; length: integer); external;
(* From DISP *)
function getChar: ascii; external;
procedure escInit(var flg: boolean); external;
(* Initialization and wrapup routines: calibrate, initArms & killArms *)
function getPromptChar: ascii;
var ch: ascii;
begin
repeat ch := getChar until ord(ch) <> 12B; (* Read one character *)
if ord(ch) = 15B then ch := ' '; (* Convert CR to space *)
ppChar(ch); ppOutNow; (* and echo it *)
if ('a' <= ch) and (ch <= 'z') then ch := chr(ord(ch)-40B); (* To upper case *)
getPromptChar := ch;
end;
procedure calibrate; external;
procedure calibrate;
var b: boolean; i,calbits: integer; ch: ascii;
function bitOn(i: integer): boolean;
begin bitOn := (msg↑.bits AND i) <> 0 end; (* *** non-standard Pascal *** *)
procedure whichArm;
begin
case i of (* tell which arm/hand *)
1: pp5('GARM ',4);
2: pp5('GHAND',5);
3: pp5('RARM ',4);
4: pp5('RHAND',5);
end;
end;
function powerOn: boolean;
var b: boolean;
begin
case i of
1: b := bitOn(garmpower);
2: b := bitOn(ghandpower);
3: b := bitOn(rarmpower);
4: b := bitOn(rhandpower);
end;
powerOn := b;
end;
begin (* hand-shaking code to calibrate arms *)
begin
for i := 1 to 4 do (* try to init just the PUMA's & hands for now *)
begin
repeat
with msg↑ do
begin
ch := ' ';
cmd := initarmscmd;
case i of
1: begin dev := garmdev; calbits := garmcal end;
2: begin dev := ghanddev; calbits := ghandcal end;
3: begin dev := rarmdev; calbits := rarmcal end;
4: begin dev := rhanddev; calbits := rhandcal end;
end;
getReply(true); (* send over init command & wait for reply *)
b := ok and powerOn;
if not ok then
pp20L('Couldn''t initialize ',20)
else if not b then
pp20L('Power off for ',14);
if not b then begin whichArm; ppOutNow end;
if ok then (* try to calibrate PUMA's *)
begin
while not b do (* get power turned on *)
begin
pp20L('Turn on arm high pow',20); pp20 ('er (Type SPACE to co',20);
pp20 ('ntinue, any other to',20); pp10 (' abort): ',9);
ppOutNow;
ch := getPromptChar;
if ch <> ' ' then (* any letter will abort *)
begin
pp10L(' Aborted ',8);
if not bitOn(calbits) then pp20(' - not calibrated ',17);
ppLine;
ppOutNow;
b := true; (* so we leave power up loop *)
end
else
begin (* keep trying *)
getReply(true); (* retry the init command & check power *)
b := ok and powerOn;
end;
end;
if ch <> ' ' then b := false
else b := bitOn(calbits);
if (ch = ' ') and not b then (* if not already calibrated ... *)
begin
pp20L('Type Y to calibrate ',20); whichArm;
ppOutNow;
ch := getPromptChar;
if (ch = 'Y') then
begin
cmd := calibcmd;
getReply(true); (* go calibrate arm *)
b := ok;
if b then pp20L('Calibration complete',20)
else begin pp20L('Error while calibrat',20); pp5('ing ',3); end;
end
else begin pp20L(' Aborted - not calib',20); pp5('rated',5); end;
ppLine; ppOutNow;
end;
end;
end;
if not b then
begin
pp20L('Type Y to try again:',20); ppchar(' '); ppOutNow;
ch := getPromptChar;
b := (ch <> 'Y');
end
until b;
end;
end;
end;
procedure initArms; external;
procedure initArms;
var b: boolean; i: integer; ch: ascii;
begin
initMsg(msg,msgp); (* connect to message buffer *)
msgp := false;
(* b := startArm; (* get ARM servo running *)
(* *** *) b := true; (* Someday this will work... *)
if b then
begin
pp20L('Type "Y" to calibrat',20); pp10('e arms: ',8);
ppOutNow;
if getPromptChar = 'Y' then calibrate;
ppLine;
end
else
begin (* Complain if error during startup *)
(* Probably should set some global flag so we don't try to talk to ARM *)
(* or maybe even exit the program *)
pp20L('Error during ARM sta',20); pp20('rtup! Arms not init',20);
pp10('ialized. ',8); ppLine end;
end;
procedure killarms; external;
procedure killarms;
begin
msg↑.cmd := killarmscmd;
sendCmd;
end;
(* Initialization routines: consDef, initWorld *)
{ consDef is one of the first initialization routines called, so let's }
{ tell what we're doing while we're in here. }
procedure consDef; external;
procedure consDef;
var i: integer;
begin
for i:=1 to 15 do writeln;
writeln(' Welcome to AL Version 2.A');
for i:=1 to 3 do writeln;
writeln(' Initializing...');
for i:=1 to 8 do writeln;
xhat := vmake(1,0,0); xhat↑.refcnt := 1000;
yhat := vmake(0,1,0); yhat↑.refcnt := 1000;
zhat := vmake(0,0,1); zhat↑.refcnt := 1000;
nilvect := vmake(0,0,0); nilvect↑.refcnt := 1000;
niltrans := tmake(vsaxwr(zhat,0.0),nilvect); niltrans↑.refcnt := 1000;
(* ypark := tmake(vsaxwr(yhat,180.0),vmake(43.5,2.325,6.86)); *)
(* bpark := tmake(vsaxwr(yhat,180.0),vmake(43.53125,56.855,9.95875)); *)
gpark := tmake(vsaxwr(zhat,180.0),vmake(83.2,46.13,67.7));
rpark := tmake(niltrans,vmake(84.8,12.87,67.7));
gpark↑.refcnt := 1000;
rpark↑.refcnt := 1000;
end;
procedure initWorld; external;
procedure initWorld;
var v: varidefp; e: enventryp; i,j: integer; envir: environp;
b: boolean;
begin
initArms; (* *** should this go here ??? *** *)
etime := 0;
curtime := 0;
activeInts := nil; (* zero the various queues *)
clkQueue := nil;
readQueue := nil;
allPdbs := nil;
curInt := nil;
allEvents := nil;
resched := false;
iSingleThreadMode := false;
sysEnv := newEheader; (* set up system variables *)
with sysEnv↑ do
begin
parent := nil;
block := nil;
procp := false;
envir := newEnvironment;
env[0] := envir;
for i := 1 to 4 do env[i] := nil;
end;
i := 0;
j := -1;
v := sysVars; (* get list of predefined system variables *) (* Instead of "getsysvars" *)
while v <> nil do
begin
(* need to handle devices specially - especially scalar devices *)
e := enterEntry(i,j,envir,sysEnv,v);
b := v↑.offset in [0,2,4,6,8,12];
(* offsets: arms: 0,4 hands: 2,6 driver/vise: 8,12 *)
if b then e↑.etype := frametype; (* so we get a frame for scalar devices *)
makeVar(e,v,v↑.tbits); (* make variable environment entry *)
if b then (* set up device values *)
with e↑.f↑ do
begin
ftype := false; (* it's a device *)
sdev := v↑.vtype = svaltype; (* indicate if scalar *)
if sdev then sdest := 0
else
begin
tdest := niltrans;
appr := nil;
depr := nil;
end;
case v↑.offset div 2 of (* set Mechanism bits *)
0: mech := GARMDEV; (* garm *)
1: mech := GHANDDEV; (* ghand *)
2: mech := RARMDEV; (* rarm *)
3: mech := RHANDDEV; (* rhand *)
4: mech := DRIVERDEV; (* driver *)
6: mech := VISEDEV; (* vise *)
otherwise {do nothing - shouldn't happen.. };
end;
end;
v := v↑.next
end;
for i := j+1 to 9 do envir↑.vals[i] := nil;
speedfactor := getEntry(0,20);
e := getEntry(0,4); (* offset for garm = 4 *)
garm := e↑.f; (* remember frame used for green arm *)
curInt := getPdb;
escInit(escapeI); (* enable escape-I interrupts *)
rewrite(talk,'tt3:'); (* speech synthesizer is on tt3: *)
writeln(talk,chr(5),'20P'); (* Set up standard AL voice (a la jjc) *)
end;