perm filename IMOVE1.2[EAL,HE]3 blob
sn#706562 filedate 1983-04-19 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00007 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 {$NOMAIN Main "MOVE" statement interpreter }
C00029 00003 { Externally defined routines: }
C00033 00004 procedure doMove external
C00037 00005 procedure doMode1
C00045 00006 procedure doMode2
C00063 00007 begin {doMove - main body}
C00065 ENDMK
C⊗;
{$NOMAIN Main "MOVE" statement interpreter }
const
FTABLE = 400B; (* Force trans (C) in table coordinates *)
FHAND = 0B; (* " " " " hand coordinate system *)
(* control bits for trajectory specs: movesegcmd & movehdrcmd *)
Viaptcb = 1; Joint1cb = 1; (* 1B *)
Deptptcb = 2; Joint2cb = 2; (* 2B *)
Apprptcb = 4; Joint3cb = 4; (* 4B *)
Destptcb = 8; Joint4cb = 8; (* 10B *)
Veloccb = 16; Joint5cb = 16; (* 20B *)
Codecb = 32; Joint6cb = 32; (* 40B *)
Durlbcb = 64; (* 100B *)
Durubcb = 128; (* 200B *)
Dureqcb = 192; (* 300B *)
Byptcb = 256; Linearcb = 256; (* 400B *)
Nullingcb = 512; (* 1000B *)
Shouldercb = 1024; Wobblecb = 1024; (* 2000B *)
Rightcb = 2048; Speedfcb = 2048; (* 4000B *)
Elbowcb = 4096; Loadcb = 4096; (* 10000B *)
Upcb = 8192; (* 20000B *)
Wristcb = 16384; (* 40000B *)
Flipcb = 32768; (* 100000B *)
(* 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
signaltype,
waittype: (event: nodep);
movetype,
jtmovetype,
operatetype,
opentype,
closetype,
centertype,
floattype,
setbasetype,
stoptype: (cf, clauses: nodep);
cmtype: (oncond: nodep; conclusion: statementp;
deferCm, exprCm: boolean; cdef: varidefp);
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);
arrivalnode:(evar: varidefp);
wrtnode,
deprnode,
apprnode,
destnode: (loc: nodep; code: statementp);
byptnode,
viaptnode: (vlist: boolean; via,vclauses: nodep; vcode: statementp);
durnode: (durrel: reltypes; durval: nodep);
velocitynode,
sfacnode,
wobblenode,
swtnode: (clval: nodep);
nullingnode, (* true = nonulling *)
wristnode, (* = don't zero force wrist *)
cwnode, (* = counter_clockwise *)
elbownode, (* = elbow up *)
shouldernode, (* = right shoulder *)
flipnode, (* = don't flip wrist *)
linearnode: (notp: boolean); (* = linear motion *)
ffnode: (ff,cf: nodep; csys, pdef: boolean); (* true = world, false = hand *)
loadnode: (loadval,loadvec: nodep; lcsys: boolean); (* lcsys = csys above *)
forcenode: (ftype: forcetypes; frel: reltypes; fval, fvec, fframe: nodep);
stiffnode: (fv, mv, cocff: nodep);
gathernode: (gbits: integer);
cmonnode: (cmon: statementp; errhandlerp: boolean);
errornode: (eexpr: nodep);
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;
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;
*) dum2: array[1..141] of dump;
(* 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 newTrans: transp; external;
procedure relTrans(t: transp); external;
function newNode: nodep; external;
procedure relNode(n: nodep); external;
(* From IAUX1A *)
procedure push (n: nodep); external;
function pop: nodep; external;
procedure upTrans (var t: transp; tp: transp); external;
function getVar (level, offset: byte): enventryp; external;
function gtVarn (n: nodep): enventryp; external;
function getNval(n: nodep; var b: boolean): nodep; external;
function getEvent: eventp; external;
procedure freeEvent(e: eventp); external;
procedure killNode(n: nodep); external;
procedure sendCmd; external;
procedure sendTrans(tr: transp); external;
function feval (f: framep): transp; external;
(* From RSXMSG *)
procedure signalArm; external;
(* Arithmetic Routines *)
function ttmul (t1,t2: transp): transp; external;
function tinvrt (t: transp): transp; external;
function svmul (s: scalar; v: vectorp): vectorp; external;
function tmake (t: transp; v: vectorp): transp; external;
function tvadd (t: transp; v: vectorp): transp; external;
(* From IAUX2A *)
function whereArm (mech: integer): transp; external;
(* From IROOT *)
function m1Forcebits(fn: nodep; var negv: boolean): integer; external;
function m1GetMechbits: integer; external;
procedure m1MvStart; external;
procedure m1MvEnd; external;
procedure m1MvRetry; external;
(* Display-related Routines *)
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;
procedure ppInt(i: integer); external;
procedure ppReal(r: real); external;
procedure ppStrng(length: integer; s: strngp); external;
(* From DISP *)
procedure beep; external;
procedure iMove1Get; external;
procedure iMove1Get; begin end;
procedure doMove; external;
(* This fella has been changed also from the original version on the 10, *)
(* since the procedure was too big to optimize, etc. The 2 big parts of *)
(* the main CASE stmnt have been made into procedures. *)
procedure doMove;
var appr,depr,dest,bydest,arrv,wobble,sfac,dur,ffr,stiff,gather,zwrist,n: nodep;
elbow,shoulder,flip,load,linear,cl,val,val1,val2: nodep;
t,tl,tb: transp; st: statementp; e: enventryp; fr: framep;
r: real; fbits,nsegs,mechbits,i,j,cmForce,useForce,jtnum: integer;
b,b1,b2,nulling,apprp,deprp,jointp: boolean; ev: eventp;
function getLoc(n: nodep): transp;
var tp: transp; b: boolean;
begin
n := getNval(n,b);
tp := n↑.t;
if b then relNode(n);
(* if t <> nil then tp := ttmul(t,tp); now done by ARM *)
getLoc := tp;
end;
function getDepr(n: nodep; b: boolean): transp;
var tp: transp; v: vectorp;
begin
if n↑.ltype = svaltype then tp := tmake(niltrans,svmul(n↑.s,zhat))
else if n↑.ltype = vectype then tp := tmake(niltrans,n↑.v)
else tp := n↑.t;
if b then relnode(n);
(* if t <> nil then tp := ttmul(t,tp); now done by ARM *)
getDepr := tp;
end;
procedure getCode(s: statementp);
var e: enventryp;
begin
if s = nil then e := nil
else
begin
with s↑ do
if stype = signaltype then e := gtVarn(event)
else e := gtVarn(oncond);
msg↑.evt := e↑.evt; (* event to signal for code *)
msg↑.bits := msg↑.bits + CODECB;
end;
end;
procedure sendJt(r: real; n: nodep; b: boolean);
begin
sendCmd;
msg↑.t[jtnum] := r; (* send over joint value *)
if b then relNode(n);
sendCmd;
end;
procedure setConfigBits;
var cbits: integer;
begin
cbits := 0;
if elbow <> nil then
if elbow↑.notp then cbits := elbowcb + upcb else cbits := elbowcb;
if shoulder <> nil then
if shoulder↑.notp then cbits := cbits + shouldercb + rightcb
else cbits := cbits + shouldercb;
if flip <> nil then
if flip↑.notp then cbits := cbits + wristcb + flipcb
else cbits := cbits + wristcb;
with msg↑ do bits := bits + cbits;
end;
procedure doMode1;
var i: integer;
begin
with curInt↑ do
begin (* set up force system, enable all cmons *)
if not jointp then
begin
e := gtVarn(spc↑.cf); (* remember what we're moving *)
mech := e↑.f;
mechbits := m1GetMechbits;
if mech↑.ftype then (* check it's a device *)
if mech↑.dev = nil then
begin (* yow! frame that's not affixed to an arm *)
pp20L('Control frame not af',20); pp20('fixed to any device:',20);
pp20(' Assuming garm ',14); ppLine;
end;
end
else
begin
with st↑.cf↑.arg1↑.vari↑ do
e := getVar(level,offset);
mech := e↑.f; (* remember what we're moving *)
mechbits := e↑.f↑.mech;
val := getNval(st↑.cf↑.arg2↑.lval,b); (* now see which joint is wanted *)
i := round(val↑.s); (* get joint # *)
if (i < 0) or (6 < i) then (* bad joint # *)
begin
pp20L('Joint number out of ',20); pp20('range - using jt 1 ',18);
ppLine;
i := 1;
end;
if not b then
begin val := newNode; val↑.ntype := leafnode; val↑.ltype := svaltype end;
val↑.s := i; (* remember joint # for later *)
end;
ffr := nil;
stiff := nil;
gather := nil;
zwrist := nil;
cmForce := 0;
useForce := 0;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
with cl↑ do
begin
case ntype of
ffnode: ffr := cl;
stiffnode: stiff := cl;
gathernode: gather := cl;
wristnode: zwrist := cl;
forcenode: useForce := useForce + 1;
cmonnode: if cmon↑.oncond↑.ntype = forcenode then cmForce := cmForce + 1;
otherwise begin (* don't care *) end;
end;
cl := next;
end;
if (ffr <> nil) or (cmForce + useForce > 0) or (gather <> nil) then
begin
msg↑.cmd := setccmd;
msg↑.dev := mechbits; (* tell which arm *)
msg↑.bits := FTABLE; (* assume this *)
if ffr <> nil then
begin
val1 := getNval(ffr↑.ff,b); (* get force frame value *)
if not ffr↑.csys then msg↑.bits := 0;
sendTrans(val1↑.t); (* send command & trans over *)
if b then relNode(val1);
end
else sendTrans(niltrans); (* send command & trans over *)
signalArm; (* wake up ARM servo background job *)
end;
if zwrist <> nil then b := zwrist↑.notp
else if (ffr <> nil) or (stiff <> nil) then b := true
else b := false;
if b then
begin
msg↑.cmd := zerowristcmd; (* tell arm servo to zero wrist *)
msg↑.dev := mechbits; (* tell which wrist *)
sendCmd;
end;
if stiff <> nil then
begin
val1 := getNval(stiff↑.fv,b1); (* get force vector *)
val2 := getNval(stiff↑.mv,b2); (* get moment vector *)
with msg↑ do
begin
cmd := setstiffcmd;
dev := mechbits; (* tell which arm *)
for i := 1 to 3 do
begin
t[i] := val1↑.v↑.val[i];
t[i+3] := val2↑.v↑.val[i];
end;
end;
sendCmd; (* send stiffnesses over *)
if b1 then killNode(val1);
if b2 then killNode(val2);
end
else if useForce > 0 then
begin (* add default stiffness *)
with msg↑ do
begin
cmd := setstiffcmd;
dev := mechbits; (* tell which arm *)
for i := 1 to 3 do
begin
t[i] := 40;
t[i+3] := 100;
end;
end;
sendCmd; (* send stiffnesses over *)
end;
if gather <> nil then
begin
with msg↑ do
begin
cmd := gathercmd;
dev := mechbits; (* tell which arm *)
bits := gather↑.gbits;
end;
sendCmd; (* send gather command over *)
end;
if useForce > 0 then (* any bias forces? *)
begin
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
begin
with cl↑ do
if ntype = forcenode then (* check for bias forces *)
begin
val1 := getNval(cl↑.fval,b); (* get force magnitude *)
r := val1↑.s;
if b then relnode(val1);
fbits := m1forcebits(cl,b);
if b then r := -r;
with msg↑ do
begin
cmd := biasoncmd;
dev := mechbits; (* tell with which arm *)
bits := fbits;
mag := r;
end;
sendCmd; (* tell arm about bias force *)
end;
cl := cl↑.next;
end;
end;
m1MvStart; (* enable all condition monitors for move *)
if jointp then push(val);
mode := 2;
end;
end {doMode1};
procedure doMode2;
begin
with curInt↑ do
begin (* set up motion specs for arm code & send it over *)
ev := getEvent; (* event to use for signalling when motion finishes *)
ev↑.count := -1;
ev↑.waitlist := curInt;
mechbits := m1getMechbits;
if jointp then
begin val := pop; jtnum := round(val↑.s); relNode(val) end;
nsegs := 0;
if mech↑.ftype then
if mech↑.dev <> nil then fr := mech↑.dev (* get frame for device *)
else fr := garm
else fr := mech;
nulling := true; (* no nulling is the default *)
dest := nil;
bydest := nil;
wobble := nil;
sfac := nil;
dur := nil;
elbow := nil;
shoulder := nil;
flip := nil;
load := nil;
linear := nil;
arrv := nil;
appr := nil;
depr := nil;
if not jointp then
begin
apprp := true; (* assume default approach *)
deprp := fr↑.depr <> nil; (* default departure if last had approach *)
end
else
begin
apprp := false; (* joint moves don't use default deproaches *)
deprp := false;
end;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
with cl↑ do
begin
case ntype of
destnode: begin dest := cl; nsegs := nsegs + 1 end;
wobblenode: wobble := cl;
elbownode: elbow := cl;
shouldernode: shoulder := cl;
flipnode: flip := cl;
loadnode: load := cl;
sfacnode: sfac := cl;
durnode: dur := cl;
linearnode: linear := cl;
nullingnode: nulling := notp;
apprnode: begin
appr := cl;
if loc = nil then apprp := false (* approach = nildeproach *)
else begin apprp := true; nsegs := nsegs + 1 end
end;
deprnode: begin
depr := cl;
if loc = nil then deprp := false (* departure = nildeproach *)
else begin deprp := true; nsegs := nsegs + 1 end
end;
viaptnode: nsegs := nsegs + 1;
byptnode: begin bydest := cl; nsegs := nsegs + 1 end;
cmonnode: if cmon↑.oncond↑.ntype = arrivalnode then arrv := cmon↑.oncond;
otherwise begin (* don't care *) end;
end;
cl := next;
end;
if (dest <> nil) then bydest := nil
else apprp := appr <> nil; (* no default approach if no dest *)
if mech↑.ftype then tb := feval(mech); (* get current cf position *)
if deprp and (depr = nil) then
nsegs := nsegs + 1; (* add in default departure seg *)
if apprp and (appr = nil) then
with dest↑.loc↑ do (* add default approach point *)
if ((ntype = leafnode) and (ltype = varitype)) or
((ntype = exprnode) and (op = arefop)) then
nsegs := nsegs + 1 (* add in default approach seg *)
else apprp := false; (* don't want default approach *)
if mech↑.ftype then
begin (* get offset trans to take cf to arm *)
t := whereArm(mechbits); (* Get current device pos *)
t := ttmul(tb,tinvrt(t)); (* compute offset *)
end
else t := niltrans; (* no offset needed *)
with msg↑ do
begin
cmd := movehdrcmd;
dev := mechbits;
if jointp then
case jtnum of
1: bits := Joint1cb;
2: bits := Joint2cb;
3: bits := Joint3cb;
4: bits := Joint4cb;
5: bits := Joint5cb;
6: bits := Joint6cb;
otherwise bits := Joint1cb;
end
else bits := 0;
if nulling then bits := bits + Nullingcb;
if load <> nil then bits := bits + Loadcb;
if linear <> nil then (* straight line motion? *)
if linear↑.notp then bits := bits + Linearcb;
n := nsegs;
evt := ev;
end;
if sfac <> nil then
begin (* use local speed factor *)
val := getNval(sfac↑.clval,b);
msg↑.sfac := val↑.s;
if b then relnode(val);
end
else
begin (* use global speed factor *)
msg↑.sfac := speedfactor↑.s;
end;
if dur <> nil then (* duration *)
begin
val := getNval(dur↑.durval,b);
msg↑.dur := val↑.s;
if dur↑.durrel < seqop then i := DURLBCB
else if dur↑.durrel > seqop then i := DURUBCB
else i := DUREQCB;
msg↑.bits := msg↑.bits + i;
if b then relnode(val);
end;
if wobble <> nil then (* wobble *)
begin
val := getNval(wobble↑.clval,b);
msg↑.wobble := val↑.s;
msg↑.bits := msg↑.bits + WOBBLECB;
if b then relnode(val);
end;
sendTrans(t); (* tell arm we're starting a motion & what's being moved *)
if load <> nil then
with msg↑ do (* indicate load for arm *)
begin
cmd := setloadcmd;
if load↑.lcsys then bits := FTABLE (* in World or Hand? *)
else bits := FHAND;
val1 := getNval(load↑.loadval,b); (* mass of load *)
dur := val1↑.s;
if b then relnode(val1);
if load↑.loadvec <> nil then
begin
val1 := getNval(load↑.loadvec,b); (* where load is located *)
with val1↑.v↑ do
begin v1 := val[1]; v2 := val[2]; v3 := val[3] end;
if b then relnode(val1);
end
else begin v1 := 0; v2 := 0; v3 := 0 end;
sendCmd; (* tell ARM about the load *)
end;
msg↑.cmd := movesegcmd; (* now get values for trajectory points *)
if deprp then (* departure: loc & event *)
begin
msg↑.bits := Deptptcb;
setConfigBits; (* indicate any specified configuration *)
if depr = nil then tl := fr↑.depr (* default departure point *)
else
begin (* explicit departure point *)
msg↑.bits := Deptptcb + Byptcb; (* incremental motion *)
n := getNval(depr↑.loc,b);
getCode(depr↑.code);
if not jointp then tl := getDepr(n,b)
else sendJt(n↑.s,n,b);
end;
if not jointp then sendTrans(tl);
end;
cl := spc↑.clauses;
while cl <> nil do (* run through clauses *)
begin
with cl↑ do
if (ntype = viaptnode) or (ntype = byptnode) then
begin (* VIA or BY: loc, duration, velocity & event *)
if ntype = viaptnode then msg↑.bits := Viaptcb
else if cl = bydest then msg↑.bits := Destptcb + Byptcb
else msg↑.bits := Viaptcb + Byptcb;
setConfigBits; (* indicate any specified configuration *)
if jointp then val1 := getNval(via,b1)
else if ntype = viaptnode then tb := getLoc(via)
else
begin
n := getNval(via,b);
if n↑.ltype = vectype then tb := tmake(niltrans,n↑.v) else tb := n↑.t;
if b then relnode(n);
end;
val2 := vclauses;
while val2 <> nil do (* check for any specified duration *)
if val2↑.ntype = durnode then
begin
val := getNval(val2↑.durval,b);
msg↑.dur := val↑.s;
if val2↑.durrel < seqop then i := Durlbcb
else if val2↑.durrel > seqop then i := Durubcb
else i := Dureqcb;
msg↑.bits := msg↑.bits + i;
if b then relNode(val);
val2 := nil;
end
else val2 := val2↑.next;
val2 := vclauses;
while val2 <> nil do (* check for any specified velocity *)
if val2↑.ntype = velocitynode then
begin
val := getNval(val2↑.clval,b);
msg↑.bits := msg↑.bits + Veloccb;
with val↑.v↑ do
begin
msg↑.v1 := val[1];
msg↑.v2 := val[2];
msg↑.v3 := val[3];
end;
if b then relNode(val);
val2 := nil;
end
else val2 := val2↑.next;
val2 := vclauses;
while val2 <> nil do (* finally deal with any configuration specs *)
begin
with msg↑ do
if val2↑.ntype = shouldernode then
begin
if shoulder = nil then
begin
bits := bits + Shouldercb;
if val2↑.notp then bits := bits + rightcb
end
else if val2↑.notp and (not shoulder↑.notp) then
bits := bits + rightcb
else if (not val2↑.notp) and shoulder↑.notp then
bits := bits - rightcb;
end
else if val2↑.ntype = elbownode then
begin
if elbow = nil then
begin
bits := bits + elbowcb;
if val2↑.notp then bits := bits + upcb
end
else if val2↑.notp and (not elbow↑.notp) then
bits := bits + upcb
else if (not val2↑.notp) and elbow↑.notp then
bits := bits - upcb;
end
else if val2↑.ntype = flipnode then
begin
if flip = nil then
begin
bits := bits + wristcb;
if val2↑.notp then bits := bits + flipcb
end
else if val2↑.notp and (not flip↑.notp) then
bits := bits + flipcb
else if (not val2↑.notp) and flip↑.notp then
bits := bits - flipcb;
end;
val2 := val2↑.next;
end;
getCode(cl↑.vcode);
if not jointp then sendTrans(tb) else sendJt(val1↑.s,val1,b1);
end;
cl := cl↑.next;
end;
if apprp then (* approach: loc & event *)
begin
msg↑.bits := Apprptcb;
setConfigBits; (* indicate any specified configuration *)
if appr <> nil then
begin (* explicit approach point *)
n := getNval(appr↑.loc,b);
getCode(appr↑.code);
end;
if not jointp then
begin
tb := getLoc(dest↑.loc); (* need to get destination location *)
tb↑.refcnt := tb↑.refcnt + 1; (* make sure we keep it for later *)
if appr <> nil then
begin
tl := getDepr(n,b); (* explicit approach point *)
tl := ttmul(tb,tl); (* shift to proper coord sys *)
end
else
begin (* default appoach point *)
tl := tvadd(tb,svmul(3,zhat));
(* if t <> nil then tl := ttmul(t,tl); now done by ARM *)
end;
tb↑.refcnt := tb↑.refcnt - 1;
upTrans(fr↑.appr,tl); (* save it for next motion *)
sendTrans(tl);
end
else
begin (* joint motion *)
val1 := getNval(dest↑.loc,b1); (* need to get destination location *)
r := val1↑.s + n↑.s; (* shift to proper coord sys *)
sendJt(r,n,b);
end
end
else
begin
if dest <> nil then
if not jointp then tb := getLoc(dest↑.loc) (* get dest for below *)
else val1 := getNval(dest↑.loc,b1);
upTrans(fr↑.appr,nil); (* remember no default depr for next motion *)
end;
(* destination: loc & event *)
if jointp then
begin tb := newTrans; tb↑.val[1,1] := val1↑.s; tb↑.val[1,2] := jtnum end;
uptrans(fr↑.tdest,tb); (* copy dest for later use *)
if dest <> nil then
begin
msg↑.bits := Destptcb;
setConfigBits; (* indicate any specified configuration *)
if arrv <> nil then
begin
with arrv↑.evar↑ do e := getVar(level,offset);
msg↑.evt := e↑.evt; (* event to signal for code *)
msg↑.bits := Destptcb + Codecb;
end;
if not jointp then sendTrans(tb) else sendJt(val1↑.s,val1,b1);
end;
beep; (* beep the terminal to warn that a move is about to start *)
signalArm; (* finally let background job deal with traj *)
mode := 3;
curInt↑.status := devicewait; (* don't for simulation version *)
curInt := nil;
resched := true; (* swap someone else in *)
end;
end {doMode2};
begin {doMove - main body}
with curInt↑ do
begin
st := spc; (* remember MOVE statement *)
jointp := st↑.stype = jtmovetype; (* is it a joint motion? *)
case mode of
1: doMode1;
2: doMode2;
3: m1MvEnd; (* do end of motion cleanup, run error handler, etc. *)
4: m1MvRetry; (* deal with user response if there was an error *)
end;
if curInt <> nil then (* in case we're waiting for an error response *)
if spc = st↑.next then
begin (* doesn't appear to have been any errors *)
if mech↑.ftype then (* get frame for device *)
if mech↑.dev <> nil then fr := mech↑.dev
else fr := garm
else fr := mech;
upTrans(fr↑.depr,fr↑.appr); (* update default departure point *)
end;
end;
end;