#include <rccl.h>
#include <errorCodes.h>
#include "manex.560.h"

COMPLY_CTRL_BLK *complyInit (mnp, tgain, rgain)
MANIP *mnp;
float tgain, rgain;
{
        COMPLY_CTRL_BLK *cbk;
        TRSF *comply;

        extern computeForce();
        extern complyFxn();

        comply = allocTrans (NULL, UNDEF);                      /*1*/
        cbk = ALLOC_MEM (NULL, COMPLY_CTRL_BLK, UNDEF);
        cbk->tgain = tgain;                                     /*2*/
        cbk->rgain = rgain;
        cbk->comply = comply;
        cbk->mnp = mnp;
        transMotionEval (comply, complyFxn, (int)cbk, mnp);     /*3*/
        return (cbk);
}

complyRun (cbk)
COMPLY_CTRL_BLK *cbk;
{
        cbk->init = YES;
        runMonitorFxn (cbk->mnp, computeForce, (int)cbk);       /*4*/
        rcclBlock();
}

main()
{
        TRSF_PTR b, e;
        POS_PTR p1;
        MANIP *mnp;
        JNTS rcclpark;
        COMPLY_CTRL_BLK *cbk;

        rcclSetOptions (RCCL_ERROR_EXIT);

        e = allocTransXyz (NULL, UNDEF, 0.0, 0.0, TOOLZ);
        b = allocTransRot (NULL, UNDEF, B_X, B_Y, B_Z, xunit, 180.0);

        mnp = rcclCreate (getDefaultRobot(), 0);
        cbk = complyInit (mnp, 0.01, 0.0);                      /*5*/

        p1 = makePosition (NULL, T6, e, cbk->comply, EQ, b, TL, cbk->comply);

        rcclStart();
        complyRun (cbk);                                        /*6*/

        setMod (mnp, 'c');
        setTime (mnp, F_DEFAULT, F_UNDEF);
        comply (mnp, "fx fy fz", 0.0, 0.0, 0.0);                /*7*/
        move (mnp, p1);

        while (1)                                               /*8*/
         { FORCE f;

           accessMem (cbk, &cbk->force, &f, 6*sizeof(float));   /*9*/
           printVf ("%10.4f\n", (float*)&f, 6);
           delay (500.0);
         }
}
