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

wheelFxn (t, arg)
TRSF_PTR t;
int arg;
{
        ROTATE_CTRL_BLK *rcb;
        static float alpha = 0.0;
                                                                /*12*/
        if ((rcb = (ROTATE_CTRL_BLK*)getMemByName("rcb")) == NULL)
         { rciAbort (0, "wheelFxn(): can't find argument object\n");
           return;
         }
#ifdef VXWORKS_CFG
        if (!rcb->wheelFxnInit)
         { alpha = 0.0;
           rcb->wheelFxnInit = 1;
         }
#endif
        alpha += rcb->wheelRPS * rcclGetInterval() / 1000.0;    /*13*/
        
        t->p.x = rcb->wheelRadius * COS(alpha * PIT2);
        t->p.y = rcb->wheelRadius * SIN(alpha * PIT2);
}

tableFxn (t, arg, mnp)
TRSF_PTR t;
int arg;
MANIP *mnp;
{
        ROTATE_CTRL_BLK *rcb;
        static float alpha = 0.0;
        static done = 0;
                                                                /*14*/
        if ((rcb = (ROTATE_CTRL_BLK*)getMemByName("rcb")) == NULL)
         { rciAbort (0, "tableFxn(): can't find argument object\n");
           return;
         }
#ifdef VXWORKS_CFG
        if (!rcb->tableFxnInit)
         { alpha = 0.0;
           done = 0;
           rcb->tableFxnInit = 1;
         }
#endif
        alpha += 360.0 * rcb->tableRPS * rcclGetInterval() / 1000.0;
        rotToTrsf (t, zunit, alpha);

        if (alpha >= 90.0 && !done)                             /*16*/
         { stopCurrentMotion (mnp, 1);
           done = 1;
         }
}
