Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/MotionControl/WakeUpEngine.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file WakeUpEngine.cpp
    00003 *
    00004 * Implementation of class WakeUpEngine.
    00005 *
    00006 * @author Andreas Heinze
    00007 */
    00008 #include "Tools/Math/Common.h"
    00009
    00010 #include "WakeUpEngine.h"
    00011 #include "Tools/Debugging/Debugging.h"
    00012
    00013 WakeUpEngine::WakeUpEngine():lastTimeRunFrameNumber(0), finished(true)
    00014 {
    00015
    00016   frameNumberOfKeyFrame[0] =   0;
    00017   frameNumberWhenDefaultPidDataIsReached = 20;
    00018   frameNumberOfKeyFrame[1] = 100;
    00019   frameNumberOfKeyFrame[2] = 200;
    00020   frameNumberOfKeyFrame[3] = 260;
    00021   frameNumberOfKeyFrame[4] = 320;
    00022   frameNumberOfKeyFrame[5] = 340;
    00023
    00024
    00025 #ifdef _WIN32
    00026   frameNumberOfKeyFrame[0] =   0;
    00027   frameNumberWhenDefaultPidDataIsReached = 1;
    00028   frameNumberOfKeyFrame[1] = 2;
    00029   frameNumberOfKeyFrame[2] = 3;
    00030   frameNumberOfKeyFrame[3] = 4;
    00031   frameNumberOfKeyFrame[4] = 5;
    00032   frameNumberOfKeyFrame[5] = 6;
    00033 #endif
    00034
    00035   /*/ old getup data
    00036   position[JointData::legFR1][2] =   -50000;
    00037   position[JointData::legFR2][2] =  1213000;
    00038   position[JointData::legFR3][2] =   737000;
    00039
    00040   position[JointData::legFL1][2] =  -111000;
    00041   position[JointData::legFL2][2] =  1230000;
    00042   position[JointData::legFL3][2] =   777000;
    00043
    00044   position[JointData::legHL1][2] =    93000;
    00045   position[JointData::legHL2][2] =  1207000;
    00046   position[JointData::legHL3][2] =  1028000;
    00047
    00048   position[JointData::legHR1][2] =   142000;
    00049   position[JointData::legHR2][2] =  1144000;
    00050   position[JointData::legHR3][2] =  1142000;
    00051
    00052   position[JointData::legFR1][3] = -300000;
    00053   position[JointData::legFR2][3] =  250000;
    00054   position[JointData::legFR3][3] = 1800000;
    00055
    00056   position[JointData::legFL1][3] = -300000;
    00057   position[JointData::legFL2][3] =  250000;
    00058   position[JointData::legFL3][3] = 1800000;
    00059
    00060   position[JointData::legHL1][3] = -600000;
    00061   position[JointData::legHL2][3] =  150000;
    00062   position[JointData::legHL3][3] = 1600000;
    00063
    00064   position[JointData::legHR1][3] = -600000;
    00065   position[JointData::legHR2][3] =  150000;
    00066   position[JointData::legHR3][3] = 1600000;*/
    00067
    00068   //new GetUp move by Denis Fisseler
    00069   position[JointData::legFR1][2] = 0;
    00070   position[JointData::legFR2][2] = 2400000;
    00071   position[JointData::legFR3][2] = 2200000;
    00072   position[JointData::legFL1][2] = 0;
    00073   position[JointData::legFL2][2] = 2400000;
    00074   position[JointData::legFL3][2] = 2200000;
    00075   position[JointData::legHR1][2] = 0;
    00076   position[JointData::legHR2][2] = 2400000;
    00077   position[JointData::legHR3][2] = 2200000;
    00078   position[JointData::legHL1][2] = 0;
    00079   position[JointData::legHL2][2] = 2400000;
    00080   position[JointData::legHL3][2] = 2200000;
    00081
    00082   position[JointData::legFR1][3] = 1000000;
    00083   position[JointData::legFR2][3] = 0;
    00084   position[JointData::legFR3][3] = 2200000;
    00085   position[JointData::legFL1][3] = 1000000;
    00086   position[JointData::legFL2][3] = 0;
    00087   position[JointData::legFL3][3] = 2200000;
    00088   position[JointData::legHR1][3] = 1100000;
    00089   position[JointData::legHR2][3] = 0;
    00090   position[JointData::legHR3][3] = 2200000;
    00091   position[JointData::legHL1][3] = 1100000;
    00092   position[JointData::legHL2][3] = 0;
    00093   position[JointData::legHL3][3] = 2200000;
    00094
    00095   position[JointData::legFR1][4] = -800000;
    00096   position[JointData::legFR2][4] = 0;
    00097   position[JointData::legFR3][4] = 2200000;
    00098   position[JointData::legFL1][4] = -800000;
    00099   position[JointData::legFL2][4] = 0;
    00100   position[JointData::legFL3][4] = 2200000;
    00101   position[JointData::legHR1][4] = -500000;
    00102   position[JointData::legHR2][4] = 0;
    00103   position[JointData::legHR3][4] = 2200000;
    00104   position[JointData::legHL1][4] = -500000;
    00105   position[JointData::legHL2][4] = 0;
    00106   position[JointData::legHL3][4] = 2200000;
    00107
    00108   position[JointData::legFR1][5] = -260000;
    00109   position[JointData::legFR2][5] = 240000;
    00110   position[JointData::legFR3][5] = 2050000;
    00111   position[JointData::legFL1][5] = -260000;
    00112   position[JointData::legFL2][5] = 240000;
    00113   position[JointData::legFL3][5] = 2050000;
    00114   position[JointData::legHR1][5] = -600000;
    00115   position[JointData::legHR2][5] = 160000;
    00116   position[JointData::legHR3][5] = 1700000;
    00117   position[JointData::legHL1][5] = -600000;
    00118   position[JointData::legHL2][5] = 160000;
    00119   position[JointData::legHL3][5] = 1700000;
    00120 }
    00121
    00122 WakeUpEngine::~WakeUpEngine()
    00123 {
    00124 }
    00125
    00126 void WakeUpEngine::start(const SensorData& sensorData, PIDData& pidData)
    00127 {
    00128   counter = 0;
    00129   currentKeyFrame = 0;
    00130
    00131   PIDData defaultPid;
    00132
    00133   int i;
    00134   for(i = 0; i < JointData::numOfJoint; i++)
    00135     {
    00136       pidData.p[i] = 0;
    00137       pidData.i[i] = 0;
    00138       pidData.d[i] = 0;
    00139     }
    00140   for(i = 0; i < JointData::numOfJoint; i++)
    00141   {
    00142     diffP[i] = (double)(defaultPid.p[i] - pidData.p[i]) / (double)frameNumberWhenDefaultPidDataIsReached;
    00143     diffI[i] = (double)(defaultPid.i[i] - pidData.i[i]) / (double)frameNumberWhenDefaultPidDataIsReached;
    00144     diffD[i] = (double)(defaultPid.d[i] - pidData.d[i]) / (double)frameNumberWhenDefaultPidDataIsReached;
    00145   }
    00146
    00147   position[JointData::legFL1][0] = sensorData.data[SensorData::legFL1];
    00148   position[JointData::legFL2][0] = sensorData.data[SensorData::legFL2];
    00149   position[JointData::legFL3][0] = sensorData.data[SensorData::legFL3];
    00150
    00151   position[JointData::legFR1][0] = sensorData.data[SensorData::legFR1];
    00152   position[JointData::legFR2][0] = sensorData.data[SensorData::legFR2];
    00153   position[JointData::legFR3][0] = sensorData.data[SensorData::legFR3];
    00154
    00155   position[JointData::legHL1][0] = sensorData.data[SensorData::legHL1];
    00156   position[JointData::legHL2][0] = sensorData.data[SensorData::legHL2];
    00157   position[JointData::legHL3][0] = sensorData.data[SensorData::legHL3];
    00158
    00159   position[JointData::legHR1][0] = sensorData.data[SensorData::legHR1];
    00160   position[JointData::legHR2][0] = sensorData.data[SensorData::legHR2];
    00161   position[JointData::legHR3][0] = sensorData.data[SensorData::legHR3];
    00162
    00163   position[JointData::legFL1][1] = sensorData.data[SensorData::legFL1];
    00164   position[JointData::legFL2][1] = 1200000;
    00165   position[JointData::legFL3][1] = sensorData.data[SensorData::legFL3];
    00166
    00167   position[JointData::legFR1][1] = sensorData.data[SensorData::legFR1];
    00168   position[JointData::legFR2][1] = 1200000;
    00169   position[JointData::legFR3][1] = sensorData.data[SensorData::legFR3];
    00170
    00171   position[JointData::legHL1][1] = sensorData.data[SensorData::legHL1];
    00172   position[JointData::legHL2][1] = 1200000;
    00173   position[JointData::legHL3][1] = sensorData.data[SensorData::legHL3];
    00174
    00175   position[JointData::legHR1][1] = sensorData.data[SensorData::legHR1];
    00176   position[JointData::legHR2][1] = 1200000;
    00177   position[JointData::legHR3][1] = sensorData.data[SensorData::legHR3];
    00178
    00179   int keyFrame;
    00180   for(keyFrame = 0; keyFrame < numberOfKeyFrames - 1; keyFrame++)
    00181   {
    00182     for(i = JointData::legFR1; i <= JointData::legHL3; i++)
    00183     {
    00184         stepSize[i][keyFrame] = (position[i][keyFrame+1] - position[i][keyFrame]) / (frameNumberOfKeyFrame[keyFrame + 1] - frameNumberOfKeyFrame[keyFrame]);
    00185     }
    00186   }
    00187 }
    00188
    00189
    00190 bool WakeUpEngine::execute(unsigned int frameNumber, const SensorData& sensorData, JointData& jointData, PIDData& pidData)
    00191 {
    00192   if(finished || abs(int(frameNumber - lastTimeRunFrameNumber)) > 100)
    00193   {
    00194      finished = false;
    00195      start(sensorData, pidData);
    00196   }
    00197
    00198   lastTimeRunFrameNumber = frameNumber;
    00199
    00200   counter++;
    00201   int i;
    00202   if(counter < frameNumberWhenDefaultPidDataIsReached)
    00203   {
    00204     for(i = 0; i < JointData::numOfJoint; i++)
    00205     {
    00206       pidData.p[i] = (int)(counter * diffP[i]);
    00207       pidData.i[i] = (int)(counter * diffI[i]);
    00208       pidData.d[i] = (int)(counter * diffD[i]);
    00209     }
    00210   }
    00211   if(counter == frameNumberWhenDefaultPidDataIsReached)
    00212   {
    00213     pidData.setToDefaults();
    00214   }
    00215
    00216   if(counter > frameNumberOfKeyFrame[currentKeyFrame+1]) currentKeyFrame++;
    00217
    00218   if(currentKeyFrame == numberOfKeyFrames){
    00219
    00220     //OUTPUT(idText, text, "wakeUP finish!");
    00221     finished = true;
    00222     return false;
    00223   }
    00224
    00225   int positionInCurrentPhase = counter - frameNumberOfKeyFrame[currentKeyFrame];
    00226
    00227   for(i = JointData::legFR1; i <= JointData::legHL3; i++)
    00228   {
    00229     jointData.data[i] = position[i][currentKeyFrame] + positionInCurrentPhase * stepSize[i][currentKeyFrame];
    00230   }
    00231
    00232   finished = false;
    00233   return true;
    00234 }