COMMENT ⊗   VALID 00008 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	{$NOMAIN	Individual statement interpreters }
C00005 00003	procedure doPrint external
C00006 00004	procedure doPause external
C00007 00005	procedure doAbort external
C00008 00006	procedure doSetbase external
C00010 00007	procedure doWrist external
C00014 00008	procedure doRetry external
C00016 ENDMK
C⊗;
{$NOMAIN	Individual statement interpreters }

const
	GARMDEV   = 1;			(* device numbers for ARM *)
	FTABLE = (*400B*) 256;		(* Force trans (C) in table coordinates *)
	FHAND  = 0;			(*   "	 "    "   " hand coordinate system *)

%include ialhdr.pas;

{ Externally defined routines: }

	(* From ALLOC *)
function newVector: vectorp;					external;
procedure relVector(v: vectorp);				external;
procedure relNode(n: nodep);					external;

	(* From IAUX1A *)
function pop: nodep;						external;
function getELev(hdr: envheaderp): integer;			external;
function gtVarn (n: nodep): enventryp;				external;
function getNval(n: nodep; var b: boolean): nodep;		external;
procedure sendCmd;						external;
procedure sendTrans(tr: transp);				external;

	(* From IAUX1B *)
procedure sleep(whenV: integer);				external;
procedure prntPlist(n: nodep);					external;

	(* From IAUX2A *)
procedure getReply(sendIt: boolean);				external;
procedure killEnv;						external;

	(* From IAUX2B *)
function cmonCheck: boolean;					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;
procedure ppDelChar; 						external;

	(* From RSXMSG *)
procedure signalArm;                                         	external;

procedure doPrint; external;
procedure doPrint;
 begin
 with curInt↑ do
  begin			(* print everything out *)
  prntplist(spc↑.plist);
  mode := 0;
  spc := spc↑.next;
  end;
 end;

procedure doPause; external;
procedure doPause;
 var i: integer; n: nodep;
 begin
 n := pop;
 i := round(n↑.s * 60);			(* get pause time (in 60Hz ticks) *)
 relNode(n);
 curInt↑.mode := 0;			(* get ready for next statement *)
 curInt↑.spc := curInt↑.spc↑.next;
 sleep(i);				(* put us to sleep for a while *)
 end;

procedure doAbort; external;
procedure doAbort;
 begin
 with curInt↑ do
  begin				(* print everything out *)
  if spc↑.debugLev = 0 then
    begin			(* a real abort *)
    msg↑.cmd := abortcmd;   	(* Send over ABORT command to ARM module *)
    sendCmd;
    prntplist(spc↑.plist);
    spc := spc↑.next;
    pp10L('Aborting  ',8);
    running := false;			(* break to debugger *)
    end
   else if debugLevel = spc↑.debugLev then
    running := false			(* break if debugger process *)
   else spc := spc↑.next;		(* just ignore it *)
  mode := 0;
  end;
 end;

procedure doSetbase; external;
procedure doSetbase;
 var mechbits: integer; e: enventryp;

 procedure complain;
  begin		(* yow! frame that's not affixed to a device *)
  pp20L('Attempt to zero unkn',20); pp20('own wrist: assuming ',20);
  pp5('GARM ',4); ppLine;
  mechbits := GARMDEV;
  end;

 begin
 with curInt↑ do
  begin
  if spc↑.cf = nil then complain
   else
    begin
    e := gtVarn(spc↑.cf);		(* see which wrist we're zeroing *)
    with e↑.f↑ do
     if ftype then		(* a frame - is it affixed to a device? *)
       if dev <> nil then mechbits := dev↑.mech
	else complain
      else					(* a device *)
       if not sdev then mechbits := mech
	else complain;		(* currently scalar devices are no good *)
    end;
  msg↑.cmd := zerowristcmd;		(* tell ARM servo to zero wrist *)
  msg↑.dev := mechbits;
  sendCmd;
  mode := 0;
  spc := spc↑.next;
  end;
 end;

procedure doWrist; external;
procedure doWrist;
 var e,fv,tv: enventryp; b: boolean;
     t: transp; v: vectorp; i: integer; val: nodep;

 procedure complain;
  begin		(* yow! frame that's not affixed to a device *)
  pp20L('Attempt to read unkn',20); pp20('own wrist: assuming ',20);
  pp5('GARM ',4); ppLine;
  i := GARMDEV;
  end;

 begin
 with curInt↑ do
  begin
  if spc↑.arm = nil then complain
   else
    begin
    e := gtVarn(spc↑.arm);		(* see which wrist we're zeroing *)
    with e↑.f↑ do
     if ftype then		(* a frame - is it affixed to a device? *)
       if dev <> nil then i := dev↑.mech
	else complain
      else					(* a device *)
       if not sdev then i := mech
	else complain;		(* currently scalar devices are no good *)
    end;
  if spc↑.ff <> nil then
    begin
    val := getNval(spc↑.ff,b);	(* get force frame value *)
    t := val↑.t;
    end
   else begin t := niltrans; b := false end;
  fv := gtVarn(spc↑.fvec);		(* get where to store results *)
  tv := gtVarn(spc↑.tvec);
  if fv↑.v <> nil then			(* flush any old values *)
    with fv↑.v↑ do
     begin
     refcnt := refcnt - 1;
     if refcnt <= 0 then relVector(fv↑.v);
     end;
  if tv↑.v <> nil then
    with tv↑.v↑ do
     begin
     refcnt := refcnt - 1;
     if refcnt <= 0 then relVector(tv↑.v);
     end;
  with msg↑ do
   begin
   cmd := wristcmd;
   dev := i;
   if spc↑.csys then bits := FTABLE else bits := FHAND;
   end;
  sendTrans(t);				(* send command & trans over *)
  signalArm;				(* tell ARM *)
  if b then relNode(val);
  getReply(false);			(* have ARM servo read wrist *)
  v := newVector;
  for i := 1 to 3 do v↑.val[i] := msg↑.t[i];
  fv↑.v := v;				(* store away force vector *)
  v↑.refcnt := 1;
  v := newVector;
  for i := 1 to 3 do v↑.val[i] := msg↑.t[i+3];
  tv↑.v := v;				(* store away torque vector *)
  v↑.refcnt := 1;
  mode := 0;
  spc := spc↑.next;
  end;
 end;

procedure doRetry; external;
procedure doRetry;
 var b: boolean;
 begin
 with curInt↑ do
  begin
  if spc↑.rparent <> nil then
    begin
    b := true;
    while b and (spc↑.olevel < getELev(env)) do
     begin	(* make sure all cmon's in outer environments have finished *)
     b := cmonCheck;
     if b then killEnv;		(* flush all environments out to move *)
     end;
    if b then			(* no cmons now running *)
      begin
(* *** might need to clean up stack some here (fornodes) *** *)
      spc := spc↑.rcode;	(* go redo the previous motion *)
      mode := 0;
      end
     else sleep(30);		(* give cmons time to finish *)
    end
   else
    begin
    spc := spc↑.next;		(* just go on to next statement *)
    mode := 0;
    end;
  end;
 end;