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

#define MSEC_PER_MINUTE (60.0*1000.0);
#define RPM             5

rotzFxn (t, arg)
TRSF *t;
int arg;
{
        /* produce a rotation about z at 5 rpm */

        static float ang = 0.0;
        ang += 360.0*RPM*rcclGetInterval()/MSEC_PER_MINUTE;
        rotToTrsf (t, zunit, ang);
}

forceMonitor (arg, mnp)
int arg;
MANIP *mnp;
{
        char *fxnName = "forceMonitor()";

        COMPLY_CTRL_BLK *cbk;
        int limitMask;
        FORCE limitValues;      

        if ((cbk = (COMPLY_CTRL_BLK*)getMemByAddr((void*)arg)) == NULL)
         { rciAbort (0, "%s -- can't find control block\n", fxnName);
           return;
         }
        getActiveForceLimit (mnp, &limitMask, &limitValues);    /*24*/

        if (limitMask && cbk->valid)                            /*25*/
         {
           if (   (limitMask & ALONG_X &&
                   FABS(cbk->force.f.x) > limitValues.f.x)
               || (limitMask & ALONG_Y &&
                   FABS(cbk->force.f.y) > limitValues.f.y)
               || (limitMask & ALONG_Z &&
                   FABS(cbk->force.f.z) > limitValues.f.z)
               || (limitMask & ABOUT_X &&
                   FABS(cbk->force.m.x) > limitValues.m.x)
               || (limitMask & ABOUT_Y &&
                   FABS(cbk->force.m.y) > limitValues.m.y)
               || (limitMask & ABOUT_Z &&
                   FABS(cbk->force.m.z) > limitValues.m.z))
            { stopCurrentMotion (mnp, ON_FORCE);                /*26*/
              rciPrintVf ("LIMIT X %11.4f\n", &cbk->force, 6);
            }
         }
}
