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