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@3:5f5d75cba8e1, 2014-12-05 (annotated)

Committer:
kalbers
Date:
Fri Dec 05 21:35:37 2014 +0000
Revision:
3:5f5d75cba8e1
Child:
13:c701f1122797
Added DataGlove and DataWifi Class, cleaned up main to use these files.

Who changed what in which revision?

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