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@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?

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 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 }