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@14:0c4a26dc6873, 2014-12-07 (annotated)

Committer:
naren
Date:
Sun Dec 07 19:50:47 2014 +0000
Revision:
14:0c4a26dc6873
Parent:
13:c701f1122797
Child:
15:b48ac00af0b8
Added new glove parsing; ; Existing bugs: ; Getting stuck at GloveWifi::GetDataFromBuffer; LED glows yellow (can it be the problem)?

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