Diff: DataGlove.cpp

Revision:

35:b6fb5dd65a98

Parent:

32:bf5f545621aa

Child:

37:996bd22a633f

--- a/DataGlove.cpp	Thu Dec 11 06:37:46 2014 +0000
+++ b/DataGlove.cpp	Thu Dec 11 06:44:30 2014 +0000
@@ -8,11 +8,11 @@
 
 DataGlove::DataGlove()
 {
-	notConnectedCount = 0;
-	gl = new Glove;
-	correction = new Glove;
-	corrected = new Glove;
-	ReceiveCount = 0;
+    notConnectedCount = 0;
+    gl = new Glove;
+    correction = new Glove;
+    corrected = new Glove;
+    ReceiveCount = 0;
     numReceived = 0;
     NewData = false;
     //Initialize correction values to 0
@@ -20,19 +20,19 @@
     correction->pitch = 0.0;
     correction->yaw = 0.0;
     for (int i = 0; i < 5; i++)
-    	correction->fingers[i] = 0.0;
+        correction->fingers[i] = 0.0;
 }
 
 DataGlove::~DataGlove()
 {
-	delete(gl);
+    delete(gl);
 }
 
 void DataGlove::Init()
 {
-	GloveSocket.Init();
-	GloveSocket.Connect();
-	StreamData(STREAM_FINGERS_QUATERNION);
+    GloveSocket.Init();
+    GloveSocket.Connect();
+    StreamData(STREAM_FINGERS_QUATERNION);
 }
 
 void DataGlove::StreamData(uint8_t DataType)
@@ -58,7 +58,14 @@
     GloveSocket.SendDataToGlove(SendBuf, 6);
 }
 
-void DataGlove::Receive()
+
+Glove DataGlove::ReceiveBlocking()
+{
+    while(Receive() == false);
+    return *gl;
+}
+
+bool DataGlove::Receive()
 {
     raw = false;
     numReceived += GloveSocket.GetDataFromBuffer(Buf, 1);
@@ -76,7 +83,7 @@
             if ((numReceived  - 3 )< pkglen) 
             {
                 if(numReceived == 0) notConnectedCount++;
-                return;
+                return false;
             }
             for (u = 3; u <= pkglen; u++)
             {
@@ -91,11 +98,12 @@
                 timeOfArrival = (Buf[6] << 24) + (Buf[7] << 16) + (Buf[8] << 8) + Buf[9];
                 id = (Buf[4] << 8) + Buf[5];
                 pkgtype = Buf[3];
-                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]);
-                }
+                ReceiveCount++;
+                /*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:
                     {
@@ -128,6 +136,7 @@
                 Buf[0] = 0; //Clear Token so no repeats.
                 notConnectedCount = 0;
                 numReceived = 0;
+                return true;
             }
             else
             {
@@ -136,34 +145,34 @@
             }
         }
         // 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);
+        /*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];
-        }
+            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 {
+   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) {
+        /*if (notConnectedCount > 200) {
             printf("Connection broke! Trying to re-establish... %d %c\r\n",numReceived,Buf[0]);
             //GloveSocket.Reconnect();
             //StreamData(STREAM_QUATERNION);
             notConnectedCount = 0;
-        }
+        }*/
         numReceived = 0;
-        notConnectedCount++;
+        //notConnectedCount++;
     }
-    
+    return false;
 }
 
 void DataGlove::ExtractFingersQuat()
@@ -317,16 +326,17 @@
 
 Glove DataGlove::GetCurrentValues()
 {
-	return *gl;
+    return *gl;
 }
+
 bool DataGlove::CheckForNewValues()
 {
-	if(NewData == true)
-	{
-		NewData = false;
-		return true;
-	}else
-	{
-		return false;
-	}
+    if(NewData == true)
+    {
+        NewData = false;
+        return true;
+    }else
+    {
+        return false;
+    }
 }
\ No newline at end of file