perm filename IMOVE2.2[EAL,HE]1 blob sn#676471 filedate 1982-09-27 generic text, type C, neo UTF8
COMMENT ⊗   VALID 00008 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	{$NOMAIN	Small "move-related" statement interpreters }
C00003 00003	{ Externally defined routines: }
C00006 00004	procedure doCmon external
C00012 00005	procedure doOperate external
C00016 00006	procedure doOpen (* & doClose *) external
C00022 00007	procedure doCenter external
C00024 00008	procedure doStop external
C00026 ENDMK
C⊗;
{$NOMAIN	Small "move-related" statement interpreters }

%include ialhdr.pas;

const
	BARMDEV   = 3;
	VISEDEV   = 5;

	YELARM = 1B;		(* Yellow arm *)
	BLUARM = 4B;		(* Blue arm *)

	SIGMAG = 20000B;	(* Test only magnitude of forces *)
	SIGGE = 100000B;	(* Start cmon if force ≥ specified value *)
	SIGLT = 0B;		(*   "	  "  "	  "   <	    "	    " *)

	nullingcb = 1B;		(* control bits for trajectory specs *)
	dureqcb = 60B;
	destptcb = 10000B;

{ Externally defined routines: }

	(* From ALLOC *)
procedure relNode(n: nodep);					external;

	(* From IAUX1A *)
function pop: nodep;						external;
function getEntry (level, offset: byte): enventryp; 		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 sendCmd;						external;

	(* From RSXMSG *)
procedure signalArm;                                         	external;

	(* From IAUX1B *)
procedure addPdb(var plist: pdbp; pn: pdbp);			external;
procedure sleep(whenV: integer);				external;

	(* From IAUX2B *)
procedure cmonEnable(e: enventryp);				external;

	(* From IROOT *)
function m2Forcebits(fn: nodep; var negv: boolean): integer;	external;
function m2GetMechbits: integer;				external;
procedure m2MvStart;						external;
procedure m2MvEnd;						external;
procedure m2MvRetry;						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 iMove2Get; external;
procedure iMove2Get; begin end;
procedure doCmon; external;
procedure doCmon;
var e: enventryp; n: nodep; b: boolean; val: nodep; r: real; fbits: integer;
 begin
 with curInt↑ do
  case mode of
1: begin
   if not spc↑.deferCm then	(* check it's not a deferred cmon *)
     begin			(* need to enable the cmon *)
     with spc↑.cdef↑ do
      cmonEnable(getEntry(level,offset));	(* enable cmon control block *)
     end;
   mode := 0;
   spc := spc↑.next;
   end;

2: begin			(* deal with ON condition *)
   n := nil;
   mode := 3;			(* set up for doing conclusion next time *)
   if spc↑.exprCm then
     begin			(* test if expression is now true *)
     n := pop;			(* get expression value *)
     if n↑.s = 0.0 then
       begin
       sleep(20);		(* no good - try again in 0.33 seconds *)
       mode := 0;
       end;
     end
    else if spc↑.oncond↑.ntype = durnode then
     begin			(* duration cmon *)
     n := pop;
     sleep(round(n↑.s * 60));	(* get wait time (in 60Hz ticks) *)
     end
    else if spc↑.oncond↑.ntype = forcenode then
     begin					(* force sensing *)
     val := getNval(spc↑.oncond↑.fval,b);	(* get force magnitude *)
     r := val↑.s;
     if b then relNode(val);
     fbits := m2Forcebits(spc↑.oncond,b);
     with spc↑.oncond↑ do
      begin
      if (ftype = absforce) or (ftype = abstorque) then fbits := fbits + SIGMAG;
      if b then begin r := -r; if frel < seqop then fbits := fbits + SIGGE end
       else if frel >= seqop then fbits := fbits + SIGGE;
      end;
    (* deal with which arm here *) fbits := fbits + BLUARM; (* for now *)
     cm↑.fbits := fbits;		(* remember bits in cmoncb *)
     with msg↑ do
      begin
      cmd := forcesigcmd;
      bits := fbits;
      evt := cm↑.evt;
      mag := r;
      end;
     sendCmd;
     cm↑.evt↑.count := -1;
     cm↑.evt↑.waitlist := curInt;	(* put us on event waitlist *)
     curInt↑.status := forcewait;
     curInt := nil;			(* swap in someone else *)
     resched := true;
     end
    else if spc↑.oncond↑.ntype = departingnode then
     begin			(* departing cmon *)
     sleep(30);			(* wait 0.5 seconds (in 60Hz ticks) *)
     end
    else
     begin			(* event cmon *)
     if spc↑.oncond↑.ntype = arrivalnode then
       with spc↑.oncond↑.evar↑ do e := getVar(level,offset)
      else e := gtVarn(spc↑.oncond);
     cm↑.evt := e↑.evt;		(* save pointer to event we're waiting on *)
     e↑.evt↑.count := e↑.evt↑.count - 1;
     if e↑.evt↑.count <= 0 then (* hasn't been signalled yet, need to wait *)
       begin
       addPdb(e↑.evt↑.waitlist,curInt);	(* add us to wait list *)
       curInt↑.status := eventqueue;
       curInt := nil;			(* swap in someone else *)
       resched := true;
       end;
     end;
   if n <> nil then relNode(n);
   end;

3: begin
   mode := 0;
   if cm↑.enabled then		(* check that we're still enabled *)
     begin
     cm↑.running := true;	(* set up current cmon status *)
     cm↑.enabled := false;
     spc := spc↑.conclusion;
     end
    else
     begin
     curInt↑.status := nullqueue;
     curInt := nil;		(* we should go away *)
     resched := true;		(* now swap in highest priority process *)
     end;
   end;

  end;
 end;

procedure doOperate; external;
procedure doOperate;
 var durcl,vel,torquecl,cl,v: nodep; e: enventryp; b,ccw: boolean; ev: eventp;
 begin				(* deal with driver *)
 with curInt↑ do
  case mode of
1:  begin
    e := gtVarn(spc↑.cf);	(* remember what we're moving *)
    mech := e↑.f;
    m2MvStart;			(* enable all condition monitors for move *)
    mode := 2;
    end;

2:  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;

    durcl := nil;
    vel := nil;
    torquecl := nil;
    ccw := false;
    cl := spc↑.clauses;
    while cl <> nil do			(* run through clauses *)
     with cl↑ do
      begin
      if ntype = durnode then durcl := cl
       else if ntype = forcenode then
	begin
	if ftype = torque then torquecl := cl
	 else if ftype = angvelocity then vel := cl
	end
       else if ntype = cwnode then ccw := notp;
      cl := next;
      end;

    with msg↑ do
     begin
     cmd := operatecmd;
     dev := m2GetMechbits;
     bits := 0;
     evt := ev;
     dur := 5.0;		(* default values *)
     v1 := 60.0;		(* angular velocity *)
     v2 := 0.0;			(* torque *)

     if durcl <> nil then
       begin
       v := getNval(durcl↑.durval,b);		(* get duration value *)
       dur := v↑.s;
       if b then relNode(v);
       end;

     if vel <> nil then
       begin
       v := getNval(vel↑.fval,b);		(* get angular velocity value *)
       v1 := v↑.s;
       if b then relNode(v);
       end;

     if torquecl <> nil then
       begin
       v := getNval(torquecl↑.fval,b);		(* get torque value *)
       v2 := v↑.s;
       if b then relNode(v);
       end;

     if ccw then
       begin				(* turning counterclockwise *)
       v1 := - v1;
       v2 := - v2;
       end;
     end;

    sendCmd;				(* pass info to ARM servo *)
    mode := 3;
    curInt↑.status := devicewait; 	(* don't for simulation version *)
    curInt := nil;
    resched := true;			(* swap someone else in *)
    end;

3:  m2MvEnd;	(* do end of motion cleanup, run error handler, etc. *)

4:  m2MvRetry;	(* deal with user response if there was an error *)

  end;

 end;

procedure doOpen; (* & doClose *) external;
procedure doOpen; 
 var dest,sfac,durcl,swt,cl,v: nodep; e: enventryp; ev: eventp;
     opening,dtime,sf,swtime: real; mechbits: integer; b,nulling: boolean;
 begin
 with curInt↑ do
  case mode of
1:  begin
    e := gtVarn(spc↑.cf);	(* remember what we're moving *)
    mech := e↑.f;
    m2MvStart;		(* enable all condition monitors for move *)
    mode := 2;
    end;

2:  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 := m2GetMechbits;

(* run through clauses for dest, duration & speed factor/stop wait time specs *)
    dest := nil;
    durcl := nil;
    sfac := nil;
    swt := nil;
    nulling := true;			(* nonulling is the default *)
    cl := spc↑.clauses;
    while cl <> nil do			(* run through clauses *)
     with cl↑ do
      begin
      if ntype = destnode then dest := cl
       else if ntype = durnode then durcl := cl
       else if ntype = sfacnode then sfac := cl
       else if ntype = swtnode then swt := cl
       else if ntype = nullingnode then nulling := notp;
      cl := next;
      end;

    if sfac = nil then sf := speedfactor↑.s	(* use global speed factor *)
     else
      begin
      v := getNval(sfac↑.clval,b);		(* get local speed factor value *)
      sf := v↑.s;
      if b then relNode(v);
      end;

    if durcl = nil then dtime := 0
     else
      begin
      v := getNval(durcl↑.durval,b);		(* get duration value *)
      dtime := v↑.s;
      if b then relNode(v);
      end;

    if swt = nil then swtime := 0
     else
      begin
      v := getNval(swt↑.clval,b);		(* get stop wait time value *)
      swtime := v↑.s;
      if b then relNode(v);
      end;

    if dest = nil then
      begin
      opening := 0;
      mech↑.sdest := -1;			(* so we know there was no dest *)
      end
     else
      begin
      v := getNval(dest↑.loc,b);		(* get opening value *)
      opening := v↑.s;
      mech↑.sdest := opening;			(* remember it *)
      if b then relNode(v);
      end;

    with msg↑ do
     begin
     dev := mechbits;
     evt := ev;
     if nulling then bits := NULLINGCB else bits := 0;
     if dest = nil then
       begin
       pos := 0.0;
       if spc↑.stype = opentype then bits := 3 else bits := 1;
       end
      else
       begin
       pos := opening;
       bits := bits + DESTPTCB;		(* indicate we're specifying opening *)
       end;
     if durcl = nil then dur := 0.0
      else
       begin
       dur := dtime;
       bits := bits + DUREQCB;
       end;
     sfac := sf;

     if mechbits = VISEDEV then
       begin
       cmd := operatecmd;		(* vise uses an operate command *)
       if swt = nil then
	 if dest = nil then v2 := 0.25 else v2 := 0.0	(* default values *)
	else v2 := swtime;
       if durcl = nil then dur := 8.0;
       sendCmd;
       end
      else
       begin
       cmd := movehdrcmd;			(* deal with hand *)
       sendCmd;
       signalArm;	(* since movehdr normally followed by movesegs *)
       end;
     end;

    mode := 3;
    curInt↑.status := devicewait; 	(* don't for simulation version *)
    curInt := nil;
    resched := true;			(* swap someone else in *)
    end;

3:  m2MvEnd;	(* do end of motion cleanup, run error handler, etc. *)

4:  m2MvRetry;	(* deal with user response if there was an error *)

  end;

 end;

procedure doCenter; external;
procedure doCenter;
 var e: enventryp; ev: eventp;
 begin
 with curInt↑ do
  case mode of
1:  begin
    e := gtVarn(spc↑.cf);	(* remember what we're moving *)
    mech := e↑.f;
    m2MvStart;		(* enable all condition monitors for move *)
    mode := 2;
    end;

2:  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;
    with msg↑ do
     begin
     cmd := centercmd;
     dev := m2GetMechbits;
     bits := 0;
     evt := ev;
     end;
    sendCmd;				(* initiate the center operation *)
    mode := 3;
    curInt↑.status := devicewait; 	(* don't for simulation version *)
    curInt := nil;
    resched := true;			(* swap someone else in *)
    end;

3:  m2MvEnd;	(* do end of motion cleanup, run error handler, etc. *)

4:  m2MvRetry;	(* deal with user response if there was an error *)

  end;

 end;

procedure doStop; external;
procedure doStop;
 var mechbits: integer; e: enventryp;
 begin
 with curInt↑ do
  begin
  if spc↑.cf = nil then mechbits := m2GetMechbits		(* use current mech *)
   else
    begin
    e := gtVarn(spc↑.cf);		(* see what we're stopping *)
    with e↑.f↑ do
     if ftype then
       if dev <> nil then mechbits := dev↑.mech
	else
	 begin		(* yow! frame that's not affixed to a device *)
	 pp20L('Attempt to stop fram',20); pp20('e not affixed to any',20);
	 pp20(' device: Assuming ba',20); pp5('rm   ',2); ppLine;
	 mechbits := BARMDEV;
	 end
      else mechbits := mech;
    end;
  with msg↑ do
   begin
   cmd := stopcmd;
   dev := mechbits;
   end;
  sendCmd;				(* tell arm servo to stop device *)
  mode := 0;
  spc := spc↑.next;
  end;
 end;