#include <rci.h>

startupFxn (td)
RCI_DESC *td;
{
        RCI_RBT *rbt = TASKROBOT (td, 0);
        (*rbt->startup)(td, rbt);
	JOINT_LEVEL_CONTROL(rbt);
        return (1);
}

zerogravFxn (td)
RCI_DESC *td;
{
        RCI_RBT *rbt = TASKROBOT (td, 0);
        HOW *how = rbt->how;
        KYN *kyn = rbt->kyn;
        void *var = rbt->var;
	int *enable = (int*)getMemByName ("enable");

        float load[MAXJNTS];
        int j;

	ROBOT_WAKEUP(rbt);
/*	while (!(ROBOT_READY(rbt)))
	   ;
*/
        (*rbt->input)(td, rbt);

	if (*enable)
	 {
	   gravload (load, how->ang, kyn);
   	   for (j = 0; j < rbt->jls->numJ; j++)
	    { SET_JTORQUE (rbt, load[j], j);
	    }
	 }

        (*rbt->output) (td, rbt);
        return (1);
}

releaseFxn (td, arg, code)
RCI_DESC *td;
int arg, code;
{
        RCI_RBT *rbt = TASKROBOT (td, 0);
        int rcode;

        if (code == RELEASE_REQ)
         { rcode = RBT_NORMAL;
         }
        else
         { rcode = RBT_HARDERROR;
         }
        (*rbt->release)(td, rbt, rcode);
        return (1);
}
