Cubic Hand project for EECS 249A course.
Dependencies: MMA8451Q TSI cc3000_hostdriver_mbedsocket NVIC_set_all_priorities mbed Multi_WS2811
Diff: DataGlove.cpp
- Revision:
- 18:6737bf53ccd1
- Parent:
- 15:b48ac00af0b8
- Child:
- 19:e751d075e2e6
diff -r c2e59ada97ee -r 6737bf53ccd1 DataGlove.cpp --- a/DataGlove.cpp Mon Dec 08 03:29:39 2014 +0000 +++ b/DataGlove.cpp Mon Dec 08 18:56:42 2014 +0000 @@ -10,6 +10,7 @@ { notConnectedCount = 0; gl = new Glove; + ReceiveCount = 0; } DataGlove::~DataGlove() @@ -21,16 +22,16 @@ { GloveSocket.Init(); GloveSocket.Connect(); - StreamData(); + StreamData(STREAM_QUATERNION); } -void DataGlove::StreamData() +void DataGlove::StreamData(uint8_t DataType) { SendBuf[0] = '$'; SendBuf[1] = 0x0A; SendBuf[2] = 0x03; // Streaming quaternion data - SendBuf[3] = STREAM_QUATERNION; + SendBuf[3] = DataType; SendBuf[4] = (SendBuf[0]+SendBuf[1]+SendBuf[2]+SendBuf[3])%256; SendBuf[5] = '#'; GloveSocket.SendDataToGlove(SendBuf, 6); @@ -62,147 +63,31 @@ 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; + ExtractFingersQuat(); } 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); + ExtractQuat(); + printf("%d\r\n", ReceiveCount++); + //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; + ExtractFingersRaw(); } 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; + ExtractRaw(); 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; + ExtractFingers(); } break; } @@ -222,11 +107,156 @@ if (notConnectedCount > 20) { printf("Connection broke! Trying to re-establish...\r\n"); GloveSocket.Reconnect(); - StreamData(); + StreamData(STREAM_QUATERNION); notConnectedCount = 0; } } -void DataGlove::Parse() +void DataGlove::ExtractFingersQuat() +{ + 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; + +} +void DataGlove::ExtractQuat() +{ + 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; + +} + +void DataGlove::ExtractFingersRaw() { + 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; +} + +void DataGlove::ExtractRaw() +{ + 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; +} + +void DataGlove::ExtractFingers() +{ + 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; } \ No newline at end of file