Cubic Hand project for EECS 249A course.
Dependencies: MMA8451Q TSI cc3000_hostdriver_mbedsocket NVIC_set_all_priorities mbed Multi_WS2811
DataGlove.cpp
- Committer:
- naren
- Date:
- 2014-12-07
- Revision:
- 14:0c4a26dc6873
- Parent:
- 13:c701f1122797
- Child:
- 15:b48ac00af0b8
File content as of revision 14:0c4a26dc6873:
#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; gl = new Glove; } DataGlove::~DataGlove() { delete(gl); } void DataGlove::Init() { GloveSocket.Init(); GloveSocket.Connect(); SendBuf[0] = '$'; SendBuf[1] = 0x0A; SendBuf[2] = 0x03; // Streaming quaternion data SendBuf[3] = STREAM_QUATERNION; SendBuf[4] = (SendBuf[0]+SendBuf[1]+SendBuf[2]+SendBuf[3])%256; SendBuf[5] = '#'; GloveSocket.SendDataToGlove(SendBuf, 6); } void DataGlove::Receive() { uint8_t *Buf, *buffer; int8_t numReceived; bool raw = false; unsigned char bcc; int pkglen, u; int timeOfArrival, id, pkgtype; int j, k, y; double roll, yaw, pitch; double norm, test; int q0, q1, q2, q3; double q00, q11, q22, q33; printf("beginning of receive\r\n"); numReceived = GloveSocket.GetDataFromBuffer(Buf, 1); printf("received %d bytes\r\n", numReceived); if (Buf[0] == '$') { bcc = '$'; numReceived = GloveSocket.GetDataFromBuffer(Buf, 2); if ((numReceived == 2) && (Buf[0] == 0x0a)) { pkglen = Buf[1]; bcc += Buf[0]; bcc += Buf[1]; numReceived = GloveSocket.GetDataFromBuffer(Buf, pkglen); if (numReceived < pkglen) { notConnectedCount++; } for (u = 0; u < pkglen - 2; u++) { bcc += Buf[u]; } if ((numReceived == pkglen) && (bcc == Buf[pkglen-2])) { memcpy(buffer, Buf, numReceived); timeOfArrival = (buffer[3] << 24) + (buffer[4] << 16) + (buffer[5] << 8) + buffer[6]; id = (buffer[1] << 8) + buffer[2]; pkgtype = buffer[0]; switch (pkgtype){ case STREAM_FINGERS_QUATERNION: { k = 7; q0 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3]; k += 4; q1 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3]; k += 4; q2 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3]; k += 4; q3 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3]; k += 4; for (y = 0; y < 5; y++) { gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[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; } break; case STREAM_QUATERNION: { k = 7; q0 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3]; k += 4; q1 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3]; k += 4; q2 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3]; k += 4; q3 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[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; printf("roll: %f\tpitch: %f\tyaw: %f\r\n", gl->roll, gl->pitch, gl->yaw); } break; case STREAM_FINGERS_RAW: { k = 7; for (j = 0; j < 3; j++){ gl->gyro[j] = (buffer[k] << 8) + (buffer[k+1]); if (gl->gyro[j] > 0x7fff) gl->gyro[j] -= 0x10000; k += 2; } for (j = 0; j < 3; j++){ gl->magn[j] = (buffer[k] << 8) + (buffer[k+1]); if (gl->magn[j] > 0x7fff) gl->magn[j] -= 0x10000; k += 2; } for (j = 0; j < 3; j++){ gl->accel[j] = (buffer[k] << 8) + (buffer[k+1]); if (gl->accel[j]>0x7fff) gl->accel[j] -= 0x10000; k += 2; } for (y = 0; y < 5; y++){ gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k+1]); k += 2; } gl->roll = 0.0; gl->pitch = 0.0; gl->yaw = 0.0; gl->lastPkgTime = timeOfArrival; } break; case STREAM_RAW: { k = 7; for (j = 0; j < 3; j++){ gl->gyro[j] = (buffer[k] << 8) + (buffer[k+1]); if (gl->gyro[j] > 0x7fff) gl->gyro[j] -= 0x10000; k += 2; } for (j = 0; j < 3; j++){ gl->magn[j] = (buffer[k] << 8) + (buffer[k+1]); if (gl->magn[j] > 0x7fff) gl->magn[j] -= 0x10000; k += 2; } for (j = 0; j < 3; j++){ gl->accel[j] = (buffer[k] << 8) + (buffer[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; printf("%f %f %f %f %f %f\r\n", gl->gyro[0], gl->gyro[1], gl->gyro[2], gl->accel[0], gl->accel[1], gl->accel[2]); } break; case STREAM_FINGERS: { k = 7; for (y = 0; y < 5; y++){ gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k+1]); k += 2; } gl->roll = 0.0; gl->pitch = 0.0; gl->yaw = 0.0; gl->lastPkgTime = timeOfArrival; } break; } notConnectedCount = 0; } else{ if (bcc!=Buf[pkglen-2]) notConnectedCount++; } } } else { printf("couldn't get data %d\r\n", notConnectedCount); notConnectedCount++; } printf("just before re-establishing connection\r\n"); // Re-establishing communication in case no data is received for 1s (20 frames per second*1 = 20) if (notConnectedCount > 20) { printf("Connection broke! Trying to re-establish...\r\n"); GloveSocket.Disconnect(); printf("Disconnected wifi\r\n"); notConnectedCount = 0; GloveSocket.Connect(); } wait(0.1); } void DataGlove::Parse() { }