Model-Based Team
Cubic Hand project for EECS 249A course.
Dependencies: MMA8451Q TSI cc3000_hostdriver_mbedsocket NVIC_set_all_priorities mbed Multi_WS2811
Diff: DataGlove.cpp
- Revision:
- 14:0c4a26dc6873
- Parent:
- 13:c701f1122797
- Child:
- 15:b48ac00af0b8
--- a/DataGlove.cpp Sat Dec 06 23:25:32 2014 +0000
+++ b/DataGlove.cpp Sun Dec 07 19:50:47 2014 +0000
@@ -1,23 +1,31 @@
#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.Init();
GloveSocket.Connect();
SendBuf[0] = '$';
SendBuf[1] = 0x0A;
SendBuf[2] = 0x03;
- SendBuf[3] = 3;
+ // Streaming quaternion data
+ SendBuf[3] = STREAM_QUATERNION;
SendBuf[4] = (SendBuf[0]+SendBuf[1]+SendBuf[2]+SendBuf[3])%256;
SendBuf[5] = '#';
GloveSocket.SendDataToGlove(SendBuf, 6);
@@ -25,48 +33,208 @@
void DataGlove::Receive()
{
- numReceived = GloveSocket.GetDataFromBuffer(Buf, 99);
- if(numReceived >= 42 && Buf[0] == '$') {
- gyrox = ((int16_t *)Buf)[5];
- gyroy = ((int16_t *)Buf)[6];
- gyroz = ((int16_t *)Buf)[7];
- accelx = ((int16_t *)Buf)[11];
- accely = ((int16_t *)Buf)[12];
- accelz = ((int16_t *)Buf)[13];
- finger1 = ((int16_t *)Buf)[14];
- finger2 = ((int16_t *)Buf)[15];
- finger3 = ((int16_t *)Buf)[16];
- finger4 = ((int16_t *)Buf)[17];
- finger5 = ((int16_t *)Buf)[18];
- // Switching MSB and LSB since data is the other way around
- gyrox = ((gyrox>>8)&(0x00FF))|((gyrox<<8)&(0xFF00));
- gyroy = ((gyroy>>8)&(0x00FF))|((gyroy<<8)&(0xFF00));
- gyroz = ((gyroz>>8)&(0x00FF))|((gyroz<<8)&(0xFF00));
- accelx = ((accelx>>8)&(0x00FF))|((accelx<<8)&(0xFF00));
- accely = ((accely>>8)&(0x00FF))|((accely<<8)&(0xFF00));
- accelz = ((accelz>>8)&(0x00FF))|((accelz<<8)&(0xFF00));
- finger1 = ((finger1>>8)&(0x00FF))|((finger1<<8)&(0xFF00));
- finger2 = ((finger2>>8)&(0x00FF))|((finger2<<8)&(0xFF00));
- finger3 = ((finger3>>8)&(0x00FF))|((finger3<<8)&(0xFF00));
- finger4 = ((finger4>>8)&(0x00FF))|((finger4<<8)&(0xFF00));
- finger5 = ((finger5>>8)&(0x00FF))|((finger5<<8)&(0xFF00));
- //printf("F1: %d\tF2: %d\tF3: %d\tF4: %d\tF5: %d\r\n", finger1, finger2, finger3, finger4, finger5);
- printf("GX: %d\tGY: %d\tGZ: %d\r\n", gyrox, gyroy, gyroz);
- //printf("AX: %d\tAY: %d\tAZ: %d\r\n", accelx, accely, accelz);
- wait(0.05);
- notConnectedCount = 0;
+ 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 {
- notConnectedCount += 1;
+ 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();
+ GloveSocket.Disconnect();
printf("Disconnected wifi\r\n");
notConnectedCount = 0;
GloveSocket.Connect();
}
+ wait(0.1);
}
void DataGlove::Parse()
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 23 Oct 2014 |
| Imports: | 4 |
| Forks: | 1 |
| Commits: | 57 |
| Dependents: | 0 |
| Dependencies: | 6 |
| Followers: | 6 |
| Issues: | 0 |
This repository is Public (Unlisted).
