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@35:b6fb5dd65a98, 2014-12-11 (annotated)

Committer:
kalbers
Date:
Thu Dec 11 06:44:30 2014 +0000
Revision:
35:b6fb5dd65a98
Parent:
32:bf5f545621aa
Child:
37:996bd22a633f
Fixed Merge Issues

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