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

Committer:
kalbers
Date:
Thu Dec 11 15:28:27 2014 +0000
Revision:
46:a7c041aaf259
Parent:
42:4e1aaab14886
Child:
47:6d0d7bafcb6f
Changed Init function for Dataglove to stream nothing first then the correct data.

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 {
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 19:e751d075e2e6 63 numReceived += GloveSocket.GetDataFromBuffer(Buf, 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 42:4e1aaab14886 164 gl.fingers[y] = 0.1 * ((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 {
robertbui 41:367cab0162de 198 gl.fingers[y] = 0.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 {
kalbers 42:4e1aaab14886 244 gl.fingers[y] = 0.1 * ((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 42:4e1aaab14886 290 gl.fingers[y] = 0.1 * ((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 }