File content as of revision 13:c701f1122797:

#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()
{
}