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
C00009 00005	procedure doAbort external
C00011 00006	procedure doSetbase external
C00012 00007	procedure doWrist external
C00014 00008	procedure doRetry external
C00016 ENDMK
C⊗;
{$NOMAIN	Individual statement interpreters }

%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;
procedure sendCmd;						external;

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

	(* From IAUX2A *)
procedure getReply;						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;

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;
 begin
 with curInt↑ do
  begin
  msg↑.cmd := zerowristcmd;		(* tell ARM servo to zero wrist *)
  sendCmd;
  mode := 0;
  spc := spc↑.next;
  end;
 end;

procedure doWrist; external;
procedure doWrist;
 var fv,tv: enventryp; v: vectorp; i: integer;
 begin
 with curInt↑ do
  begin
  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;
  msg↑.cmd := wristcmd;
  getReply;				(* 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;