Revision 17:c2e59ada97ee, committed 2014-12-08

Comitter:

joseoyola

Date:

Mon Dec 08 03:29:39 2014 +0000

Parent:

16:a3de0c0dbe33

Parent:

15:b48ac00af0b8

Child:

18:6737bf53ccd1

Child:

20:c2bc970bbff8

Commit message:

Merged.

Changed in this revision

--- a/DataGlove.cpp	Mon Dec 08 03:28:50 2014 +0000
+++ b/DataGlove.cpp	Mon Dec 08 03:29:39 2014 +0000
@@ -1,23 +1,36 @@
 #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();
+	StreamData();
+}
+
+void DataGlove::StreamData()
+{
     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,47 +38,192 @@
 
 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;
+    raw = false;
+    numReceived = GloveSocket.GetDataFromBuffer(Buf, 1);
+    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++;
     }
     // 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");
+        GloveSocket.Reconnect();
+        StreamData();
         notConnectedCount = 0;
-        GloveSocket.Connect();
     }
 }
 

--- a/DataGlove.h	Mon Dec 08 03:28:50 2014 +0000
+++ b/DataGlove.h	Mon Dec 08 03:29:39 2014 +0000
@@ -2,12 +2,24 @@
 
 #pragma once
 
+struct Glove {
+    double roll;
+    double pitch;
+    double yaw;
+    double fingers[5];
+    double gyro[3];
+    double magn[3];
+    double accel[3];
+    int lastPkgTime;
+};
+
 class DataGlove
 {
 	public:
 		DataGlove();
 		~DataGlove();
 		void Init();
+		void StreamData();
 		void Receive();
         int16_t finger1, finger2, finger3, finger4, finger5;
         int16_t accelx, accely, accelz;
@@ -15,10 +27,20 @@
         
 	private:
 		void Parse();
-        uint8_t Buf[100];
         uint8_t SendBuf[20];
         int16_t notConnectedCount;
-		int numReceived;
+		int8_t numReceived;
+	    uint8_t Buf[256], buffer[256];
+	    bool raw;
+	    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;
+		Glove *gl;
         GloveWifi GloveSocket;
 
 

--- a/GloveWifi.cpp	Mon Dec 08 03:28:50 2014 +0000
+++ b/GloveWifi.cpp	Mon Dec 08 03:29:39 2014 +0000
@@ -44,9 +44,9 @@
 void GloveWifi::Connect()
 {
     
-    wifi = new cc3000(PTD4, PTC9, PTD0, SPI(PTD2, PTD3, PTD1), SSID, PASSWORD, WPA2, false); 
+    //wifi = new cc3000(PTD4, PTC9, PTD0, SPI(PTD2, PTD3, PTD1), SSID, PASSWORD, WPA2, false); 
+    wifi = new cc3000(PTD4, PTC9, PTC4, SPI(PTC6, PTC7, PTC5), SSID, PASSWORD, WPA2, false);
     wifi->init();
-    socket = new TCPSocketConnection;
     if (wifi->connect() == -1) 
     {
         printf("Failed to connect. Please verify connection details and try again. \r\n");
@@ -54,6 +54,7 @@
     {
         printf("IP address: %s \r\n", wifi->getIPAddress());
     }
+    socket = new TCPSocketConnection;
     while (socket->connect(ECHO_SERVER_ADDRESS, ECHO_SERVER_PORT) < 0) 
     {
         printf("Unable to connect to (%s) on port (%d) \r\n", ECHO_SERVER_ADDRESS, ECHO_SERVER_PORT);
@@ -61,6 +62,7 @@
     }
     socket->set_blocking(true);
     printf("Connected!\r\n");
+    
 }
 
 void GloveWifi::Disconnect()
@@ -71,8 +73,22 @@
     delete wifi;
 }
 
+void GloveWifi::Reconnect()
+{
+    delete socket;
+    socket = new TCPSocketConnection;
+    while (socket->connect(ECHO_SERVER_ADDRESS, ECHO_SERVER_PORT) < 0) 
+    {
+        printf("Unable to connect to (%s) on port (%d) \r\n", ECHO_SERVER_ADDRESS, ECHO_SERVER_PORT);
+        wait(1);
+    }
+    socket->set_blocking(true);
+    printf("Reconnected!\r\n");
+}
+
 uint8_t GloveWifi::GetDataFromBuffer(uint8_t * buf, uint16_t size)
 {
+    //printf("Buffer Size: %d\r\n", wifi->_simple_link.get_transmit_error());
     return socket->receive((char *)buf, size);
 }
 

--- a/GloveWifi.h	Mon Dec 08 03:28:50 2014 +0000
+++ b/GloveWifi.h	Mon Dec 08 03:29:39 2014 +0000
@@ -14,6 +14,7 @@
 		void Init();
 		void Connect();
 		void Disconnect();
+		void Reconnect();
 		uint8_t GetDataFromBuffer(uint8_t * buf, uint16_t size);
 		uint8_t SendDataToGlove(uint8_t * buf, uint16_t size);
 

--- a/main.cpp	Mon Dec 08 03:28:50 2014 +0000
+++ b/main.cpp	Mon Dec 08 03:29:39 2014 +0000
@@ -16,8 +16,8 @@
     pc.printf("DataGlove Cube Client\r\n");
     while(true) 
     {
-        //MasterGlove.Receive();
-        pc.printf("Looping \r\n");
-        wait(0.5);
+        MasterGlove.Receive();
+        //pc.printf("Looping \r\n");
+        //wait(0.1);
      }
 }