Modules/MotionControl/DebugMotionControl.cpp

Go to the documentation of this file.

/**
* @file DebugMotionControl.cpp
*
* Implementation of class DebugMotionControl
* 
* @author Max Risler
*/

#include "DebugMotionControl.h"

void DebugMotionControl::execute()
{
  for (int i = 0; i < jointDataBufferNumOfFrames; i++)
  {
    JointData &currentFrame = jointDataBuffer.frame[i];
    
    if (sequencer.isRunning())
      //execute sequence
      sequencer.nextSequenceData(currentFrame);
    else
      //do nothing if sequence empty
      currentFrame = JointData();
    
    // stay as forced for joints not used
    executeStayAsForced(sensorDataBuffer.lastFrame(), currentFrame);
    
    // the obgligatory GT tail wag
    // other direction to distinguish obviously from DefaultMotionControl
    if (currentFrame.data[JointData::tailTilt] == jointDataInvalidValue)
    {
      int j=tailcount++;
      if (j>64) j = 128-j;
      currentFrame.data[JointData::tailTilt] = 10000*(j - 32);
      tailcount &= 127;
    }
    if (currentFrame.data[JointData::tailPan] == jointDataInvalidValue)
      currentFrame.data[JointData::tailPan] = 0;
    
    //stabilize(0, motionRequest, currentFrame, odometryData, sensorData);
    currentFrame.data[JointData::tailTilt] = 0;
    currentFrame.data[JointData::tailPan] = 0;
  }
  clip();
}

bool DebugMotionControl::handleMessage(InMessage& message)
{
  switch(message.getMessageID())
  {
  case idJointDataSequence:
    message.bin >> sequencer;
    sequencer.start();
    return true;
  }
  return false;
}

#define CLIP(x,min,max) \
  if (x > max) x=max; \
  else if (x < min) x=min;


void DebugMotionControl::clip()
{
  for (int i = 0; i < jointDataBufferNumOfFrames; i++)
  {
    CLIP(jointDataBuffer.frame[i].data[JointData::legFR1], toMicroRad(jointLimitLeg1FN), toMicroRad(jointLimitLeg1FP));
    CLIP(jointDataBuffer.frame[i].data[JointData::legFR2], toMicroRad(jointLimitLeg2N), toMicroRad(jointLimitLeg2P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legFR3], toMicroRad(jointLimitLeg3N), toMicroRad(jointLimitLeg3P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legFL1], toMicroRad(jointLimitLeg1FN), toMicroRad(jointLimitLeg1FP));
    CLIP(jointDataBuffer.frame[i].data[JointData::legFL2], toMicroRad(jointLimitLeg2N), toMicroRad(jointLimitLeg2P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legFL3], toMicroRad(jointLimitLeg3N), toMicroRad(jointLimitLeg3P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legHR1], toMicroRad(jointLimitLeg1HN), toMicroRad(jointLimitLeg1HP));
    CLIP(jointDataBuffer.frame[i].data[JointData::legHR2], toMicroRad(jointLimitLeg2N), toMicroRad(jointLimitLeg2P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legHR3], toMicroRad(jointLimitLeg3N), toMicroRad(jointLimitLeg3P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legHL1], toMicroRad(jointLimitLeg1HN), toMicroRad(jointLimitLeg1HP));
    CLIP(jointDataBuffer.frame[i].data[JointData::legHL2], toMicroRad(jointLimitLeg2N), toMicroRad(jointLimitLeg2P));
    CLIP(jointDataBuffer.frame[i].data[JointData::legHL3], toMicroRad(jointLimitLeg3N), toMicroRad(jointLimitLeg3P));
    CLIP(jointDataBuffer.frame[i].data[JointData::neckTilt], toMicroRad(jointLimitNeckTiltN), toMicroRad(jointLimitNeckTiltP));
    CLIP(jointDataBuffer.frame[i].data[JointData::headPan], toMicroRad(jointLimitHeadPanN), toMicroRad(jointLimitHeadPanP));
    CLIP(jointDataBuffer.frame[i].data[JointData::headTilt], toMicroRad(jointLimitHeadTiltN), toMicroRad(jointLimitHeadTiltP));
  }
}


#define SET_STAYASFORCED(joint,tolerance) \
setStayAsForcedValue(sensorData,jointData, SensorData::joint,JointData::joint,tolerance)

void DebugMotionControl::executeStayAsForced(
                                             const SensorData& sensorData,
                                             JointData& jointData
                                             )
{
  SET_STAYASFORCED(neckTilt,100000);
  SET_STAYASFORCED(headPan,100000);
  SET_STAYASFORCED(headTilt,50000);
  SET_STAYASFORCED(legFL1,100000);
  SET_STAYASFORCED(legHL1,100000);
  SET_STAYASFORCED(legFR1,100000);
  SET_STAYASFORCED(legHR1,100000);
  SET_STAYASFORCED(legFL2,100000);
  SET_STAYASFORCED(legHL2,100000);
  SET_STAYASFORCED(legFR2,100000);
  SET_STAYASFORCED(legHR2,100000);
  SET_STAYASFORCED(legFL3,100000);
  SET_STAYASFORCED(legHL3,100000);
  SET_STAYASFORCED(legFR3,100000);
  SET_STAYASFORCED(legHR3,100000);
  SET_STAYASFORCED(mouth,100000);
}

void DebugMotionControl::setStayAsForcedValue(
                                              const SensorData& sensorData,
                                              JointData& jointData,
                                              SensorData::sensors sensor,
                                              JointData::JointID joint,
                                              long tolerance
                                              )
{
  if (jointData.data[joint] == jointDataInvalidValue)
  {
    if (
      ((stayAsForcedOldValue.data[joint] - sensorData.data[sensor]) > tolerance)  ||
      ((stayAsForcedOldValue.data[joint] - sensorData.data[sensor]) < -tolerance)
      )
      jointData.data[joint] = stayAsForcedOldValue.data[joint] = sensorData.data[sensor];
    else
      jointData.data[joint] = stayAsForcedOldValue.data[joint];
  }
}

---