Robert Bui
/
CubicHand
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: MMA8451Q Multi_WS2811 NVIC_set_all_priorities TSI cc3000_hostdriver_mbedsocket mbed
Fork of
CubicHand
by 
Model-Based Team
Revision 21:5b987194387d, committed 2014-12-10
- Comitter:
- joseoyola
- Date:
- Wed Dec 10 22:46:44 2014 +0000
- Parent:
- 20:c2bc970bbff8
- Parent:
- 19:e751d075e2e6
- Child:
- 22:0cc5b7e55419
- Child:
- 24:554ea084eddf
- Commit message:
- Merged.
Changed in this revision
--- a/DataGlove.cpp Wed Dec 10 22:45:49 2014 +0000
+++ b/DataGlove.cpp Wed Dec 10 22:46:44 2014 +0000
@@ -10,6 +10,8 @@
{
notConnectedCount = 0;
gl = new Glove;
+ ReceiveCount = 0;
+ numReceived = 0;
}
DataGlove::~DataGlove()
@@ -21,212 +23,267 @@
{
GloveSocket.Init();
GloveSocket.Connect();
- StreamData();
+ StreamData(STREAM_QUATERNION);
}
-void DataGlove::StreamData()
+void DataGlove::StreamData(uint8_t DataType)
{
SendBuf[0] = '$';
SendBuf[1] = 0x0A;
SendBuf[2] = 0x03;
// Streaming quaternion data
- SendBuf[3] = STREAM_QUATERNION;
+ SendBuf[3] = DataType;
SendBuf[4] = (SendBuf[0]+SendBuf[1]+SendBuf[2]+SendBuf[3])%256;
SendBuf[5] = '#';
GloveSocket.SendDataToGlove(SendBuf, 6);
}
+void DataGlove::StopSampling()
+{
+ SendBuf[0] = '$';
+ SendBuf[1] = 0x0B;
+ SendBuf[2] = 0x02;
+ // Streaming quaternion data
+ SendBuf[3] = (SendBuf[0]+SendBuf[1]+SendBuf[2])%256;
+ SendBuf[4] = '#';
+ GloveSocket.SendDataToGlove(SendBuf, 6);
+}
+
void DataGlove::Receive()
{
raw = false;
- numReceived = GloveSocket.GetDataFromBuffer(Buf, 1);
- if (Buf[0] == '$') {
+ numReceived += GloveSocket.GetDataFromBuffer(Buf, 1);
+ //if(ReceiveCount++%25 == 0) printf("%d %c\r\n", ReceiveCount, Buf[0]);
+ if (Buf[0] == '$' && numReceived > 0)
+ {
bcc = '$';
- numReceived = GloveSocket.GetDataFromBuffer(Buf, 2);
- if ((numReceived == 2) && (Buf[0] == 0x0a)) {
- pkglen = Buf[1];
- bcc += Buf[0];
+ numReceived += GloveSocket.GetDataFromBuffer((Buf+numReceived), 2);
+ if ((numReceived > 2) && (Buf[1] == 0x0a))
+ {
+ pkglen = Buf[2];
bcc += Buf[1];
- numReceived = GloveSocket.GetDataFromBuffer(Buf, pkglen);
- if (numReceived < pkglen) {
- notConnectedCount++;
+ bcc += Buf[2];
+ numReceived += GloveSocket.GetDataFromBuffer((Buf+numReceived), pkglen - numReceived + 3);
+ if ((numReceived - 3 )< pkglen)
+ {
+ if(numReceived == 0) notConnectedCount++;
+ return;
}
- for (u = 0; u < pkglen - 2; u++) {
+ for (u = 3; u <= pkglen; u++)
+ {
bcc += Buf[u];
}
- if ((numReceived == pkglen) && (bcc == Buf[pkglen-2])) {
- memcpy(buffer, Buf, numReceived);
- timeOfArrival = (buffer[3] << 24) + (buffer[4] << 16) + (buffer[5] << 8) + buffer[6];
+ if ((numReceived - 3 == pkglen) && (bcc == Buf[pkglen+1]))
+ {
+ memcpy(buffer, (Buf + 3), numReceived - 3);
+ /*timeOfArrival = (buffer[3] << 24) + (buffer[4] << 16) + (buffer[5] << 8) + buffer[6];
id = (buffer[1] << 8) + buffer[2];
- pkgtype = buffer[0];
+ pkgtype = buffer[0];*/
+ timeOfArrival = (Buf[6] << 24) + (Buf[7] << 16) + (Buf[8] << 8) + Buf[9];
+ id = (Buf[4] << 8) + Buf[5];
+ pkgtype = Buf[3];
+ if(ReceiveCount++%25 == 0) printf("%d %c\r\n", ReceiveCount, Buf[0]);
switch (pkgtype){
case STREAM_FINGERS_QUATERNION:
{
- k = 7;
- q0 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
- k += 4;
- q1 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
- k += 4;
- q2 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
- k += 4;
- q3 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
- k += 4;
- for (y = 0; y < 5; y++) {
- gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k + 1]);
- k += 2;
- }
- q00 = q0 / 32768.0;
- q11 = q1 / 32768.0;
- q22 = q2 / 32768.0;
- q33 = q3 / 32768.0;
-
- norm = sqrt(q00 * q00 + q11 * q11 + q22 * q22 + q33 * q33);
- test = q00 * q11 + q22 * q33;
-
- roll = 180.0 * atan2(2 * (q00 * q11 + q22 * q33), 1 - 2 * (q11 * q11 + q22 * q22)) / 3.1415;
- pitch = 180.0 * asin(2 * (q00 * q22 - q33 * q11)) / 3.1415;
- yaw = 180.0 * atan2(2 * (q00 * q33 + q11 * q22), 1 - 2 * (q22 * q22 + q33 * q33)) / 3.1415;
-
- gl->roll = roll;
- gl->pitch = pitch;
- gl->yaw = yaw;
- gl->lastPkgTime = timeOfArrival;
+ ExtractFingersQuat();
}
break;
case STREAM_QUATERNION:
{
- k = 7;
- q0 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
- k += 4;
- q1 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
- k += 4;
- q2 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
- k += 4;
- q3 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
- k += 4;
- for (y = 0; y < 5; y++) {
- gl->fingers[y] = 0.0;
- }
- q00 = q0 / 32768.0;
- q11 = q1 / 32768.0;
- q22 = q2 / 32768.0;
- q33 = q3 / 32768.0;
-
- norm = sqrt(q00 * q00 + q11 * q11 + q22 * q22 + q33 * q33);
- test = q00 * q11 + q22 * q33;
- roll = 180.0 * atan2(2 * (q00 * q11 + q22 * q33), 1 - 2 * (q11 * q11 + q22 * q22)) / 3.1415;
- pitch = 180.0 * asin(2 * (q00 * q22 - q33 * q11)) / 3.1415;
- yaw = 180.0 * atan2(2 * (q00 * q33 + q11 * q22), 1 - 2 * (q22 * q22 + q33 * q33)) / 3.1415;
-
- gl->roll = roll;
- gl->pitch = pitch;
- gl->yaw = yaw;
- gl->lastPkgTime = timeOfArrival;
- printf("roll: %f\tpitch: %f\tyaw: %f\r\n", gl->roll, gl->pitch, gl->yaw);
+ ExtractQuat();
+ //printf("roll: %f\tpitch: %f\tyaw: %f\r\n", gl->roll, gl->pitch, gl->yaw);
}
break;
case STREAM_FINGERS_RAW:
{
- k = 7;
- for (j = 0; j < 3; j++){
- gl->gyro[j] = (buffer[k] << 8) + (buffer[k+1]);
- if (gl->gyro[j] > 0x7fff)
- gl->gyro[j] -= 0x10000;
- k += 2;
- }
- for (j = 0; j < 3; j++){
- gl->magn[j] = (buffer[k] << 8) + (buffer[k+1]);
- if (gl->magn[j] > 0x7fff)
- gl->magn[j] -= 0x10000;
- k += 2;
- }
- for (j = 0; j < 3; j++){
- gl->accel[j] = (buffer[k] << 8) + (buffer[k+1]);
- if (gl->accel[j]>0x7fff)
- gl->accel[j] -= 0x10000;
- k += 2;
- }
- for (y = 0; y < 5; y++){
- gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k+1]);
- k += 2;
- }
-
- gl->roll = 0.0;
- gl->pitch = 0.0;
- gl->yaw = 0.0;
- gl->lastPkgTime = timeOfArrival;
+ ExtractFingersRaw();
}
break;
case STREAM_RAW:
{
- k = 7;
- for (j = 0; j < 3; j++){
- gl->gyro[j] = (buffer[k] << 8) + (buffer[k+1]);
- if (gl->gyro[j] > 0x7fff)
- gl->gyro[j] -= 0x10000;
- k += 2;
- }
- for (j = 0; j < 3; j++){
- gl->magn[j] = (buffer[k] << 8) + (buffer[k+1]);
- if (gl->magn[j] > 0x7fff)
- gl->magn[j] -= 0x10000;
- k += 2;
- }
- for (j = 0; j < 3; j++){
- gl->accel[j] = (buffer[k] << 8) + (buffer[k+1]);
- if (gl->accel[j] > 0x7fff)
- gl->accel[j] -= 0x10000;
- k += 2;
- }
- int y = 0;
- for (y = 0; y < 5; y++){
- gl->fingers[y] = 0.0;
- }
-
- gl->roll = 0.0;
- gl->pitch = 0.0;
- gl->yaw = 0.0;
- gl->lastPkgTime = timeOfArrival;
+ ExtractRaw();
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]);
}
break;
case STREAM_FINGERS:
{
- k = 7;
- for (y = 0; y < 5; y++){
- gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k+1]);
- k += 2;
- }
-
- gl->roll = 0.0;
- gl->pitch = 0.0;
- gl->yaw = 0.0;
- gl->lastPkgTime = timeOfArrival;
+ ExtractFingers();
}
break;
}
+ Buf[0] = 0; //Clear Token so no repeats.
notConnectedCount = 0;
+ numReceived = 0;
}
- else{
- if (bcc!=Buf[pkglen-2])
+ else
+ {
+ if (bcc!=Buf[pkglen+1])
notConnectedCount++;
}
}
}
else {
//printf("couldn't get data %d\r\n", notConnectedCount);
+ // Re-establishing communication in case no data is received for 1s (20 frames per second*1 = 20)
+ if (notConnectedCount > 200) {
+ printf("Connection broke! Trying to re-establish... %d \r\n",numReceived);
+ //GloveSocket.Reconnect();
+ //StreamData(STREAM_QUATERNION);
+ notConnectedCount = 0;
+ }
+ numReceived = 0;
notConnectedCount++;
}
- // Re-establishing communication in case no data is received for 1s (20 frames per second*1 = 20)
- if (notConnectedCount > 20) {
- printf("Connection broke! Trying to re-establish...\r\n");
- GloveSocket.Reconnect();
- StreamData();
- notConnectedCount = 0;
+
+}
+
+void DataGlove::ExtractFingersQuat()
+{
+ k = 7;
+ q0 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
+ k += 4;
+ q1 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
+ k += 4;
+ q2 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
+ k += 4;
+ q3 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
+ k += 4;
+ for (y = 0; y < 5; y++)
+ {
+ gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k + 1]);
+ k += 2;
}
+ q00 = q0 / 32768.0;
+ q11 = q1 / 32768.0;
+ q22 = q2 / 32768.0;
+ q33 = q3 / 32768.0;
+
+ norm = sqrt(q00 * q00 + q11 * q11 + q22 * q22 + q33 * q33);
+ test = q00 * q11 + q22 * q33;
+
+ roll = 180.0 * atan2(2 * (q00 * q11 + q22 * q33), 1 - 2 * (q11 * q11 + q22 * q22)) / 3.1415;
+ pitch = 180.0 * asin(2 * (q00 * q22 - q33 * q11)) / 3.1415;
+ yaw = 180.0 * atan2(2 * (q00 * q33 + q11 * q22), 1 - 2 * (q22 * q22 + q33 * q33)) / 3.1415;
+
+ gl->roll = roll;
+ gl->pitch = pitch;
+ gl->yaw = yaw;
+ gl->lastPkgTime = timeOfArrival;
+
+}
+void DataGlove::ExtractQuat()
+{
+ k = 7;
+ q0 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
+ k += 4;
+ q1 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
+ k += 4;
+ q2 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
+ k += 4;
+ q3 = (buffer[k] << 24) + (buffer[k+1] << 16) + (buffer[k+2] << 8) + buffer[k+3];
+ k += 4;
+ for (y = 0; y < 5; y++)
+ {
+ gl->fingers[y] = 0.0;
+ }
+ q00 = q0 / 32768.0;
+ q11 = q1 / 32768.0;
+ q22 = q2 / 32768.0;
+ q33 = q3 / 32768.0;
+
+ norm = sqrt(q00 * q00 + q11 * q11 + q22 * q22 + q33 * q33);
+ test = q00 * q11 + q22 * q33;
+ roll = 180.0 * atan2(2 * (q00 * q11 + q22 * q33), 1 - 2 * (q11 * q11 + q22 * q22)) / 3.1415;
+ pitch = 180.0 * asin(2 * (q00 * q22 - q33 * q11)) / 3.1415;
+ yaw = 180.0 * atan2(2 * (q00 * q33 + q11 * q22), 1 - 2 * (q22 * q22 + q33 * q33)) / 3.1415;
+
+ gl->roll = roll;
+ gl->pitch = pitch;
+ gl->yaw = yaw;
+ gl->lastPkgTime = timeOfArrival;
+
}
-void DataGlove::Parse()
+void DataGlove::ExtractFingersRaw()
+{
+ k = 7;
+ for (j = 0; j < 3; j++)
+ {
+ gl->gyro[j] = (buffer[k] << 8) + (buffer[k+1]);
+ if (gl->gyro[j] > 0x7fff)
+ gl->gyro[j] -= 0x10000;
+ k += 2;
+ }
+ for (j = 0; j < 3; j++)
+ {
+ gl->magn[j] = (buffer[k] << 8) + (buffer[k+1]);
+ if (gl->magn[j] > 0x7fff)
+ gl->magn[j] -= 0x10000;
+ k += 2;
+ }
+ for (j = 0; j < 3; j++)
+ {
+ gl->accel[j] = (buffer[k] << 8) + (buffer[k+1]);
+ if (gl->accel[j]>0x7fff)
+ gl->accel[j] -= 0x10000;
+ k += 2;
+ }
+ for (y = 0; y < 5; y++)
+ {
+ gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k+1]);
+ k += 2;
+ }
+
+ gl->roll = 0.0;
+ gl->pitch = 0.0;
+ gl->yaw = 0.0;
+ gl->lastPkgTime = timeOfArrival;
+}
+
+void DataGlove::ExtractRaw()
{
+ k = 7;
+ for (j = 0; j < 3; j++){
+ gl->gyro[j] = (buffer[k] << 8) + (buffer[k+1]);
+ if (gl->gyro[j] > 0x7fff)
+ gl->gyro[j] -= 0x10000;
+ k += 2;
+ }
+ for (j = 0; j < 3; j++){
+ gl->magn[j] = (buffer[k] << 8) + (buffer[k+1]);
+ if (gl->magn[j] > 0x7fff)
+ gl->magn[j] -= 0x10000;
+ k += 2;
+ }
+ for (j = 0; j < 3; j++){
+ gl->accel[j] = (buffer[k] << 8) + (buffer[k+1]);
+ if (gl->accel[j] > 0x7fff)
+ gl->accel[j] -= 0x10000;
+ k += 2;
+ }
+ int y = 0;
+ for (y = 0; y < 5; y++){
+ gl->fingers[y] = 0.0;
+ }
+
+ gl->roll = 0.0;
+ gl->pitch = 0.0;
+ gl->yaw = 0.0;
+ gl->lastPkgTime = timeOfArrival;
+}
+
+void DataGlove::ExtractFingers()
+{
+ k = 7;
+ for (y = 0; y < 5; y++){
+ gl->fingers[y] = 0.1 * ((buffer[k] << 8) + buffer[k+1]);
+ k += 2;
+ }
+
+ gl->roll = 0.0;
+ gl->pitch = 0.0;
+ gl->yaw = 0.0;
+ gl->lastPkgTime = timeOfArrival;
}
\ No newline at end of file
--- a/DataGlove.h Wed Dec 10 22:45:49 2014 +0000
+++ b/DataGlove.h Wed Dec 10 22:46:44 2014 +0000
@@ -19,18 +19,16 @@
DataGlove();
~DataGlove();
void Init();
- void StreamData();
void Receive();
int16_t finger1, finger2, finger3, finger4, finger5;
int16_t accelx, accely, accelz;
int16_t gyrox, gyroy, gyroz;
private:
- void Parse();
uint8_t SendBuf[20];
int16_t notConnectedCount;
- int8_t numReceived;
- uint8_t Buf[256], buffer[256];
+ long numReceived;
+ char Buf[256], buffer[256];
bool raw;
unsigned char bcc;
int pkglen, u;
@@ -40,8 +38,17 @@
double norm, test;
int q0, q1, q2, q3;
double q00, q11, q22, q33;
+ long ReceiveCount;
Glove *gl;
GloveWifi GloveSocket;
-
-
+
+ private:
+ void Parse();
+ void StreamData(uint8_t DataType);
+ void StopSampling();
+ void ExtractFingersQuat();
+ void ExtractQuat();
+ void ExtractFingersRaw();
+ void ExtractRaw();
+ void ExtractFingers();
};
\ No newline at end of file
--- a/GloveWifi.cpp Wed Dec 10 22:45:49 2014 +0000
+++ b/GloveWifi.cpp Wed Dec 10 22:46:44 2014 +0000
@@ -12,7 +12,8 @@
#define ECHO_SERVER_PORT 2000
#define MMA8451_I2C_ADDRESS (0x1d<<1)
-GloveWifi::GloveWifi()
+GloveWifi::GloveWifi():
+DropCount(0)
{
}
@@ -60,8 +61,8 @@
printf("Unable to connect to (%s) on port (%d) \r\n", ECHO_SERVER_ADDRESS, ECHO_SERVER_PORT);
wait(1);
}
- socket->set_blocking(true);
printf("Connected!\r\n");
+ socket->set_blocking(false, 10);
}
@@ -82,14 +83,29 @@
printf("Unable to connect to (%s) on port (%d) \r\n", ECHO_SERVER_ADDRESS, ECHO_SERVER_PORT);
wait(1);
}
- socket->set_blocking(true);
+ socket->set_blocking(false, 10);
printf("Reconnected!\r\n");
}
-uint8_t GloveWifi::GetDataFromBuffer(uint8_t * buf, uint16_t size)
+int GloveWifi::GetDataFromBuffer(char * buf, int size)
{
//printf("Buffer Size: %d\r\n", wifi->_simple_link.get_transmit_error());
- return socket->receive((char *)buf, size);
+ int numR = 0;
+ numR = socket->receive((char *)buf, size);
+ if(numR == -1)
+ {
+ DropCount++;
+ }else
+ {
+ DropCount = 0;
+ }
+ if(DropCount > 200)
+ {
+ printf("!\r\n");
+ Reconnect();
+ DropCount = 0;
+ }
+ return numR;
}
uint8_t GloveWifi::SendDataToGlove(uint8_t * buf, uint16_t size)
--- a/GloveWifi.h Wed Dec 10 22:45:49 2014 +0000
+++ b/GloveWifi.h Wed Dec 10 22:46:44 2014 +0000
@@ -15,11 +15,12 @@
void Connect();
void Disconnect();
void Reconnect();
- uint8_t GetDataFromBuffer(uint8_t * buf, uint16_t size);
+ int GetDataFromBuffer(char * buf, int size);
uint8_t SendDataToGlove(uint8_t * buf, uint16_t size);
private:
TCPSocketConnection * socket;
cc3000 * wifi;
+ uint32_t DropCount;
};
\ No newline at end of file