Revision 25:7a866b3d79bb, committed 2014-12-11

Comitter:

joseoyola

Date:

Thu Dec 11 04:24:28 2014 +0000

Parent:

24:554ea084eddf

Parent:

23:a8de4f87cada

Child:

26:0933d7f2fe78

Child:

29:50be685a3f54

Commit message:

Merged

Changed in this revision

--- a/DataGlove.cpp	Thu Dec 11 04:23:31 2014 +0000
+++ b/DataGlove.cpp	Thu Dec 11 04:24:28 2014 +0000
@@ -10,8 +10,17 @@
 {
 	notConnectedCount = 0;
 	gl = new Glove;
+	correction = new Glove;
+	corrected = new Glove;
 	ReceiveCount = 0;
     numReceived = 0;
+    NewData = false;
+    //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 +32,7 @@
 {
 	GloveSocket.Init();
 	GloveSocket.Connect();
-	StreamData(STREAM_QUATERNION);
+	StreamData(STREAM_FINGERS_QUATERNION);
 }
 
 void DataGlove::StreamData(uint8_t DataType)
@@ -82,7 +91,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 +105,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 +115,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;
 
@@ -113,6 +124,7 @@
                     }
                     break;
                 }
+                NewData = true;
                 Buf[0] = 0; //Clear Token so no repeats.
                 notConnectedCount = 0;
                 numReceived = 0;
@@ -123,12 +135,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;
@@ -286,4 +313,20 @@
     gl->pitch = 0.0;
     gl->yaw = 0.0;
     gl->lastPkgTime = timeOfArrival;
+}
+
+Glove DataGlove::GetCurrentValues()
+{
+	return *gl;
+}
+bool DataGlove::CheckForNewValues()
+{
+	if(NewData == true)
+	{
+		NewData = false;
+		return true;
+	}else
+	{
+		return false;
+	}
 }
\ No newline at end of file

--- a/DataGlove.h	Thu Dec 11 04:23:31 2014 +0000
+++ b/DataGlove.h	Thu Dec 11 04:24:28 2014 +0000
@@ -20,6 +20,12 @@
 		~DataGlove();
 		void Init();
 		void Receive();
+		
+		//Returns the latest glove values in a structure.
+		Glove GetCurrentValues();
+		//Returns true if new data since last function call, false otherwise. 
+		bool CheckForNewValues();
+		
         int16_t finger1, finger2, finger3, finger4, finger5;
         int16_t accelx, accely, accelz;
         int16_t gyrox, gyroy, gyroz;
@@ -39,7 +45,13 @@
 	    int q0, q1, q2, q3;
 	    double q00, q11, q22, q33;
 	    long ReceiveCount;
+	    bool NewData;
+	    // Stores glove data
 		Glove *gl;
+		// Stores the correction matrix
+		Glove *correction;
+		// Stores the corrected glove values
+		Glove *corrected;
         GloveWifi GloveSocket;
         
     private: