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

#ifdef VXWORKS_CFG
#define main rotate_260_
#endif

main ()
{
        TRSF_PTR table0, table, p, wheel;
        POS_PTR p0, pt;
        MANIP *mnp;
        ROTATE_CTRL_BLK *rcb;
        JNTS rcclpark;
        char *robotName;

        int tableFxn();
        int wheelFxn();
        int mid;

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

        p = allocTransRot ("P", UNDEF, P_X, P_Y, P_Z, xunit, 180.0);
        table0 = allocTransRot ("TABLE0", UNDEF, 0.0, 0.0, 0.0, zunit, -45.0);
        wheel = allocTrans ("WHEEL", UNDEF);
        table = allocTrans ("TABLE", UNDEF);

        rcb = ALLOC_MEM ("rcb", ROTATE_CTRL_BLK, UNDEF);

        rcb->wheelRadius = 40.0;
        rcb->wheelRPS = .50;
        rcb->tableRPS = 0.02;

        p0 = makePosition ("p0", T6, EQ, table0, p, wheel, TL, T6);
        pt = makePosition ("pt", T6, EQ, table0, table, p, wheel, TL, T6);

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

        transEval (wheel, wheelFxn, 0);

        movej (mnp, &rcclpark);

        setMod (mnp, 'c');
        move (mnp, p0);
        mid = stop (mnp, F_UNDEF);

        transMotionEval (table, tableFxn, 0, mnp);
        setTime (mnp, F_DEFAULT, F_UNDEF);
        move (mnp, pt);
        movej (mnp, &rcclpark);
        stop (mnp, 1000.0);
        
        waitFor (motionStatus(mid) == M_RUNNING);
        delay (10000.0);
        stopMotion (mid, 0);

        waitForCompleted (mnp);
        rcclRelease (1);
}
