Modules/MotionControl/WakeUpEngine.cpp

Go to the documentation of this file.

/**
* @file WakeUpEngine.cpp
* 
* Implementation of class WakeUpEngine.
*
* @author Andreas Heinze
*/
#include "Tools/Math/Common.h"

#include "WakeUpEngine.h"
#include "Tools/Debugging/Debugging.h"

WakeUpEngine::WakeUpEngine():lastTimeRunFrameNumber(0), finished(true)
{

  frameNumberOfKeyFrame[0] =   0;
  frameNumberWhenDefaultPidDataIsReached = 20;
  frameNumberOfKeyFrame[1] = 100;
  frameNumberOfKeyFrame[2] = 200;
  frameNumberOfKeyFrame[3] = 260;
  frameNumberOfKeyFrame[4] = 320;
  frameNumberOfKeyFrame[5] = 340;
  

#ifdef _WIN32
  frameNumberOfKeyFrame[0] =   0;
  frameNumberWhenDefaultPidDataIsReached = 1;
  frameNumberOfKeyFrame[1] = 2;
  frameNumberOfKeyFrame[2] = 3;
  frameNumberOfKeyFrame[3] = 4;
  frameNumberOfKeyFrame[4] = 5;
  frameNumberOfKeyFrame[5] = 6;
#endif

  /*/ old getup data
  position[JointData::legFR1][2] =   -50000;
  position[JointData::legFR2][2] =  1213000; 
  position[JointData::legFR3][2] =   737000;

  position[JointData::legFL1][2] =  -111000;
  position[JointData::legFL2][2] =  1230000; 
  position[JointData::legFL3][2] =   777000;
  
  position[JointData::legHL1][2] =    93000;
  position[JointData::legHL2][2] =  1207000; 
  position[JointData::legHL3][2] =  1028000;

  position[JointData::legHR1][2] =   142000;
  position[JointData::legHR2][2] =  1144000; 
  position[JointData::legHR3][2] =  1142000;
  
  position[JointData::legFR1][3] = -300000;
  position[JointData::legFR2][3] =  250000;
  position[JointData::legFR3][3] = 1800000;

  position[JointData::legFL1][3] = -300000;
  position[JointData::legFL2][3] =  250000;
  position[JointData::legFL3][3] = 1800000;

  position[JointData::legHL1][3] = -600000;
  position[JointData::legHL2][3] =  150000;
  position[JointData::legHL3][3] = 1600000;
  
  position[JointData::legHR1][3] = -600000;
  position[JointData::legHR2][3] =  150000;
  position[JointData::legHR3][3] = 1600000;*/

  //new GetUp move by Denis Fisseler
  position[JointData::legFR1][2] = 0;
  position[JointData::legFR2][2] = 2400000;
  position[JointData::legFR3][2] = 2200000;
  position[JointData::legFL1][2] = 0;
  position[JointData::legFL2][2] = 2400000;
  position[JointData::legFL3][2] = 2200000;
  position[JointData::legHR1][2] = 0;
  position[JointData::legHR2][2] = 2400000;
  position[JointData::legHR3][2] = 2200000;
  position[JointData::legHL1][2] = 0;
  position[JointData::legHL2][2] = 2400000;
  position[JointData::legHL3][2] = 2200000;

  position[JointData::legFR1][3] = 1000000;
  position[JointData::legFR2][3] = 0;
  position[JointData::legFR3][3] = 2200000;
  position[JointData::legFL1][3] = 1000000;
  position[JointData::legFL2][3] = 0;
  position[JointData::legFL3][3] = 2200000;
  position[JointData::legHR1][3] = 1100000;
  position[JointData::legHR2][3] = 0;
  position[JointData::legHR3][3] = 2200000;
  position[JointData::legHL1][3] = 1100000;
  position[JointData::legHL2][3] = 0;
  position[JointData::legHL3][3] = 2200000;

  position[JointData::legFR1][4] = -800000;
  position[JointData::legFR2][4] = 0;
  position[JointData::legFR3][4] = 2200000;
  position[JointData::legFL1][4] = -800000;
  position[JointData::legFL2][4] = 0;
  position[JointData::legFL3][4] = 2200000;
  position[JointData::legHR1][4] = -500000;
  position[JointData::legHR2][4] = 0;
  position[JointData::legHR3][4] = 2200000;
  position[JointData::legHL1][4] = -500000;
  position[JointData::legHL2][4] = 0;
  position[JointData::legHL3][4] = 2200000;

  position[JointData::legFR1][5] = -260000;
  position[JointData::legFR2][5] = 240000;
  position[JointData::legFR3][5] = 2050000;
  position[JointData::legFL1][5] = -260000;
  position[JointData::legFL2][5] = 240000;
  position[JointData::legFL3][5] = 2050000;
  position[JointData::legHR1][5] = -600000;
  position[JointData::legHR2][5] = 160000;
  position[JointData::legHR3][5] = 1700000;
  position[JointData::legHL1][5] = -600000;
  position[JointData::legHL2][5] = 160000;
  position[JointData::legHL3][5] = 1700000;
}

WakeUpEngine::~WakeUpEngine()
{
}

void WakeUpEngine::start(const SensorData& sensorData, PIDData& pidData)
{
  counter = 0;
  currentKeyFrame = 0;
  
  PIDData defaultPid;
  
  int i;
  for(i = 0; i < JointData::numOfJoint; i++)
    {
      pidData.p[i] = 0;
      pidData.i[i] = 0; 
      pidData.d[i] = 0; 
    }
  for(i = 0; i < JointData::numOfJoint; i++)
  {
    diffP[i] = (double)(defaultPid.p[i] - pidData.p[i]) / (double)frameNumberWhenDefaultPidDataIsReached;
    diffI[i] = (double)(defaultPid.i[i] - pidData.i[i]) / (double)frameNumberWhenDefaultPidDataIsReached;
    diffD[i] = (double)(defaultPid.d[i] - pidData.d[i]) / (double)frameNumberWhenDefaultPidDataIsReached;
  }
  
  position[JointData::legFL1][0] = sensorData.data[SensorData::legFL1];
  position[JointData::legFL2][0] = sensorData.data[SensorData::legFL2];
  position[JointData::legFL3][0] = sensorData.data[SensorData::legFL3];

  position[JointData::legFR1][0] = sensorData.data[SensorData::legFR1];
  position[JointData::legFR2][0] = sensorData.data[SensorData::legFR2];
  position[JointData::legFR3][0] = sensorData.data[SensorData::legFR3];

  position[JointData::legHL1][0] = sensorData.data[SensorData::legHL1];
  position[JointData::legHL2][0] = sensorData.data[SensorData::legHL2];
  position[JointData::legHL3][0] = sensorData.data[SensorData::legHL3];

  position[JointData::legHR1][0] = sensorData.data[SensorData::legHR1];
  position[JointData::legHR2][0] = sensorData.data[SensorData::legHR2];
  position[JointData::legHR3][0] = sensorData.data[SensorData::legHR3];

  position[JointData::legFL1][1] = sensorData.data[SensorData::legFL1];
  position[JointData::legFL2][1] = 1200000;
  position[JointData::legFL3][1] = sensorData.data[SensorData::legFL3];

  position[JointData::legFR1][1] = sensorData.data[SensorData::legFR1];
  position[JointData::legFR2][1] = 1200000;
  position[JointData::legFR3][1] = sensorData.data[SensorData::legFR3];

  position[JointData::legHL1][1] = sensorData.data[SensorData::legHL1];
  position[JointData::legHL2][1] = 1200000;
  position[JointData::legHL3][1] = sensorData.data[SensorData::legHL3];

  position[JointData::legHR1][1] = sensorData.data[SensorData::legHR1];
  position[JointData::legHR2][1] = 1200000;
  position[JointData::legHR3][1] = sensorData.data[SensorData::legHR3];

  int keyFrame;
  for(keyFrame = 0; keyFrame < numberOfKeyFrames - 1; keyFrame++)
  {
    for(i = JointData::legFR1; i <= JointData::legHL3; i++)
    {
        stepSize[i][keyFrame] = (position[i][keyFrame+1] - position[i][keyFrame]) / (frameNumberOfKeyFrame[keyFrame + 1] - frameNumberOfKeyFrame[keyFrame]);
    }
  }
}


bool WakeUpEngine::execute(unsigned int frameNumber, const SensorData& sensorData, JointData& jointData, PIDData& pidData)
{
  if(finished || abs(int(frameNumber - lastTimeRunFrameNumber)) > 100)
  {
     finished = false;
     start(sensorData, pidData);
  }
   
  lastTimeRunFrameNumber = frameNumber;

  counter++;
  int i;
  if(counter < frameNumberWhenDefaultPidDataIsReached)
  {
    for(i = 0; i < JointData::numOfJoint; i++)
    {
      pidData.p[i] = (int)(counter * diffP[i]);
      pidData.i[i] = (int)(counter * diffI[i]); 
      pidData.d[i] = (int)(counter * diffD[i]); 
    }
  }
  if(counter == frameNumberWhenDefaultPidDataIsReached)
  {
    pidData.setToDefaults();
  }

  if(counter > frameNumberOfKeyFrame[currentKeyFrame+1]) currentKeyFrame++;

  if(currentKeyFrame == numberOfKeyFrames){
    
    //OUTPUT(idText, text, "wakeUP finish!");
    finished = true;
    return false;
  }

  int positionInCurrentPhase = counter - frameNumberOfKeyFrame[currentKeyFrame];

  for(i = JointData::legFR1; i <= JointData::legHL3; i++)
  {
    jointData.data[i] = position[i][currentKeyFrame] + positionInCurrentPhase * stepSize[i][currentKeyFrame];
  }

  finished = false;
  return true;
}

---