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

extern pivotFxn();

#ifdef VXWORKS_CFG
#define main pivot_560_
#endif

main()
{
        TRSF_PTR e, b, handle, rot0, rot1, roty;
        POS_PTR p0, p1, pt, pd;
        MANIP *mnp;
        JNTS rcclpark;
        char *robotName;

        rcclSetOptions (RCCL_ERROR_EXIT);
        robotName = getDefaultRobot();
        if (!getRobotPosition (rcclpark.v, "rcclpark", robotName))
         { printf ("position 'rcclpark' not defined for robot\n");
           exit(-1);
         }

        e = allocTransXyz ("E", UNDEF, 0.0, 0.0, TOOLZ);
        b = allocTransRot ("B", UNDEF, B_X, B_Y, B_Z, xunit, 180.0);
        handle = allocTransXyz ("HANDLE", UNDEF, 0.0, 0.0, -300.0);
        rot0 = allocTransRpy ("ROT0", UNDEF, 0.0, 0.0, 0.0, 0.0, -30.0, 0.0);
        rot1 = allocTransRpy ("ROT1", UNDEF, 0.0, 0.0, 0.0, 0.0,  30.0, 0.0);
        roty = allocTrans ("ROTY", UNDEF);

        p0 = makePosition ("p0", T6, e, EQ, b, rot0, handle, TL, e);
        pt = makePosition ("pt", T6, e, EQ, b, roty, handle, TL, e);
        p1 = makePosition ("p1", T6, e, EQ, b, rot1, handle, TL, e);
        pd = makePosition ("pd", T6, e, EQ, b, rot0, handle, TL, handle);

        mnp = rcclCreate (robotName, 0);
        rcclStart();

	movej (mnp, &rcclpark);

        setMod (mnp, 'c');
        distance (mnp, "dz", -75.0);                            /*1*/
        move (mnp, p0);
        move (mnp, p0);

        setTime (mnp, 200.0, 12000.0);                          /*2*/

        *roty = *rot0;
        transMotionEval (roty, pivotFxn, 0, mnp);               /*3*/
        move (mnp, pt);
        distance (mnp, "dz", -75.0);                            /*4*/
        move (mnp, p1);

        move (mnp, p1);                                         /*5*/
        setTime (mnp, 200.0, 12000.0);
        move (mnp, pd);
        distance (mnp, "dz", -75.0);
        move (mnp, p0);

        setMod (mnp, 'j');
        movej (mnp, &rcclpark);
        stop (mnp, 1000.0);

        waitForCompleted (mnp);                                 /*6*/
        rcclRelease (1);
}
