Cubic Hand project for EECS 249A course.
Dependencies: MMA8451Q TSI cc3000_hostdriver_mbedsocket NVIC_set_all_priorities mbed Multi_WS2811
DataGlove.cpp
- Committer:
- kalbers
- Date:
- 2014-12-05
- Revision:
- 3:5f5d75cba8e1
- Child:
- 13:c701f1122797
File content as of revision 3:5f5d75cba8e1:
#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() { }