cphidgetservo.c

Go to the documentation of this file.

#include "stdafx.h"
#include "cphidgetservo.h"
#include "cusb.h"
#include "csocket.h"
#include "cthread.h"

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

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

        phid->motorPositionMaxLimit = PUNI_DBL;
        phid->motorPositionMinLimit = PUNI_DBL;
        phid->fullStateEcho = PUNK_BOOL;

        for (i = 0; i<SERVO_MAXSERVOS; i++)
        {
                phid->motorPositionMax[i] = PUNI_DBL;
                phid->motorPositionMin[i] = PUNI_DBL;
                phid->motorPosition[i] = PUNK_DBL;
                phid->motorEngagedState[i] = PUNK_BOOL;
                phid->motorPositionEcho[i] = PUNI_DBL;
                phid->motorEngagedStateEcho[i] = PUNI_BOOL;
                phid->servoParams[i].state = PUNI_BOOL;
        }

        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(Servo)
        int i = 0;

        TESTPTR(phid);
        
        //Make sure no old writes are still pending
        phid->outputPacketLen = 0;

        //Setup max/min values
        switch(phid->phid.deviceIDSpec)
        {
                case PHIDID_SERVO_1MOTOR:
                case PHIDID_SERVO_4MOTOR:
                        if (phid->phid.deviceVersion == 200)
                        {
                                phid->motorPositionMaxLimit = 2550;
                                phid->motorPositionMinLimit = 1;
                                phid->fullStateEcho = PFALSE;
                        }
                        else if ((phid->phid.deviceVersion >= 300) && (phid->phid.deviceVersion < 313))
                        {
                                phid->motorPositionMaxLimit = 4095;
                                phid->motorPositionMinLimit = 1;
                                phid->fullStateEcho = PFALSE;
                        }
                        else if ((phid->phid.deviceVersion >= 313) && (phid->phid.deviceVersion < 400))
                        {
                                phid->motorPositionMaxLimit = 4095;
                                phid->motorPositionMinLimit = 1;
                                phid->fullStateEcho = PTRUE;
                        }
                        else
                                return EPHIDGET_BADVERSION;
                        break;
                case PHIDID_SERVO_1MOTOR_OLD:
                case PHIDID_SERVO_4MOTOR_OLD:
                        if (phid->phid.deviceVersion == 200)
                        {
                                phid->motorPositionMaxLimit = 2550;
                                phid->motorPositionMinLimit = 1;
                                phid->fullStateEcho = PFALSE;
                        }
                        else
                                return EPHIDGET_BADVERSION;
                        break;
                default:
                        return EPHIDGET_UNEXPECTED;
        }
        
        for (i = 0; i<phid->phid.attr.servo.numMotors; i++)
        {
                //set data arrays to unknown
                phid->motorPositionEcho[i] = PUNK_DBL;
                phid->motorEngagedStateEcho[i] = PUNK_BOOL;
                //init servo type and max/min
                phid->servoParams[i] = Phid_Servo_Types[getServoParameterIndex(PHIDGET_SERVO_DEFAULT)];
                if(phid->servoParams[i].max_us > phid->motorPositionMaxLimit)
                        phid->motorPositionMax[i] = phid->motorPositionMaxLimit;
                else
                        phid->motorPositionMax[i] = phid->servoParams[i].max_us;
                phid->motorPositionMin[i] = phid->servoParams[i].min_us;
        }

        //issue a read for devices that return output data
        if(phid->fullStateEcho)
        {
                CPhidget_read((CPhidgetHandle)phid);
        }

        //recover what we can - if anything isn't filled out, it's PUNK anyways
        for (i = 0; i<phid->phid.attr.servo.numMotors; i++)
        {
                phid->motorPosition[i] = phid->motorPositionEcho[i];
                phid->motorEngagedState[i] = phid->motorEngagedStateEcho[i];
        }

        return EPHIDGET_OK;
}

//dataInput - parses device packets
CPHIDGETDATA(Servo)
        int i = 0;
        double position[SERVO_MAXSERVOS];
        double lastPosition[SERVO_MAXSERVOS];

        if (length<0) return EPHIDGET_INVALIDARG;
        TESTPTR(phid);
        TESTPTR(buffer);

        ZEROMEM(position, sizeof(position));
        ZEROMEM(lastPosition, sizeof(lastPosition));

        //Parse device packets - store data locally
        switch(phid->phid.deviceIDSpec)
        {
                case PHIDID_SERVO_1MOTOR:
                        if ((phid->phid.deviceVersion >= 313) && (phid->phid.deviceVersion < 400))
                        {
                                        position[0] = (((unsigned short)buffer[0]) << 5) + buffer[1];
                        }
                        else
                                return EPHIDGET_UNEXPECTED;
                        break;
                case PHIDID_SERVO_4MOTOR:
                        if ((phid->phid.deviceVersion >= 313) && (phid->phid.deviceVersion < 400))
                        {
                                for (i = 0; i < phid->phid.attr.servo.numMotors; i++)
                                {
                                        position[i] = (((unsigned short)buffer[i*2]) << 5) + buffer[(i*2) + 1];
                                }
                        }
                        else
                                return EPHIDGET_UNEXPECTED;
                        break;
                case PHIDID_SERVO_1MOTOR_OLD:
                case PHIDID_SERVO_4MOTOR_OLD:
                        return EPHIDGET_UNSUPPORTED;
                default:
                        return EPHIDGET_UNEXPECTED;
        }

        //Make sure values are within defined range, and store to structure
        for (i = 0; i < phid->phid.attr.servo.numMotors; i++)
        {
                lastPosition[i] = phid->motorPositionEcho[i];
                if(position[i] > phid->motorPositionMaxLimit || position[i] < phid->motorPositionMinLimit)
                {
                        if(position[i] == 0)
                                phid->motorEngagedStateEcho[i] = PFALSE;
                        else
                                phid->motorEngagedStateEcho[i] = PUNK_BOOL;
                        phid->motorPositionEcho[i] = PUNK_DBL;
                }
                else
                {
                        phid->motorPositionEcho[i] = position[i];
                        phid->motorEngagedStateEcho[i] = PTRUE;
                }
        }
        
        //send out any events for changed data
        for (i = 0; i < phid->phid.attr.servo.numMotors; i++)
        {
                if(phid->motorPositionEcho[i] != PUNK_DBL && phid->motorPositionEcho[i] != lastPosition[i] && phid->motorEngagedStateEcho[i] == PTRUE)
                {
                        FIRE(PositionChange, i, servo_us_to_degrees(phid->servoParams[i], phid->motorPositionEcho[i], PTRUE));
                        //Deprecated
                        FIRE(MotorPositionChange, i, servo_us_to_degrees(phid->servoParams[i], phid->motorPositionEcho[i], PTRUE));
                }
        }

        return EPHIDGET_OK;
}

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

        for (i = 0; i < phid->phid.attr.servo.numMotors; i++)
        {
                if(phid->motorPositionEcho[i] != PUNK_DBL && phid->motorEngagedStateEcho[i] == PTRUE)
                {
                        FIRE(PositionChange, i, servo_us_to_degrees(phid->servoParams[i], phid->motorPositionEcho[i], PTRUE));
                        //Deprecated
                        FIRE(MotorPositionChange, i, servo_us_to_degrees(phid->servoParams[i], phid->motorPositionEcho[i], PTRUE));
                }
        }

        return EPHIDGET_OK;
}

//getPacket - used by write thread to get the next packet to send to device
CGETPACKET_BUF(Servo)

//sendpacket - sends a packet to the device asynchronously, blocking if the 1-packet queue is full
CSENDPACKET_BUF(Servo)

//makePacket - constructs a packet using current device state
CMAKEPACKETINDEXED(Servo)
        int i = 0;

        TESTPTRS(phid, buffer);

        switch(phid->phid.deviceIDSpec)
        {
                case PHIDID_SERVO_1MOTOR:
                        if (phid->phid.deviceVersion == 200) //this version may not exist in this product id
                        {
                                buffer[0] = 0;
                                if(phid->motorPosition[0] == PUNK_DBL || phid->motorEngagedState[0] == PFALSE)
                                        buffer[1] = 0;
                                else
                                        buffer[1] = (unsigned char)round(phid->motorPosition[Index] / 10.0);
                        }
                        else if ((phid->phid.deviceVersion >= 300) && (phid->phid.deviceVersion < 400))
                        {
                                int microsecondPulse = 0;

                                if(phid->motorPosition[0] == PUNK_DBL || phid->motorEngagedState[0] == PFALSE)
                                        microsecondPulse = 0;
                                else
                                        microsecondPulse = round(phid->motorPosition[0]);

                                buffer[0] = (unsigned char)(microsecondPulse & 0xFF);
                                buffer[1] = (unsigned char)(microsecondPulse >> 8);
                        }
                        else
                                return EPHIDGET_UNEXPECTED;
                        break;
                case PHIDID_SERVO_4MOTOR:
                        if (phid->phid.deviceVersion == 200) //this version may not exist in this product id
                        {
                                if (Index == 0) buffer[0] = 2;
                                if (Index == 1) buffer[0] = 3;
                                if (Index == 2) buffer[0] = 0;
                                if (Index == 3) buffer[0] = 1;
                                if(phid->motorPosition[Index] == PUNK_DBL || phid->motorEngagedState[Index] == PFALSE)
                                        buffer[1] = 0;
                                else
                                        buffer[1] = (unsigned char)round(phid->motorPosition[Index]/10.0);
                        }
                        else if ((phid->phid.deviceVersion >= 300) && (phid->phid.deviceVersion < 400))
                        {
                                int microsecondPulse[4];

                                ZEROMEM(microsecondPulse, sizeof(microsecondPulse));

                                for (i = 0; i<phid->phid.attr.servo.numMotors; i++)
                                {
                                        if(phid->motorPosition[i] == PUNK_DBL || phid->motorEngagedState[i] == PFALSE)
                                        {
                                                microsecondPulse[i] = 0;
                                                phid->motorEngagedState[i] = PFALSE;
                                                if (!(phid->fullStateEcho))
                                                        phid->motorEngagedStateEcho[i] = PFALSE;
                                        }
                                        else
                                        {
                                                microsecondPulse[i] = round(phid->motorPosition[i]);
                                                phid->motorEngagedState[i] = PTRUE;
                                                if (!(phid->fullStateEcho))
                                                        phid->motorEngagedStateEcho[i] = PTRUE;
                                        }
                                }

                                buffer[0] = (unsigned char)(microsecondPulse[0] & 0xFF);
                                buffer[1] = (unsigned char)((microsecondPulse[0] >> 8) & 0x0F);

                                buffer[2] = (unsigned char)(microsecondPulse[1] & 0xFF);
                                buffer[1] |= (unsigned char)((microsecondPulse[1] >> 4) & 0xF0);

                                buffer[3] = (unsigned char)(microsecondPulse[2] & 0xFF);
                                buffer[4] = (unsigned char)((microsecondPulse[2] >> 8) & 0x0F);

                                buffer[5] = (unsigned char)(microsecondPulse[3] & 0xFF);
                                buffer[4] |= (unsigned char)((microsecondPulse[3] >> 4) & 0xF0);
                        }
                        else
                                return EPHIDGET_UNEXPECTED;
                        break;
                case PHIDID_SERVO_1MOTOR_OLD:
                        if (phid->phid.deviceVersion == 200)
                        {
                                buffer[0] = 0;
                                if(phid->motorPosition[0] == PUNK_DBL || phid->motorEngagedState[0] == PFALSE)
                                        buffer[1] = (unsigned char)0;
                                else
                                        buffer[1] = (unsigned char)round(phid->motorPosition[Index]/10.0);
                        }
                        else
                                return EPHIDGET_UNEXPECTED;
                        break;
                case PHIDID_SERVO_4MOTOR_OLD:
                        if (phid->phid.deviceVersion == 200)
                        {
                                if (Index == 0) buffer[0] = 2;
                                if (Index == 1) buffer[0] = 3;
                                if (Index == 2) buffer[0] = 0;
                                if (Index == 3) buffer[0] = 1;
                                if(phid->motorPosition[Index] == PUNK_DBL || phid->motorEngagedState[Index] == PFALSE)
                                        buffer[1] = (unsigned char)0;
                                else
                                        buffer[1] = (unsigned char)round(phid->motorPosition[Index]/10.0);
                        }
                        else
                                return EPHIDGET_UNEXPECTED;
                        break;
                default:
                        return EPHIDGET_UNEXPECTED;
        }

        return EPHIDGET_OK;
}

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

//create and initialize a device structure
CCREATE(Servo, PHIDCLASS_SERVO)

//event setup functions
CFHANDLE(Servo, PositionChange, int, double)

CGET(Servo,MotorCount,int)
        TESTPTRS(phid,pVal) 
        TESTDEVICETYPE(PHIDCLASS_SERVO)
        TESTATTACHED

        MASGN(phid.attr.servo.numMotors)
}

CGETINDEX(Servo,Position,double)
        TESTPTRS(phid,pVal) 
        TESTDEVICETYPE(PHIDCLASS_SERVO)
        TESTATTACHED
        TESTINDEX(phid.attr.servo.numMotors)
        TESTMASGN(motorPositionEcho[Index], PUNK_DBL)

        if(phid->motorEngagedStateEcho[Index] != PTRUE) {*pVal = PUNK_DBL; return EPHIDGET_UNKNOWNVAL;}

        *pVal = servo_us_to_degrees(phid->servoParams[Index], phid->motorPositionEcho[Index], PTRUE);
        return EPHIDGET_OK;
}
CSETINDEX(Servo,Position,double)
        TESTPTR(phid) 
        TESTDEVICETYPE(PHIDCLASS_SERVO)
        TESTATTACHED
        TESTINDEX(phid.attr.servo.numMotors)
        TESTRANGE(servo_us_to_degrees(phid->servoParams[Index], phid->motorPositionMin[Index], PFALSE), 
                servo_us_to_degrees(phid->servoParams[Index], phid->motorPositionMax[Index], PFALSE))

        newVal = servo_degrees_to_us(phid->servoParams[Index], newVal);

        //Note: setting a position will always engage a servo
        if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG))
        {
                if(phid->motorEngagedState[Index] != PTRUE)
                        CPhidgetServo_setEngaged(phid, Index, PTRUE);
                ADDNETWORKKEYINDEXED(Position, "%lE", motorPosition);
        }
        else
        {
                phid->motorEngagedState[Index] = PTRUE;
                SENDPACKETINDEXED(Servo, motorPosition[Index], Index);
                if (!(phid->fullStateEcho))
                {
                        phid->motorEngagedStateEcho[Index] = PTRUE;
                        if (phid->motorPositionEcho[Index] == PUNK_BOOL || phid->motorPositionEcho[Index] != newVal)
                        {
                                phid->motorPositionEcho[Index] = newVal;
                                {
                                        FIRE(PositionChange, Index, servo_us_to_degrees(phid->servoParams[Index], newVal, PTRUE));
                                        //Deprecated
                                        FIRE(MotorPositionChange, Index, servo_us_to_degrees(phid->servoParams[Index], newVal, PTRUE));
                                }
                        }
                }
        }

        return EPHIDGET_OK;
}

CGETINDEX(Servo,PositionMax,double)
        TESTPTRS(phid,pVal)     
        TESTDEVICETYPE(PHIDCLASS_SERVO)
        TESTATTACHED
        TESTINDEX(phid.attr.servo.numMotors)
        TESTMASGN(motorPositionMax[Index], PUNK_DBL)

        *pVal = servo_us_to_degrees(phid->servoParams[Index], phid->motorPositionMax[Index], PFALSE);
        return EPHIDGET_OK;
}

CGETINDEX(Servo,PositionMin,double)
        TESTPTRS(phid,pVal)     
        TESTDEVICETYPE(PHIDCLASS_SERVO)
        TESTATTACHED
        TESTINDEX(phid.attr.servo.numMotors)
        TESTMASGN(motorPositionMin[Index], PUNK_DBL)

        *pVal = servo_us_to_degrees(phid->servoParams[Index], phid->motorPositionMin[Index], PFALSE);
        return EPHIDGET_OK;
}

CGETINDEX(Servo,Engaged,int)
        TESTPTRS(phid,pVal) 
        TESTDEVICETYPE(PHIDCLASS_SERVO)
        TESTATTACHED
        TESTINDEX(phid.attr.servo.numMotors)
        TESTMASGN(motorEngagedStateEcho[Index], PUNK_BOOL)

        MASGN(motorEngagedStateEcho[Index])
}
CSETINDEX(Servo,Engaged,int)
        TESTPTR(phid) 
        TESTDEVICETYPE(PHIDCLASS_SERVO)
        TESTATTACHED
        TESTINDEX(phid.attr.servo.numMotors)
        TESTRANGE(PFALSE, PTRUE)

        if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG))
                ADDNETWORKKEYINDEXED(Engaged, "%d", motorEngagedState);
        else
                SENDPACKETINDEXED(Servo, motorEngagedState[Index], Index);

        return EPHIDGET_OK;
}

int setupNewServoParams(CPhidgetServoHandle phid, int Index, CPhidgetServoParameters params)
{
        char newVal[256];

        //Only let raw us mode set the position to 0 - servo mode forces the use of engaged
        if(params.servoType == PHIDGET_SERVO_RAW_us_MODE)
                phid->motorPositionMinLimit = 0;
        else
                phid->motorPositionMinLimit = 1;

        //Set the max/min
        //make sure we don't set mac higher then the limit
        if(params.max_us > phid->motorPositionMaxLimit)
                phid->motorPositionMax[Index] = phid->motorPositionMaxLimit;
        else
                phid->motorPositionMax[Index] = params.max_us;

        phid->motorPositionMin[Index] = params.min_us;

        if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG))
        {
                //make sure webservice sends along motorPositionMinLimit which may have changed
                sprintf(newVal, "%d,%lE,%lE,%lE", params.servoType, params.min_us, params.max_us, params.us_per_degree);
                ADDNETWORKKEYINDEXED(ServoParameters, "%s", servoParamString);
        }

        phid->servoParams[Index] = params;

        return EPHIDGET_OK;
}

CGETINDEX(Servo,ServoType,CPhidget_ServoType)
        TESTPTRS(phid,pVal) 
        TESTDEVICETYPE(PHIDCLASS_SERVO)
        TESTATTACHED
        TESTINDEX(phid.attr.servo.numMotors)

        MASGN(servoParams[Index].servoType)
}

CSETINDEX(Servo,ServoType,CPhidget_ServoType)
        TESTPTR(phid) 
        TESTDEVICETYPE(PHIDCLASS_SERVO)
        TESTATTACHED
        TESTINDEX(phid.attr.servo.numMotors)
        //can't set to User defined with this function
        TESTRANGE(PHIDGET_SERVO_DEFAULT, PHIDGET_SERVO_USER_DEFINED-1)

        setupNewServoParams(phid, Index, Phid_Servo_Types[getServoParameterIndex(newVal)]);

        return EPHIDGET_OK;
}

PHIDGET21_API int CCONV CPhidgetServo_setServoParameters(CPhidgetServoHandle phid, int Index, double min_us, double max_us, double degrees)
{
        CPhidgetServoParameters params;
        TESTPTR(phid) 
        TESTDEVICETYPE(PHIDCLASS_SERVO)
        TESTATTACHED
        TESTINDEX(phid.attr.servo.numMotors)

        //Sanity checking of the values
        if(min_us < phid->motorPositionMinLimit)
                return EPHIDGET_INVALIDARG;
        if(max_us > phid->motorPositionMaxLimit)
                return EPHIDGET_INVALIDARG;
        if(max_us <= min_us)
                return EPHIDGET_INVALIDARG;
        if(degrees <= 0 || degrees > 1440)
                return EPHIDGET_INVALIDARG;

        params.servoType = PHIDGET_SERVO_USER_DEFINED;
        params.min_us = min_us;
        params.max_us = max_us;
        params.us_per_degree = (max_us - min_us)/degrees;

        return setupNewServoParams(phid, Index, params);
}

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

CFHANDLE(Servo, MotorPositionChange, int, double)
CGET(Servo,NumMotors,int)
        return CPhidgetServo_getMotorCount(phid, pVal);
}
CGETINDEX(Servo,MotorPosition,double)
        return CPhidgetServo_getPosition(phid, Index, pVal);
}
CSETINDEX(Servo,MotorPosition,double)
        return CPhidgetServo_setPosition(phid, Index, newVal);
}
CGETINDEX(Servo,MotorPositionMax,double)
        return CPhidgetServo_getPositionMax(phid, Index, pVal);
}
CGETINDEX(Servo,MotorPositionMin,double)
        return CPhidgetServo_getPositionMin(phid, Index, pVal);
}
CGETINDEX(Servo,MotorOn,int)
        return CPhidgetServo_getEngaged(phid, Index, pVal);
}
CSETINDEX(Servo,MotorOn,int)
        return CPhidgetServo_setEngaged(phid, Index, newVal);
}