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

# Modules/MotionControl/DebugMotionControl.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file DebugMotionControl.cpp
    00003 *
    00004 * Implementation of class DebugMotionControl
    00005 *
    00006 * @author Max Risler
    00007 */
    00008
    00009 #include "DebugMotionControl.h"
    00010
    00011 void DebugMotionControl::execute()
    00012 {
    00013   for (int i = 0; i < jointDataBufferNumOfFrames; i++)
    00014   {
    00015     JointData &currentFrame = jointDataBuffer.frame[i];
    00016
    00017     if (sequencer.isRunning())
    00018       //execute sequence
    00019       sequencer.nextSequenceData(currentFrame);
    00020     else
    00021       //do nothing if sequence empty
    00022       currentFrame = JointData();
    00023
    00024     // stay as forced for joints not used
    00025     executeStayAsForced(sensorDataBuffer.lastFrame(), currentFrame);
    00026
    00027     // the obgligatory GT tail wag
    00028     // other direction to distinguish obviously from DefaultMotionControl
    00029     if (currentFrame.data[JointData::tailTilt] == jointDataInvalidValue)
    00030     {
    00031       int j=tailcount++;
    00032       if (j>64) j = 128-j;
    00033       currentFrame.data[JointData::tailTilt] = 10000*(j - 32);
    00034       tailcount &= 127;
    00035     }
    00036     if (currentFrame.data[JointData::tailPan] == jointDataInvalidValue)
    00037       currentFrame.data[JointData::tailPan] = 0;
    00038
    00039     //stabilize(0, motionRequest, currentFrame, odometryData, sensorData);
    00040     currentFrame.data[JointData::tailTilt] = 0;
    00041     currentFrame.data[JointData::tailPan] = 0;
    00042   }
    00043   clip();
    00044 }
    00045
    00046 bool DebugMotionControl::handleMessage(InMessage& message)
    00047 {
    00048   switch(message.getMessageID())
    00049   {
    00050   case idJointDataSequence:
    00051     message.bin >> sequencer;
    00052     sequencer.start();
    00053     return true;
    00054   }
    00055   return false;
    00056 }
    00057
    00058 #define CLIP(x,min,max) \
    00059   if (x > max) x=max; \
    00060   else if (x < min) x=min;
    00061
    00062
    00063 void DebugMotionControl::clip()
    00064 {
    00065   for (int i = 0; i < jointDataBufferNumOfFrames; i++)
    00066   {
    00067     CLIP(jointDataBuffer.frame[i].data[JointData::legFR1], toMicroRad(jointLimitLeg1FN), toMicroRad(jointLimitLeg1FP));
    00068     CLIP(jointDataBuffer.frame[i].data[JointData::legFR2], toMicroRad(jointLimitLeg2N), toMicroRad(jointLimitLeg2P));
    00069     CLIP(jointDataBuffer.frame[i].data[JointData::legFR3], toMicroRad(jointLimitLeg3N), toMicroRad(jointLimitLeg3P));
    00070     CLIP(jointDataBuffer.frame[i].data[JointData::legFL1], toMicroRad(jointLimitLeg1FN), toMicroRad(jointLimitLeg1FP));
    00071     CLIP(jointDataBuffer.frame[i].data[JointData::legFL2], toMicroRad(jointLimitLeg2N), toMicroRad(jointLimitLeg2P));
    00072     CLIP(jointDataBuffer.frame[i].data[JointData::legFL3], toMicroRad(jointLimitLeg3N), toMicroRad(jointLimitLeg3P));
    00073     CLIP(jointDataBuffer.frame[i].data[JointData::legHR1], toMicroRad(jointLimitLeg1HN), toMicroRad(jointLimitLeg1HP));
    00074     CLIP(jointDataBuffer.frame[i].data[JointData::legHR2], toMicroRad(jointLimitLeg2N), toMicroRad(jointLimitLeg2P));
    00075     CLIP(jointDataBuffer.frame[i].data[JointData::legHR3], toMicroRad(jointLimitLeg3N), toMicroRad(jointLimitLeg3P));
    00076     CLIP(jointDataBuffer.frame[i].data[JointData::legHL1], toMicroRad(jointLimitLeg1HN), toMicroRad(jointLimitLeg1HP));
    00077     CLIP(jointDataBuffer.frame[i].data[JointData::legHL2], toMicroRad(jointLimitLeg2N), toMicroRad(jointLimitLeg2P));
    00078     CLIP(jointDataBuffer.frame[i].data[JointData::legHL3], toMicroRad(jointLimitLeg3N), toMicroRad(jointLimitLeg3P));
    00079     CLIP(jointDataBuffer.frame[i].data[JointData::neckTilt], toMicroRad(jointLimitNeckTiltN), toMicroRad(jointLimitNeckTiltP));
    00080     CLIP(jointDataBuffer.frame[i].data[JointData::headPan], toMicroRad(jointLimitHeadPanN), toMicroRad(jointLimitHeadPanP));
    00081     CLIP(jointDataBuffer.frame[i].data[JointData::headTilt], toMicroRad(jointLimitHeadTiltN), toMicroRad(jointLimitHeadTiltP));
    00082   }
    00083 }
    00084
    00085
    00086 #define SET_STAYASFORCED(joint,tolerance) \
    00087 setStayAsForcedValue(sensorData,jointData, SensorData::joint,JointData::joint,tolerance)
    00088
    00089 void DebugMotionControl::executeStayAsForced(
    00090                                              const SensorData& sensorData,
    00091                                              JointData& jointData
    00092                                              )
    00093 {
    00094   SET_STAYASFORCED(neckTilt,100000);
    00095   SET_STAYASFORCED(headPan,100000);
    00096   SET_STAYASFORCED(headTilt,50000);
    00097   SET_STAYASFORCED(legFL1,100000);
    00098   SET_STAYASFORCED(legHL1,100000);
    00099   SET_STAYASFORCED(legFR1,100000);
    00100   SET_STAYASFORCED(legHR1,100000);
    00101   SET_STAYASFORCED(legFL2,100000);
    00102   SET_STAYASFORCED(legHL2,100000);
    00103   SET_STAYASFORCED(legFR2,100000);
    00104   SET_STAYASFORCED(legHR2,100000);
    00105   SET_STAYASFORCED(legFL3,100000);
    00106   SET_STAYASFORCED(legHL3,100000);
    00107   SET_STAYASFORCED(legFR3,100000);
    00108   SET_STAYASFORCED(legHR3,100000);
    00109   SET_STAYASFORCED(mouth,100000);
    00110 }
    00111
    00112 void DebugMotionControl::setStayAsForcedValue(
    00113                                               const SensorData& sensorData,
    00114                                               JointData& jointData,
    00115                                               SensorData::sensors sensor,
    00116                                               JointData::JointID joint,
    00117                                               long tolerance
    00118                                               )
    00119 {
    00120   if (jointData.data[joint] == jointDataInvalidValue)
    00121   {
    00122     if (
    00123       ((stayAsForcedOldValue.data[joint] - sensorData.data[sensor]) > tolerance)  ||
    00124       ((stayAsForcedOldValue.data[joint] - sensorData.data[sensor]) < -tolerance)
    00125       )
    00126       jointData.data[joint] = stayAsForcedOldValue.data[joint] = sensorData.data[sensor];
    00127     else
    00128       jointData.data[joint] = stayAsForcedOldValue.data[joint];
    00129   }
    00130 }