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@19:e751d075e2e6, 2014-12-08 (annotated)

Committer:
kalbers
Date:
Mon Dec 08 23:54:33 2014 +0000
Revision:
19:e751d075e2e6
Parent:
18:6737bf53ccd1
Child:
22:0cc5b7e55419
Put Reconnect error recovering in GloveWifi instead of DataGlove;

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