Modules/HeadControl/GT2005HeadControl/GT2005HeadControl.cpp

Go to the documentation of this file.

/**
* @file GT2005HeadControl.cpp
*
* Implementation of class GT2005HeadControl
*
* @author Marc Dassler
* @author Jan Hoffmann
* @author Uwe Düffert
* @author Martin Lötzsch
*/


#include "Platform/GTAssert.h"
#include "GT2005HeadControl.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Actorics/Kinematics.h"
#include "Tools/Math/Common.h"
#include "Tools/FieldDimensions.h"
#include "Platform/SystemCall.h"


#ifdef _WIN32
#pragma warning(disable:4355) // VC++: "this" in initializer list is ok
#endif

GT2005HeadControl::GT2005HeadControl(HeadControlInterfaces& interfaces)
: Xabsl2HeadControl(interfaces, SolutionRequest::hc_gt2005), 
headPathPlanner(sensorDataBuffer),
symbols(interfaces,*this,basicBehaviors.basicBehaviorDirectedScanForLandmarks),
basicBehaviors(errorHandler,*this,*this,headPathPlanner,lastScanWasLeft,cameraInfo),
lastScanWasLeft(true), 
lastHeadControlMode(HeadControlMode::none),
lastOdometryData(), lastRobotPose()
{
  Xabsl2FileInputSource file("Xabsl2/HeadCtrl/gt05-ic.dat");
  init(file);

  headPathPlanner.lastNeckTilt = ((double )sensorDataBuffer.lastFrame().data[SensorData::neckTilt])/1000000.0;
  headPathPlanner.lastHeadPan  = ((double )sensorDataBuffer.lastFrame().data[SensorData::headPan])/1000000.0;
  headPathPlanner.lastHeadTilt = ((double )sensorDataBuffer.lastFrame().data[SensorData::headTilt])/1000000.0;

  const RobotDimensions& robotDimensions = getRobotConfiguration().getRobotDimensions();  
  cameraInfo = CameraInfo::CameraInfo(getRobotConfiguration().getRobotDesign());

  setupMainAngles();

  jointRangeNeckTilt = Range<double>(robotDimensions.jointLimitNeckTiltN,robotDimensions.jointLimitNeckTiltP);
  jointRangeHeadPan  = Range<double>(robotDimensions.jointLimitHeadPanN, robotDimensions.jointLimitHeadPanP);
  jointRangeHeadTilt = Range<double>(robotDimensions.jointLimitHeadTiltN,robotDimensions.jointLimitHeadTiltP);

  // these values are the default settings for standard pid values
  speedNeckTilt = 0.002000;
  speedHeadPan  = 0.005350;
  speedHeadTilt = 0.002550;

  // setting speed in headpath planner
  headPathPlanner.headPathSpeedNeckTilt = speedNeckTilt;
  headPathPlanner.headPathSpeedHeadPan  = speedHeadPan;
  headPathPlanner.headPathSpeedHeadTilt = speedHeadTilt;

  calibrationReset();

  useCommunicatedBall = true;


}

int GT2005HeadControl::getLastSeenBeaconIndex()
{
  return landmarksState.lastSeenBeaconIndex();
}
long GT2005HeadControl::getTimeOfLastSeenBeacon(int index)
{
  return landmarksState.timeOfLastSeenBeacon(index);
}

long GT2005HeadControl::getTimeBetweenSeen2LastBeacons(int index)
{
  return landmarksState.timeBetweenSeen2LastBeacons(index);
}

bool GT2005HeadControl::headPanIsLeft()
{
  return ((((double )sensorDataBuffer.lastFrame().data[SensorData::headPan]))>0);
}

void GT2005HeadControl::getSensorHeadAngles(Vector3<double>& pos)
{
  pos.x = ((double )sensorDataBuffer.lastFrame().data[SensorData::neckTilt])/1000000.0;
  pos.y = ((double )sensorDataBuffer.lastFrame().data[SensorData::headPan])/1000000.0;
  pos.z = ((double )sensorDataBuffer.lastFrame().data[SensorData::headTilt])/1000000.0;
}

void GT2005HeadControl::searchForBallRight()
{
  Vector3<double> points[]={  headDown,
                              headMiddleRight,
                              headRight,
                              headRight};

  long durations[] =   {100,100,200,160};
  headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>));

  lastScanWasLeft = false;
}

void GT2005HeadControl::searchForBallLeft()
{
  Vector3<double> points[]={  headDown,
                              headMiddleLeft,
                              headLeft,
                              headLeft};

  long durations[] =   {100,100,200,160};
  headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>));

  lastScanWasLeft = true;
}

double GT2005HeadControl::headPositionDistanceToActualPosition(Vector3<double> comp,bool leftSide)
{
  double pos;
  pos = ((double)sensorDataBuffer.lastFrame().data[SensorData::headPan])/1000000.0;

  return -fabs(pos-comp.y);;

}
void GT2005HeadControl::registerSymbolsAndBasicBehaviors()
{
  symbols.registerSymbols(*pEngine);
  basicBehaviors.registerBasicBehaviors(*pEngine);
}

void GT2005HeadControl::execute()
{
  pidData.setToDefaults();
  symbols.update();
  
  if(headIsBlockedBySpecialActionOrWalk) 
  {
    headPathPlanner.lastNeckTilt = ((double )sensorDataBuffer.lastFrame().data[SensorData::neckTilt])/1000000.0;
    headPathPlanner.lastHeadPan  = ((double )sensorDataBuffer.lastFrame().data[SensorData::headPan])/1000000.0;
    headPathPlanner.lastHeadTilt = ((double )sensorDataBuffer.lastFrame().data[SensorData::headTilt])/1000000.0;
  }

  if(sensorDataBuffer.lastFrame().data[SensorData::chin] == 1)
  {
    if(
      robotState.getButtonPressed(BodyPercept::backBack) &&
      robotState.getButtonDuration(BodyPercept::backBack) > 1000 &&
      profiler.profilerCollectMode == GTXabsl2Profiler::collectProfiles
      )
    {
      profiler.profilerCollectMode = GTXabsl2Profiler::dontCollectProfiles;
      profiler.profilerWriteMode = GTXabsl2Profiler::writeCompleteProfiles;
    }

    if(
      robotState.getButtonPressed(BodyPercept::backFront) &&
      robotState.getButtonDuration(BodyPercept::backFront) > 1000
      )
    {
      profiler.profilerCollectMode = GTXabsl2Profiler::collectProfiles;
    }
  }

  executeEngine();   
  DEBUG_RESPONSE("LED, tail, mouth: show position standard deviation with mouth", showVisualizationQuality(); );
  lastHeadControlMode = headControlMode.headControlMode;
}



void GT2005HeadControl::showVisualizationQuality()
{
  
  if ((headMotionRequest.mouth >= 0) && (headControlMode.headControlMode != HeadControlMode::catchBall))
  { 
    double maxStandardDeviation = 500, mouthRange = -1000000;

    MODIFY("headControl:maxVarianceForMouthOutput", maxStandardDeviation);
    MODIFY("headControl:maxMouthRange", mouthRange);

    double relativeStandardDeviation = sqrt(robotPose.getPositionVariance())/maxStandardDeviation;
    if (relativeStandardDeviation > 1) 
      relativeStandardDeviation = 1;
    headMotionRequest.mouth = (long )( mouthRange*relativeStandardDeviation);
  }
}

  

bool GT2005HeadControl::handleMessage(InMessage& message)
{
  return Xabsl2HeadControl::handleMessage(message);
}


void GT2005HeadControl::beginBallSearchAt(Vector2<double> ballPosition2d)
{
  Vector3<double> ballPosition3d (ballPosition2d.x,ballPosition2d.y,ballRadius);    

  Vector3<double> leftRightSweepTop,
                  leftRightSweepBottom,
                  halfLeftRightSweep,
                  halfLeftRightSweepBottom;

  Vector2<double> toBall = ballPosition2d - robotPose.translation;
  double angleToBall = normalize(atan2(toBall.y,toBall.x))-robotPose.rotation;
  
  // center the ball view in the middle of the image
  // if the ball is near, the ball should be seen, if we look halfway down
  // constante definition of distance to ball
  enum { ballNearBy = 500 };
  Vector2<int> cameraImageOffset(0,25);
  
  double neckTilt,headPan,headTilt;
  simpleLookAtPointOnField(ballPosition3d,cameraImageOffset,neckTilt,headPan,headTilt);
  Vector3<double> ballAngles (neckTilt,headPan,headTilt);

  if (angleToBall>0)
  {
    leftRightSweepTop        = headLeft;
    leftRightSweepBottom     = headLeftDown;
    halfLeftRightSweep       = headMiddleLeft;
    halfLeftRightSweepBottom = headMiddleLeftDown;

  }
  else
  {
    leftRightSweepTop         = headRight;
    leftRightSweepBottom      = headRightDown;
    halfLeftRightSweep        = headMiddleRight;
    halfLeftRightSweepBottom  = headMiddleRightDown;
  }

  Vector3<double> points[]={ballAngles,
                            ballAngles,
                            leftRightSweepBottom,
                            leftRightSweepTop,
                            halfLeftRightSweep,
                            headUp,
                            headDown};

  long durations[] = {0,100,160,120,160,100,80};
  headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>));
  lastScanWasLeft = (angleToBall>0);


}
void GT2005HeadControl::setJointsDirect(double neckTilt, double headPan, double headTilt, double mouth)
{
  headPathPlanner.lastNeckTilt = jointRangeNeckTilt.limit(neckTilt);
  headPathPlanner.lastHeadPan  = jointRangeHeadPan.limit(headPan);
  headPathPlanner.lastHeadTilt = jointRangeHeadTilt.limit(headTilt);

  headMotionRequest.tilt  = toMicroRad(neckTilt);
  headMotionRequest.pan   = toMicroRad(headPan);
  headMotionRequest.roll  = toMicroRad(headTilt);
  headMotionRequest.mouth = toMicroRad(mouth);
}

void GT2005HeadControl::setJoints(double neckTilt, double headPan, double headTilt, double speed, double mouth)
{
  const double closeToDesiredAngles = 0.01;
  
  //double panCausedByRobotRotation = currentOdometryData.rotation - lastOdometryData.rotation;

  // test if head has reached his position
  setJointsIsCloseToDestination = headPathPlanner.headPositionReached(Vector3<double> (neckTilt,headPan,headTilt));

  setJointsMaxPanReached = false;

  Vector2<double> 
    direction(headTilt - headPathPlanner.lastHeadTilt, 
    headPan - (headPathPlanner.lastHeadPan/* + panCausedByRobotRotation*/)); 
 
  if (speed > 0)
  {
    Vector2<double> directionWithSpeed = direction;
    directionWithSpeed.normalize();
    directionWithSpeed *= speed/125;
    if (directionWithSpeed.abs() < direction.abs())
      direction = directionWithSpeed;
  }

  if (direction.abs() < closeToDesiredAngles)
    setJointsIsCloseToDestination++;
  else
    setJointsIsCloseToDestination = 0;

  headPathPlanner.lastNeckTilt = neckTilt;
  headPathPlanner.lastHeadPan  += direction.y;
  headPathPlanner.lastHeadTilt += direction.x;

  headPathPlanner.lastNeckTilt = jointRangeNeckTilt.limit(headPathPlanner.lastNeckTilt);
  headPathPlanner.lastHeadPan  = jointRangeHeadPan.limit(headPathPlanner.lastHeadPan);
  headPathPlanner.lastHeadTilt = jointRangeHeadTilt.limit(headPathPlanner.lastHeadTilt);

  if (fabs(headPathPlanner.lastHeadPan) == jointRangeHeadPan.max)
    setJointsMaxPanReached = true;

  headMotionRequest.tilt  = (long)(headPathPlanner.lastNeckTilt*1000000.0);
  headMotionRequest.pan   = (long)(headPathPlanner.lastHeadPan *1000000.0);
  headMotionRequest.roll  = (long)(headPathPlanner.lastHeadTilt*1000000.0);
#ifndef _WIN32
  //headMotionRequest.roll += 150000; // add a little to compensate for the head's weight...;
#endif
  headMotionRequest.mouth = (long)(mouth*1000000);

  if(lastRobotPose != robotPose)
  {
    lastOdometryData = currentOdometryData;
    lastRobotPose = robotPose;
  }
}




// tools
void GT2005HeadControl::aimAtLandmark(int landmark, double& neckTilt, double& headPan, double& headTilt)
{
  if(targetPointOnLandmark[landmark].height > 0)
  {
    simpleLookAtPointOnField(
      targetPointOnLandmark[landmark].position, 
      Vector2<int>(0, -cameraInfo.resolutionHeight/3), 
      neckTilt, headPan, headTilt);
  }
  else
  {
    simpleLookAtPointOnField(
      targetPointOnLandmark[landmark].position, 
      Vector2<int>(0, 0), //+cameraInfo.resolutionHeight/3), 
      neckTilt, headPan, headTilt);
  }
}



void GT2005HeadControl::getLookAtBallAngles(const Vector2<double> ballOnField, double& neckTilt, double& headPan, double& headTilt)
{
 
  Vector2<double> ballPos = Geometry::fieldCoord2Relative(robotPose, ballOnField);
  if (ballPos.abs() < 100) // clip ball distances close to and inside of the robot
    ballPos.x = 100;   

  double minBClose = 300, maxBClose = 2000;
  Range<double> ballCloseRange(minBClose, maxBClose);
  double ballIsClose = ballPos.abs();
  ballIsClose = ballCloseRange.limit(ballIsClose);
  ballIsClose = (ballIsClose - minBClose)/(maxBClose-minBClose);

  simpleLookAtPointRelativeToRobot(
    Vector3<double>(ballPos.x, ballPos.y, 1.8*ballRadius), 
    Vector2<int>(0, int( (.167-ballIsClose)*cameraInfo.resolutionHeight/3)), 
    neckTilt, headPan, headTilt);
}

void GT2005HeadControl::simpleLookAtPointRelativeToRobot(const Vector3<double> pos, Vector2<int> offsetInImage, 
                                    double& neckTilt, double& headPan, double& headTilt)
{
  Vector3<double> target(pos);

  /** transform the targets position to the robot's "neck coord. system": */
  target.z -= bodyPosture.neckHeightCalculatedFromLegSensors;
  RotationMatrix bodyRotation;
  bodyRotation.rotateY(-bodyPosture.bodyTiltCalculatedFromLegSensors).  
    rotateX(bodyPosture.bodyRollCalculatedFromLegSensors); /** what about the order of the rotations? */
  target = bodyRotation*target;                            /** rotate by the body's rotation(s) */


  neckTilt = -0.0;                                    /** first try the calculation of head joints with fixed neck tilt: */
  if (!simpleLookAtPointFixNeckTilt(target, neckTilt, headPan, headTilt))  
  {
    if (headTilt < jointLimitHeadTiltN) 
    {
      neckTilt = headTilt - jointLimitHeadTiltN;
      headTilt = jointLimitHeadTiltN;
    }
    else if (headTilt > jointLimitHeadTiltP)
  {
    // clip to maximum possible joint angle
    headTilt = jointLimitHeadTiltP;
  }

    //lookAtPointFixHeadTilt(target, neckTilt, headPan, headTilt);
  }

  /** now that the angles are found, add the offset-in-image: */

  /** clip image offset to image bounderies: */
  const Range<int> cameraResY(-cameraInfo.resolutionHeight / 2, cameraInfo.resolutionHeight / 2);
  const Range<int> cameraResX(-cameraInfo.resolutionWidth / 2, cameraInfo.resolutionWidth / 2);
  offsetInImage.x = cameraResX.limit(offsetInImage.x);
  offsetInImage.y = cameraResY.limit(offsetInImage.y);

  /** add angles for offset in image: */
  headPan  += ((double)offsetInImage.x/cameraInfo.resolutionWidth)*cameraInfo.openingAngleWidth;
  headTilt += ((double)offsetInImage.y/cameraInfo.resolutionHeight)*cameraInfo.openingAngleHeight;
 
  /** perform clipping to ranges of possible joint values: */
  const Range<double> jointRangeNeckTilt(jointLimitNeckTiltN,jointLimitNeckTiltP);
  const Range<double> jointRangeHeadPan(jointLimitHeadPanN, jointLimitHeadPanP);
  const Range<double> jointRangeHeadTilt(jointLimitHeadTiltN, jointLimitHeadTiltP);
  neckTilt = jointRangeNeckTilt.limit(neckTilt);
  headPan = jointRangeHeadPan.limit(headPan);
  headTilt = jointRangeHeadTilt.limit(headTilt);
}


void GT2005HeadControl::simpleLookAtPointOnField(const Vector3<double> pos, Vector2<int> offsetInImage, 
                                    double& neckTilt, double& headPan, double& headTilt)
{
  /** transform the targets position to the robot's "coord. system": */
  Vector3<double> target(pos);
  Pose2D robotPose = this->robotPose.getPose();
  target.x -= robotPose.translation.x;                /** transform from world to relative coordinates */
  target.y -= robotPose.translation.y;
 
  RotationMatrix bodyRotation;
  bodyRotation.rotateZ(-robotPose.getAngle());        /** what about the order of the rotations? */
  target = bodyRotation*target;                       /** rotate by the body's rotation(s) */

  simpleLookAtPointRelativeToRobot(target, offsetInImage, neckTilt, headPan, headTilt);
}


bool GT2005HeadControl::simpleLookAtPointFixNeckTilt(const Vector3<double> &aim, const double& neckTilt, double& headPan, double& headTilt)
{
  Vector3<double> target(aim);
  
  RotationMatrix rotationByNeckTilt;
  rotationByNeckTilt.rotateY(neckTilt);     /** transformation from "neck-coord" into cam. coord., i.e. */
  target.z -= distanceNeckToPanCenter;      /** translation by distanceNeckToPanCenter   */
  target = rotationByNeckTilt*target;       /** a rotation by the neck-joint Angle followed by */
   
  headPan = atan2(target.y, target.x);      /** get the pan angle from looking at where the target is in the xy plane */
                                            /** never mind clipping: if (headPan>=pi) headPan -= pi2; else if (headPan<-pi) headPan += pi2; */
  RotationMatrix rotationByHeadPan; 
  rotationByHeadPan.rotateZ(-headPan);  
  target = rotationByHeadPan*target;        /** perform (pan-) rotation on target */

  headTilt = atan2(target.z, target.x);     /** get the headTilt angle by taking the angle to the target in the XZ-Plane */
                                            /** again, no clipping: if (headTilt >= pi) headTilt -= pi2; else if (headTilt < -pi) headTilt += pi2; */      
                                            /** For debugging: Rotate camera coord. system by the tilt just found. If everything was done correctly, target should have y = z = 0.0 */
                                            /** RotationMatrix rotationByHeadTilt; rotationByHeadTilt.rotateY(headTilt); target = rotationByHeadTilt*target; */

  /** if values are our of bounds, return 0 so a different method can be applied! */
  if ((headTilt < jointLimitHeadTiltN) || (headTilt > jointLimitHeadTiltP) || 
    (headPan < jointLimitHeadPanN) || (headPan > jointLimitHeadPanP))
    return false;

  return true;
}



int GT2005HeadControl::calculateClosestLandmark(double direction, double nextLeftOrRight)
{
  return GT2005HeadControl::calculateClosestLandmark(robotPose, direction, nextLeftOrRight);
}

int GT2005HeadControl::calculateClosestLandmark(Pose2D pose, double direction, double nextLeftOrRight, bool disregardFieldMarkings)
{
  int i, closest = -1;
  double smallestAngle = 3;

  LINE(headControlField, 
    pose.translation.x, pose.translation.y, 
    pose.translation.x + 4000*cos(direction+pose.rotation), pose.translation.y + 4000*sin(direction+pose.rotation), 
    20, 0, Drawings::blue);

  for (i = 0; i < numOfLandmarks; i++)
  {
    if ((disregardFieldMarkings) && (targetPointOnLandmark[i].position.z == 0))
    {
      CIRCLE(headControlField, 
        targetPointOnLandmark[i].position.x, targetPointOnLandmark[i].position.y, 35, 
        2, 0, 1);
      continue; 
    }
    else
    {
    CIRCLE(headControlField, 
      targetPointOnLandmark[i].position.x, targetPointOnLandmark[i].position.y, 35, 
      2, 0, Drawings::blue);
    }
    Vector2<double> landMarkInRobotCoordinates(targetPointOnLandmark[i].position.x, targetPointOnLandmark[i].position.y);
    landMarkInRobotCoordinates -= pose.translation;
    double distToLandmark = landMarkInRobotCoordinates.abs();

    if (((targetPointOnLandmark[i].height > 0) && (distToLandmark > 250)) ||       // Landmarks such as beacons and goals can be useful from far away
       ((targetPointOnLandmark[i].height == 0) && (distToLandmark > 250) && (distToLandmark < 1500)))         // Landmarks on the floor are only useful if when they're close
    {
      double angleToLandmark;
      
      if (nextLeftOrRight == 0)
      {
        angleToLandmark = fabs(normalize(
          landMarkInRobotCoordinates.angle() - pose.rotation - direction)); 
      }
      else
      {
        angleToLandmark = sgn(nextLeftOrRight)*(normalize(
          landMarkInRobotCoordinates.angle() - pose.rotation - direction)); 
      }

      if((0.0 <= angleToLandmark ) && (angleToLandmark < smallestAngle))
      {
        //CIRCLE(headControlField, targetPointOnLandmark[i].position.x, targetPointOnLandmark[i].position.y, 50, 2, 0, Drawings::blue);

        smallestAngle = angleToLandmark;
        closest = i;
      }
    }
  }
  return closest;
}

void GT2005HeadControl::setupMainAngles()
{   
    headLeft.x = 0.052400; 
  headLeft.y = 1.573200; 
  headLeft.z = -0.120000;

  headLeftDown.x = 0.052400;
  headLeftDown.y = 1.573200;
  headLeftDown.z = -0.350000;
  
  headRight.x = 0.052400; 
  headRight.y = -1.573200; 
  headRight.z = -0.0120000;

  headRightDown.x = 0.052400;
  headRightDown.y = -1.573200;
  headRightDown.z = -0.350000;

  headMiddleLeft.x = 0.052400;
  headMiddleLeft.y = 0.560000;
  headMiddleLeft.z = 0.062000;

  headMiddleLeftDown.x = -0.1;
  headMiddleLeftDown.y = 0.560000;
  headMiddleLeftDown.z = -0.2;

  headMiddleRight.x = 0.052400;
  headMiddleRight.y = -0.560000;
  headMiddleRight.z = 0.062000;

  headMiddleRightDown.x = -0.1;
  headMiddleRightDown.y = -0.560000;
  headMiddleRightDown.z = -0.2;

    headUp.x = 0.052400;
    headUp.y = 0;
    headUp.z = 0.034500;

    headDown.x = -0.55000;
    headDown.y = 0;
    headDown.z = -2.8;
}


void GT2005HeadControl::calibrateHeadSpeed()
{
  // First look up-left,wait some time (roboter has to get up)
  // second look from left to right time (time this)
  // third look up down, using tilt1 (time this)
  // fourth look up down, using tilt2 (time this)

  Vector3<double> calibrationTilt1Down (-1.385604,0.0,0.040000);
  Vector3<double> calibrationTilt1Up (0.052359,0.0,0.040000);

  Vector3<double> calibrationTilt2Down (0.0,0.0,-0.326175);
  Vector3<double> calibrationTilt2Up (0.0,0.0,0.750630);

  Vector3<double> calibrationLeft  (0.052400, 1.608598,-0.120000);
  Vector3<double> calibrationRight (0.052400,-1.608598,-0.120000);
  
  enum {calibrationRounds = 3};

  if (headPathPlanner.isLastPathFinished() || calibrationState == calibrationStateStart)
  {
    switch(calibrationState)
    {
    case calibrationStateStart:
    {
      Vector3<double> points[]={headUp,headUp};
      long durations[] = {0,1000};
      
      headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>));
      calibrationState = calibrationStateLeft;
      calibrationRoundCount = 0;
      calibrationSuccessfulRounds = 0;
      speedNeckTilt = 0;
      speedHeadPan = 0;
      speedHeadTilt = 0;
      break;
    }
    case calibrationStateLeft:
    {
      Vector3<double> points[]={calibrationLeft,calibrationLeft};
      long durations[] = {2000,200};
      headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>),false);
      calibrationState = calibrationStateLeftWait;
      calibrationTime = SystemCall::getCurrentSystemTime();
      break;
    }
    case calibrationStateLeftWait:
      if (headPositionReached(calibrationLeft) || isTimedOut())
        calibrationState = calibrationStateRight;
      break;
    case calibrationStateRight:
    {
        Vector3<double> points[]={calibrationRight};
      long durations[] = {0};
      headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>),false);
      calibrationState = calibrationStateRightWait;
      calibrationTime = SystemCall::getCurrentSystemTime();
      break;
    }
    case calibrationStateRightWait:
      if (headPositionReached(calibrationRight) || isTimedOut())
      {
        if (!isTimedOut())
        {
          speedHeadPan += fabs(calibrationLeft.y-calibrationRight.y) / (SystemCall::getTimeSince(calibrationTime)+0.0001);
          calibrationSuccessfulRounds++;
        }
        else
          calibrationTimeOutsPan++;
        if (calibrationRoundCount < calibrationRounds-1)
        {
          calibrationState = calibrationStateLeft;
          calibrationRoundCount++;
        }
        else
        {
          // calibration of joint ready
          speedHeadPan /= calibrationSuccessfulRounds;
          calibrationSuccessfulRounds = 0; 
          calibrationRoundCount = 0;
          calibrationState = calibrationStateDownTilt1;
        }
      }
      break;
    case calibrationStateDownTilt1:
    {
      Vector3<double> points[]={calibrationTilt1Down,calibrationTilt1Down};
      long durations[] = {1500,200};
      headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>),false);
      calibrationState = calibrationStateDownTilt1Wait;
      calibrationTime = SystemCall::getCurrentSystemTime();
      break;
    }
    case calibrationStateDownTilt1Wait:
      if (headPositionReached(calibrationTilt1Down) || isTimedOut())
        calibrationState = calibrationStateUpTilt1;
      break;
    case calibrationStateUpTilt1:
    {
      Vector3<double> points[]={calibrationTilt1Up};
      long durations[] = {0};
      headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>),false);
      calibrationState = calibrationStateUpTilt1Wait;
      calibrationTime = SystemCall::getCurrentSystemTime();
      break;
    }
    case calibrationStateUpTilt1Wait:
      if (headPositionReached(calibrationTilt1Up) || isTimedOut())
      {
        if (!isTimedOut())
        {
          speedNeckTilt += fabs(calibrationTilt1Down.x-calibrationTilt1Up.x) / (SystemCall::getTimeSince(calibrationTime)+0.0001);
          calibrationSuccessfulRounds++;
        }
        else
          calibrationTimeOutsTilt1++;
        if (calibrationRoundCount < calibrationRounds-1)
        {
          calibrationState = calibrationStateDownTilt1;
          calibrationRoundCount++;
        }
        else
        {
          // calibration of joint ready
          speedNeckTilt /= calibrationSuccessfulRounds;
          calibrationSuccessfulRounds = 0; 
          calibrationRoundCount = 0;
          calibrationState = calibrationStateDownTilt2;
        }
      }
      break;
    case calibrationStateDownTilt2:
    {
      Vector3<double> points[]={calibrationTilt2Down,calibrationTilt2Down};
      long durations[] = {1500,200};
      headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>),false);
      calibrationState = calibrationStateDownTilt2Wait;
      calibrationTime = SystemCall::getCurrentSystemTime();
      break;
    }
    case calibrationStateDownTilt2Wait:
      if (headPositionReached(calibrationTilt2Down) || isTimedOut())
        calibrationState = calibrationStateUpTilt2;
      break;
    case calibrationStateUpTilt2:
    {
      Vector3<double> points[]={calibrationTilt2Up};
      long durations[] = {0};
      headPathPlanner.init(points,durations,sizeof(points)/sizeof(Vector3<double>),false);
      calibrationState = calibrationStateUpTilt2Wait;
      calibrationTime = SystemCall::getCurrentSystemTime();
      break;
    }
    case calibrationStateUpTilt2Wait:
      if (headPositionReached(calibrationTilt2Up) || isTimedOut())
      {
        if (!isTimedOut())
        {
          speedHeadTilt += fabs(calibrationTilt2Down.z-calibrationTilt2Up.z) / (SystemCall::getTimeSince(calibrationTime)+0.0001);
          calibrationSuccessfulRounds++;
        }
        else
          calibrationTimeOutsTilt2++;
        if (calibrationRoundCount < calibrationRounds-1)
        {
          calibrationState = calibrationStateDownTilt2;
          calibrationRoundCount++;
        }
        else
        {
          // calibration of joint ready
          speedHeadTilt /= calibrationSuccessfulRounds;
          calibrationSuccessfulRounds = 0; 
          calibrationRoundCount = 0;
          calibrationState = calibrationStateUseResults;
        }
      }

      break;
    case calibrationStateUseResults:
      if (   calibrationTimeOutsTilt1 == 0
          && calibrationTimeOutsPan   == 0
          && calibrationTimeOutsTilt2 == 0)
      {
        OUTPUT(idText,text,"Headspeed calibration was successful");
      }
      else
      {
        OUTPUT(idText,text,"Headspeed calibration failed. ");
        OUTPUT(idText,text,"Check the values of function headPositionReached or buy a new head !");
        OUTPUT(idText,text,"Rounds: " << calibrationRounds << "; Timeouts: Tilt1 = " << calibrationTimeOutsTilt1 << "; Pan = " << calibrationTimeOutsPan << "; Tilt2 = " << calibrationTimeOutsTilt2 );
      }
      // if too much timeout  occured, set to standard values
      if (calibrationTimeOutsTilt1-calibrationRounds == 0) speedNeckTilt = 0.001500;
      if (calibrationTimeOutsPan-calibrationRounds == 0)   speedHeadPan  = 0.005350;
      if (calibrationTimeOutsTilt2-calibrationRounds == 0) speedHeadTilt = 0.002350;

      OUTPUT(idText,text,"speedTilt1:" << speedNeckTilt);
      OUTPUT(idText,text,"speedPan:" << speedHeadPan);
      OUTPUT(idText,text,"speedTilt2:" << speedHeadTilt);


      // setting speed in headpath planner
      headPathPlanner.headPathSpeedNeckTilt = (double) speedNeckTilt;
      headPathPlanner.headPathSpeedHeadPan   = (double) speedHeadPan;
      headPathPlanner.headPathSpeedHeadTilt = (double) speedHeadTilt;
      calibrationState = calibrationStateReady;
      break;
    default:
    case calibrationStateReady:
      break;
    }
  }
}

---