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] = 300;

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

  position[JointData::legFL1][2] =  -111000;
  position[JointData::legFL2][2] =  1230000; 
  position[JointData::legFL3][2] =   777000;

  position[JointData::legFR1][2] =   -50000;
  position[JointData::legFR2][2] =  1213000; 
  position[JointData::legFR3][2] =   737000;
  
  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::legFL1][3] = -300000;
  position[JointData::legFL2][3] =  250000;
  position[JointData::legFL3][3] = 1800000;
  
  position[JointData::legFR1][3] = -300000;
  position[JointData::legFR2][3] =  250000;
  position[JointData::legFR3][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;
}

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;
}




/*
* Change log :
* 
* $Log: WakeUpEngine.cpp,v $
* Revision 1.5  2004/06/15 17:15:21  roefer
* Warning removed
*
* Revision 1.4  2004/06/14 14:53:43  spranger
* put everything into execute (start is called from there)
* motioncontrol cleanup
*
* Revision 1.3  2004/05/24 14:50:01  juengel
* faster
*
* Revision 1.2  2004/05/23 20:29:27  juengel
* Faster WakeUp in simulation.
*
* Revision 1.1.1.1  2004/05/22 17:20:37  cvsadm
* created new repository GT2004_WM
*
* Revision 1.4  2004/05/22 14:28:13  juengel
* First working version.
*
* Revision 1.3  2004/05/19 13:32:39  heinze
* worked at the engine
*
* Revision 1.2  2004/05/03 15:29:48  roefer
* Warnings removed
*
* Revision 1.1  2004/05/03 09:45:01  heinze
* Added WakeUpEngine.
*
*/

---