Diff: DataGlove.cpp

Revision:

22:0cc5b7e55419

Parent:

19:e751d075e2e6

Child:

23:a8de4f87cada

--- a/DataGlove.cpp	Wed Dec 10 22:46:44 2014 +0000
+++ b/DataGlove.cpp	Thu Dec 11 04:07:54 2014 +0000
@@ -10,8 +10,16 @@
 {
 	notConnectedCount = 0;
 	gl = new Glove;
+	correction = new Glove;
+	corrected = new Glove;
 	ReceiveCount = 0;
     numReceived = 0;
+    //Initialize correction values to 0
+    correction->roll = 0.0;
+    correction->pitch = 0.0;
+    correction->yaw = 0.0;
+    for (int i = 0; i < 5; i++)
+    	correction->fingers[i] = 0.0;
 }
 
 DataGlove::~DataGlove()
@@ -23,7 +31,7 @@
 {
 	GloveSocket.Init();
 	GloveSocket.Connect();
-	StreamData(STREAM_QUATERNION);
+	StreamData(STREAM_FINGERS_QUATERNION);
 }
 
 void DataGlove::StreamData(uint8_t DataType)
@@ -82,7 +90,11 @@
                 timeOfArrival = (Buf[6] << 24) + (Buf[7] << 16) + (Buf[8] << 8) + Buf[9];
                 id = (Buf[4] << 8) + Buf[5];
                 pkgtype = Buf[3];
-                if(ReceiveCount++%25 == 0) printf("%d %c\r\n", ReceiveCount, Buf[0]);
+                if(ReceiveCount > 501 && ReceiveCount++%25 == 0) {//printf("%d %c\r\n", ReceiveCount, Buf[0]);
+                	printf("%f %f %f %f %f %f %f %f\r\n", corrected->roll, corrected->pitch, corrected->yaw, 
+                	corrected->fingers[0], corrected->fingers[1], corrected->fingers[2], 
+                	corrected->fingers[3], corrected->fingers[4]);
+                }
                 switch (pkgtype){
                     case STREAM_FINGERS_QUATERNION:
                     {
@@ -92,7 +104,6 @@
                     case STREAM_QUATERNION:
                     {
                         ExtractQuat();
-                        //printf("roll: %f\tpitch: %f\tyaw: %f\r\n", gl->roll, gl->pitch, gl->yaw);
                     }
                     break;
                     case STREAM_FINGERS_RAW:
@@ -103,7 +114,6 @@
                     case STREAM_RAW:
                     {
                         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;
 
@@ -123,12 +133,27 @@
                     notConnectedCount++;
             }
         }
+        // Normalize the data for the first 1000 samples
+        if (ReceiveCount < 500) {
+        	correction->roll = (correction->roll*ReceiveCount + gl->roll)/(ReceiveCount+1);
+        	correction->pitch = (correction->pitch*ReceiveCount + gl->pitch)/(ReceiveCount+1);
+        	correction->yaw = (correction->yaw*ReceiveCount + gl->yaw)/(ReceiveCount+1);
+        	for (int iter = 0; iter < 5; iter++)
+        		correction->fingers[iter] = (correction->fingers[iter]*ReceiveCount + gl->fingers[iter])/(ReceiveCount+1);
+        }
+        else {
+        	corrected->roll = gl->roll - correction->roll;
+        	corrected->pitch = gl->pitch - correction->pitch;
+        	corrected->yaw = gl->yaw - correction->yaw;
+        	for (int iter = 0; iter < 5; iter++)
+        		corrected->fingers[iter] = gl->fingers[iter] - correction->fingers[iter];
+        }
     }
     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 \r\n",numReceived);
+            printf("Connection broke! Trying to re-establish... %d %c\r\n",numReceived,Buf[0]);
             //GloveSocket.Reconnect();
             //StreamData(STREAM_QUATERNION);
             notConnectedCount = 0;