D:/SVN/Scol/trunk/plugins/glove5DT/src/GloveObject.cpp

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


/* HEADER INCLUDE */
#include "Glove.h"
#include <string.h>


/* GLOBAL VARIABLES */
// Thresholds for calibration procedure
float f_thresholdUpperCoeff = 0.3f, f_thresholdLowerCoeff = 0.3f;


/*------------------------------- CALLBACK FUNCTION -------------------------------*/
void GloveCallback(LPVOID param) 
{
        // 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();
}


/*-------------------------- CONSTRUCTOR AND DESTRUCTOR ---------------------------*/
GloveObject::GloveObject() : Thread() 
{
        SetGlove(NULL);
        SetPort(NULL);
        SetShowRaw(false);
        SetGloveType(FD_GLOVENONE);
        SetType("?");
        SetNbrOfSensors(-1);
        SetGestureIndex(-1);
  SetCalibrationDone(true);
  SetInitValuesDone(true);
  SetInitDone(false);
  SetUSBIndex(0);
  SetGestureMode(GESTURE_WITH_THUMB);

  // Thread handling
  this->start();
  this->isRunning = true;
}

GloveObject::~GloveObject() 
{
  // Destroy dynamic table containing data glove serial number 
  //delete [] szSerialNumber;  

  // Thread handling
  this->isRunning = false;
  this->stop();
}


/*------------------------------ GETTERS AND SETTERS ------------------------------*/
fdGlove * GloveObject::GetGlove() 
{
        return this->pGlove;
}

void GloveObject::SetGlove(fdGlove * glove)
{
        this->pGlove = glove;
}

char * GloveObject::GetPort() 
{
        return this->szPort;
}

void GloveObject::SetPort(char * port) 
{
        this->szPort = port;
}

bool GloveObject::IsShowRaw() 
{
        return this->bShowRaw;
}

void GloveObject::SetShowRaw(bool showRaw) 
{
        this->bShowRaw = showRaw;
}

int GloveObject::GetGloveType() 
{
        return this->iGloveType;
}

void GloveObject::SetGloveType(int gloveType) 
{
        this->iGloveType = gloveType;
}

char * GloveObject::GetType() 
{
        return this->szType;
}

void GloveObject::SetType(char * type) 
{
        this->szType = type;
}

int GloveObject::GetGloveHand() 
{
        return this->iGloveHand;
}

void GloveObject::SetGloveHand(int gloveHand) 
{
        this->iGloveHand = gloveHand;
}

char * GloveObject::GetHand() 
{
        return this->szHand;
}

void GloveObject::SetHand(char * hand) 
{
        this->szHand = hand;
}

int GloveObject::GetNbrOfSensors() 
{
        return this->iNbrOfSensors;
}

void GloveObject::SetNbrOfSensors(int nbSensors) 
{
        this->iNbrOfSensors = nbSensors;
}

std::string GloveObject::GetSerialNumber() 
{
  return this->szSerialNumber;
}

void GloveObject::SetSerialNumber(std::string serialNumber) 
{
  this->szSerialNumber = serialNumber;
}

bool GloveObject::GetCalibrationDone()
{
  return this->bCalibrationDone;
}

void GloveObject::SetCalibrationDone(bool status) 
{
  this->bCalibrationDone = status;
}

bool GloveObject::GetInitValuesDone() 
{
  return this->bInitValuesDone;
}

void GloveObject::SetInitValuesDone(bool status) 
{
  this->bInitValuesDone = status;
}

bool GloveObject::GetInitDone() 
{
  return this->bInitDone;
}

void GloveObject::SetInitDone(bool status) 
{
  this->bInitDone = status;
}

int GloveObject::GetUSBIndex() 
{
  return this->iUSBIndex;
}

void GloveObject::SetUSBIndex(int index) 
{
  this->iUSBIndex = index;
}

int GloveObject::GetGestureMode() 
{
  return this->iGestureMode;
}

void GloveObject::SetGestureMode(int gestureMode) 
{
  this->iGestureMode = gestureMode; 
}


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

void GloveObject::SetGestureIndex(int gestureIndex) 
{
        this->iGestureIndex = gestureIndex;
}


/*----- Return new gesture index between 0 and 31 (thumb handled) -----*/
int GloveObject::GetGestureWithThumbIndex() 
{
  int *pIndexValues, gestureValue = 0, thresholdDiff = 0;
  pIndexValues = new int[GetNbrOfSensors()];

  // Retrieve the current sensors values and the thresholds defined during auto calibration
  fdGetSensorRawAll(GetGlove(), pValues);
  fdGetCalibrationAll(GetGlove(), pMax, pMin);
  
  // Define a new index position (boolean) for each finger
  for (int i=0; i<GetNbrOfSensors(); i++) 
  {
          // Define new thresholds
    thresholdDiff = pMax[i] - pMin[i];
    pMax[i] -= (int)(f_thresholdUpperCoeff*thresholdDiff);
    pMin[i] += (int)(f_thresholdLowerCoeff*thresholdDiff);
        
          // Adjust thresholds with a coefficient
    // Normal position : -1
    if (pValues[i] >= pMin[i] && pValues[i] <= pMax[i]) 
    {
      pIndexValues[i] = -1;
    }
        
    // Closed finger : 0
    else if (pValues[i] < pMin[i]) 
    {
      pIndexValues[i] = 0;
    }
        
    // Opened finger : 1
    else if (pValues[i] > pMax[i]) 
    {
      pIndexValues[i] = 1;
    }
  }
  
  // Define new gesture index (-1 if normal gesture, otherwise between 0 and 31)
  if (pIndexValues[FD_THUMBNEAR] == -1 || pIndexValues[FD_INDEXNEAR] == -1 || pIndexValues[FD_MIDDLENEAR] == -1 || pIndexValues[FD_RINGNEAR] == -1  
                                       || pIndexValues[FD_LITTLENEAR] == -1) 
  {
    // Unrecognizable gesture
    gestureValue = -1;
  }
  
  // Otherwise, combine the index values of each finger
  else 
  {
    // Index between 0 and 31 (32 gestures are possible)
    gestureValue = pIndexValues[FD_THUMBNEAR] + 2*pIndexValues[FD_INDEXNEAR] + 4*pIndexValues[FD_MIDDLENEAR] + 8*pIndexValues[FD_RINGNEAR]
                                              + 16*pIndexValues[FD_LITTLENEAR];
  }
  return gestureValue;
}

void GloveObject::SetGestureWithThumbIndex(int gestureWithThumbIndex) 
{
        this->iGestureTHIndex = gestureWithThumbIndex;
}


/*-------------------------------- ADDITIONAL FUNCTIONS ---------------------------------*/
/*----- Initialize glove object -----*/
bool GloveObject::Init(int gestureMode) 
{
  // Check for any available USB glove
        unsigned short aPID[5];
        int nNumFound = 5, nChosen = 0;
  char portName[10] = "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
    SetGlove(fdOpen(portName, true));
    if (!GetGlove()) 
    {
      // Initialization failed : data glove already opened
      MMechostr(MSKDEBUG, "-> Init ...failed : data glove n.%d already opened\n", i);
    }
    else 
    {
      // Set USB port and index
      SetPort(portName);
      SetUSBIndex(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[13];
  fdGetSerialNumber(GetGlove(), tmpserial);
  MMechostr(MSKDEBUG, "Glove 5DT serial : %s", tmpserial);
  szSerialNumber = tmpserial;
  SetSerialNumber(szSerialNumber);

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

  // Initialize data glove gesture mode
  SetGestureMode(gestureMode);

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

                // Get Glove Type
                case FD_GLOVE7:    
                        SetType("5DT Glove"); 
                        break;

                case FD_GLOVE7W:   
                        SetType("5DT Wireless Glove"); 
                        break;

                case FD_GLOVE16:   
                        SetType("16DT Glove");
                        break;

                case FD_GLOVE16W:  
                        SetType("16DT Wireless Glove"); 
                        break;          

                case FD_GLOVE5U: 
                        SetType("DG5 Ultra Series"); 
                        break;

                case FD_GLOVE5UW: 
                        SetType("DG5 Wireless Ultra Series"); 
                        break;

                case FD_GLOVE5U_USB: 
                        SetType("DG5 Ultra USB");
                        break;

                case FD_GLOVE14U: 
                        SetType("DG14 Ultra Series"); 
                        break;

                case FD_GLOVE14UW: 
                        SetType("DG14 Wireless Ultra Series"); 
                        break;

                case FD_GLOVE14U_USB: 
                        SetType("DG14 Ultra USB"); 
                        break;          
        }
        SetGloveHand(fdGetGloveHand(pGlove));
        if (GetGloveHand() == FD_HAND_RIGHT)
                SetHand("Right");
        else
                SetHand("Left");

        // Set Number of Glove Sensors
        SetNbrOfSensors(fdGetNumSensors(pGlove));

        // Init successful
  SetInitDone(true);
        return true;
}


/*----- Initialize data glove sensors values -----*/
void GloveObject::InitValues() 
{
        // Get Number of Values to Retrieve
        pValues = new unsigned short[GetNbrOfSensors()];
        pMax = new unsigned short[GetNbrOfSensors()];
        pMin = new unsigned short[GetNbrOfSensors()];

        // Define Minimum and Maximum Values Before Calibration
        for (int i=0; i<GetNbrOfSensors(); i++) 
  {
                pMin[i] = 0;
                pMax[i] = 4095;
        }

  // Update initialization procedure status
  SetInitValuesDone(true);
}


/*----- Set callback to be called when a new packet is received by the driver -----*/
void GloveObject::SetCallback() 
{
        // Set the update function callback (once the calibration is done)
        fdSetCallback(GetGlove(), (void*)GloveCallback, this);
}


/*----- Delete callback -----*/
void GloveObject::DeleteCallback() 
{
        // Delete the callback function previously added (in order to stop the acquisition of sensors values)
        fdSetCallback(GetGlove(), NULL, NULL);
}


/*----- To be called by glove callback function -----*/
void GloveObject::UpdateGlove() 
{
        double angleX = 0.0, angleY = 0.0;
        // If and only if a valid pointer to the glove device is available
        if (GetGlove()) 
  {
                fdGetSensorRawAll(GetGlove(), pValues);                     // Update sensors values

    // Retrieve the current gesture according to the gesture mode
    if (GetGestureMode() == 0) 
    {
                  SetGestureIndex(fdGetGesture(GetGlove()));              // Retrieve and set current gesture index
                  PostMessage(HScol, GLOVE_NEWDATA_WITHOUT_THUMB_CB, (int)this, (LPARAM)NULL);
    }
    else 
    {
      SetGestureWithThumbIndex(fdGetGesture(GetGlove()));   // Retrieve and set current gesture index (with thumb)
      PostMessage(HScol, GLOVE_NEWDATA_CB, (int)this, (LPARAM)NULL);
    }

                // Notify Scol window that a new hand gesture has been made
                PostMessage(HScol, GLOVE_HAND_CB, (int)this, (LPARAM)NULL);
        }
}


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


/*----- Calibrate automatically or manually the glove -----*/
void GloveObject::AutoCalibrate(bool bAuto) 
{
  time_t startTime, currentTime;

  // Notify Scol window that the calibration has started
  PostMessage(HScol, GLOVE_CALIBRATION_START_CB, (int)this, (LPARAM)NULL);

        // ---------- Case 1 : Auto Calibration
  if (bAuto) 
  {
    MMechostr(MSKDEBUG, "-> AutoCalibrate ...calibration mode : AUTO\n");
    // Reset calibration values
          for (int i=0; i<18; i++) 
    {
      pMax[i] = 0;
                        pMin[i] = 4095;
          }
    fdSetCalibrationAll(GetGlove(), pMax, pMin);

    // Enable auto calibration
    fdSetAutoCalibrate(GetGlove(), TRUE);

          // Start calibration : the process will last 5 seconds
    // During this period, the user must open/close his hand several times to complete calibration process
          time(&startTime);
          do 
    {
                  time(&currentTime);
          } while ((currentTime - startTime) < 5);

    // Disable auto calibration
    fdSetAutoCalibrate(GetGlove(), FALSE);
  }

        // ---------- Case 2 : Manual Calibration
        else 
  {
    MMechostr(MSKDEBUG, "-> AutoCalibrate ...calibration mode : MANUAL\n");
                // Default Values to Start Calibration
                for (int i=0; i<GetNbrOfSensors(); i++) 
    {
                        pMax[i] = 0;
                        pMin[i] = 4095;
                }

                // Get Calibration Start Time
                time(&startTime);
                do 
    {
                        // Get Sensors Values
                        fdGetSensorRawAll(GetGlove(), pValues);

                        // For each value, we get the minimum and maximum
                        for (int i=0; i<GetNbrOfSensors(); i++)
      {
                                if(pValues[i]!=0) {
                                        if (pValues[i] > pMax[i])
                                                pMax[i] = pValues[i];
                                        
                                        if (pValues[i] < pMin[i]) 
                                                pMin[i] = pValues[i];
                                }
                        }

                        // Get Current Time
                        time(&currentTime);

                } while ((currentTime - startTime) < 5);
        }

  // Notify Scol window that the calibration has ended
  PostMessage(HScol, GLOVE_CALIBRATION_END_CB, (int)this, (LPARAM)NULL);

  // Update calibration status
  MMechostr(MSKDEBUG, "-> AutoCalibrate ...calibration done for %s.\n", GetType());
  SetCalibrationDone(true);
}


/*----- Thread handling -----*/
void GloveObject::run() 
{
  try 
  {
    while(isRunning) 
    {
      // TODO : Handle USB disconnection of the data glove
      // Will be implemented by 5DT Technical support in a few days

      // Calibration
      if (!GetCalibrationDone()) 
      {
        AutoCalibrate(true);            
      }

      // Initialize data glove sensors values
      else if (!GetInitValuesDone()) 
      {
        InitValues();
      }
    }
    // Release connection to the data glove
    if (Close() == 0) 
    {
      SetGlove(NULL);
    }
  }
  catch (ThreadException ex) 
  {
    MMechostr(MSKDEBUG, "error thread : %s\n", ex.getMessage().c_str());
  }             
}