/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Vincent Hayward.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*---------------------------------------------------------------------------
 *
 * MODULE NAME:	robotIO
 *
 * DESCRIPTION: Functions for communicating between the RCCL trajectory
 *		generator and the robot, using the the robot control 
 *		interface (RCI).
 *
 * CAVEATS:	These routines are sort of top-heavy now that the presentation
 *		layer of RCI has become as sophisticated as it has.
 *
 * AUTHOR/DATE:	Vincent Hayward, Purdue University, 1983, and John Lloyd.
 *
 *--------------------------------------------------------------------------*/

#include "../h/cdefs.h"
#include "../h/rciTask.h"
#include "../h/robot_kynvar.h"

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

#define J1	  00	/* indicies into an array of joint values */
#define J2	  01
#define J3	  02

#define J4	  03
#define J5	  04
#define J6	  05

#define NmToNmm 1000.0	/* Newton-meters to Newton-millimeters */
#define NmmToNm 0.001	/* Newton-millimeters to Newton-meters */
#define SMALL 1.0e-5	

output_setpoints (mpb)
register MOTION_PARAMETER_BLOCK *mpb;
{
	/* Called once every trajectory cycle to output the setpoints
	   the manipulator. We should use a loop, but unrolling the 
	   per-joint loop gives a mondo speed, and we really need the
	   speed, especially on VAXen.

	   All the routine does is output the setpoint joint angle,
	   do limit checking, and (if selected) output a computed
	   gravity bias.
	 */

	register RCI_RBT *rbt = mpb->rbt;
	float gravLoad[6];

	KYN *kyn;
	HOW *how;
	register JLS *jls = rbt->jls;
	register float value;

	int code = 0;			/* return code */
	int gravityBias = (mpb->mcb->mode & GRAVITY_FEEDFORWARD);

	/* A place to call the inverse model */

	if (gravityBias)
	 { gravloadVar (gravLoad, rbt->var, rbt->kyn);

	   kyn = rbt->kyn;	/* we will need these to do */
	   how = rbt->how;	/* friction adjustment */
	 }

	value = mpb->j6.v[0];
	if (jls->jmin[J1] < value && value < jls->jmax[J1])
	 { MOVE_ANGLE (rbt, value, J1);
	 }
	else
	 { code |= (1<<J1);
	 }
	if (gravityBias)
	 { SET_JTORQUE_BIAS (rbt, gravLoad[J1], J1);
	 }

	value = mpb->j6.v[1];
	if (jls->jmin[J2] < value && value < jls->jmax[J2])
	 { MOVE_ANGLE (rbt, value, J2);
	 }
	else
	 { code |= (1<<J2);
	 }
	if (gravityBias)
	 { SET_JTORQUE_BIAS (rbt, gravLoad[J2], J2);
	 }
	
	value = mpb->j6.v[2];
	if (jls->jmin[J3] < value && value < jls->jmax[J3])
	 { MOVE_ANGLE (rbt, value, J3);
	 }
	else
	 { code |= (1<<J3);
	 }
	if (gravityBias)
	 { SET_JTORQUE_BIAS (rbt, gravLoad[J3], J3);
	 }

	value = mpb->j6.v[3];
	if (jls->jmin[J4] < value && value < jls->jmax[J4])
	 { MOVE_ANGLE (rbt, value, J4);
	 }
	else
	 { code |= (1<<J4);
	 }
	if (gravityBias)
	 { SET_JTORQUE_BIAS (rbt, gravLoad[J4], J4);
	 }

	value = mpb->j6.v[4];
	if (jls->jmin[J5] < value && value < jls->jmax[J5])
	 { MOVE_ANGLE (rbt, value, J5);
	 }
	else
	 { code |= (1<<J5);
	 }
	if (gravityBias)
	 { SET_JTORQUE_BIAS (rbt, gravLoad[J5], J5);
	 }

	value = mpb->j6.v[5];
	if (jls->jmin[J6] < value && value < jls->jmax[J6])
	 { MOVE_ANGLE (rbt, value, J6);
	 }
	else
	 { code |= (1<<J6);
	 }
	if (gravityBias)
	 { SET_JTORQUE_BIAS (rbt, gravLoad[J6], J6);
	 }

	if (mpb->handPosSet)
	 { SET_HANDPOS (rbt, mpb->handPos);
	   mpb->handPosSet = 0;
	 }

	return(code);
}
