/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 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:	getLimit
 *
 * DESCRIPTION:	Routines to get the motion limit specifications currently
 *		active for a particular manipulator.
 *
 * AUTHOR/DATE:	John Lloyd, Fall 1989
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include "../h/cdefs.h"
#include "../h/rcclDefs.h"
#include "../h/robotMath.h"

#include "../rccl/motionRequest.h"

#define NmToNmm	1000.0

getActiveForceLimit(mnp, mask, force)
MANIP *mnp;
unsigned long *mask;
register FORCE *force;
{
	/* RCCL primitive to read back the force limit spec. currently
	   active on a particular manipulator.
	 */

	char *fxnName = "getActiveForceLimit()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	register int index;

	do
	 { index = mcb->pathsegIndex;
	   if (force)
	    { *force = mcb->activeForceLimVals[index&1];
	      force->m.x /= NmToNmm;
	      force->m.y /= NmToNmm;
	      force->m.z /= NmToNmm;
	    }
	   *mask = mcb->activeForceLimMask[index&1];
	 }
	while ((index&~1) != (mcb->pathsegIndex&~1));
	return (1);
}

getActiveDispLimit(mnp, mask, diff)
MANIP *mnp;
unsigned long *mask;
register DIFF *diff;
{
	/* RCCL primitive to read back the diff limit spec. currently
	   active on a particular manipulator.
	 */

	char *fxnName = "getActiveDispLimit()";

	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;
	register int index;

	do
	 { index = mcb->pathsegIndex;
	   if (diff)
	    { *diff = mcb->activeDiffLimVals[index&1];
	      diff->r.x *= RADTODEG;
	      diff->r.y *= RADTODEG;
	      diff->r.z *= RADTODEG;
	    }
	   *mask = mcb->activeDiffLimMask[index&1];
	 }
	while ((index&~1) != (mcb->pathsegIndex&~1));
	return (1);
}
