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@41:367cab0162de, 2014-12-11 (annotated)

Committer:
robertbui
Date:
Thu Dec 11 12:08:27 2014 +0000
Revision:
41:367cab0162de
Parent:
37:996bd22a633f
Child:
42:4e1aaab14886
intermediate debugging.

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