Diff: DataGlove.cpp

Revision:

3:5f5d75cba8e1

Child:

13:c701f1122797

diff -r 587189fb6d87 -r 5f5d75cba8e1 DataGlove.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DataGlove.cpp	Fri Dec 05 21:35:37 2014 +0000
@@ -0,0 +1,75 @@
+#include "DataGlove.h"
+
+DataGlove::DataGlove():
+notConnectedCount(0)
+{
+
+}
+
+DataGlove::~DataGlove()
+{
+
+}
+
+void DataGlove::Init()
+{
+	GloveSocket.Init();
+	GloveSocket.Connect();
+    SendBuf[0] = '$';
+    SendBuf[1] = 0x0A;
+    SendBuf[2] = 0x03;
+    SendBuf[3] = 3;
+    SendBuf[4] = (SendBuf[0]+SendBuf[1]+SendBuf[2]+SendBuf[3])%256;
+    SendBuf[5] = '#';
+    GloveSocket.SendDataToGlove(SendBuf, 6);
+}
+
+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;
+    }
+    else {
+        notConnectedCount += 1;
+    }
+    // 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");
+        notConnectedCount = 0;
+        GloveSocket.Connect();
+    }
+}
+
+void DataGlove::Parse()
+{
+}
\ No newline at end of file