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;