diff options
Diffstat (limited to 'cphidgetencoder.c')
| -rw-r--r-- | cphidgetencoder.c | 455 |
1 files changed, 455 insertions, 0 deletions
diff --git a/cphidgetencoder.c b/cphidgetencoder.c new file mode 100644 index 0000000..3c1991f --- /dev/null +++ b/cphidgetencoder.c @@ -0,0 +1,455 @@ +#include "stdafx.h" +#include "cphidgetencoder.h" +#include <stdio.h> +#include "cusb.h" +#include "csocket.h" +#include "cthread.h" + +// === Internal Functions === // + +//clearVars - sets all device variables to unknown state +CPHIDGETCLEARVARS(Encoder) + int i = 0; + for (i = 0; i<ENCODER_MAXINPUTS; i++) + { + phid->inputState[i] = PUNK_BOOL; + } + for (i = 0; i<ENCODER_MAXENCODERS; i++) + { + phid->encoderPosition[i] = 0; + phid->encoderTimeStamp[i] = PUNK_INT; + phid->indexPosition[i] = PUNK_INT; + phid->enableStateEcho[i] = 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(Encoder) + int i = 0; + unsigned char buffer[8] = { 0 }; + int result; + + TESTPTR(phid); + + //Make sure no old writes are still pending + phid->outputPacketLen = 0; + + //set data arrays to unknown, initial states + for (i = 0; i<phid->phid.attr.encoder.numInputs; i++) + { + phid->inputState[i] = PUNK_BOOL; + } + for (i = 0; i<phid->phid.attr.encoder.numEncoders; i++) + { + phid->encoderPosition[i] = 0; //position set to zero on each attach + phid->encoderTimeStamp[i] = PUNK_INT; + phid->indexPosition[i] = PUNK_INT; + switch(phid->phid.deviceIDSpec) { + case PHIDID_ENCODER_HS_4ENCODER_4INPUT: + phid->enableState[i] = PUNK_BOOL; + phid->enableStateEcho[i] = PUNK_BOOL; + break; + case PHIDID_ENCODER_1ENCODER_1INPUT: + case PHIDID_ENCODER_HS_1ENCODER: + phid->enableState[i] = PTRUE; + phid->enableStateEcho[i] = PTRUE; + break; + default: + break; + } + } + + //send out any initial pre-read packets + switch(phid->phid.deviceIDSpec) { + case PHIDID_ENCODER_1ENCODER_1INPUT: + if (phid->phid.deviceVersion <= 100) + { + ZEROMEM(buffer,8); + LOG(PHIDGET_LOG_INFO,"Sending workaround startup packet"); + if ((result = CUSBSendPacket((CPhidgetHandle)phid, buffer)) != EPHIDGET_OK) + return result; + } + break; + case PHIDID_ENCODER_HS_1ENCODER: + case PHIDID_ENCODER_HS_4ENCODER_4INPUT: + default: + break; + } + + //issue a read on the 4-input HS to get enable states + switch(phid->phid.deviceIDSpec) { + case PHIDID_ENCODER_HS_4ENCODER_4INPUT: + if (phid->phid.deviceVersion >= 100 && phid->phid.deviceVersion < 200) + { + CPhidget_read((CPhidgetHandle)phid); + } + break; + case PHIDID_ENCODER_1ENCODER_1INPUT: + case PHIDID_ENCODER_HS_1ENCODER: + default: + break; + } + + //fill in enabled states + for (i = 0; i<phid->phid.attr.encoder.numEncoders; i++) + { + phid->enableState[i] = phid->enableStateEcho[i]; + } + + return EPHIDGET_OK; +} + +//dataInput - parses device packets +CPHIDGETDATA(Encoder) + int i=0,j=0; + + unsigned char input[ENCODER_MAXINPUTS]; + unsigned char indexTrue[ENCODER_MAXINPUTS]; + unsigned char enabledEcho[ENCODER_MAXINPUTS]; + short positionChange[ENCODER_MAXENCODERS]; + short indexChange[ENCODER_MAXENCODERS]; + int packetTime[ENCODER_MAXENCODERS]; + int curTime[ENCODER_MAXENCODERS]; + unsigned short timeChange = 0, timeStamp = 0; + int timeChangeInt[ENCODER_MAXENCODERS]; + + unsigned char lastInputState[ENCODER_MAXINPUTS]; + + if (length<0) return EPHIDGET_INVALIDARG; + TESTPTR(phid); + TESTPTR(buffer); + + ZEROMEM(input, sizeof(input)); + ZEROMEM(positionChange, sizeof(positionChange)); + ZEROMEM(lastInputState, sizeof(lastInputState)); + ZEROMEM(indexTrue, sizeof(indexTrue)); + ZEROMEM(indexChange, sizeof(indexChange)); + ZEROMEM(packetTime, sizeof(packetTime)); + ZEROMEM(curTime, sizeof(curTime)); + ZEROMEM(timeChangeInt, sizeof(timeChangeInt)); + + switch(phid->phid.deviceIDSpec) + { + case PHIDID_ENCODER_1ENCODER_1INPUT: + /* OLD version here... */ + if (phid->phid.deviceVersion < 110) + { + if (buffer[1] & 0x04) + input[0] = PFALSE; + else + input[0] = PTRUE; + + positionChange[0] = (signed char)buffer[2]; + + timeStamp = (((unsigned short)buffer[4]) << 8) + buffer[3]; + } + /* NEW version 1-encoder = 1.10+*/ + else if (phid->phid.deviceVersion >= 110 && phid->phid.deviceVersion < 300) + { + if (buffer[1] & 0x01) + input[0] = PFALSE; + else + input[0] = PTRUE; + + positionChange[0] = (signed char)buffer[4]; + + timeStamp = (((unsigned short)buffer[3]) << 8) + buffer[2]; + } + else + return EPHIDGET_UNEXPECTED; + break; + case PHIDID_ENCODER_HS_1ENCODER: + /* high speed encoder */ + if (phid->phid.deviceVersion >= 300 && phid->phid.deviceVersion < 400) { + + //this will work for 8 inputs before we need to change the protocol + //currently no high-speed encoder has any inputs + for (i = 0, j = 1; i < (phid->phid.attr.encoder.numInputs); i++, j<<=1) + { + if (buffer[1] & j) + input[i] = PFALSE; + else + input[i] = PTRUE; + } + + //this will work for two encoders before we need to change the protocol + for(i=0;i<phid->phid.attr.encoder.numEncoders;i++) { + positionChange[i] = (((unsigned short)buffer[2*i+5]) << 8) + buffer[2*i+4]; + } + + timeStamp = (((unsigned short)buffer[3]) << 8) + buffer[2]; + } + else + return EPHIDGET_UNEXPECTED; + break; + case PHIDID_ENCODER_HS_4ENCODER_4INPUT: + /* high speed encoder 4 input, with enable and index */ + if (phid->phid.deviceVersion >= 100 && phid->phid.deviceVersion < 200) + { + //this will work for two encoders before we need to change the protocol + for(i=0;i<phid->phid.attr.encoder.numEncoders;i++) { + positionChange[i] = (((unsigned short)buffer[10*i+0]) << 8) + buffer[10*i+1] - 0x7fff; + indexChange[i] = (((unsigned short)buffer[10*i+2]) << 8) + buffer[10*i+3] - 0x7fff; + packetTime[i] = (((int)buffer[10*i+4]) << 16) + (((unsigned short)buffer[10*i+5]) << 8) + buffer[10*i+6]; + curTime[i] = (((unsigned short)buffer[10*i+7]) << 8) + buffer[10*i+8]; + indexTrue[i] = (buffer[10*i+9] & 0x01) ? PTRUE : PFALSE; + enabledEcho[i] = (buffer[10*i+9] & 0x02) ? PTRUE : PFALSE; + if (buffer[10*i+9] & 0x04) + input[i] = PFALSE; + else + input[i] = PTRUE; + } + } + 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.encoder.numInputs; i++) + { + lastInputState[i] = phid->inputState[i]; + phid->inputState[i] = input[i]; + } + for(i=0; i < phid->phid.attr.encoder.numEncoders; i++) + { + //check for over/undershoots + if((positionChange[i] > 0 && (phid->encoderPosition[i] + positionChange[i]) < phid->encoderPosition[i]) + || (positionChange[i] < 0 && (phid->encoderPosition[i] + positionChange[i]) > phid->encoderPosition[i])) + { + char error_buffer[50]; + FIRE_ERROR(EEPHIDGET_WRAP, "Encoder %d position is wrapping around.", i); + } + + phid->encoderPosition[i] += positionChange[i]; + + //Different timing stuff for different devices + switch(phid->phid.deviceIDSpec) + { + case PHIDID_ENCODER_1ENCODER_1INPUT: + case PHIDID_ENCODER_HS_1ENCODER: + //this handles wraparounds because we're using unsigned shorts + timeChange = (timeStamp - phid->encoderTimeStamp[i]); + + //TODO: this misses the case where timeChange > 65 seconds - we would need to add timing to the library + if (timeChange > 30000 || phid->encoderTimeStamp[i] == PUNK_INT) + timeChangeInt[i] = PUNK_INT; + else + timeChangeInt[i] = timeChange; + + phid->encoderTimeStamp[i] = timeStamp; + break; + case PHIDID_ENCODER_HS_4ENCODER_4INPUT: + //timing + if (phid->encoderTimeStamp[i] == PUNK_INT) + phid->encoderTimeStamp[i] = 0; + + phid->encoderTimeStamp[i] += packetTime[i]; + + if(positionChange[i] != 0) + { + timeChangeInt[i] = phid->encoderTimeStamp[i] - curTime[i]; + // convert 1/12,000,000 to microseconds + timeChangeInt[i] = round(timeChangeInt[i] / 12.0); + + phid->encoderTimeStamp[i] = curTime[i]; + } + //index + if(indexTrue[i]) + phid->indexPosition[i] = phid->encoderPosition[i] - indexChange[i]; + //enabled echo + phid->enableStateEcho[i] = enabledEcho[i]; + break; + default: + return EPHIDGET_UNEXPECTED; + } + } + + //send out any events for changed data + for (i = 0; i < phid->phid.attr.encoder.numInputs; i++) + { + if(phid->inputState[i] != lastInputState[i]) + FIRE(InputChange, i, phid->inputState[i]); + } + for(i=0; i < phid->phid.attr.encoder.numEncoders; i++) + { + if(positionChange[i] != 0) + FIRE(PositionChange, i, timeChangeInt[i], positionChange[i]); + } + + return EPHIDGET_OK; +} + +//eventsAfterOpen - sends out an event for all valid data, used during attach initialization +CPHIDGETINITEVENTS(Encoder) + + for (i = 0; i < phid->phid.attr.encoder.numInputs; i++) + { + if (phid->inputState[i] != PUNK_BOOL) + { + FIRE(InputChange, i, phid->inputState[i]); + } + } + return EPHIDGET_OK; +} + +//getPacket - used by write thread to get the next packet to send to device +CGETPACKET_BUF(Encoder) + +//sendpacket - sends a packet to the device asynchronously, blocking if the 1-packet queue is full +CSENDPACKET_BUF(Encoder) + +//makePacket - constructs a packet using current device state +CMAKEPACKET(Encoder) + int i = 0, j = 0; + + TESTPTRS(phid, buffer); + + switch(phid->phid.deviceIDSpec) + { + case PHIDID_ENCODER_HS_4ENCODER_4INPUT: + if ((phid->phid.deviceVersion >= 100) && (phid->phid.deviceVersion < 200)) + { + //have to make sure that everything to be sent has some sort of default value if the user hasn't set a value + for (i = 0; i<phid->phid.attr.encoder.numEncoders; i++) + { + if (phid->enableState[i] == PUNK_BOOL) + phid->enableState[i] = PFALSE; + } + + //construct the packet + for (i = 0, j = 1; i < phid->phid.attr.encoder.numEncoders; i++, j<<=1) + { + if (phid->enableState[i]) + buffer[0] |= j; + } + } + else + return EPHIDGET_UNEXPECTED; + break; + case PHIDID_ENCODER_HS_1ENCODER: + case PHIDID_ENCODER_1ENCODER_1INPUT: + return EPHIDGET_UNSUPPORTED; //these versions don't have OUT packets + default: + return EPHIDGET_UNEXPECTED; + } + return EPHIDGET_OK; +} + +// === Exported Functions === // + +//create and initialize a device structure +CCREATE(Encoder, PHIDCLASS_ENCODER) + +//event setup functions +CFHANDLE(Encoder, InputChange, int, int) +CFHANDLE(Encoder, PositionChange, int, int, int) + +CGET(Encoder,InputCount,int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_ENCODER) + TESTATTACHED + + MASGN(phid.attr.encoder.numInputs) +} + +CGETINDEX(Encoder,InputState,int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_ENCODER) + TESTATTACHED + TESTINDEX(phid.attr.encoder.numInputs) + TESTMASGN(inputState[Index], PUNK_BOOL) + + MASGN(inputState[Index]) +} + +CGET(Encoder,EncoderCount,int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_ENCODER) + TESTATTACHED + + MASGN(phid.attr.encoder.numEncoders) +} + +CGETINDEX(Encoder,Position,int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_ENCODER) + TESTATTACHED + TESTINDEX(phid.attr.encoder.numEncoders) + + MASGN(encoderPosition[Index]) +} +CSETINDEX(Encoder,Position,int) + TESTPTR(phid) + TESTDEVICETYPE(PHIDCLASS_ENCODER) + TESTATTACHED + TESTINDEX(phid.attr.encoder.numEncoders) + + if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG)) + ADDNETWORKKEYINDEXED(ResetPosition, "%d", encoderPosition); + else + phid->encoderPosition[Index] = newVal; + + return EPHIDGET_OK; +} + +CGETINDEX(Encoder,IndexPosition,int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_ENCODER) + TESTATTACHED + TESTINDEX(phid.attr.encoder.numEncoders) + TESTMASGN(indexPosition[Index], PUNK_INT) + + MASGN(indexPosition[Index]) +} + +CGETINDEX(Encoder,Enabled,int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_ENCODER) + TESTATTACHED + TESTINDEX(phid.attr.encoder.numEncoders) + TESTMASGN(enableStateEcho[Index], PUNK_BOOL) + + MASGN(enableStateEcho[Index]) +} +CSETINDEX(Encoder,Enabled,int) + TESTPTR(phid) + TESTDEVICETYPE(PHIDCLASS_ENCODER) + TESTATTACHED + + //Only supported on the new 4-encoder board + switch(phid->phid.deviceIDSpec) { + case PHIDID_ENCODER_HS_4ENCODER_4INPUT: + TESTINDEX(phid.attr.encoder.numEncoders) + TESTRANGE(PFALSE, PTRUE) + if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG)) + ADDNETWORKKEYINDEXED(Enabled, "%d", enableState); + else + SENDPACKET(Encoder, enableState[Index]); + return EPHIDGET_OK; + case PHIDID_ENCODER_1ENCODER_1INPUT: + case PHIDID_ENCODER_HS_1ENCODER: + default: + return EPHIDGET_UNSUPPORTED; + } + +} + +// === Deprecated Functions === // + +CGETINDEX(Encoder,EncoderPosition,int) + return CPhidgetEncoder_getPosition(phid, Index, pVal); +} +CSETINDEX(Encoder,EncoderPosition,int) + return CPhidgetEncoder_setPosition(phid, Index, newVal); +} +CGET(Encoder,NumInputs,int) + return CPhidgetEncoder_getInputCount(phid, pVal); +} +CGET(Encoder,NumEncoders,int) + return CPhidgetEncoder_getEncoderCount(phid, pVal); +} |