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;