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
DataGlove.cpp
- Committer:
- kalbers
- Date:
- 2014-12-07
- Revision:
- 15:b48ac00af0b8
- Parent:
- 14:0c4a26dc6873
- Child:
- 18:6737bf53ccd1
File content as of revision 15:b48ac00af0b8:
#include "DataGlove.h"
#define STREAM_FINGERS_QUATERNION 1
#define STREAM_QUATERNION 2
#define STREAM_FINGERS_RAW 3
#define STREAM_RAW 4
#define STREAM_FINGERS 5
DataGlove::DataGlove()
{
notConnectedCount = 0;
gl = new Glove;
}
DataGlove::~DataGlove()
{
delete(gl);
}
void DataGlove::Init()
{
GloveSocket.Init();
GloveSocket.Connect();
StreamData();
}
void DataGlove::StreamData()
{
SendBuf[0] = '$';
SendBuf[1] = 0x0A;
SendBuf[2] = 0x03;
// Streaming quaternion data
SendBuf[3] = STREAM_QUATERNION;
SendBuf[4] = (SendBuf[0]+SendBuf[1]+SendBuf[2]+SendBuf[3])%256;
SendBuf[5] = '#';
GloveSocket.SendDataToGlove(SendBuf, 6);
}
void DataGlove::Receive()
{
raw = false;
numReceived = GloveSocket.GetDataFromBuffer(Buf, 1);
if (Buf[0] == '$') {
bcc = '$';
numReceived = GloveSocket.GetDataFromBuffer(Buf, 2);
if ((numReceived == 2) && (Buf[0] == 0x0a)) {
pkglen = Buf[1];
bcc += Buf[0];
bcc += Buf[1];
numReceived = GloveSocket.GetDataFromBuffer(Buf, pkglen);
if (numReceived < pkglen) {
notConnectedCount++;
}
for (u = 0; u < pkglen - 2; 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];
id = (buffer[1] << 8) + buffer[2];
pkgtype = buffer[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;
}
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);
}
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;
}
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;
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;
}
break;
}
notConnectedCount = 0;
}
else{
if (bcc!=Buf[pkglen-2])
notConnectedCount++;
}
}
}
else {
//printf("couldn't get data %d\r\n", notConnectedCount);
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::Parse()
{
}