From 0b624384cd52be20e61284551d832b499d7b7707 Mon Sep 17 00:00:00 2001 From: Jonathan McCrohan Date: Sat, 14 Apr 2012 12:56:48 +0100 Subject: Imported Upstream version 2.1.8.20120216 --- cphidgetmotorcontrol.c | 1022 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1022 insertions(+) create mode 100644 cphidgetmotorcontrol.c (limited to 'cphidgetmotorcontrol.c') diff --git a/cphidgetmotorcontrol.c b/cphidgetmotorcontrol.c new file mode 100644 index 0000000..73b1363 --- /dev/null +++ b/cphidgetmotorcontrol.c @@ -0,0 +1,1022 @@ +#include +#include +#include "stdafx.h" +#include "cphidgetmotorcontrol.h" +#include "cusb.h" +#include "csocket.h" +#include "cthread.h" + +// === Internal Functions === // + +//clearVars - sets all device variables to unknown state +CPHIDGETCLEARVARS(MotorControl) + int i = 0; + + phid->accelerationMax = PUNI_DBL; + phid->accelerationMin = PUNI_DBL; + + for (i = 0; iinputState[i] = PUNI_BOOL; + } + for (i = 0; imotorSpeedEcho[i] = PUNI_DBL; + phid->motorSensedCurrent[i] = PUNI_DBL; + phid->motorSensedBackEMF[i] = PUNI_DBL; + phid->motorSetSpeedEcho[i] = PUNK_DBL; + phid->motorAccelerationEcho[i] = PUNI_DBL; + phid->motorBrakingEcho[i] = PUNI_DBL; + phid->backEMFSensingStateEcho[i] = PUNI_BOOL; + + phid->motorSpeed[i] = PUNK_DBL; + phid->motorAcceleration[i] = PUNK_DBL; + phid->motorBraking[i] = PUNK_DBL; + phid->backEMFSensingState[i] = PUNK_BOOL; + phid->motorErrors[i] = 0; + } + phid->supplyVoltage = PUNI_DBL; + for(i = 0; iencoderPositionEcho[i] = 0; + phid->encoderTimeStamp[i] = 0; + phid->encoderPositionDelta[i] = 0; + } + for(i = 0; isensorRawValue[i] = PUNI_INT; + phid->sensorValue[i] = PUNI_INT; + } + phid->ratiometricEcho = PUNI_BOOL; + phid->ratiometric = PUNK_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(MotorControl) + int i = 0; + int readtries = 0; + + TESTPTR(phid); + + //Make sure no old writes are still pending + phid->outputPacketLen = 0; + + //Setup max/min values + switch(phid->phid.deviceIDSpec) + { + case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT: + case PHIDID_MOTORCONTROL_HC_2MOTOR: + if ((phid->phid.deviceVersion >= 100) && (phid->phid.deviceVersion < 200)) + { + phid->accelerationMax = 100; + phid->accelerationMin = round_double((1.0 / 10.23), 2); + } + else + return EPHIDGET_BADVERSION; + break; + case PHIDID_MOTORCONTROL_1MOTOR: + if ((phid->phid.deviceVersion >= 100) && (phid->phid.deviceVersion < 200)) + { + phid->accelerationMax = 100/0.016; //0-100 in 16ms + phid->accelerationMin = round_double(100/4.08, 2); //0-100 in 4.08 seconds + } + else + return EPHIDGET_BADVERSION; + break; + default: + return EPHIDGET_UNEXPECTED; + } + + //initialize triggers, set data arrays to unknown + for (i = 0; iphid.attr.motorcontrol.numInputs; i++) + { + phid->inputState[i] = PUNK_BOOL; + } + for (i = 0; iphid.attr.motorcontrol.numMotors; i++) + { + phid->motorSpeedEcho[i] = PUNK_DBL; + phid->motorSensedCurrent[i] = PUNK_DBL; + phid->motorSensedBackEMF[i] = PUNK_DBL; + phid->motorSetSpeedEcho[i] = PUNK_DBL; + phid->motorAccelerationEcho[i] = PUNK_DBL; + //set some defaults for devices that don't support these + switch(phid->phid.deviceIDSpec) + { + case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT: + case PHIDID_MOTORCONTROL_HC_2MOTOR: + phid->motorBrakingEcho[i] = 0; + phid->backEMFSensingStateEcho[i] = PFALSE; + break; + case PHIDID_MOTORCONTROL_1MOTOR: + phid->motorBrakingEcho[i] = PUNK_DBL; + phid->backEMFSensingStateEcho[i] = PUNK_BOOL; + break; + default: + break; + } + phid->motorErrors[i] = 0; + } + phid->supplyVoltage = PUNK_DBL; + phid->lastVoltage = PUNK_DBL; + for(i = 0; iphid.attr.motorcontrol.numEncoders; i++) + { + phid->encoderPositionEcho[i] = 0; + phid->encoderTimeStamp[i] = 0; + phid->encoderPositionDelta[i] = 0; + } + for(i = 0; iphid.attr.motorcontrol.numSensors; i++) + { + phid->sensorRawValue[i] = PUNK_INT; + phid->sensorValue[i] = PUNK_INT; + } + phid->ratiometricEcho = PUNK_BOOL; + + phid->lastPacketCount = PUNK_INT; + + //read in initial state + switch(phid->phid.deviceIDSpec) + { + case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT: + case PHIDID_MOTORCONTROL_1MOTOR: + readtries = 1; + break; + case PHIDID_MOTORCONTROL_HC_2MOTOR: + readtries = phid->phid.attr.motorcontrol.numMotors * 2; + break; + default: + return EPHIDGET_UNEXPECTED; + } + while(readtries-- > 0) + { + CPhidget_read((CPhidgetHandle)phid); + for (i = 0; iphid.attr.motorcontrol.numMotors; i++) + if(phid->motorSpeedEcho[i] == PUNK_DBL) + break; + if(i==phid->phid.attr.motorcontrol.numMotors) break; + } + + //recover what we can, set others to unknown + for (i = 0; iphid.attr.motorcontrol.numMotors; i++) + { + if(phid->motorSetSpeedEcho[i] != PUNK_DBL) + phid->motorSpeed[i] = phid->motorSetSpeedEcho[i]; + else + phid->motorSpeed[i] = phid->motorSpeedEcho[i]; + phid->motorAcceleration[i] = phid->motorAccelerationEcho[i]; + phid->motorBraking[i] = phid->motorBrakingEcho[i]; + phid->backEMFSensingState[i] = phid->backEMFSensingStateEcho[i]; + } + for(i = 0; iphid.attr.motorcontrol.numEncoders; i++) + { + phid->encoderPositionDelta[i] = phid->encoderPositionEcho[i]; + } + phid->ratiometric = phid->ratiometricEcho; + + return EPHIDGET_OK; +} + +//dataInput - parses device packets +CPHIDGETDATA(MotorControl) + int i = 0, j = 0; + + if (length < 0) return EPHIDGET_INVALIDARG; + TESTPTR(phid); + TESTPTR(buffer); + + switch(phid->phid.deviceIDSpec) + { + /* Original Motor controller */ + case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT: + if ((phid->phid.deviceVersion >= 100) && (phid->phid.deviceVersion < 200)) + { + double speed[MOTORCONTROL_MAXMOTORS]; + double lastSpeed[MOTORCONTROL_MAXMOTORS]; + unsigned char input[MOTORCONTROL_MAXINPUTS]; + unsigned char lastInputState[MOTORCONTROL_MAXINPUTS]; + unsigned char error[MOTORCONTROL_MAXINPUTS]; + + ZEROMEM(speed, sizeof(speed)); + ZEROMEM(lastSpeed, sizeof(lastSpeed)); + ZEROMEM(input, sizeof(input)); + ZEROMEM(lastInputState, sizeof(lastInputState)); + ZEROMEM(error, sizeof(error)); + + //Parse device packet - store data locally + for (i = 0, j = 1; i < phid->phid.attr.motorcontrol.numInputs; i++, j<<=1) + { + if (buffer[0] & j) + input[i] = PTRUE; + else + input[i] = PFALSE; + } + for (i = 0, j = 1; i < phid->phid.attr.motorcontrol.numMotors; i++, j<<=1) + { + speed[i] = (char)buffer[4 + i]; + speed[i] = round_double(((speed[i] * 100) / 127.0), 2); + + //errors + if (buffer[1] & j) + { + error[i] = PTRUE; + } + } + + //Make sure values are within defined range, and store to structure + for (i = 0; i < phid->phid.attr.motorcontrol.numInputs; i++) + { + lastInputState[i] = phid->inputState[i]; + phid->inputState[i] = input[i]; + } + for (i = 0; iphid.attr.motorcontrol.numMotors; i++) + { + lastSpeed[i] = phid->motorSpeedEcho[i]; + phid->motorSpeedEcho[i] = speed[i]; + } + + //send out any events for changed data + for (i = 0; i < phid->phid.attr.motorcontrol.numInputs; i++) + { + if(phid->inputState[i] != PUNK_BOOL && phid->inputState[i] != lastInputState[i]) + FIRE(InputChange, i, phid->inputState[i]); + } + for (i = 0; iphid.attr.motorcontrol.numMotors; i++) + { + if(phid->motorSpeedEcho[i] != PUNK_DBL && phid->motorSpeedEcho[i] != lastSpeed[i]) + { + FIRE(VelocityChange, i, phid->motorSpeedEcho[i]); + //Deprecated + FIRE(MotorChange, i, phid->motorSpeedEcho[i]); + } + if(error[i]) + { + char error_buffer[50]; + FIRE_ERROR(EEPHIDGET_OVERCURRENT, "Motor %d exceeded 1.5 Amp current limit.", i); + } + } + + } + else + return EPHIDGET_UNEXPECTED; + break; + /* HC motor controller - packets are indexed */ + case PHIDID_MOTORCONTROL_HC_2MOTOR: + if ((phid->phid.deviceVersion >= 100) && (phid->phid.deviceVersion < 200)) + { + int index = 0; + int error[MOTORCONTROL_MAXINPUTS]; + double speed = 0, current = 0; + double lastSpeed = 0, lastCurrent = 0; + + ZEROMEM(error, sizeof(error)); + + //Parse device packet - store data locally + index = buffer[3]; + + speed = (signed char)buffer[4]; + speed = round_double(((speed * 100) / 127.0), 2); + + //NOTE: current sense is only accurate at +100 and -100 velocity + current = (unsigned int)(((unsigned char)buffer[6] << 8) | (unsigned char)buffer[7]); + current -= 5; + if(current < 0) current = 0; + current /= 51.2; //volts + current = (current * 11370) / 1500; //amps + + if (!(buffer[1] & 0x10)) error[0] |= 0x01; + if (!(buffer[1] & 0x20)) error[1] |= 0x01; + if (!(buffer[1] & 0x40)) error[0] |= 0x02; + if (!(buffer[1] & 0x80)) error[1] |= 0x02; + + //Make sure values are within defined range, and store to structure + lastSpeed = phid->motorSpeedEcho[index]; + phid->motorSpeedEcho[index] = speed; + lastCurrent = phid->motorSensedCurrent[index]; + phid->motorSensedCurrent[index] = current; + + //send out any events for changed data + if(phid->motorSpeedEcho[index] != PUNK_DBL && phid->motorSpeedEcho[index] != lastSpeed) + { + FIRE(VelocityChange, index, phid->motorSpeedEcho[index]); + //Deprecated + FIRE(MotorChange, index, phid->motorSpeedEcho[index]); + } + if(phid->motorSensedCurrent[index] != PUNK_DBL && phid->motorSensedCurrent[index] != lastCurrent) + FIRE(CurrentChange, index, phid->motorSensedCurrent[index]); + if(phid->motorSensedCurrent[index] != PUNK_DBL) + FIRE(CurrentUpdate, index, phid->motorSensedCurrent[index]); + for (i = 0; iphid.attr.motorcontrol.numMotors; i++) + { + char error_buffer[50]; + //There are two error conditions - but as far as I can tell they both mean the same thing + if(error[i]) + { + FIRE_ERROR(EEPHIDGET_OVERTEMP, "Motor %d overtemperature or short detected.", i); + } + } + } + else + return EPHIDGET_UNEXPECTED; + break; + /* 1-Motor controller with encoder and sensor inputs */ + case PHIDID_MOTORCONTROL_1MOTOR: + if ((phid->phid.deviceVersion >= 100) && (phid->phid.deviceVersion < 200)) + { + int i=0,j=0; + double speed = 0, current = 0, backEmf = 0, backEmfPos = 0, backEmfNeg = 0, supplyVoltage = 0, setSpeed = 0, setAccel = 0, setBraking = 0; + double lastSpeed = 0, lastCurrent = 0; + int encoderPos = 0, timeChangeInt = 0; + unsigned short encoderTimeChange = 0, encoderTime = 0; + int packetCount = 0; + unsigned char ratioEn = 0, dir = 0, bEMFValid = 0, bEMFenabled = 0, lowVoltageErr = 0, overTempErr = 0, setDir = 0; + int sensor[MOTORCONTROL_MAXSENSORS], sensorRaw[MOTORCONTROL_MAXSENSORS]; + unsigned char input[MOTORCONTROL_MAXINPUTS]; + unsigned char lastInputState[MOTORCONTROL_MAXINPUTS]; + int encoderPositionChange = 0; + char error_buffer[128]; + ZEROMEM(sensorRaw, sizeof(sensorRaw)); + ZEROMEM(sensor, sizeof(sensor)); + ZEROMEM(input, sizeof(input)); + ZEROMEM(lastInputState, sizeof(lastInputState)); + + //Read in data + for (i = 0, j = 1; i < phid->phid.attr.motorcontrol.numInputs; i++, j<<=1) + { + if (buffer[0] & j) + input[i] = PTRUE; + else + input[i] = PFALSE; + } + + overTempErr = ((buffer[1] & 0x01) ? PTRUE : PFALSE); + lowVoltageErr = ((buffer[1] & 0x02) ? PTRUE : PFALSE); + bEMFValid = ((buffer[1] & 0x04) ? PTRUE : PFALSE); + dir = ((buffer[1] & 0x08) ? PTRUE : PFALSE); + ratioEn = ((buffer[1] & 0x10) ? PTRUE : PFALSE); + bEMFenabled = ((buffer[1] & 0x20) ? PTRUE : PFALSE); + setDir = ((buffer[1] & 0x40) ? PTRUE : PFALSE); + + for (i = 0; i < phid->phid.attr.motorcontrol.numSensors; i++) + { + sensorRaw[i] = ((unsigned char)buffer[i*2 + 4] << 8) + (unsigned char)buffer[i*2 + 5]; + sensorRaw[i] = round(sensorRaw[i] * 1.001); + if(sensorRaw[i] > 0xfff) + sensorRaw[i] = 0xfff; + sensor[i] = round((double)sensorRaw[i] / 4.095); + } + + //TODO: check this calculation + current = (unsigned int)(((unsigned char)buffer[2] << 8) + (unsigned char)buffer[3]); + current = ((current * 5) / 4096.0); //voltage on adc + current = (current / 150.0); //Current through resistor (0.24% of current through motor) + current = round_double(current / 0.0024, 3); //Normalized Current + + backEmfPos = (unsigned int)(((unsigned char)buffer[10] << 8) + (unsigned char)buffer[11]); + backEmfNeg = (unsigned int)(((unsigned char)buffer[12] << 8) + (unsigned char)buffer[13]); + backEmf = backEmfPos - backEmfNeg; + backEmf = round_double(((backEmf * 80) / 4096.0), 3); + + supplyVoltage = (unsigned int)(((unsigned char)buffer[8] << 8) + (unsigned char)buffer[9]); + supplyVoltage = round_double(((supplyVoltage * 80) / 2048.0), 3); //80v full scale on ADC + + //LOG(PHIDGET_LOG_DEBUG, "Supply Voltage: %0.3lf", supplyVoltage); + + speed = ((unsigned char)buffer[20] / 255.0) * 100.0; + if(!dir) + speed = -speed; + + setSpeed = ((unsigned char)buffer[21] / 255.0) * 100.0; + if(!setDir) + setSpeed = -setSpeed; + + setAccel = ((unsigned char)buffer[22] / 255.0) * phid->accelerationMax; + setBraking = ((unsigned char)buffer[23] / 255.0) * 100.0; + + packetCount = (unsigned char)buffer[24]; + + encoderPos = (signed int)(((unsigned char)buffer[14] << 24) + + ((unsigned char)buffer[15] << 16) + + ((unsigned char)buffer[16] << 8) + + (unsigned char)buffer[17]); + + encoderTime = (unsigned int)(((unsigned char)buffer[18] << 8) + (unsigned char)buffer[19]); + + //Do stuff with the data + //Make sure values are within defined range, and store to structure + for (i = 0; i < phid->phid.attr.motorcontrol.numInputs; i++) + { + lastInputState[i] = phid->inputState[i]; + phid->inputState[i] = input[i]; + } + + lastSpeed = phid->motorSpeedEcho[0]; + phid->motorSpeedEcho[0] = speed; + + lastCurrent = phid->motorSensedCurrent[0]; + phid->motorSensedCurrent[0] = current; + + phid->motorSetSpeedEcho[0] = setSpeed; + phid->motorAccelerationEcho[0] = setAccel; + phid->motorBrakingEcho[0] = setBraking; + + if(bEMFValid) + phid->motorSensedBackEMF[0] = backEmf; + if(!bEMFenabled) + phid->motorSensedBackEMF[0] = PUNK_DBL; + phid->backEMFSensingStateEcho[0] = bEMFenabled; + + phid->supplyVoltage = supplyVoltage; + + //check for over/undershoots on encoder + if(((int)(phid->encoderPositionEcho[0] - phid->encoderPositionDelta[0]) > 2000000000 + && (int)(encoderPos - phid->encoderPositionDelta[0]) < -2000000000) + || ((int)(phid->encoderPositionEcho[0] - phid->encoderPositionDelta[0]) < -2000000000 + && (int)(encoderPos - phid->encoderPositionDelta[0]) > 2000000000)) + { + char error_buffer[50]; + FIRE_ERROR(EEPHIDGET_WRAP, "Encoder position is wrapping around."); + } + + encoderPositionChange = encoderPos - phid->encoderPositionEcho[0]; + phid->encoderPositionEcho[0] = encoderPos; + + //this handles wraparounds because we're using unsigned shorts + encoderTimeChange = (encoderTime - phid->encoderTimeStamp[0]); + + //timeout is 20 seconds + if (encoderTimeChange > 60000 || phid->encoderTimeStamp[0] == PUNK_INT) + timeChangeInt = PUNK_INT; + else + timeChangeInt = encoderTimeChange; + + phid->encoderTimeStamp[0] = encoderTime; + + for (i = 0; i < phid->phid.attr.motorcontrol.numSensors; i++) + { + phid->sensorValue[i] = sensor[i]; + phid->sensorRawValue[i] = sensorRaw[i]; + } + phid->ratiometricEcho = ratioEn; + + //Send out events + for (i = 0; i < phid->phid.attr.motorcontrol.numInputs; i++) + { + if(phid->inputState[i] != PUNK_BOOL && phid->inputState[i] != lastInputState[i]) + FIRE(InputChange, i, phid->inputState[i]); + } + if(phid->motorSpeedEcho[0] != PUNK_DBL && phid->motorSpeedEcho[0] != lastSpeed) + { + FIRE(VelocityChange, 0, phid->motorSpeedEcho[0]); + //Deprecated + FIRE(MotorChange, 0, phid->motorSpeedEcho[0]); + } + if(phid->motorSensedCurrent[0] != PUNK_DBL && phid->motorSensedCurrent[0] != lastCurrent) + FIRE(CurrentChange, 0, phid->motorSensedCurrent[0]); + + if(phid->motorSensedCurrent[0] != PUNK_DBL) + FIRE(CurrentUpdate, 0, phid->motorSensedCurrent[0]); + + for (i = 0; i < phid->phid.attr.motorcontrol.numSensors; i++) + FIRE(SensorUpdate, i, phid->sensorValue[i]); + + if(encoderPositionChange != 0) + FIRE(EncoderPositionChange, 0, timeChangeInt, encoderPositionChange); + FIRE(EncoderPositionUpdate, 0, encoderPositionChange); + + if(bEMFValid) + FIRE(BackEMFUpdate, 0, backEmf); + + //Error Events: + + //This is true when MC33931 status pin is LOW (active) + // could be under-voltage lockout, over-temperature, or short-circuit + // over-temperature and short-circuit conditions need to be cleared by toggling D1 or D2 + if(overTempErr && !(phid->motorErrors[0] & MOTORCONTROL_ERRORFLAG_OVERTEMP)) + { + phid->motorErrors[0] |= MOTORCONTROL_ERRORFLAG_OVERTEMP; + FIRE_ERROR(EEPHIDGET_OVERTEMP, "Over-temperature / Short-circuit condition detected."); + } + else if(!overTempErr && (phid->motorErrors[0] & MOTORCONTROL_ERRORFLAG_OVERTEMP)) + { + phid->motorErrors[0] &= ~MOTORCONTROL_ERRORFLAG_OVERTEMP; + FIRE_ERROR(EEPHIDGET_OK, "Over-temperature / Short-circuit condition ended."); + } + //if supply voltage is >5v, then we have likely already recovered from this error. This happens when the power is first plugged in + //This error happens when the power supply voltage falls, not when it starts low + if(lowVoltageErr && supplyVoltage < 5 && !(phid->motorErrors[0] & MOTORCONTROL_ERRORFLAG_UNDERVOLTAGE_LOCKOUT)) + { + phid->motorErrors[0] |= MOTORCONTROL_ERRORFLAG_UNDERVOLTAGE_LOCKOUT; + FIRE_ERROR(EEPHIDGET_BADPOWER, "Under-voltage Lockout condition detected. Supply voltage is %0.2lfv", supplyVoltage); + } + else if(!lowVoltageErr && (phid->motorErrors[0] & MOTORCONTROL_ERRORFLAG_UNDERVOLTAGE_LOCKOUT)) + { + phid->motorErrors[0] &= ~MOTORCONTROL_ERRORFLAG_UNDERVOLTAGE_LOCKOUT; + FIRE_ERROR(EEPHIDGET_OK, "Under-voltage Lockout condition ended."); + } + //Errors I detect - not error flags from the Motor Control IC: + if(supplyVoltage < 1 && !(phid->motorErrors[0] & MOTORCONTROL_ERRORFLAG_NOPOWER)) + { + phid->motorErrors[0] |= MOTORCONTROL_ERRORFLAG_NOPOWER; + FIRE_ERROR(EEPHIDGET_BADPOWER, "Power supply is unplugged or non-functional. Supply voltage is %0.2lfv", supplyVoltage); + } + else if(supplyVoltage <= 7 && !(phid->motorErrors[0] & MOTORCONTROL_ERRORFLAG_LOWPOWER)) + { + phid->motorErrors[0] |= MOTORCONTROL_ERRORFLAG_LOWPOWER; + FIRE_ERROR(EEPHIDGET_BADPOWER, "Power supply voltage is too low. Supply voltage is %0.2lfv", supplyVoltage); + } + if(supplyVoltage >= 40 && !(phid->motorErrors[0] & MOTORCONTROL_ERRORFLAG_DANGERPOWER)) + { + phid->motorErrors[0] |= MOTORCONTROL_ERRORFLAG_DANGERPOWER; + FIRE_ERROR(EEPHIDGET_BADPOWER, "DANGER: Over-voltage condition, braking motor. Supply voltage is %0.2lfv", supplyVoltage); + } + else if(supplyVoltage >= 34 && !(phid->motorErrors[0] & MOTORCONTROL_ERRORFLAG_HIGHPOWER)) + { + phid->motorErrors[0] |= MOTORCONTROL_ERRORFLAG_HIGHPOWER; + FIRE_ERROR(EEPHIDGET_BADPOWER, "Power supply voltage is too high. Supply voltage is %0.2lfv", supplyVoltage); + } + if(supplyVoltage > 7.5 && supplyVoltage < 33 + && (phid->motorErrors[0] & (MOTORCONTROL_ERRORFLAG_NOPOWER | MOTORCONTROL_ERRORFLAG_LOWPOWER | MOTORCONTROL_ERRORFLAG_HIGHPOWER | MOTORCONTROL_ERRORFLAG_DANGERPOWER))) + { + phid->motorErrors[0] &= ~(MOTORCONTROL_ERRORFLAG_NOPOWER | MOTORCONTROL_ERRORFLAG_LOWPOWER | MOTORCONTROL_ERRORFLAG_HIGHPOWER | MOTORCONTROL_ERRORFLAG_DANGERPOWER); + FIRE_ERROR(EEPHIDGET_OK, "Power supply voltage has returned to valid range. Supply voltage is %0.2lfv", supplyVoltage); + } + + if((phid->lastPacketCount != PUNK_INT) && ((phid->lastPacketCount+1)&0xFF) != packetCount) + { + FIRE_ERROR_NOQUEUE(EEPHIDGET_PACKETLOST, "One or more data packets were lost"); + } + phid->lastPacketCount = packetCount; + } + else + return EPHIDGET_UNEXPECTED; + break; + default: + return EPHIDGET_UNEXPECTED; + + } + + return EPHIDGET_OK; +} + +//eventsAfterOpen - sends out an event for all valid data, used during attach initialization +CPHIDGETINITEVENTS(MotorControl) + + for (i = 0; iphid.attr.motorcontrol.numInputs; i++) + { + if(phid->inputState[i] != PUNK_BOOL) + FIRE(InputChange, i, phid->inputState[i]); + } + for (i = 0; iphid.attr.motorcontrol.numMotors; i++) + { + if(phid->motorSpeedEcho[i] != PUNK_DBL) + { + FIRE(VelocityChange, i, phid->motorSpeedEcho[i]); + //Deprecated + FIRE(MotorChange, i, phid->motorSpeedEcho[i]); + } + if(phid->motorSensedCurrent[i] != PUNK_DBL && phid->motorSensedCurrent[i] != PUNI_DBL) + FIRE(CurrentChange, i, phid->motorSensedCurrent[i]); + } + + //Note: don't send BackEMFUpdate, SensorUpdate, CurrentUpdate events - they come at a set rate. + + return EPHIDGET_OK; +} + +//getPacket - used by write thread to get the next packet to send to device +CGETPACKET_BUF(MotorControl) + +//sendpacket - sends a packet to the device asynchronously, blocking if the 1-packet queue is full +CSENDPACKET_BUF(MotorControl) + +//makePacket - constructs a packet using current device state +CMAKEPACKETINDEXED(MotorControl) + int velocity = 0, accel = 0, braking = 0; + unsigned char dir = 0; + + TESTPTRS(phid, buffer); + + switch(phid->phid.deviceIDSpec) + { + case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT: + case PHIDID_MOTORCONTROL_HC_2MOTOR: + 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 + if(phid->motorSpeed[Index] == PUNK_DBL) + phid->motorSpeed[Index] = 0; //not moving + if(phid->motorAcceleration[Index] == PUNK_DBL) + phid->motorAcceleration[Index] = phid->accelerationMax / 2; //mid-range + + velocity = (int)round((phid->motorSpeed[Index] * 127.0) / 100.0); + accel = (int)round(phid->motorAcceleration[Index] * 10.23); + + buffer[0] = (unsigned char)Index; + buffer[1] = (unsigned char)(velocity & 0xff); + buffer[2] = (unsigned char)((accel >> 8) & 0x0f); + buffer[3] = (unsigned char)(accel & 0xff); + } + else + return EPHIDGET_UNEXPECTED; + break; + case PHIDID_MOTORCONTROL_1MOTOR: + 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 + if(phid->motorSpeed[Index] == PUNK_DBL) + phid->motorSpeed[Index] = 0; //not moving + if(phid->motorBraking[Index] == PUNK_DBL) + phid->motorBraking[Index] = 0; //not braking + if(phid->motorAcceleration[Index] == PUNK_DBL) + phid->motorAcceleration[Index] = phid->accelerationMax / 2; //mid-range + if(phid->motorSpeed[Index] >= 0) + { + velocity = (int)round((phid->motorSpeed[Index] * 255.0) / 100.0); + dir = 1; + } + else + { + velocity = (int)round((-phid->motorSpeed[Index] * 255.0) / 100.0); + dir = 0; + } + accel = (int)round((phid->motorAcceleration[Index] / phid->accelerationMax) * 255); + braking = (int)round((phid->motorBraking[Index] * 255.0) / 100.0); + if(phid->ratiometric == PUNK_BOOL) + phid->ratiometric = PTRUE; + if(phid->backEMFSensingState[0] == PUNK_BOOL) + phid->backEMFSensingState[0] = PFALSE; + + buffer[0] = (unsigned char)(dir | (phid->ratiometric==PTRUE?0x04:0x00) | (phid->backEMFSensingState[0]==PTRUE?0x02:0x00)); + buffer[1] = (unsigned char)(velocity & 0xff); + buffer[2] = (unsigned char)(accel & 0xff); + buffer[3] = (unsigned char)(braking & 0xff); + } + else + return EPHIDGET_UNEXPECTED; + break; + default: + return EPHIDGET_UNEXPECTED; + } + + return EPHIDGET_OK; +} + +// === Exported Functions === // + +//create and initialize a device structure +CCREATE(MotorControl, PHIDCLASS_MOTORCONTROL) + +//event setup functions +CFHANDLE(MotorControl, InputChange, int, int) +CFHANDLE(MotorControl, VelocityChange, int, double) +CFHANDLE(MotorControl, CurrentChange, int, double) +CFHANDLE(MotorControl, EncoderPositionChange, int, int, int) +CFHANDLE(MotorControl, EncoderPositionUpdate, int, int) +CFHANDLE(MotorControl, BackEMFUpdate, int, double) +CFHANDLE(MotorControl, SensorUpdate, int, int) +CFHANDLE(MotorControl, CurrentUpdate, int, double) + +CGET(MotorControl,MotorCount,int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + + MASGN(phid.attr.motorcontrol.numMotors) +} + +CGETINDEX(MotorControl,Velocity,double) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numMotors) + TESTMASGN(motorSpeedEcho[Index], PUNK_DBL) + + MASGN(motorSpeedEcho[Index]) +} +CSETINDEX(MotorControl,Velocity,double) + TESTPTR(phid) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numMotors) + TESTRANGE(-100, 100) + + if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG)) + ADDNETWORKKEYINDEXED(Velocity, "%lE", motorSpeed); + else + SENDPACKETINDEXED(MotorControl, motorSpeed[Index], Index); + + return EPHIDGET_OK; +} + +CGETINDEX(MotorControl,Acceleration,double) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numMotors) + TESTMASGN(motorAcceleration[Index], PUNK_DBL) + + MASGN(motorAcceleration[Index]) +} +CSETINDEX(MotorControl,Acceleration,double) + TESTPTR(phid) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numMotors) + TESTRANGE(phid->accelerationMin, phid->accelerationMax) + + if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG)) + ADDNETWORKKEYINDEXED(Acceleration, "%lE", motorAcceleration); + else + SENDPACKETINDEXED(MotorControl, motorAcceleration[Index], Index); + + return EPHIDGET_OK; +} + +CGETINDEX(MotorControl,AccelerationMax,double) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numMotors) + TESTMASGN(accelerationMax, PUNK_DBL) + + MASGN(accelerationMax) +} + +CGETINDEX(MotorControl,AccelerationMin,double) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numMotors) + TESTMASGN(accelerationMin, PUNK_DBL) + + MASGN(accelerationMin) +} + +CGETINDEX(MotorControl,Current,double) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + + //Only supported on HC/1-motor + switch(phid->phid.deviceIDSpec) + { + case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT: + return EPHIDGET_UNSUPPORTED; + case PHIDID_MOTORCONTROL_HC_2MOTOR: + case PHIDID_MOTORCONTROL_1MOTOR: + TESTINDEX(phid.attr.motorcontrol.numMotors) + TESTMASGN(motorSensedCurrent[Index], PUNK_DBL) + MASGN(motorSensedCurrent[Index]) + default: + return EPHIDGET_UNEXPECTED; + } +} + +CGETINDEX(MotorControl,BackEMFSensingState,int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numMotors) + TESTMASGN(backEMFSensingStateEcho[Index], PUNK_BOOL) + + //Note: will return false on devices that don't support backEMF sensing + MASGN(backEMFSensingStateEcho[Index]) +} +CSETINDEX(MotorControl,BackEMFSensingState,int) + TESTPTR(phid) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numMotors) + TESTRANGE(PFALSE, PTRUE) + + //Only supported on 1-motor + switch(phid->phid.deviceIDSpec) + { + case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT: + case PHIDID_MOTORCONTROL_HC_2MOTOR: + return EPHIDGET_UNSUPPORTED; + case PHIDID_MOTORCONTROL_1MOTOR: + if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG)) + ADDNETWORKKEYINDEXED(BackEMFState, "%d", backEMFSensingState); + else + SENDPACKETINDEXED(MotorControl, backEMFSensingState[Index], Index); + break; + default: + return EPHIDGET_UNEXPECTED; + } + + return EPHIDGET_OK; +} + +CGETINDEX(MotorControl,BackEMF,double) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + + //Only supported on 1-motor + switch(phid->phid.deviceIDSpec) + { + case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT: + case PHIDID_MOTORCONTROL_HC_2MOTOR: + return EPHIDGET_UNSUPPORTED; + case PHIDID_MOTORCONTROL_1MOTOR: + TESTINDEX(phid.attr.motorcontrol.numMotors) + TESTMASGN(motorSensedBackEMF[Index], PUNK_DBL) + MASGN(motorSensedBackEMF[Index]) + default: + return EPHIDGET_UNEXPECTED; + } +} + +CGET(MotorControl,SupplyVoltage,double) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + + //Only supported on 1-motor + switch(phid->phid.deviceIDSpec) + { + case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT: + case PHIDID_MOTORCONTROL_HC_2MOTOR: + return EPHIDGET_UNSUPPORTED; + case PHIDID_MOTORCONTROL_1MOTOR: + TESTMASGN(supplyVoltage, PUNK_DBL) + MASGN(supplyVoltage) + default: + return EPHIDGET_UNEXPECTED; + } +} + +CGETINDEX(MotorControl,Braking,double) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numMotors) + TESTMASGN(motorBrakingEcho[Index], PUNK_DBL) + + MASGN(motorBrakingEcho[Index]) +} +CSETINDEX(MotorControl,Braking,double) + TESTPTR(phid) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numMotors) + TESTRANGE(0, 100) + + //Only supported on 1-motor + switch(phid->phid.deviceIDSpec) + { + case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT: + case PHIDID_MOTORCONTROL_HC_2MOTOR: + return EPHIDGET_UNSUPPORTED; + case PHIDID_MOTORCONTROL_1MOTOR: + if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG)) + ADDNETWORKKEYINDEXED(Braking, "%lE", motorBraking); + else + SENDPACKETINDEXED(MotorControl, motorBraking[Index], Index); + break; + default: + return EPHIDGET_UNEXPECTED; + } + + return EPHIDGET_OK; +} + +CGET(MotorControl,InputCount,int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + + MASGN(phid.attr.motorcontrol.numInputs) +} + +CGETINDEX(MotorControl,InputState,int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numInputs) + TESTMASGN(inputState[Index], PUNK_BOOL) + + MASGN(inputState[Index]) +} + +CGET(MotorControl,EncoderCount,int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + + MASGN(phid.attr.motorcontrol.numEncoders) +} + +CGETINDEX(MotorControl,EncoderPosition,int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numEncoders) + + *pVal = (phid->encoderPositionEcho[Index] - phid->encoderPositionDelta[Index]); + return EPHIDGET_OK; +} +CSETINDEX(MotorControl,EncoderPosition,int) + TESTPTR(phid) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numEncoders) + + if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG)) + { + //newVal = phid->encoderPositionEcho[Index] + newVal; + ADDNETWORKKEYINDEXED(ResetEncoderPosition, "%d", encoderPositionDelta); + } + else + phid->encoderPositionDelta[Index] = phid->encoderPositionEcho[Index] + newVal; + + return EPHIDGET_OK; +} + +CGET(MotorControl, SensorCount, int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + + MASGN(phid.attr.motorcontrol.numSensors) +} + +CGETINDEX(MotorControl, SensorValue, int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numSensors) + TESTMASGN(sensorValue[Index], PUNK_INT) + + MASGN(sensorValue[Index]) +} + +CGETINDEX(MotorControl, SensorRawValue, int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + TESTINDEX(phid.attr.motorcontrol.numSensors) + TESTMASGN(sensorRawValue[Index], PUNK_INT) + + MASGN(sensorRawValue[Index]) +} +CGET(MotorControl, Ratiometric, int) + TESTPTRS(phid,pVal) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + + //Only supported on 1-motor + switch(phid->phid.deviceIDSpec) + { + case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT: + case PHIDID_MOTORCONTROL_HC_2MOTOR: + return EPHIDGET_UNSUPPORTED; + case PHIDID_MOTORCONTROL_1MOTOR: + TESTMASGN(ratiometricEcho, PUNK_BOOL) + MASGN(ratiometricEcho) + default: + return EPHIDGET_UNEXPECTED; + } +} +CSET(MotorControl, Ratiometric, int) + TESTPTR(phid) + TESTDEVICETYPE(PHIDCLASS_MOTORCONTROL) + TESTATTACHED + + //Only supported on 1-motor + switch(phid->phid.deviceIDSpec) + { + case PHIDID_MOTORCONTROL_LV_2MOTOR_4INPUT: + case PHIDID_MOTORCONTROL_HC_2MOTOR: + return EPHIDGET_UNSUPPORTED; + case PHIDID_MOTORCONTROL_1MOTOR: + if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG)) + ADDNETWORKKEY(Ratiometric, "%d", ratiometric); + else + SENDPACKETINDEXED(MotorControl, ratiometric, 0); + break; + default: + return EPHIDGET_UNEXPECTED; + } + + return EPHIDGET_OK; +} + +// === Deprecated Functions === // + +CFHANDLE(MotorControl, MotorChange, int, double) +CGETINDEX(MotorControl,MotorSpeed,double) + return CPhidgetMotorControl_getVelocity(phid, Index, pVal); +} +CSETINDEX(MotorControl,MotorSpeed,double) + return CPhidgetMotorControl_setVelocity(phid, Index, newVal); +} +CGET(MotorControl,NumMotors,int) + return CPhidgetMotorControl_getMotorCount(phid, pVal); +} +CGET(MotorControl,NumInputs,int) + return CPhidgetMotorControl_getInputCount(phid, pVal); +} -- cgit v1.2.3