Cubic Hand project for EECS 249A course.
Dependencies: MMA8451Q TSI cc3000_hostdriver_mbedsocket NVIC_set_all_priorities mbed Multi_WS2811
DataGlove.cpp
- Committer:
- kalbers
- Date:
- 2014-12-11
- Revision:
- 46:a7c041aaf259
- Parent:
- 42:4e1aaab14886
- Child:
- 47:6d0d7bafcb6f
File content as of revision 46:a7c041aaf259:
#include "DataGlove.h" #define STREAM_FINGERS_QUATERNION 1 #define STREAM_QUATERNION 2 #define STREAM_FINGERS_RAW 3 #define STREAM_RAW 4 #define STREAM_FINGERS 5 DataGlove::DataGlove() { notConnectedCount = 0; ReceiveCount = 0; numReceived = 0; NewData = false; } DataGlove::~DataGlove() { } void DataGlove::Init() { GloveSocket.Init(); GloveSocket.Connect(); StreamData(0); StreamData(STREAM_FINGERS_QUATERNION); } void DataGlove::StreamData(uint8_t DataType) { SendBuf[0] = '$'; SendBuf[1] = 0x0A; SendBuf[2] = 0x03; // Streaming quaternion data SendBuf[3] = DataType; SendBuf[4] = (SendBuf[0]+SendBuf[1]+SendBuf[2]+SendBuf[3])%256; SendBuf[5] = '#'; GloveSocket.SendDataToGlove(SendBuf, 6); } void DataGlove::StopSampling() { SendBuf[0] = '$'; SendBuf[1] = 0x0B; SendBuf[2] = 0x02; // Streaming quaternion data SendBuf[3] = (SendBuf[0]+SendBuf[1]+SendBuf[2])%256; SendBuf[4] = '#'; GloveSocket.SendDataToGlove(SendBuf, 6); } Glove DataGlove::ReceiveBlocking() { while(Receive() == false); return gl; } bool DataGlove::Receive() { //raw = false; numReceived += GloveSocket.GetDataFromBuffer(Buf, 1); //if(ReceiveCount++%25 == 0) printf("%d %c\r\n", ReceiveCount, Buf[0]); if (Buf[0] == '$' && numReceived > 0) { bcc = '$'; numReceived += GloveSocket.GetDataFromBuffer((Buf+numReceived), 2); if ((numReceived > 2) && (Buf[1] == 0x0a)) { pkglen = Buf[2]; bcc += Buf[1]; bcc += Buf[2]; numReceived += GloveSocket.GetDataFromBuffer((Buf+numReceived), pkglen - numReceived + 3); if ((numReceived - 3 )< pkglen) { if(numReceived == 0) notConnectedCount++; return false; } for (u = 3; u <= pkglen; u++) { bcc += Buf[u]; } if ((numReceived - 3 == pkglen) && (bcc == Buf[pkglen+1])) { //memcpy(buffer, (Buf + 3), numReceived - 3); /*timeOfArrival = (buffer[3] << 24) + (buffer[4] << 16) + (buffer[5] << 8) + buffer[6]; id = (buffer[1] << 8) + buffer[2]; pkgtype = buffer[0];*/ timeOfArrival = (Buf[6] << 24) + (Buf[7] << 16) + (Buf[8] << 8) + Buf[9]; id = (Buf[4] << 8) + Buf[5]; pkgtype = Buf[3]; ReceiveCount++; switch (pkgtype){ case STREAM_FINGERS_QUATERNION: { ExtractFingersQuat(); } break; case STREAM_QUATERNION: { ExtractQuat(); } break; case STREAM_FINGERS_RAW: { ExtractFingersRaw(); } break; case STREAM_RAW: { ExtractRaw(); } break; case STREAM_FINGERS: { ExtractFingers(); } break; } NewData = true; Buf[0] = 0; //Clear Token so no repeats. notConnectedCount = 0; numReceived = 0; return true; } else { if (bcc!=Buf[pkglen+1]) notConnectedCount++; } } } else { //printf("couldn't get data %d\r\n", notConnectedCount); // Re-establishing communication in case no data is received for 1s (20 frames per second*1 = 20) if (notConnectedCount > 200) { //printf("Connection broke! Trying to re-establish... %d %c\r\n",numReceived,Buf[0]); //GloveSocket.Reconnect(); StreamData(STREAM_FINGERS_QUATERNION); notConnectedCount = 0; }//*/ numReceived = 0; notConnectedCount++; } return false; } void DataGlove::ExtractFingersQuat() { k = 10;//7; q0 = (Buf[k] << 24) + (Buf[k+1] << 16) + (Buf[k+2] << 8) + Buf[k+3]; k += 4; q1 = (Buf[k] << 24) + (Buf[k+1] << 16) + (Buf[k+2] << 8) + Buf[k+3]; k += 4; q2 = (Buf[k] << 24) + (Buf[k+1] << 16) + (Buf[k+2] << 8) + Buf[k+3]; k += 4; q3 = (Buf[k] << 24) + (Buf[k+1] << 16) + (Buf[k+2] << 8) + Buf[k+3]; k += 4; for (y = 0; y < 5; y++) { gl.fingers[y] = 0.1 * ((Buf[k] << 8) + Buf[k + 1]); k += 2; } q00 = q0 / 32768.0; q11 = q1 / 32768.0; q22 = q2 / 32768.0; q33 = q3 / 32768.0; norm = sqrt(q00 * q00 + q11 * q11 + q22 * q22 + q33 * q33); test = q00 * q11 + q22 * q33; roll = 180.0 * atan2(2 * (q00 * q11 + q22 * q33), 1 - 2 * (q11 * q11 + q22 * q22)) / 3.1415; pitch = 180.0 * asin(2 * (q00 * q22 - q33 * q11)) / 3.1415; yaw = 180.0 * atan2(2 * (q00 * q33 + q11 * q22), 1 - 2 * (q22 * q22 + q33 * q33)) / 3.1415; gl.roll = roll; gl.pitch = pitch; gl.yaw = yaw; gl.lastPkgTime = timeOfArrival; } void DataGlove::ExtractQuat() { k = 10;//7; q0 = (Buf[k] << 24) + (Buf[k+1] << 16) + (Buf[k+2] << 8) + Buf[k+3]; k += 4; q1 = (Buf[k] << 24) + (Buf[k+1] << 16) + (Buf[k+2] << 8) + Buf[k+3]; k += 4; q2 = (Buf[k] << 24) + (Buf[k+1] << 16) + (Buf[k+2] << 8) + Buf[k+3]; k += 4; q3 = (Buf[k] << 24) + (Buf[k+1] << 16) + (Buf[k+2] << 8) + Buf[k+3]; k += 4; for (y = 0; y < 5; y++) { gl.fingers[y] = 0.0; } q00 = q0 / 32768.0; q11 = q1 / 32768.0; q22 = q2 / 32768.0; q33 = q3 / 32768.0; norm = sqrt(q00 * q00 + q11 * q11 + q22 * q22 + q33 * q33); test = q00 * q11 + q22 * q33; roll = 180.0 * atan2(2 * (q00 * q11 + q22 * q33), 1 - 2 * (q11 * q11 + q22 * q22)) / 3.1415; pitch = 180.0 * asin(2 * (q00 * q22 - q33 * q11)) / 3.1415; yaw = 180.0 * atan2(2 * (q00 * q33 + q11 * q22), 1 - 2 * (q22 * q22 + q33 * q33)) / 3.1415; gl.roll = roll; gl.pitch = pitch; gl.yaw = yaw; gl.lastPkgTime = timeOfArrival; } void DataGlove::ExtractFingersRaw() { k = 10;//7; for (j = 0; j < 3; j++) { gl.gyro[j] = (Buf[k] << 8) + (Buf[k+1]); if (gl.gyro[j] > 0x7fff) gl.gyro[j] -= 0x10000; k += 2; } for (j = 0; j < 3; j++) { gl.magn[j] = (Buf[k] << 8) + (Buf[k+1]); if (gl.magn[j] > 0x7fff) gl.magn[j] -= 0x10000; k += 2; } for (j = 0; j < 3; j++) { gl.accel[j] = (Buf[k] << 8) + (Buf[k+1]); if (gl.accel[j]>0x7fff) gl.accel[j] -= 0x10000; k += 2; } for (y = 0; y < 5; y++) { gl.fingers[y] = 0.1 * ((Buf[k] << 8) + Buf[k+1]); k += 2; } gl.roll = 0.0; gl.pitch = 0.0; gl.yaw = 0.0; gl.lastPkgTime = timeOfArrival; } void DataGlove::ExtractRaw() { k = 10;//7; for (j = 0; j < 3; j++){ gl.gyro[j] = (Buf[k] << 8) + (Buf[k+1]); if (gl.gyro[j] > 0x7fff) gl.gyro[j] -= 0x10000; k += 2; } for (j = 0; j < 3; j++){ gl.magn[j] = (Buf[k] << 8) + (Buf[k+1]); if (gl.magn[j] > 0x7fff) gl.magn[j] -= 0x10000; k += 2; } for (j = 0; j < 3; j++){ gl.accel[j] = (Buf[k] << 8) + (Buf[k+1]); if (gl.accel[j] > 0x7fff) gl.accel[j] -= 0x10000; k += 2; } int y = 0; for (y = 0; y < 5; y++){ gl.fingers[y] = 0.0; } gl.roll = 0.0; gl.pitch = 0.0; gl.yaw = 0.0; gl.lastPkgTime = timeOfArrival; } void DataGlove::ExtractFingers() { k = 10;//7; for (y = 0; y < 5; y++){ gl.fingers[y] = 0.1 * ((Buf[k] << 8) + Buf[k+1]); k += 2; } gl.roll = 0.0; gl.pitch = 0.0; gl.yaw = 0.0; gl.lastPkgTime = timeOfArrival; } Glove DataGlove::GetCurrentValues() { return gl; } bool DataGlove::CheckForNewValues() { if(NewData == true) { NewData = false; return true; }else { return false; } }