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

#ifdef VXWORKS_CFG
#define main zigzag_260_
#endif

main ()
{
        TRSF_PTR z, e, conv, t1, t2;
        POS_PTR pt1, pt2;
        MANIP *mnp;
        JNTS rcclpark;
        char *robotName;

        int convfn();
        int i;

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

        z = allocTransXyz ("Z", UNDEF, 0.0, 0.0, ZBASE);
        e = allocTransXyz ("E", UNDEF, 0.0, 0.0, TOOLZ);
        conv = allocTransRot ("CONV", UNDEF, C_X, C_Y, C_Z, xunit, 180.0);
        t1 = allocTransXyz ("T1", UNDEF, -200.0,  30.0, 0.0);
        t2 = allocTransXyz ("T2", UNDEF, -200.0, -30.0, 0.0);

        pt1 = makePosition ("pt1", z, T6, e, EQ, conv, t1, TL, e);
        pt2 = makePosition ("pt2", z, T6, e, EQ, conv, t2, TL, e);

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

        movej (mnp, &rcclpark);
        waitForCompleted (mnp);

        move (mnp, pt1);
        stop (mnp, 0.0);
        waitForCompleted (mnp);

        setMod (mnp, 'c');

        transEval (conv, convfn, /*speed=*/15);

        for (i=0; i<8; ++i)
         { move (mnp, pt1);
           stop (mnp, 1000.0);
           move (mnp, pt2);
           stop (mnp, 1000.0);
           if (i == 3)
            { waitForCompleted (mnp);
              transEval (conv, convfn, -15);
            }
         }
        movej (mnp, &rcclpark);
        stop (mnp, 1000.0);
        waitForCompleted (mnp);
        rcclRelease (1);
}
