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@18:6737bf53ccd1, 2014-12-08 (annotated)

Committer:
kalbers
Date:
Mon Dec 08 18:56:42 2014 +0000
Revision:
18:6737bf53ccd1
Parent:
15:b48ac00af0b8
Child:
19:e751d075e2e6
Split Glove Data Extraction into Members Functions;

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