GloveObject.cpp

//###################################################################################
//#                                                                     Glove Object                                                                                    #
//#                                                             Used To Handle a 5DT Glove                                                                    #
//#                                                                       Author :                                                                            #
//#                                                                   Aymeric SUTEAU                                                                                    #
//#                                                                    LISA - ANGERS                                                                                    #
//###################################################################################


/* HEADER INCLUDE */
#include "GloveObject.h"
#include "GloveCalibration.h"
#include "GlovePlugin.h"
#include <string>

/*------------------------------- CALLBACK FUNCTION -------------------------------*/
void GloveCallback(LPVOID param) 
{
  // Lower the message rate to 
        // Cast void pointer to GloveObject type before using
        GloveObject *pGloveObject = (GloveObject*) param;
        // If we retrieve a valid pointer, we call the function which will update the sensors values
        if (pGloveObject)
                pGloveObject->UpdateGlove();
  // Hack to lower the message rate
  // Sleep(10);
}


/*-------------------------- CONSTRUCTOR AND DESTRUCTOR ---------------------------*/
GloveObject::GloveObject()
{
        pGlove = 0;
  calibration = 0;
  iUSBPort = 0;
        iType = FD_GLOVENONE;
        NbrOfSensors = -1;
        gestureIndex = -1;
  bCalibrationDone = true;
        pSensorValues = new float[NUMBEROFSENSOR];
  for (int i = 0; i < NUMBEROFSENSOR; i++)
  {
    *(pSensorValues + i) = 0.0f;
  }
}

GloveObject::~GloveObject() 
{
  // Destroy dynamic table containing data glove serial number 
  //delete [] szSerialNumber;  
  
  // Delete the callback function previously added (in order to stop the acquisition of sensors values)
        fdSetCallback(pGlove, NULL, NULL);

  // close the usb port
  Close();

  if (pSensorValues)
  {
    delete[] pSensorValues;
    pSensorValues = 0;
  }
  if (calibration)
  {
    delete calibration;
    calibration = 0;
  }
}


/*------------------------------ GETTERS AND SETTERS ------------------------------*/

std::string GloveObject::GetUSBPortName() 
{
        return sUSBPort;
}

int GloveObject::GetTypeID() 
{
        return iType;
}
std::string GloveObject::GetTypeName() 
{
        return sType;
}

int GloveObject::GetHandIndex() 
{
        return iHand;
}

std::string GloveObject::GetHandName() 
{
        return sHand;
}

int GloveObject::GetNbrOfSensors() 
{
        return NbrOfSensors;
}

std::string GloveObject::GetSerialNumber() 
{
  return serialNumber;
}

int GloveObject::GetUSBPortIndex() 
{
  return iUSBPort;
}

float* GloveObject::GetSensorValues()
{
   return pSensorValues;
}

fdGlove* GloveObject::GetGlove()
{
  return pGlove;
}

/*----- Return gesture index between 0 and 15 (thumb not handled) -----*/
int GloveObject::GetGestureIndex() 
{
        return this->gestureIndex;
}

void GloveObject::SetGestureIndex(int driverGesture) 
{
  // We get from driver
  // Gesture : 0 closed except for thumb
  // Thumb : 16 closed, 32 half closed, 0 open 
  // -1 for an unrecognized gesture
  
  // We want : 0 for closed , 1 for open in binary
  // And beginning with the thumb
  
  if (driverGesture != -1)
  {

    driverGesture = driverGesture<<1;

    // Thumb closed or half closed
    if (driverGesture & 0x60) // 0x60=0x30<<1=0b00110000<<1 meaning 48=16+32 fingers (both are thumb)
            gestureIndex = driverGesture & (0x1E); // 0x1E=0x0F<<1=0b00011110 meaning that the thumb will be set to 0 and others fingers will not be changed
    // Thumb open
    else
            gestureIndex = driverGesture | 0x01; // we have already moved the fingers so we just set the thumb at 1
  }
}

/*-------------------------------- ADDITIONAL FUNCTIONS ---------------------------------*/
/*----- Initialize glove object -----*/
bool GloveObject::Init() 
{
  // Check for any available USB glove
        unsigned short aPID[5];
        int nNumFound = 5, nChosen = 0;
  char portName[5] = "USB";
  bool bInitOk = false;
  //szSerialNumber = new char[12];

  // Scan USB ports
  fdScanUSB(aPID, nNumFound);
  // DEBUG : MMechostr(MSKDEBUG, "-> Init ...gloves found : %d\n", nNumFound);
  for (int i = 0; i < nNumFound; i++)
  {
    // Update the name of current USB port
    sprintf(portName, "USB%d", i);

                // Try to open the first device of the list, else try to open the next one
    pGlove = fdOpen(portName);
    if (pGlove == 0) 
    {
      // Initialization failed : data glove already opened
      MMechostr(MSKDEBUG, "-> Init ...failed : data glove n.%d already opened\n", i);
    }
    else 
    {
      // Set USB port and index
      sUSBPort = portName;
      iUSBPort = i;
      MMechostr(MSKDEBUG, "-> Init ...successful\n");
      bInitOk = true;
      break;
    }
        }

  // If no gloves have been found, we must exit the function
  if (!bInitOk) 
  {
    MMechostr(MSKDEBUG, "-> Init ...failed : couldn't open any data glove\n");
    return false;
  }

  // Retrieve the data glove serial number
  char tmpserial[20];
  fdGetSerialNumber(pGlove, tmpserial);
  MMechostr(MSKDEBUG, "Glove 5DT serial : %s", tmpserial);
  serialNumber = tmpserial;

  // Disable auto calibration (enabled by default)
  // fdSetAutoCalibrate(pGlove, false);

  // Set Glove Type and Hand Type
        iType = fdGetGloveType(pGlove);
        switch (iType) 
  {
                // Not a Glove : we consider that the initialization of the device was a failure
                case FD_GLOVENONE: 
                        sType="None";
                        return false; 
                        break;

                // Get Glove Type
                case FD_GLOVE7:    
                        sType="5DT Glove"; 
                        break;

                case FD_GLOVE7W:
                        sType="5DT Wireless Glove";
                        break;

                case FD_GLOVE16:
                        sType="16DT Glove";
                        break;

                case FD_GLOVE16W:
                        sType="16DT Wireless Glove";
                        break;

                case FD_GLOVE5U:
                        sType="DG5 Ultra Series";
                        break;

                case FD_GLOVE5UW:
                        sType="DG5 Wireless Ultra Series";
                        break;

                case FD_GLOVE5U_USB:
                        sType="DG5 Ultra USB";
                        break;

                case FD_GLOVE14U:
                        sType="DG14 Ultra Series";
                        break;

                case FD_GLOVE14UW: 
                        sType="DG14 Wireless Ultra Series";
                        break;

                case FD_GLOVE14U_USB:
                        sType="DG14 Ultra USB";
                        break;
        }
        iHand=fdGetGloveHand(pGlove);
        if (iHand == FD_HAND_RIGHT)
                sHand = "Right";
        else
                sHand = "Left";

        // Set Number of Glove Sensors
        NbrOfSensors = fdGetNumSensors(pGlove);

  // Set the update function callback (once the calibration is done)
        fdSetCallback(pGlove, (void*)GloveCallback, this);
        return true;
}

/*----- Calibtration function ------*/
void GloveObject::StartCalibration()
{
  if (calibration == 0)
  {
    //TODO : destroy calib
    calibration = new GloveCalibration(this);
    calibration->start();
  }
  else if (!calibration->GetIsRunning())
  {
    calibration->start();
  }
  else 
  {
    //TODO : MSKDEBUG to change
    MMechostr(MSKDEBUG, "Calibration already running");
  }
}

/*----- To be called by glove callback function -----*/
void GloveObject::UpdateGlove() 
{
        // If and only if a valid pointer to the glove device is available
  // And we add a hack to lower the message rate
        if (pGlove && timer.GetMilliseconds() > 10) 
  {
                fdGetSensorScaledAll(pGlove, pSensorValues);  // Update sensors values

    // Retrieve the current gesture according to the gesture mode
    // And check if its value has changed and send it to the event manager
    int oldGesture = gestureIndex;
    SetGestureIndex(fdGetGestureA(pGlove));
    if (gestureIndex != oldGesture)
    {
      // Notify Scol window that a new hand gesture has been made
                  OBJpostEvent(GLOVE_HAND_CB, (int)this, (LPARAM)gestureIndex);
    }
    
    //Create a message buffer to post the raw data
    float* rawMsg = new float[NUMBEROFSENSOR];
    for (int i = 0; i < NUMBEROFSENSOR; i++)
    {
      rawMsg[i] = pSensorValues[i];
    }

    //Send sensor values
    OBJpostEvent(GLOVE_NEWDATA_CB, (int)this, (LPARAM)rawMsg); 
    timer.Reset();
        }
}


/*----- Close glove -----*/
int GloveObject::Close() 
{
        int iErrorCode = fdClose(pGlove);   // Free the glove device and communication port
  pGlove = 0;                         // Make sure the pointer is reset
  return iErrorCode;
}