Model-Based Team / Mbed 2 deprecated CubicHand

Cubic Hand project for EECS 249A course.

Dependencies:   MMA8451Q TSI cc3000_hostdriver_mbedsocket NVIC_set_all_priorities mbed Multi_WS2811

DataGlove.cpp@13:c701f1122797, 2014-12-06 (annotated)

Committer:
kalbers
Date:
Sat Dec 06 23:25:32 2014 +0000
Revision:
13:c701f1122797
Parent:
3:5f5d75cba8e1
Child:
14:0c4a26dc6873
Fixed Glove Wifi class to initialize correctly.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kalbers 3:5f5d75cba8e1 1 #include "DataGlove.h"
kalbers 3:5f5d75cba8e1 2
kalbers 13:c701f1122797 3 DataGlove::DataGlove()
kalbers 3:5f5d75cba8e1 4 {
kalbers 13:c701f1122797 5 notConnectedCount = 0;
kalbers 3:5f5d75cba8e1 6 }
kalbers 3:5f5d75cba8e1 7
kalbers 3:5f5d75cba8e1 8 DataGlove::~DataGlove()
kalbers 3:5f5d75cba8e1 9 {
kalbers 3:5f5d75cba8e1 10
kalbers 3:5f5d75cba8e1 11 }
kalbers 3:5f5d75cba8e1 12
kalbers 3:5f5d75cba8e1 13 void DataGlove::Init()
kalbers 3:5f5d75cba8e1 14 {
kalbers 13:c701f1122797 15 //GloveSocket.Init();
kalbers 3:5f5d75cba8e1 16 GloveSocket.Connect();
kalbers 3:5f5d75cba8e1 17 SendBuf[0] = '$';
kalbers 3:5f5d75cba8e1 18 SendBuf[1] = 0x0A;
kalbers 3:5f5d75cba8e1 19 SendBuf[2] = 0x03;
kalbers 3:5f5d75cba8e1 20 SendBuf[3] = 3;
kalbers 3:5f5d75cba8e1 21 SendBuf[4] = (SendBuf[0]+SendBuf[1]+SendBuf[2]+SendBuf[3])%256;
kalbers 3:5f5d75cba8e1 22 SendBuf[5] = '#';
kalbers 3:5f5d75cba8e1 23 GloveSocket.SendDataToGlove(SendBuf, 6);
kalbers 3:5f5d75cba8e1 24 }
kalbers 3:5f5d75cba8e1 25
kalbers 3:5f5d75cba8e1 26 void DataGlove::Receive()
kalbers 3:5f5d75cba8e1 27 {
kalbers 13:c701f1122797 28 numReceived = GloveSocket.GetDataFromBuffer(Buf, 99);
kalbers 3:5f5d75cba8e1 29 if(numReceived >= 42 && Buf[0] == '$') {
kalbers 3:5f5d75cba8e1 30 gyrox = ((int16_t *)Buf)[5];
kalbers 3:5f5d75cba8e1 31 gyroy = ((int16_t *)Buf)[6];
kalbers 3:5f5d75cba8e1 32 gyroz = ((int16_t *)Buf)[7];
kalbers 3:5f5d75cba8e1 33 accelx = ((int16_t *)Buf)[11];
kalbers 3:5f5d75cba8e1 34 accely = ((int16_t *)Buf)[12];
kalbers 3:5f5d75cba8e1 35 accelz = ((int16_t *)Buf)[13];
kalbers 3:5f5d75cba8e1 36 finger1 = ((int16_t *)Buf)[14];
kalbers 3:5f5d75cba8e1 37 finger2 = ((int16_t *)Buf)[15];
kalbers 3:5f5d75cba8e1 38 finger3 = ((int16_t *)Buf)[16];
kalbers 3:5f5d75cba8e1 39 finger4 = ((int16_t *)Buf)[17];
kalbers 3:5f5d75cba8e1 40 finger5 = ((int16_t *)Buf)[18];
kalbers 3:5f5d75cba8e1 41 // Switching MSB and LSB since data is the other way around
kalbers 3:5f5d75cba8e1 42 gyrox = ((gyrox>>8)&(0x00FF))|((gyrox<<8)&(0xFF00));
kalbers 3:5f5d75cba8e1 43 gyroy = ((gyroy>>8)&(0x00FF))|((gyroy<<8)&(0xFF00));
kalbers 3:5f5d75cba8e1 44 gyroz = ((gyroz>>8)&(0x00FF))|((gyroz<<8)&(0xFF00));
kalbers 3:5f5d75cba8e1 45 accelx = ((accelx>>8)&(0x00FF))|((accelx<<8)&(0xFF00));
kalbers 3:5f5d75cba8e1 46 accely = ((accely>>8)&(0x00FF))|((accely<<8)&(0xFF00));
kalbers 3:5f5d75cba8e1 47 accelz = ((accelz>>8)&(0x00FF))|((accelz<<8)&(0xFF00));
kalbers 3:5f5d75cba8e1 48 finger1 = ((finger1>>8)&(0x00FF))|((finger1<<8)&(0xFF00));
kalbers 3:5f5d75cba8e1 49 finger2 = ((finger2>>8)&(0x00FF))|((finger2<<8)&(0xFF00));
kalbers 3:5f5d75cba8e1 50 finger3 = ((finger3>>8)&(0x00FF))|((finger3<<8)&(0xFF00));
kalbers 3:5f5d75cba8e1 51 finger4 = ((finger4>>8)&(0x00FF))|((finger4<<8)&(0xFF00));
kalbers 3:5f5d75cba8e1 52 finger5 = ((finger5>>8)&(0x00FF))|((finger5<<8)&(0xFF00));
kalbers 3:5f5d75cba8e1 53 //printf("F1: %d\tF2: %d\tF3: %d\tF4: %d\tF5: %d\r\n", finger1, finger2, finger3, finger4, finger5);
kalbers 3:5f5d75cba8e1 54 printf("GX: %d\tGY: %d\tGZ: %d\r\n", gyrox, gyroy, gyroz);
kalbers 3:5f5d75cba8e1 55 //printf("AX: %d\tAY: %d\tAZ: %d\r\n", accelx, accely, accelz);
kalbers 3:5f5d75cba8e1 56 wait(0.05);
kalbers 3:5f5d75cba8e1 57 notConnectedCount = 0;
kalbers 3:5f5d75cba8e1 58 }
kalbers 3:5f5d75cba8e1 59 else {
kalbers 3:5f5d75cba8e1 60 notConnectedCount += 1;
kalbers 3:5f5d75cba8e1 61 }
kalbers 3:5f5d75cba8e1 62 // Re-establishing communication in case no data is received for 1s (20 frames per second*1 = 20)
kalbers 3:5f5d75cba8e1 63 if (notConnectedCount > 20) {
kalbers 3:5f5d75cba8e1 64 printf("Connection broke! Trying to re-establish...\r\n");
kalbers 3:5f5d75cba8e1 65 GloveSocket.Disconnect();
kalbers 3:5f5d75cba8e1 66 printf("Disconnected wifi\r\n");
kalbers 3:5f5d75cba8e1 67 notConnectedCount = 0;
kalbers 3:5f5d75cba8e1 68 GloveSocket.Connect();
kalbers 3:5f5d75cba8e1 69 }
kalbers 3:5f5d75cba8e1 70 }
kalbers 3:5f5d75cba8e1 71
kalbers 3:5f5d75cba8e1 72 void DataGlove::Parse()
kalbers 3:5f5d75cba8e1 73 {
kalbers 3:5f5d75cba8e1 74 }