/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	controlFxns
 *
 * DESCRIPTION:	Contains the startup, control, and release functions for 
 *		the RCI trajectory generator task.
 * 
 * CAVEATS:	There is one trajectory generator task allocated per-CPU,
 *		and we make use of this fact with the occasional static
 *		variable.
 *
 * AUTHOR/DATE:	John Lloyd, Fall 1988
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <sys/time.h>

#include "../h/rciTask.h"
#include "../h/IOdevice.h"
#include "../h/rciMacros.h"
#include "../h/rciMonitor.h"
#include "../h/errorCodes.h"
#include "../h/assignMem.h"

#include "../rci/link.h"

#include "../rccl/setpoint.h"

#define TRIGGER_SET	0x1111  /* use a highly unique value */
#define POWER_FREEZE	1	/* freeze trajectory generator if */
				/* robot arm power is off */

/* Local hacks: Enable tracing of the control function execution. If one of 
   these trace options is selected (set to 1), then a trace bit will be set
   whenever the setpoint functions are active; this bit can then be 
   traced on the scope for guesstimate the function timing. These
   includes are very site specific */
#define PRI_TRACE	0	/* use a bit on a DRV11 (pri driver) */ 

extern rciCpu;			/* CPU task is running on */

/* these definitions should go into a per-task structure, but we can get
   away with this for now ... as long as there is 1 RCCL task per cpu. */
static MANIP **manipPoolReference;
MANIP *rcclManipPool[MAX_CPUS];
int rcclManipPoolSet[MAX_CPUS];
MANIP_RECORD *rcclManipTable;
int rcclTaskInitDone = UNDEF;
MOTION_PARAMETER_BLOCK *mpbListHead;
MOTION_PARAMETER_BLOCK *mpbListTail;

static triggerFxn(td)
RCI_DESC *td;
{
	/* RCI trigger function to wake up all the robots connected to
	   this trajectory generator */

	RCI_RBT *rbt;
	int k;

	rciSetTrace (0x2);	/* diagnostic oscilliscope trace bit */

	td->tcb->fxnNotice = TRIGGER_SET;
	for (k=0; k<MAX_TASKROBOTS; k++)
	 { if (rbt = TASKROBOT(td, k))
	    { ROBOT_WAKEUP(rbt);
	    }
	 }

	rciClearTrace (0x2);
}

static triggerWakeup(td)
RCI_DESC *td;
{
	/* Polling function for RCI monitor to be used with ON_TRIGGER
	   scheduling: returns true whenever any of the robots connected to
	   the current task are ready with their input data ... OR, if there
	   are no robots, always returns true as soon as we have sent the
	   trigger */ 

	RCI_RBT *rbt;
	int robotPresent = 0;
	int k;

	if (td->tcb->fxnNotice == TRIGGER_SET)
	 { for (k=0; k<MAX_TASKROBOTS; k++)
	    { if (rbt = td->rbts[k])
	       { robotPresent = 1;
		 if (ROBOT_READY (rbt))
		  { return (1);
		  }
	       }
	    }
	   if (!robotPresent)
	    { return (1);
	    }
	 }
	return (0);
}

static functionWakeup(td)
RCI_DESC *td;
{
	/* Polling function for RCI monitor to be used with ON_FUNCTION
	   scheduling: returns true whenever any of the robots connected to
	   the current task are ready with their input data.
	   If there is no robot attached, the routine aborts: this is
	   currently and illegal condition if running RCCL programs
	   ON_FUNCTION (this should be fixed).
	 */

	RCI_RBT *rbt;
	int robotPresent = 0;
	int k;

	for (k=0; k<MAX_TASKROBOTS; k++)
	 { if (rbt = td->rbts[k])
	    { robotPresent = 1;
	      if (ROBOT_READY (rbt))
	       { return (1);
	       }
	    }
	 }
	if (!robotPresent)
	 { rciAbort (0, "functionWakeup() -- no robot attached to task\n");
	   return (-1);
	 }
	return (0);
}

setpVariableInit()
{
	/* called once from the planning level, after the first call
	   to rcclCreate(), to initialize the static variables. */

	int k;

	manipPoolReference = NULL;
	bzero ((char*)rcclManipPool, sizeof(rcclManipPool));
	for (k=0; k<MAX_CPUS; k++)
	 { rcclManipPoolSet[k] = UNDEF;
	 }
	rcclManipTable = NULL;
	rcclTaskInitDone = UNDEF;
}
	
setpTaskInit(td, num)
{
	/* Called once whenever an RCCL task is created on a particular
	   CPU. This will happen during the first call to rcclCreate() 
	   which creates a trajectory task on that CPU. */

	char *fxnName = "setpTaskInit()";
	RCCL_SMEM *smem;
	RCCL_GMEM *gmem;
	int k;

	if ((smem = (RCCL_SMEM*)getSharedMemory ("RCCL_SMEM")) == NULL)
	 { rciAbort (EInternal, "%s -- can't get memory RCCL_SMEM\n", fxnName);
	   return (-1);
	 }
	if ((gmem = (RCCL_GMEM*)getGlobalMemory ("RCCL_GMEM")) == NULL)
	 { rciAbort (EInternal, "%s -- can't get memory RCCL_GMEM\n", fxnName);
	   return (-1);
	 }
	rcclManipTable = smem->manipTable;
	rcclTaskInitDone = rciCpu;
	manipPoolReference = gmem->manipPool;
	mpbListHead = mpbListTail = NULL;

	for (k=0; k<MAX_CPUS; k++)
	 { if (manipPoolReference[k])
	    { if ((rcclManipPool[k] = (MANIP*)findGlobalMemory (
					      manipPoolReference[k])) == NULL)
	       { rciAbort (EInternal, "%s -- can't find manip pool %d\n", fxnName,k);
		 return (-1);
	       }
	    }
	   else
	    { rcclManipPool[k] = NULL;
	    }
	 }
	return (1);
}

setpManipInit(td, num)
RCI_DESC *td;
int num;
{
	/* Called whenever a manipulator is created (with a call to 
	   rcclCreate()) to zero the motion control block and set 
	   up a few things */

	char *fxnName = "setpManipInit()";
	RCCL_SMEM *smem;
	MOTION_PARAMETER_BLOCK *mpb;
	MANIP *mnp, *gmnp;
	void *key;
	char *mem;
	int cpu;

	/* Attach the new manipulator. In particular, go through
	   the manip structure and remap (get the correct pointers
	   for) all the shared data fields contained in the MANIP
	   structure.
	 */

	cpu = rcclManipTable[num].cpu;

	if (rcclManipPoolSet[cpu] != rciCpu)
	 { if ((rcclManipPool[cpu] = (MANIP*)findGlobalMemory (
				     manipPoolReference[cpu])) == NULL)
	    { rciAbort (EInternal, "%s -- can't find manip pool for cpu %d\n",
				   fxnName, cpu);
	      return (-1);
	    }
	   rcclManipPoolSet[cpu] = rciCpu;
	 }
	gmnp = &rcclManipPool[ARBITER_CPU][num];	      
	mnp = &rcclManipPool[rciCpu][num];	      

	*mnp = *gmnp;

	if ((mnp->j6 = (JNTS*)getMemByAddr (gmnp->j6)) == NULL)
	 { key = (void*)gmnp->j6;
	   goto smiFindErr;
	 }
	if ((mnp->j6o = (JNTS*)getMemByAddr (gmnp->j6o)) == NULL)
	 { key = (void*)gmnp->j6o;
	   goto smiFindErr;
	 }
	if ((mnp->jbias = (JNTS*)getMemByAddr (gmnp->jbias)) == NULL)
	 { key = (void*)gmnp->jbias;
	   goto smiFindErr;
	 }
	if ((mnp->jvel = (JNTS*)getMemByAddr (gmnp->jvel)) == NULL)
	 { key = (void*)gmnp->jvel;
	   goto smiFindErr;
	 }
	if ((mnp->t6 = (TRSF*)getMemByAddr (gmnp->t6)) == NULL)
	 { key = (void*)gmnp->t6;
	   goto smiFindErr;
	 }
	if ((mnp->t6o = (TRSF*)getMemByAddr (gmnp->t6o)) == NULL)
	 { key = (void*)gmnp->t6o;
	   goto smiFindErr;
	 }
	if ((mnp->here = (TRSF*)getMemByAddr (gmnp->here)) == NULL)
	 { key = (void*)gmnp->here;
	   goto smiFindErr;
	 }
	if ((mnp->tool = (TRSF*)getMemByAddr (gmnp->tool)) == NULL)
	 { key = (void*)gmnp->tool;
	   goto smiFindErr;
	 }
	if ((mnp->rest = (TRSF*)getMemByAddr (gmnp->rest)) == NULL)
	 { key = (void*)gmnp->rest;
	   goto smiFindErr;
	 }
	if ((mnp->lastTC = (TRSF*)getTransByAddr (gmnp->lastTC)) == NULL)
	 { key = (void*)gmnp->lastTC;
	   goto smiFindErr;
	 }
	if ((mem = (char*)findGlobalMemory(gmnp->mcb))==NULL)
	 { rciAbort (EInternal, "%s -- can't get global memory\n", fxnName);
	   return (-1);
	 }
	ASSIGN_MEM (mnp->mcb, void*, sizeof(MOTION_CONTROL_BLOCK), mem);
	ASSIGN_MEM (mnp->mq, void*, sizeof(MOTION_QUEUE), mem);

	rcclManipTable[num].mnp = mnp;

	if (mnp->rbtNum != UNDEF)
	 { RCI_RBT *rbt;

	   rbt = TASKROBOT(findRciTask(mnp->taskNum), mnp->rbtNum);
	   mnp->rbt = rbt;
	   mnp->how = rbt->how;
	   mnp->jls = rbt->jls;
	   mnp->kyn = rbt->kyn;
	   mnp->var = rbt->var;
	 }

	if (cpu == rciCpu)
	 { 
	   /* get the shared memory areas and attach them */

	   if ((smem = (RCCL_SMEM*)getSharedMemory ("RCCL_SMEM")) == NULL)
	    { rciAbort(EInternal,"%s -- can't get memory RCCL_SMEM\n",fxnName);
	      return (-1);
	    }
	   mpb = (MOTION_PARAMETER_BLOCK*)smem->mpb[num];
	   bzero (mpb, sizeof(MOTION_PARAMETER_BLOCK));
	   mpb->evalListHead = mpb->evalListTail = NULL;

	   /* link the motion parameter block into the trajectory 
	      generator's list */

	   mpb->num = num;
	   mpb->mq = (MOTION_QUEUE*)mnp->mq;
	   LINK_HT (mpbListHead, mpbListTail, mpb);
	 }
	return (1);
smiFindErr:
	rciAbort (EInternal, "%s -- can't find object at 0x%x\n", fxnName, key);
	return (-1);
}

setpManipRemove(td, num)
RCI_DESC *td;
int num;
{
	/* Called whenever a manipulator is deleted (with a call to 
	   rcclDelete()) to unlink the manipulator from the active
	   list. Note that this routine is currently called on all
	   tasks everytime any manipulator is deleted, so only the
	   task that is running it will find it in the mpbList.
	 */

	MOTION_PARAMETER_BLOCK *mpb;

	for (mpb = mpbListHead; mpb; mpb = mpb->next)
	 { if (mpb->num == num)
	    { UNLINK_HT (mpbListHead, mpbListTail, mpb);
	      break;
	    }
	 }
}

setpStartup(td, arg)
RCI_DESC *td;
int arg;
{
	/* Called once every time RCCL is activated with a call to
	   rcclStart() */

	char *fxnName = "setpStartup()";

	MOTION_PARAMETER_BLOCK *mpb;
	RCI_RBT *rbt;
	RCCL_GMEM *gmem;
	register int k, j;
	MANIP_RECORD *table;

	/* get the shared memory areas and attach them */

	if ((gmem = (RCCL_GMEM*)getGlobalMemory ("RCCL_GMEM")) == NULL)
	 { rciAbort (EInternal, "%s -- can't get memory RCCL_GMEM\n", fxnName);
	   return (-1);
	 }
	td->tcb->globalMem = (void*)gmem;

	for (mpb = mpbListTail; mpb; mpb = mpb->prev)
	 { mpb->hostMan = &(gmem->manipPool[ARBITER_CPU][mpb->num]);
	   for (j=0; j<MAX_CPUS; j++)
	    { if (rcclManipPool[j])
	       { mpb->mnps[j] = &rcclManipPool[j][mpb->num];
	       }
	    }
	   mpb->mcb = (MOTION_CONTROL_BLOCK*)mpb->mnps[rciCpu]->mcb;

	   if (mpb->mnps[rciCpu]->rbtNum != UNDEF)	
	    { mpb->rbt = TASKROBOT(td, mpb->mnps[rciCpu]->rbtNum);
	    }
	   else
	    { mpb->rbt = NULL;
	    }
	   mpb->mflagBuf = gmem->mflagBuf;
	   initializeMpb (mpb, gmem);
	 }

	for (k=0; k<MAX_TASKROBOTS; k++)
	 { if (rbt = TASKROBOT(td, k))
	    { if ((rbt->startup)(td, rbt) < 0)
	       { return (-1);
	       }
	      JOINT_LEVEL_CONTROL(rbt);
	      SET_POWER_ON(rbt);
	    }
	 }
	if (td->tcb->scheduleDisp == ON_TRIGGER)
	 { rciWakeupOn (td, triggerWakeup, (int)td);
	   td->tcb->fxnNotice = 0;
	   rciSetTrigger (triggerFxn, (int)td);
	 }
	else if (td->tcb->scheduleDisp == ON_FUNCTION)
	 { rciWakeupOn (td, functionWakeup, (int)td);
	 }
	else if (td->tcb->scheduleDisp == ON_SIGNAL)
	 { rciWakeupOn (td, simRobotsReady, (int)td);
	   simSetTimer (td->tcb->interval, (struct timeval *)0);
	 }

	gmem->rendezvous[TASKCPU(td)] = 0;
	gmem->taskActive[TASKCPU(td)] = 1;

	/* initialize pause control variables */

	gmem->pauseTimeLimit = F_UNDEF;
	gmem->pauseTransTime = F_UNDEF;
	gmem->pauseReq = 0;
	gmem->pauseCtrl = TASK_RUNNING;
	gmem->pauseState[TASKCPU(td)] = TASK_RUNNING;
	gmem->pauseCount[TASKCPU(td)] = 0;
	gmem->powerOff[rciCpu] = NO;
	gmem->powerOffFreeze = NO;

	return (1);
}

#define TIMEOUT	1000000		/* about 5 seconds on MicroVAX II */

setpRendezvous()
{
	char *fxnName = "setpRendezvous()";

	/* This is bit of hack routine which syncronizes the execution of
	   the trajectory generators running on different CPUS. If RCCL
	   is running on only one CPU, this routine is essentially a noop.
	   This is true when running in simulator mode as well, since 
	   simulator mode runs everything on the arbiter CPU. */

	RCI_DESC *td;
	register RCCL_GMEM *gmem;
	int count;
	int value;
	int cpu;
	int checked;

	td = getCurrentTask();
	gmem = (RCCL_GMEM*)td->tcb->globalMem;
	count = TIMEOUT;
	value = gmem->rendezvous[TASKCPU(td)];
	checked = 0;
	for (cpu=0; cpu<MAX_CPUS; cpu++)
	 { while (gmem->taskActive[cpu] && gmem->rendezvous[cpu] < value 
					&& --count)
	      ;
	   if (count == 0)
	    { 
	      rciPrintf ("gmem->rendezvous[%d] = %d\n", cpu,
			  gmem->rendezvous[cpu]);
	      rciAbort (ETimeout,
		 "%s -- timeout waiting for %d on cpu %d\n", fxnName, value, cpu);
	      return (-1);
	    }
	   if (gmem->rendezvous[cpu] >= value)
	    { checked |= (1<<cpu);
	    }
	 }
	return (1);
}

setpCtrl(td, arg)
RCI_DESC *td;
int arg;
{
	/* Main control routine, called once per RCCL control cycle.

	   This routine basically does the following things:

		1. reads in data from each robot
		2. writes commands back to each robot (*)
		3. does rendezvous with trajectory tasks on other CPUS.
		4. sets up trigger for next control cycle: the
		   trigger function, that will wake up the robot 
		   controllers before the next control cycle, is 
		   enabled.
		5. Call the input computation routines for each manipulator.
		6. Evaluate transforms bound to functions with 
		   'transEval()'.
		7. Call the output computation routines for each manipulator.
		8. Rendezvous again with other CPUs

	   (*) If gmem->tightControlLoop is TRUE, then step (2.) is delayed
	       until after (8.). This eliminates the 1+ cycle delay we
	       otherwise have in responding to inputs. Drawback: the overall
	       cycle may have to be lengthened, since the robot controller
	       can no longer execute its commands in parallel with the 
	       trajectory generator; it has to wait until the trajectory
	       generator is finished.
	*/

	char *fxnName = "setpCtrl()";

	MOTION_PARAMETER_BLOCK *mpb;
	RCCL_GMEM *gmem = (RCCL_GMEM*)td->tcb->globalMem;
	RCI_TCB *tcb = td->tcb;
	RCI_RBT *rbt;
	int count;
	int k;
	struct timeval tv0;

#if PRI_TRACE
	unsigned short *csr = NULL;	/* used for parallel port trace bit */
#endif
	count = 2;
	rciSetTrace(0x1);

	/* If running ON_CLOCK, wakeup the robot controllers now, because
	   this was not done by the trigger function. */

	if (td->tcb->scheduleDisp == ON_CLOCK)
	 { for (k=0; k<MAX_TASKROBOTS; k++)
	    { if (rbt = TASKROBOT(td, k))
	      { if(rbt->wakeup != NULL) ROBOT_WAKEUP(rbt); /* MS */
	       }
	    }
	 }
	else if (td->tcb->scheduleDisp == ON_SIGNAL)
	 { getSystemTime (&tv0);
	 }

	/* do the ``data exchange'' for each robot: read in robot state 
	   information, and if gmem->tightControlLoop is FALSE, write out 
	   the commands which were generated during the last control cycle */

	for (k=0; k<MAX_TASKROBOTS; k++)
	 { if (rbt = TASKROBOT(td, k))
	    { if ((*rbt->input)(td, rbt) < 0)
	       { goto ctrlEnd;
	       }
#if POWER_FREEZE
	      if (rbt->how->power == 0)
	       { gmem->powerOff[rciCpu] = YES;
		 gmem->powerOffFreeze = YES;
	       }
	      else
	       { gmem->powerOff[rciCpu] = NO;
	       }
#endif
	      if (!gmem->tightControlLoop)
	       { if ((*rbt->output)(td, rbt) < 0)
		  { goto ctrlEnd;
		  }
	       }
	    }
	 }

	/* check to see if the system is being paused or resumed */

	if (gmem->pauseReq)
	 { if (gmem->pauseCtrl == TASK_RUNNING)
	    { gmem->pauseCtrl = TASK_READY_TO_PAUSE;
	    }
	 }
	else
	 { if (gmem->pauseCtrl == TASK_PAUSED)
	    { gmem->pauseCtrl = TASK_READY_TO_RESUME;
	    }
	 }

	/* post rendezvous notice saying data exchange has been done.
	   Do not proceed until all other RCCL control tasks have also done 
	   their data exchanges */

	gmem->rendezvous[rciCpu] += 1;
	count--;
	if (setpRendezvous() < 0)
	 { goto ctrlEnd;
	 }

	/* If the task has just started, call the setpoint init
	   functions. If the manipulator is a robot, turn on
	   its arm power and tell RCI to operate the robot at the
	   joint level */

	if (tcb->cycleCount == 1)
	 { for (mpb = mpbListTail; mpb; mpb = mpb->prev)
	    { setpointInit(mpb);
	    }
	   for (k=0; k<MAX_TASKROBOTS; k++)
	    { if (rbt = TASKROBOT(td, k))
	       { if (!rbt->how->calibrated)
		  { rciAbort (ERobotError,
	"%s -- robot '%s' reports it is not calibrated\n", fxnName, rbt->name);
		    goto ctrlEnd;
		  }
	       }
	    }
	 }

#if POWER_FREEZE
	if (gmem->powerOffFreeze)
	 { /* first, check to see if power is still off */

	   for (k=0; k<MAX_CPUS; k++)
	    { if (gmem->powerOff[k])
	       { break;
	       }
	    }
	   if (k == MAX_CPUS)
	    { gmem->powerOffFreeze = NO;
	    }

	   /* then complete the control cycle and exit */

	   if (gmem->tightControlLoop)
	    { for (k=0; k<MAX_TASKROBOTS; k++)
	       { if (rbt = TASKROBOT(td, k))
		  { if ((*rbt->output)(td, rbt) < 0)
		     { goto ctrlEnd;
		     }
		  }
	       }
	    }
	  if (tcb->scheduleDisp == ON_TRIGGER)
	   { tcb->fxnNotice = 0;
	     rciSetTrigger (triggerFxn, (int)td);
	   }
	  else if (tcb->scheduleDisp == ON_SIGNAL)
	   { simSetTimer (tcb->interval, &tv0);
	   }
	  goto ctrlEnd;
	 }
#endif				
	/* take care of any required pause state transitions */

	checkOnPauseState (gmem);

	/* increment the system time if we are not paused */

	if (gmem->pauseState[rciCpu] == TASK_RUNNING)
	 { rcclIncSysTime ((float)tcb->interval);
	 }

	/* If we have have done out robot output already, then
	   set the trigger for the next control cycle. Otherwise,
	   wait until the output functions have been called, because
	   the trigger communication might interfere with the output
	   communication */

	if (!gmem->tightControlLoop && tcb->scheduleDisp == ON_TRIGGER)
	 { tcb->fxnNotice = 0;
	   rciSetTrigger (triggerFxn, (int)td);
	 }

	/* call the compute routines for each manip */

	for (mpb = mpbListTail; mpb; mpb = mpb->prev)
	 { mpb->sysTime = rcclSysTime();

	   /* --- set trace bit if possible ---- */

#if PRI_TRACE
	   if (csr == NULL && mpb->rbt && tcb->scheduleDisp != ON_SIGNAL)
	    { csr = (unsigned short *)
		    ((IO_DEVICE*)mpb->rbt->dev)->location;
	      *csr |= 0x02;
	    }
#endif
	   /* --- switch modes if necessary --- */

	   if (mpb->mcb->controlModeReq)
	    { if (mpb->controlMode == TEACH_CTRL)
	       { teachWrapup (mpb);
	       }
	      mpb->controlMode = mpb->mcb->controlModeReq;
	      if (mpb->controlMode == SETPOINT_CTRL)
	       { setpointInit (mpb);
	       }
	      else if (mpb->controlMode == TEACH_CTRL)
	       { teachInit (mpb);
	       }
	      mpb->mcb->controlModeReq = 0;
	    }

	   /* --- call the regular control function --- */

	   if (mpb->controlMode == TEACH_CTRL)
	    { teachCompute(mpb);
	    }
	   else if (mpb->controlMode == SETPOINT_CTRL)
	    { setpointControl(mpb);
	    }
	 }

	/* evaluate all transforms assigned to this task */

	evalTaskTransforms(rcclSysTime());

	/* post rendezvous mark saying all transforms are evaluated */

	gmem->rendezvous[rciCpu] += 1;
	count--;

	/* call the setpoint output routines for each manip */

	for (mpb = mpbListTail; mpb; mpb = mpb->prev)
	 { if (mpb->controlMode == SETPOINT_CTRL)
	    { setpointSolve(mpb);
	    }
	 }
	if (setpRendezvous() < 0)
	 { goto ctrlEnd;
	 }

	if (gmem->tightControlLoop)
	 { for (k=0; k<MAX_TASKROBOTS; k++)
	    { if (rbt = TASKROBOT(td, k))
	       { if ((*rbt->output)(td, rbt) < 0)
		  { goto ctrlEnd;
		  }
	       }
	    }
	   if (tcb->scheduleDisp == ON_TRIGGER)
	    { tcb->fxnNotice = 0;
	      rciSetTrigger (triggerFxn, (int)td);
	    }
	 }
	if (tcb->scheduleDisp == ON_SIGNAL)
	 { simSetTimer (tcb->interval, &tv0);
	 }
ctrlEnd:
	if (count)
	 { gmem->rendezvous[rciCpu] += count;
	 }
#if PRI_TRACE
	if (csr)
	 { *csr &= ~0x02;
	 }
#endif
	rciClearTrace(0x1);
}

setpRelease(td, arg, code)
RCI_DESC *td;
int arg, code;
{
	/* Called once every time RCCL is released, either through a call
	   to rcclRelease(), or an RCI abort condition */

	RCI_RBT *rbt;
	int error;
	int k;
	RCCL_GMEM *gmem = (RCCL_GMEM*)td->tcb->globalMem;
	MOTION_PARAMETER_BLOCK *mpb;

	rciSetTrigger (NULLF, 0);	/* clear outstanding trigger fxns */
	gmem->taskActive[TASKCPU(td)] = 0;

	error = 0;
	for (k=0; k<MAX_TASKROBOTS; k++)
	 { if (rbt = TASKROBOT(td, k))
	    { int rcode;

	      rcode = (code == RELEASE_REQ) ? RBT_NORMAL : RBT_HARDERROR;
	      if ((rbt->release)(td, rbt, rcode) < 0)
	       { error = 1;
	       }
	    }
	 }
	for (mpb = mpbListTail; mpb; mpb = mpb->prev)
	 { setpointRelease(mpb);
	 }
	if (error)
	 { return (-1);
	 }
	return (1);
}

checkOnPauseState (gmem)
register RCCL_GMEM *gmem;
{
	/* What this routine is doing is really very simple, although
	   you would never know it by looking at it.

	   Suppose a pause request comes in. What we want to do
	   is make sure that each trajectory generator on each CPU
	   pauses and resumes during exactly the same cycle. This
	   is necessary because otherwise sync problems can arise.
	   For instance, one CPU gain system time on anther (the 
	   incrementing of RCCL system time is suspended while we
	   are paused.

	   To ensure that pausing and resuming is done by ALL CPUS
	   on exactly the same 'beat', we go through this long dance.

	   No double this dance can be simplified, particularly if we
	   are willing to make use of testAndSet type things, but I
	   didn't want to do that because T&S is not supported on the 
	   Q-bus and that would be another can of worms. 
	*/

	register MOTION_PARAMETER_BLOCK *mpb;
	int k;

	if (gmem->pauseState[rciCpu] == TASK_PAUSING)
	 { /* check to see that each manip is paused */
	   for (mpb = mpbListTail; mpb; mpb = mpb->prev)
	    { if (mpb->controlMode == SETPOINT_CTRL &&
		   (mpb->state != TGEN_PAUSE ||
		    mpb->cycle < 2 * mpb->tau))
	       { break;
	       }
	    }
	   if (!mpb)
	    { gmem->pauseState[rciCpu] = TASK_PAUSED;
	      if (gmem->pauseTimeLimit >= 0.0)
	       { gmem->pauseCount[rciCpu] = 
		       gmem->pauseTimeLimit/gmem->interval;
	       }
	      else
	       { gmem->pauseCount[rciCpu] = UNDEF;
	       }
	    }
	   for (k=0; k<MAX_CPUS; k++)
	    { if (gmem->taskActive[k] && gmem->pauseState[k] != TASK_PAUSED)
	       { break;
	       }
	    }
	   if (k == MAX_CPUS)
	    { gmem->pauseCtrl = TASK_PAUSED;
	    }
	 }
	if (gmem->pauseCtrl == TASK_READY_TO_PAUSE && 
	    gmem->pauseState[rciCpu] == TASK_RUNNING)
	 { /* issue a system pause request to each manipulator. Check
	      to see if the pause will be honored on this cycle
	      or the next. If this cycle, set PAUSING. This loop
	      may be called twice, but that doen't matter */

	   int allCanPause = 1;

	   for (mpb = mpbListTail; mpb; mpb = mpb->prev)
	    { mpb->systemPause = 1;
	      mpb->mcb->pauseTime = UNDEF;
	      mpb->mcb->pauseTransTime = gmem->pauseTransTime;
	      if (mpb->cycle == 0 && mpb->state != TGEN_PAUSE)
	       { allCanPause = 0;
	       }
	    }

	   if (allCanPause)
	    { gmem->pauseState[rciCpu] = TASK_READY_TO_PAUSE;
	      for (k=0; k<MAX_CPUS; k++)
	       { if (gmem->taskActive[k] && 
		     gmem->pauseState[k] != TASK_READY_TO_PAUSE)
		  { break;
		  }
	       }
	      if (k == MAX_CPUS)
	       { gmem->pauseCtrl = TASK_PAUSING;
	       }
	    }
	 }
	if (gmem->pauseCtrl == TASK_PAUSING && 
	    gmem->pauseState[rciCpu] == TASK_READY_TO_PAUSE)
	 { gmem->pauseState[rciCpu] = TASK_PAUSING;
	 }
	if (gmem->pauseCtrl == TASK_PAUSED && gmem->pauseCount[rciCpu] >= 0)
	 { if (--gmem->pauseCount[rciCpu] <= 0)
	    { gmem->pauseReq = 0;
	    }
	 }
	if (gmem->pauseCtrl == TASK_READY_TO_RESUME &&
	    gmem->pauseState[rciCpu] == TASK_PAUSED)
	 { gmem->pauseState[rciCpu] = TASK_RUNNING;

	   /* set up a resume request for each manipulator */

	   for (mpb = mpbListTail; mpb; mpb = mpb->prev)
	    { mpb->systemPause = 0;
	    }

	   for (k=0; k<MAX_CPUS; k++)
	    { if (gmem->taskActive[k] &&
		  gmem->pauseState[k] != TASK_RUNNING)
	       { break;
	       }
	    }
	   if (k == MAX_CPUS)
	    { gmem->pauseCtrl = TASK_RUNNING;
	    }
	 }
}
