cphidgetaccelerometer.c

Go to the documentation of this file.

#include "stdafx.h"
#include "cphidgetaccelerometer.h"
#include "cusb.h"
#include "math.h"
#include "csocket.h"
#include "cthread.h"

// === Internal Functions === //

//clearVars - sets all device variables to unknown state
CPHIDGETCLEARVARS(Accelerometer)
        int i = 0;

        phid->accelerationMax = PUNI_DBL;
        phid->accelerationMin = PUNI_DBL;

        for (i = 0; i<ACCEL_MAXAXES; i++)
        {
                phid->axis[i] = PUNI_DBL;
                phid->axisLastTrigger[i] = PUNK_DBL;
                phid->axisChangeTrigger[i] = PUNI_DBL;
        }
        return EPHIDGET_OK;
}

//initAfterOpen - sets up the initial state of an object, reading in packets from the device if needed
//                                used during attach initialization - on every attach
CPHIDGETINIT(Accelerometer)
        int i = 0;

        TESTPTR(phid);

        //Setup max/min values
        switch(phid->phid.deviceIDSpec)
        {
                case PHIDID_ACCELEROMETER_2AXIS:
                        if (phid->phid.deviceVersion < 200)
                        {
                                phid->accelerationMax = 2.1;
                                phid->accelerationMin = -2.1;
                        }
                        else if ((phid->phid.deviceVersion  >= 200) && (phid->phid.deviceVersion  < 300))
                        {
                                phid->accelerationMax = 10.1;
                                phid->accelerationMin = -10.1;
                        }
                        else if ((phid->phid.deviceVersion  >= 300) && (phid->phid.deviceVersion  < 400))
                        {
                                phid->accelerationMax = 5.1;
                                phid->accelerationMin = -5.1;
                        }
                        else
                                return EPHIDGET_BADVERSION;
                        break;
                case PHIDID_ACCELEROMETER_3AXIS:
                        if ((phid->phid.deviceVersion  >= 400) && (phid->phid.deviceVersion  < 500))
                        {
                                phid->accelerationMax = 3.1;
                                phid->accelerationMin = -3.1;
                        }
                        else
                                return EPHIDGET_BADVERSION;
                        break;
                default:
                        return EPHIDGET_UNEXPECTED;
        }

        //initialize triggers, set data arrays to unknown
        for (i = 0; i<phid->phid.attr.accelerometer.numAxis; i++)
        {
                phid->axis[i] = PUNK_DBL;
                phid->axisLastTrigger[i] = PUNK_DBL;
                phid->axisChangeTrigger[i] = 0.001;
        }

        //issue one read
        CPhidget_read((CPhidgetHandle)phid);

        return EPHIDGET_OK;
}

//dataInput - parses device packets
CPHIDGETDATA(Accelerometer)
        int i = 0;
        double axis[ACCEL_MAXAXES];

        if (length<0) return EPHIDGET_INVALIDARG;
        TESTPTR(phid);
        TESTPTR(buffer);
        
        ZEROMEM(axis, sizeof(axis));

        //Parse device packets - store data locally
        switch(phidG->deviceIDSpec)
        {
                case PHIDID_ACCELEROMETER_2AXIS:
                        if (phidG->deviceVersion < 200)
                        {
                                int data = 0;
                                data = (signed short)((unsigned short)buffer[0]+((unsigned short)buffer[1]<<8));
                                axis[0] = round_double((((double)data-16384) / 2000), 4);
                                data = (signed short)((unsigned short)buffer[2]+((unsigned short)buffer[3]<<8));
                                axis[1] = round_double((((double)data-16384) / 2000), 4);
                        }
                        else if ((phidG->deviceVersion  >= 200) && (phidG->deviceVersion  < 300))
                        {
                                int data = 0;
                                data = (signed short)((unsigned short)buffer[0]+((unsigned short)buffer[1]<<8));
                                axis[0] = round_double((((double)data-16384) / 650), 4);
                                data = (signed short)((unsigned short)buffer[2]+((unsigned short)buffer[3]<<8));
                                axis[1] = round_double((((double)data-16384) / 650), 4);
                        }
                        else if ((phidG->deviceVersion  >= 300) && (phidG->deviceVersion  < 400))
                        {
                                int data = 0;
                                data = ((unsigned short)buffer[0]+((unsigned short)buffer[1]<<8));
                                axis[0] = round_double((((double)(data-32768)) / 4000), 5);
                                data = ((unsigned short)buffer[2]+((unsigned short)buffer[3]<<8));
                                axis[1] = round_double((((double)(data-32768)) / 4000), 5);
                        }
                        else
                                return EPHIDGET_UNEXPECTED;
                        break;
                case PHIDID_ACCELEROMETER_3AXIS:
                        if ((phidG->deviceVersion  >= 400) && (phidG->deviceVersion  < 500))
                        {
                                int data = 0;
                                data = ((unsigned short)buffer[0]+((unsigned short)buffer[1]<<8));
                                axis[0] = round_double((((double)(data-32768)) / 6553.6), 5);
                                data = ((unsigned short)buffer[2]+((unsigned short)buffer[3]<<8));
                                axis[1] = round_double((((double)(data-32768)) / 6553.6), 5);
                                data = ((unsigned short)buffer[4]+((unsigned short)buffer[5]<<8));
                                axis[2] = round_double((((double)(data-32768)) / 6553.6), 5);
                        }
                        else
                                return EPHIDGET_UNEXPECTED;
                        break;
                default:
                        return EPHIDGET_UNEXPECTED;
        }

        //Make sure values are within defined range, and store to structure
        for (i = 0; i<phid->phid.attr.accelerometer.numAxis; i++)
        {
                if(axis[i] > phid->accelerationMax) axis[i] = phid->accelerationMax;
                if(axis[i] < phid->accelerationMin) axis[i] = phid->accelerationMin;
                phid->axis[i] = axis[i];
        }
        
        //send out any events that exceed or match the trigger
        for (i = 0; i<phid->phid.attr.accelerometer.numAxis; i++)
        {
                if (fabs(phid->axis[i] - phid->axisLastTrigger[i]) >= phid->axisChangeTrigger[i]
                        || phid->axisLastTrigger[i] == PUNK_DBL)
                {
                        FIRE(AccelerationChange, i, phid->axis[i]);
                        phid->axisLastTrigger[i] = phid->axis[i];
                }
        }

        return EPHIDGET_OK;
}

//eventsAfterOpen - sends out an event for all valid data, used during attach initialization
CPHIDGETINITEVENTS(Accelerometer)

        for (i = 0; i<phid->phid.attr.accelerometer.numAxis; i++)
        {
                if(phid->axis[i] != PUNK_DBL)
                {
                        FIRE(AccelerationChange, i, phid->axis[i]);
                        phid->axisLastTrigger[i] = phid->axis[i];
                }
        }

        return EPHIDGET_OK;
}

//getPacket - not used for accelerometer
CGETPACKET(Accelerometer)
        return EPHIDGET_UNEXPECTED;
}

// === Exported Functions === //

//create and initialize a device structure
CCREATE(Accelerometer, PHIDCLASS_ACCELEROMETER)

//event setup functions
CFHANDLE(Accelerometer, AccelerationChange, int, double)

CGET(Accelerometer,AxisCount,int)
        TESTPTRS(phid,pVal) 
        TESTDEVICETYPE(PHIDCLASS_ACCELEROMETER)
        TESTATTACHED

        MASGN(phid.attr.accelerometer.numAxis)
}

CGETINDEX(Accelerometer,Acceleration,double)
        TESTPTRS(phid,pVal)     
        TESTDEVICETYPE(PHIDCLASS_ACCELEROMETER)
        TESTATTACHED
        TESTINDEX(phid.attr.accelerometer.numAxis)
        TESTMASGN(axis[Index], PUNK_DBL)

        MASGN(axis[Index])
}

CGETINDEX(Accelerometer,AccelerationMax,double)
        TESTPTRS(phid,pVal)     
        TESTDEVICETYPE(PHIDCLASS_ACCELEROMETER)
        TESTATTACHED
        TESTINDEX(phid.attr.accelerometer.numAxis)
        TESTMASGN(accelerationMax, PUNK_DBL)

        MASGN(accelerationMax)
}

CGETINDEX(Accelerometer,AccelerationMin,double)
        TESTPTRS(phid,pVal)     
        TESTDEVICETYPE(PHIDCLASS_ACCELEROMETER)
        TESTATTACHED
        TESTINDEX(phid.attr.accelerometer.numAxis)
        TESTMASGN(accelerationMin, PUNK_DBL)

        MASGN(accelerationMin)
}

CGETINDEX(Accelerometer,AccelerationChangeTrigger,double)
        TESTPTRS(phid,pVal) 
        TESTDEVICETYPE(PHIDCLASS_ACCELEROMETER)
        TESTATTACHED
        TESTINDEX(phid.attr.accelerometer.numAxis)
        TESTMASGN(axisChangeTrigger[Index], PUNK_DBL)

        MASGN(axisChangeTrigger[Index])
}
CSETINDEX(Accelerometer,AccelerationChangeTrigger,double)
        TESTPTR(phid) 
        TESTDEVICETYPE(PHIDCLASS_ACCELEROMETER)
        TESTATTACHED
        TESTINDEX(phid.attr.accelerometer.numAxis)
        TESTRANGE(0, phid->accelerationMax - phid->accelerationMin)

        if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG))
                ADDNETWORKKEYINDEXED(Trigger, "%lE", axisChangeTrigger);
        else
                phid->axisChangeTrigger[Index] = newVal;

        return EPHIDGET_OK;
}

// === Deprecated Functions === //

CGET(Accelerometer,NumAxis,int)
        return CPhidgetAccelerometer_getAxisCount(phid, pVal);
}