Robert Bui / Mbed 2 deprecated CubicHand

Temp Fork

Dependencies:   MMA8451Q Multi_WS2811 NVIC_set_all_priorities TSI cc3000_hostdriver_mbedsocket mbed

Fork of CubicHand by Model-Based Team

DataGlove.cpp@31:bf5f545621aa, 2014-12-11 (annotated)

Committer:
robertbui
Date:
Thu Dec 11 06:08:13 2014 +0000
Revision:
31:bf5f545621aa
Parent:
30:3261015fdd17 
initial draft of gesture recognition.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kalbers 3:5f5d75cba8e1 1 #include "DataGlove.h"
kalbers 3:5f5d75cba8e1 2
naren 14:0c4a26dc6873 3 #define STREAM_FINGERS_QUATERNION 1
naren 14:0c4a26dc6873 4 #define STREAM_QUATERNION 2
naren 14:0c4a26dc6873 5 #define STREAM_FINGERS_RAW 3
naren 14:0c4a26dc6873 6 #define STREAM_RAW 4
naren 14:0c4a26dc6873 7 #define STREAM_FINGERS 5
naren 14:0c4a26dc6873 8
kalbers 13:c701f1122797 9 DataGlove::DataGlove()
kalbers 3:5f5d75cba8e1 10 {
kalbers 13:c701f1122797 11 notConnectedCount = 0;
naren 14:0c4a26dc6873 12 gl = new Glove;
naren 22:0cc5b7e55419 13 correction = new Glove;
naren 22:0cc5b7e55419 14 corrected = new Glove;
kalbers 18:6737bf53ccd1 15 ReceiveCount = 0;
kalbers 19:e751d075e2e6 16 numReceived = 0;
kalbers 23:a8de4f87cada 17 NewData = false;
naren 22:0cc5b7e55419 18 //Initialize correction values to 0
naren 22:0cc5b7e55419 19 correction->roll = 0.0;
naren 22:0cc5b7e55419 20 correction->pitch = 0.0;
naren 22:0cc5b7e55419 21 correction->yaw = 0.0;
naren 22:0cc5b7e55419 22 for (int i = 0; i < 5; i++)
naren 22:0cc5b7e55419 23 correction->fingers[i] = 0.0;
kalbers 3:5f5d75cba8e1 24 }
kalbers 3:5f5d75cba8e1 25
kalbers 3:5f5d75cba8e1 26 DataGlove::~DataGlove()
kalbers 3:5f5d75cba8e1 27 {
naren 14:0c4a26dc6873 28 delete(gl);
kalbers 3:5f5d75cba8e1 29 }
kalbers 3:5f5d75cba8e1 30
kalbers 3:5f5d75cba8e1 31 void DataGlove::Init()
kalbers 3:5f5d75cba8e1 32 {
naren 14:0c4a26dc6873 33 GloveSocket.Init();
kalbers 3:5f5d75cba8e1 34 GloveSocket.Connect();
naren 22:0cc5b7e55419 35 StreamData(STREAM_FINGERS_QUATERNION);
kalbers 15:b48ac00af0b8 36 }
kalbers 15:b48ac00af0b8 37
kalbers 18:6737bf53ccd1 38 void DataGlove::StreamData(uint8_t DataType)
kalbers 15:b48ac00af0b8 39 {
kalbers 3:5f5d75cba8e1 40 SendBuf[0] = '$';
kalbers 3:5f5d75cba8e1 41 SendBuf[1] = 0x0A;
kalbers 3:5f5d75cba8e1 42 SendBuf[2] = 0x03;
naren 14:0c4a26dc6873 43 // Streaming quaternion data
kalbers 18:6737bf53ccd1 44 SendBuf[3] = DataType;
kalbers 3:5f5d75cba8e1 45 SendBuf[4] = (SendBuf[0]+SendBuf[1]+SendBuf[2]+SendBuf[3])%256;
kalbers 3:5f5d75cba8e1 46 SendBuf[5] = '#';
kalbers 3:5f5d75cba8e1 47 GloveSocket.SendDataToGlove(SendBuf, 6);
kalbers 3:5f5d75cba8e1 48 }
kalbers 3:5f5d75cba8e1 49
kalbers 19:e751d075e2e6 50 void DataGlove::StopSampling()
kalbers 19:e751d075e2e6 51 {
kalbers 19:e751d075e2e6 52 SendBuf[0] = '$';
kalbers 19:e751d075e2e6 53 SendBuf[1] = 0x0B;
kalbers 19:e751d075e2e6 54 SendBuf[2] = 0x02;
kalbers 19:e751d075e2e6 55 // Streaming quaternion data
kalbers 19:e751d075e2e6 56 SendBuf[3] = (SendBuf[0]+SendBuf[1]+SendBuf[2])%256;
kalbers 19:e751d075e2e6 57 SendBuf[4] = '#';
kalbers 19:e751d075e2e6 58 GloveSocket.SendDataToGlove(SendBuf, 6);
kalbers 19:e751d075e2e6 59 }
kalbers 19:e751d075e2e6 60
robertbui 31:bf5f545621aa 61 void DataGlove::Receive()
kalbers 3:5f5d75cba8e1 62 {
kalbers 15:b48ac00af0b8 63 raw = false;
kalbers 19:e751d075e2e6 64 numReceived += GloveSocket.GetDataFromBuffer(Buf, 1);
kalbers 19:e751d075e2e6 65 //if(ReceiveCount++%25 == 0) printf("%d %c\r\n", ReceiveCount, Buf[0]);
kalbers 19:e751d075e2e6 66 if (Buf[0] == '$' && numReceived > 0)
kalbers 19:e751d075e2e6 67 {
naren 14:0c4a26dc6873 68 bcc = '$';
kalbers 19:e751d075e2e6 69 numReceived += GloveSocket.GetDataFromBuffer((Buf+numReceived), 2);
kalbers 19:e751d075e2e6 70 if ((numReceived > 2) && (Buf[1] == 0x0a))
kalbers 19:e751d075e2e6 71 {
kalbers 19:e751d075e2e6 72 pkglen = Buf[2];
naren 14:0c4a26dc6873 73 bcc += Buf[1];
kalbers 19:e751d075e2e6 74 bcc += Buf[2];
kalbers 19:e751d075e2e6 75 numReceived += GloveSocket.GetDataFromBuffer((Buf+numReceived), pkglen - numReceived + 3);
kalbers 19:e751d075e2e6 76 if ((numReceived - 3 )< pkglen)
kalbers 19:e751d075e2e6 77 {
kalbers 19:e751d075e2e6 78 if(numReceived == 0) notConnectedCount++;
robertbui 31:bf5f545621aa 79 return;
naren 14:0c4a26dc6873 80 }
kalbers 19:e751d075e2e6 81 for (u = 3; u <= pkglen; u++)
kalbers 19:e751d075e2e6 82 {
naren 14:0c4a26dc6873 83 bcc += Buf[u];
naren 14:0c4a26dc6873 84 }
kalbers 19:e751d075e2e6 85 if ((numReceived - 3 == pkglen) && (bcc == Buf[pkglen+1]))
kalbers 19:e751d075e2e6 86 {
kalbers 19:e751d075e2e6 87 memcpy(buffer, (Buf + 3), numReceived - 3);
kalbers 19:e751d075e2e6 88 /*timeOfArrival = (buffer[3] << 24) + (buffer[4] << 16) + (buffer[5] << 8) + buffer[6];
naren 14:0c4a26dc6873 89 id = (buffer[1] << 8) + buffer[2];
kalbers 19:e751d075e2e6 90 pkgtype = buffer[0];*/
kalbers 19:e751d075e2e6 91 timeOfArrival = (Buf[6] << 24) + (Buf[7] << 16) + (Buf[8] << 8) + Buf[9];
kalbers 19:e751d075e2e6 92 id = (Buf[4] << 8) + Buf[5];
kalbers 19:e751d075e2e6 93 pkgtype = Buf[3];
robertbui 31:bf5f545621aa 94 if(ReceiveCount > 501 && ReceiveCount++%25 == 0) {//printf("%d %c\r\n", ReceiveCount, Buf[0]);
naren 22:0cc5b7e55419 95 printf("%f %f %f %f %f %f %f %f\r\n", corrected->roll, corrected->pitch, corrected->yaw,
naren 22:0cc5b7e55419 96 corrected->fingers[0], corrected->fingers[1], corrected->fingers[2],
naren 22:0cc5b7e55419 97 corrected->fingers[3], corrected->fingers[4]);
robertbui 31:bf5f545621aa 98 }
naren 14:0c4a26dc6873 99 switch (pkgtype){
naren 14:0c4a26dc6873 100 case STREAM_FINGERS_QUATERNION:
naren 14:0c4a26dc6873 101 {
kalbers 18:6737bf53ccd1 102 ExtractFingersQuat();
naren 14:0c4a26dc6873 103 }
naren 14:0c4a26dc6873 104 break;
naren 14:0c4a26dc6873 105 case STREAM_QUATERNION:
naren 14:0c4a26dc6873 106 {
kalbers 18:6737bf53ccd1 107 ExtractQuat();
naren 14:0c4a26dc6873 108 }
naren 14:0c4a26dc6873 109 break;
naren 14:0c4a26dc6873 110 case STREAM_FINGERS_RAW:
naren 14:0c4a26dc6873 111 {
kalbers 18:6737bf53ccd1 112 ExtractFingersRaw();
naren 14:0c4a26dc6873 113 }
naren 14:0c4a26dc6873 114 break;
naren 14:0c4a26dc6873 115 case STREAM_RAW:
naren 14:0c4a26dc6873 116 {
kalbers 18:6737bf53ccd1 117 ExtractRaw();
naren 14:0c4a26dc6873 118 }
naren 14:0c4a26dc6873 119 break;
naren 14:0c4a26dc6873 120
naren 14:0c4a26dc6873 121 case STREAM_FINGERS:
naren 14:0c4a26dc6873 122 {
kalbers 18:6737bf53ccd1 123 ExtractFingers();
naren 14:0c4a26dc6873 124 }
naren 14:0c4a26dc6873 125 break;
naren 14:0c4a26dc6873 126 }
kalbers 23:a8de4f87cada 127 NewData = true;
kalbers 19:e751d075e2e6 128 Buf[0] = 0; //Clear Token so no repeats.
naren 14:0c4a26dc6873 129 notConnectedCount = 0;
kalbers 19:e751d075e2e6 130 numReceived = 0;
naren 14:0c4a26dc6873 131 }
kalbers 19:e751d075e2e6 132 else
kalbers 19:e751d075e2e6 133 {
kalbers 19:e751d075e2e6 134 if (bcc!=Buf[pkglen+1])
naren 14:0c4a26dc6873 135 notConnectedCount++;
naren 14:0c4a26dc6873 136 }
naren 14:0c4a26dc6873 137 }
naren 22:0cc5b7e55419 138 // Normalize the data for the first 1000 samples
robertbui 31:bf5f545621aa 139 if (ReceiveCount < 500) {
naren 22:0cc5b7e55419 140 correction->roll = (correction->roll*ReceiveCount + gl->roll)/(ReceiveCount+1);
naren 22:0cc5b7e55419 141 correction->pitch = (correction->pitch*ReceiveCount + gl->pitch)/(ReceiveCount+1);
naren 22:0cc5b7e55419 142 correction->yaw = (correction->yaw*ReceiveCount + gl->yaw)/(ReceiveCount+1);
naren 22:0cc5b7e55419 143 for (int iter = 0; iter < 5; iter++)
naren 22:0cc5b7e55419 144 correction->fingers[iter] = (correction->fingers[iter]*ReceiveCount + gl->fingers[iter])/(ReceiveCount+1);
naren 22:0cc5b7e55419 145 }
naren 22:0cc5b7e55419 146 else {
naren 22:0cc5b7e55419 147 corrected->roll = gl->roll - correction->roll;
naren 22:0cc5b7e55419 148 corrected->pitch = gl->pitch - correction->pitch;
naren 22:0cc5b7e55419 149 corrected->yaw = gl->yaw - correction->yaw;
naren 22:0cc5b7e55419 150 for (int iter = 0; iter < 5; iter++)
naren 22:0cc5b7e55419 151 corrected->fingers[iter] = gl->fingers[iter] - correction->fingers[iter];
robertbui 31:bf5f545621aa 152 }
kalbers 3:5f5d75cba8e1 153 }
robertbui 31:bf5f545621aa 154 else {
kalbers 15:b48ac00af0b8 155 //printf("couldn't get data %d\r\n", notConnectedCount);
kalbers 19:e751d075e2e6 156 // Re-establishing communication in case no data is received for 1s (20 frames per second*1 = 20)
robertbui 31:bf5f545621aa 157 if (notConnectedCount > 200) {
naren 22:0cc5b7e55419 158 printf("Connection broke! Trying to re-establish... %d %c\r\n",numReceived,Buf[0]);
kalbers 19:e751d075e2e6 159 //GloveSocket.Reconnect();
kalbers 19:e751d075e2e6 160 //StreamData(STREAM_QUATERNION);
kalbers 19:e751d075e2e6 161 notConnectedCount = 0;
robertbui 31:bf5f545621aa 162 }
kalbers 19:e751d075e2e6 163 numReceived = 0;
robertbui 31:bf5f545621aa 164 notConnectedCount++;
kalbers 3:5f5d75cba8e1 165 }
robertbui 31:bf5f545621aa 166
kalbers 3:5f5d75cba8e1 167 }
kalbers 3:5f5d75cba8e1 168
kalbers 18:6737bf53ccd1 169 void DataGlove::ExtractFingersQuat()
kalbers 18:6737bf53ccd1 170 {
kalbers 18:6737bf53ccd1 171 k = 7;
kalbers 18:6737bf53ccd1 172 q0 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 173 k += 4;
kalbers 18:6737bf53ccd1 174 q1 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 175 k += 4;
kalbers 18:6737bf53ccd1 176 q2 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 177 k += 4;
kalbers 18:6737bf53ccd1 178 q3 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 179 k += 4;
kalbers 18:6737bf53ccd1 180 for (y = 0; y < 5; y++)
kalbers 18:6737bf53ccd1 181 {
kalbers 18:6737bf53ccd1 182 gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k + 1]);
kalbers 18:6737bf53ccd1 183 k += 2;
kalbers 18:6737bf53ccd1 184 }
kalbers 18:6737bf53ccd1 185 q00 = q0 / 32768.0;
kalbers 18:6737bf53ccd1 186 q11 = q1 / 32768.0;
kalbers 18:6737bf53ccd1 187 q22 = q2 / 32768.0;
kalbers 18:6737bf53ccd1 188 q33 = q3 / 32768.0;
kalbers 18:6737bf53ccd1 189
kalbers 18:6737bf53ccd1 190 norm = sqrt(q00 * q00 + q11 * q11 + q22 * q22 + q33 * q33);
kalbers 18:6737bf53ccd1 191 test = q00 * q11 + q22 * q33;
kalbers 18:6737bf53ccd1 192
kalbers 18:6737bf53ccd1 193 roll = 180.0 * atan2(2 * (q00 * q11 + q22 * q33), 1 - 2 * (q11 * q11 + q22 * q22)) / 3.1415;
kalbers 18:6737bf53ccd1 194 pitch = 180.0 * asin(2 * (q00 * q22 - q33 * q11)) / 3.1415;
kalbers 18:6737bf53ccd1 195 yaw = 180.0 * atan2(2 * (q00 * q33 + q11 * q22), 1 - 2 * (q22 * q22 + q33 * q33)) / 3.1415;
kalbers 18:6737bf53ccd1 196
kalbers 18:6737bf53ccd1 197 gl->roll = roll;
kalbers 18:6737bf53ccd1 198 gl->pitch = pitch;
kalbers 18:6737bf53ccd1 199 gl->yaw = yaw;
kalbers 18:6737bf53ccd1 200 gl->lastPkgTime = timeOfArrival;
kalbers 18:6737bf53ccd1 201
kalbers 18:6737bf53ccd1 202 }
kalbers 18:6737bf53ccd1 203 void DataGlove::ExtractQuat()
kalbers 18:6737bf53ccd1 204 {
kalbers 18:6737bf53ccd1 205 k = 7;
kalbers 18:6737bf53ccd1 206 q0 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 207 k += 4;
kalbers 18:6737bf53ccd1 208 q1 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 209 k += 4;
kalbers 18:6737bf53ccd1 210 q2 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 211 k += 4;
kalbers 18:6737bf53ccd1 212 q3 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
kalbers 18:6737bf53ccd1 213 k += 4;
kalbers 18:6737bf53ccd1 214 for (y = 0; y < 5; y++)
kalbers 18:6737bf53ccd1 215 {
kalbers 18:6737bf53ccd1 216 gl->fingers[y] = 0.0;
kalbers 18:6737bf53ccd1 217 }
kalbers 18:6737bf53ccd1 218 q00 = q0 / 32768.0;
kalbers 18:6737bf53ccd1 219 q11 = q1 / 32768.0;
kalbers 18:6737bf53ccd1 220 q22 = q2 / 32768.0;
kalbers 18:6737bf53ccd1 221 q33 = q3 / 32768.0;
kalbers 18:6737bf53ccd1 222
kalbers 18:6737bf53ccd1 223 norm = sqrt(q00 * q00 + q11 * q11 + q22 * q22 + q33 * q33);
kalbers 18:6737bf53ccd1 224 test = q00 * q11 + q22 * q33;
kalbers 18:6737bf53ccd1 225 roll = 180.0 * atan2(2 * (q00 * q11 + q22 * q33), 1 - 2 * (q11 * q11 + q22 * q22)) / 3.1415;
kalbers 18:6737bf53ccd1 226 pitch = 180.0 * asin(2 * (q00 * q22 - q33 * q11)) / 3.1415;
kalbers 18:6737bf53ccd1 227 yaw = 180.0 * atan2(2 * (q00 * q33 + q11 * q22), 1 - 2 * (q22 * q22 + q33 * q33)) / 3.1415;
kalbers 18:6737bf53ccd1 228
kalbers 18:6737bf53ccd1 229 gl->roll = roll;
kalbers 18:6737bf53ccd1 230 gl->pitch = pitch;
kalbers 18:6737bf53ccd1 231 gl->yaw = yaw;
kalbers 18:6737bf53ccd1 232 gl->lastPkgTime = timeOfArrival;
kalbers 18:6737bf53ccd1 233
kalbers 18:6737bf53ccd1 234 }
kalbers 18:6737bf53ccd1 235
kalbers 18:6737bf53ccd1 236 void DataGlove::ExtractFingersRaw()
kalbers 3:5f5d75cba8e1 237 {
kalbers 18:6737bf53ccd1 238 k = 7;
kalbers 18:6737bf53ccd1 239 for (j = 0; j < 3; j++)
kalbers 18:6737bf53ccd1 240 {
kalbers 18:6737bf53ccd1 241 gl->gyro[j] = (buffer[k] << 8) + (buffer[k+1]);
kalbers 18:6737bf53ccd1 242 if (gl->gyro[j] > 0x7fff)
kalbers 18:6737bf53ccd1 243 gl->gyro[j] -= 0x10000;
kalbers 18:6737bf53ccd1 244 k += 2;
kalbers 18:6737bf53ccd1 245 }
kalbers 18:6737bf53ccd1 246 for (j = 0; j < 3; j++)
kalbers 18:6737bf53ccd1 247 {
kalbers 18:6737bf53ccd1 248 gl->magn[j] = (buffer[k] << 8) + (buffer[k+1]);
kalbers 18:6737bf53ccd1 249 if (gl->magn[j] > 0x7fff)
kalbers 18:6737bf53ccd1 250 gl->magn[j] -= 0x10000;
kalbers 18:6737bf53ccd1 251 k += 2;
kalbers 18:6737bf53ccd1 252 }
kalbers 18:6737bf53ccd1 253 for (j = 0; j < 3; j++)
kalbers 18:6737bf53ccd1 254 {
kalbers 18:6737bf53ccd1 255 gl->accel[j] = (buffer[k] << 8) + (buffer[k+1]);
kalbers 18:6737bf53ccd1 256 if (gl->accel[j]>0x7fff)
kalbers 18:6737bf53ccd1 257 gl->accel[j] -= 0x10000;
kalbers 18:6737bf53ccd1 258 k += 2;
kalbers 18:6737bf53ccd1 259 }
kalbers 18:6737bf53ccd1 260 for (y = 0; y < 5; y++)
kalbers 18:6737bf53ccd1 261 {
kalbers 18:6737bf53ccd1 262 gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k+1]);
kalbers 18:6737bf53ccd1 263 k += 2;
kalbers 18:6737bf53ccd1 264 }
kalbers 18:6737bf53ccd1 265
kalbers 18:6737bf53ccd1 266 gl->roll = 0.0;
kalbers 18:6737bf53ccd1 267 gl->pitch = 0.0;
kalbers 18:6737bf53ccd1 268 gl->yaw = 0.0;
kalbers 18:6737bf53ccd1 269 gl->lastPkgTime = timeOfArrival;
kalbers 18:6737bf53ccd1 270 }
kalbers 18:6737bf53ccd1 271
kalbers 18:6737bf53ccd1 272 void DataGlove::ExtractRaw()
kalbers 18:6737bf53ccd1 273 {
kalbers 18:6737bf53ccd1 274 k = 7;
kalbers 18:6737bf53ccd1 275 for (j = 0; j < 3; j++){
kalbers 18:6737bf53ccd1 276 gl->gyro[j] = (buffer[k] << 8) + (buffer[k+1]);
kalbers 18:6737bf53ccd1 277 if (gl->gyro[j] > 0x7fff)
kalbers 18:6737bf53ccd1 278 gl->gyro[j] -= 0x10000;
kalbers 18:6737bf53ccd1 279 k += 2;
kalbers 18:6737bf53ccd1 280 }
kalbers 18:6737bf53ccd1 281 for (j = 0; j < 3; j++){
kalbers 18:6737bf53ccd1 282 gl->magn[j] = (buffer[k] << 8) + (buffer[k+1]);
kalbers 18:6737bf53ccd1 283 if (gl->magn[j] > 0x7fff)
kalbers 18:6737bf53ccd1 284 gl->magn[j] -= 0x10000;
kalbers 18:6737bf53ccd1 285 k += 2;
kalbers 18:6737bf53ccd1 286 }
kalbers 18:6737bf53ccd1 287 for (j = 0; j < 3; j++){
kalbers 18:6737bf53ccd1 288 gl->accel[j] = (buffer[k] << 8) + (buffer[k+1]);
kalbers 18:6737bf53ccd1 289 if (gl->accel[j] > 0x7fff)
kalbers 18:6737bf53ccd1 290 gl->accel[j] -= 0x10000;
kalbers 18:6737bf53ccd1 291 k += 2;
kalbers 18:6737bf53ccd1 292 }
kalbers 18:6737bf53ccd1 293 int y = 0;
kalbers 18:6737bf53ccd1 294 for (y = 0; y < 5; y++){
kalbers 18:6737bf53ccd1 295 gl->fingers[y] = 0.0;
kalbers 18:6737bf53ccd1 296 }
kalbers 18:6737bf53ccd1 297
kalbers 18:6737bf53ccd1 298 gl->roll = 0.0;
kalbers 18:6737bf53ccd1 299 gl->pitch = 0.0;
kalbers 18:6737bf53ccd1 300 gl->yaw = 0.0;
kalbers 18:6737bf53ccd1 301 gl->lastPkgTime = timeOfArrival;
kalbers 18:6737bf53ccd1 302 }
kalbers 18:6737bf53ccd1 303
kalbers 18:6737bf53ccd1 304 void DataGlove::ExtractFingers()
kalbers 18:6737bf53ccd1 305 {
kalbers 18:6737bf53ccd1 306 k = 7;
kalbers 18:6737bf53ccd1 307 for (y = 0; y < 5; y++){
kalbers 18:6737bf53ccd1 308 gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k+1]);
kalbers 18:6737bf53ccd1 309 k += 2;
kalbers 18:6737bf53ccd1 310 }
kalbers 18:6737bf53ccd1 311
kalbers 18:6737bf53ccd1 312 gl->roll = 0.0;
kalbers 18:6737bf53ccd1 313 gl->pitch = 0.0;
kalbers 18:6737bf53ccd1 314 gl->yaw = 0.0;
kalbers 18:6737bf53ccd1 315 gl->lastPkgTime = timeOfArrival;
kalbers 23:a8de4f87cada 316 }
kalbers 23:a8de4f87cada 317
kalbers 23:a8de4f87cada 318 Glove DataGlove::GetCurrentValues()
kalbers 23:a8de4f87cada 319 {
kalbers 23:a8de4f87cada 320 return *gl;
kalbers 23:a8de4f87cada 321 }
kalbers 23:a8de4f87cada 322 bool DataGlove::CheckForNewValues()
kalbers 23:a8de4f87cada 323 {
kalbers 23:a8de4f87cada 324 if(NewData == true)
kalbers 23:a8de4f87cada 325 {
kalbers 23:a8de4f87cada 326 NewData = false;
kalbers 23:a8de4f87cada 327 return true;
kalbers 23:a8de4f87cada 328 }else
kalbers 23:a8de4f87cada 329 {
kalbers 23:a8de4f87cada 330 return false;
kalbers 23:a8de4f87cada 331 }
kalbers 3:5f5d75cba8e1 332 }