Modules/BehaviorControl/GT2005BehaviorControl/GT2005BasicBehaviors/GT2005SimpleBasicBehaviors.cpp

Go to the documentation of this file.

/** 
* @file GT2005SimpleBasicBehaviors.cpp
*
* Implementation of basic behaviors defined in simple-basic-behaviors.xml.
*
* @author Uwe Düffert
* @author Jan Hoffmann
* @author Matthias Jüngel
* @author Martin Lötzsch
* @author Max Risler
* @author Tim Laue
*/

#include "GT2005SimpleBasicBehaviors.h"
#include "Tools/Math/PIDsmoothedValue.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Math/Matrix2x2.h"
#include "Tools/Math/Common.h"
#include "Representations/Perception/SlamPercept.h"

#include "Platform/GTAssert.h"

#define slamPercept (*(SlamPercept *)specialPercept.slamData)

#ifdef APERIOS1_3_2
  #include <ERA201D1.h>
#endif

void GT2005BasicBehaviorNewGoToBall::execute()
{  
  if (maxSpeed == 0) maxSpeed = 600;
  if (maxSpeedY == 0) maxSpeedY = 600;

  double maxTurnSpeed;
  if (this->maxTurnSpeed == 0)
    maxTurnSpeed = fromDegrees(180);
  else
    maxTurnSpeed = fromDegrees(this->maxTurnSpeed);
  
  Vector2<double> ballInWorldCoord = ballModel.getKnownPosition(
    BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted);
  double angleToBall = Geometry::angleTo(robotPose.getPose(),ballInWorldCoord);
  double distanceToBall = Geometry::distanceTo(robotPose.getPose(),ballInWorldCoord);
  
  double factor;
  // if distance is between 400 and 600,
  // perform linear interpolation
  // factor = 1 - (distanceToBall - 400) / 200;
  // if (factor > 1) factor = 1;
  // if (factor < 0) factor = 0;
  
  //targetAngle *= factor;
  //angleToBall += targetAngle;  
  
  // destination = ball position in robot 
  // coordintes plus 200mm in x direction
  Vector2<double> destination(
    distanceToBall * cos(angleToBall) + 400, // ?? shouldnt be 200?
    distanceToBall * sin(angleToBall) + yOffset);
  
  double factorClipping = 1.0; // full speed

  if (distanceToBall < slowDownDistance + 50)
  {
  factorClipping = 0.9; // smoother approach
  }
  // adjust forward speed:
  // if a lot of turning is necessary, don't walk so fast!
  factor = (pi-fabs(angleToBall))/pi;
  if (factor > factorClipping) factor = factorClipping;
  if (factor < 0) factor = 0;
  
  destination.normalize();
  
  if (distanceToBall < slowDownDistance)
  {
    destination *= maxSpeed*((distanceToBall/2)/slowDownDistance);
  }
  else 
    destination *= (maxSpeed*factor);
  
  /*
  factor = .2 + distanceToBall / 500;
  if (factor > 1) factor = 1;
  if (factor < 0) factor = 0;
  
    destination *= factor;
  */
  
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType   = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
  motionRequest.walkRequest.walkParams.translation.x = destination.x;
  motionRequest.walkRequest.walkParams.translation.y = destination.y;
  
  if (motionRequest.walkRequest.walkParams.translation.y > maxSpeedY)
    motionRequest.walkRequest.walkParams.translation.y = maxSpeedY;
  
  if(angleFactor == 0)
    angleFactor = 1.0;

  if(distanceToBall > 300)
    factor = angleFactor;
  else
    factor = angleFactor + 0.5;
  /*
  factor = .2 + 1.5 * distanceToBall / 800;
  if (factor > 1.5) factor = 1.5;
  if (factor < 0) factor = 0;
  */
  motionRequest.walkRequest.walkParams.rotation = angleToBall * factor; /* this should also be factored!!!! */
  
  // clip rotation speed
  motionRequest.walkRequest.walkParams.rotation = min(motionRequest.walkRequest.walkParams.rotation, maxTurnSpeed);
  motionRequest.walkRequest.walkParams.rotation = max(motionRequest.walkRequest.walkParams.rotation, -maxTurnSpeed);  
}

void GT2005BasicBehaviorGoToBall::execute()
{
  //accelerationRestrictor.saveLastWalkParameters();
  double targetAngle;
  
  if (maxSpeed == 0) maxSpeed = 350;
  if (maxSpeedY == 0) maxSpeed = 350;
  if (slowDownDistance == 0) slowDownDistance = 400;
  if (targetAngleToBall == 0) 
    targetAngle = 0;
  else 
    targetAngle = targetAngleToBall;
  
  
  double maxTurnSpeed;
  if (this->maxTurnSpeed == 0)
    maxTurnSpeed = fromDegrees(180);
  else
    maxTurnSpeed = fromDegrees(this->maxTurnSpeed);
  
  Vector2<double> ballInWorldCoord = ballModel.getKnownPosition(
    BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted);
  double angleToBall = Geometry::angleTo(robotPose.getPose(),ballInWorldCoord);
  double distanceToBall = Geometry::distanceTo(robotPose.getPose(),ballInWorldCoord);
  
  // if distance is between 400 and 600,
  // perform linear interpolation


  //double factor = 1 - (distanceToBall - 400) / 200; // unused
  //if (factor > 1) factor = 1; // unused
  //if (factor < 0) factor = 0; // unused
  
  //targetAngle *= factor;
  //angleToBall += targetAngle;  
  
  // destination = ball position in robot 
  // coordintes plus 200mm in x direction
  Vector2<double> destination(
    distanceToBall * cos(angleToBall) + 400, 
    distanceToBall * sin(angleToBall) + targetAngleToBall + yOffset);

  double factorClipping = 1.0; // full speed

  if (distanceToBall < slowDownDistance + 50)
  {
  factorClipping = 0.9; // smoother approach
  }

  // adjust forward speed:
  // if a lot of turning is necessary, don't walk so fast!
  double factor = (pi-fabs(angleToBall))/pi;
  if (factor > factorClipping) factor = factorClipping;
  if (factor < 0) factor = 0;
  
  destination.normalize();
  destination *= (maxSpeed*factor);
  
  /*
  factor = .2 + distanceToBall / 500;
  if (factor > 1) factor = 1;
  if (factor < 0) factor = 0;
  
    destination *= factor;
  */
  
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType   = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
  motionRequest.walkRequest.walkParams.translation.x = destination.x;
  motionRequest.walkRequest.walkParams.translation.y = destination.y;
  
  if (motionRequest.walkRequest.walkParams.translation.y > maxSpeedY)
    motionRequest.walkRequest.walkParams.translation.y = maxSpeedY;
  
  factor = 1.5;
  /*
  factor = .2 + 1.5 * distanceToBall / 800;
  if (factor > 1.5) factor = 1.5;
  if (factor < 0) factor = 0;
  */
  motionRequest.walkRequest.walkParams.rotation = angleToBall * factor; /* this should also be factored!!!! */
  
  // clip rotation speed
  motionRequest.walkRequest.walkParams.rotation = min(motionRequest.walkRequest.walkParams.rotation, maxTurnSpeed);
  motionRequest.walkRequest.walkParams.rotation = max(motionRequest.walkRequest.walkParams.rotation, -maxTurnSpeed);  
}

void GT2005BasicBehaviorGoToBallPropagated::execute()
{
  //accelerationRestrictor.saveLastWalkParameters();
  
  double targetAngle;
  
  if (maxSpeed == 0) maxSpeed = 450;
  if (maxSpeedY == 0) maxSpeed = 450;
  if (targetAngleToBall == 0) 
    targetAngle = 0;
  else 
    targetAngle = targetAngleToBall;
  
  
  double maxTurnSpeed;
  if (this->maxTurnSpeed == 0)
    maxTurnSpeed = fromDegrees(180);
  else
    maxTurnSpeed = fromDegrees(this->maxTurnSpeed);

  Vector2<double> ballInWorldCoord = ballModel.propagated.positionField;
  
  double angleToBall = Geometry::angleTo(robotPose.getPose(),ballInWorldCoord);
  double distanceToBall = Geometry::distanceTo(robotPose.getPose(),ballInWorldCoord);
  
  
  // if distance is between 400 and 600,
  // perform linear interpolation
  double factor = 1 - (distanceToBall - 400) / 200;
  if (factor > 1) factor = 1;
  if (factor < 0) factor = 0;
  
  //targetAngle *= factor;
  //angleToBall += targetAngle;  
  
  // destination = ball position in robot 
  // coordintes plus 200mm in x direction
  Vector2<double> destination(
    distanceToBall * cos(angleToBall) + 400, 
    distanceToBall * sin(angleToBall) + targetAngleToBall + yOffset);
  
  // adjust forward speed:
  // if a lot of turning is necessary, don't walk so fast!
  factor = (pi-fabs(angleToBall))/pi;
  if (factor > 1) factor = 1;
  if (factor < 0) factor = 0;
  factor = 1;
  
  destination.normalize();
  destination *= (maxSpeed*factor);
  
  /*
  factor = .2 + distanceToBall / 500;
  if (factor > 1) factor = 1;
  if (factor < 0) factor = 0;
  
    destination *= factor;
  */
  
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType   = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
  motionRequest.walkRequest.walkParams.translation.x = destination.x;
  motionRequest.walkRequest.walkParams.translation.y = destination.y;
  
  if (motionRequest.walkRequest.walkParams.translation.y > maxSpeedY)
    motionRequest.walkRequest.walkParams.translation.y = maxSpeedY;
  if (motionRequest.walkRequest.walkParams.translation.y < -maxSpeedY)
    motionRequest.walkRequest.walkParams.translation.y = -maxSpeedY;
  
  factor = 1.5;
  /*
  factor = .2 + 1.5 * distanceToBall / 800;
  if (factor > 1.5) factor = 1.5;
  if (factor < 0) factor = 0;
  */
  motionRequest.walkRequest.walkParams.rotation = angleToBall * factor; /* this should also be factored!!!! */
  
  // clip rotation speed
  motionRequest.walkRequest.walkParams.rotation = min(motionRequest.walkRequest.walkParams.rotation, maxTurnSpeed);
  motionRequest.walkRequest.walkParams.rotation = max(motionRequest.walkRequest.walkParams.rotation, -maxTurnSpeed);  
}


void GT2005BasicBehaviorGoToBallWithoutTurning::execute()
{
  if (maxSpeed == 0) maxSpeed = 250;

  Vector2<double> ballInWorldCoord = ballModel.getKnownPosition(
    BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted);
  double angleToBall = Geometry::angleTo(robotPose.getPose(),ballInWorldCoord);
  double distanceToBall = Geometry::distanceTo(robotPose.getPose(),ballInWorldCoord);

  Vector2<double> destination(
    distanceToBall * cos(angleToBall), 
    distanceToBall * sin(angleToBall) );

  destination.normalize();
  destination *= maxSpeed;

  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;  
  motionRequest.walkRequest.walkParams.translation.x = destination.x;
  motionRequest.walkRequest.walkParams.translation.y = destination.y;
  
  motionRequest.walkRequest.walkParams.rotation = 0;
}


void GT2005BasicBehaviorGoaliePosition::execute()
{
  
  if (!basicBehaviorWasActiveDuringLastExecutionOfEngine) {
    goaliePose = lastRobotPose = robotPose;
    lastOdometry = odometryData;
  }
  
  // optional parameter initialising
  if (maxSpeed == 0 )
    maxSpeed = 150;
  
  //Aldi
  if (cutY == 0)
    cutY = 300; // max distance from middle of goal
  if (guardDirectToGoal == 0)
    guardDirectToGoal = 200; // if Ball.x distance to goal < this, goalie goes back to groundline

  if (guardLine == 0)
    guardLine = -150;

  // begin improved localization
  // use *goaliePose* instead of robotPose
  
  Pose2D diffPose;
  diffPose.translation = robotPose.translation - lastGoaliePose.translation;
  diffPose.rotation = (robotPose.rotation - lastGoaliePose.rotation);

  Pose2D diffOdo = odometryData - lastOdometry;
  Vector2<double> diffTranslation = (diffPose.translation*weightPose + diffOdo.translation*weightOdo);

  goaliePose.translation += diffTranslation;
  goaliePose.rotation += (diffPose.rotation*weightPose) + (diffOdo.rotation*weightOdo);
  
  CIRCLE(goaliePositionField, goaliePose.translation.x, goaliePose.translation.y, 200, 6, 0, Drawings::blue);
  CIRCLE(goaliePositionField, robotPose.translation.x, robotPose.translation.y, 180, 6, 0, Drawings::yellow);
  //CIRCLE(goaliePositionField, diffPose.translation.x, diffPose.translation.y, 140, 6, 0, Drawings::blue);
  //CIRCLE(goaliePositionField, diffOdo.translation.x, diffOdo.translation.y, 140, 6, 0, Drawings::red);
  //CIRCLE(goaliePositionField, diffTranslation.x, diffTranslation.y, 140, 6, 0, Drawings::yellow);
  
  // end improved localization
  
  // begin calculating guardPosition 
  Vector2<double> ballInWorldCoord = ballModel.getKnownPosition(
    BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted);
  double angleToBall = Geometry::angleTo(goaliePose,ballInWorldCoord);
  
  Vector2<double> guardPosition; //(Range<double>(-3000,3000),Range<double>(-200,200));
  
  // x-pos: Goalie stays *guardDirectToGoal* mm before goalline, except the half balldistance.x is shorter
  guardPosition.x = min(xPosOwnGroundline + guardDirectToGoal, xPosOwnGroundline - (xPosOwnGroundline-ballInWorldCoord.x)/2);
  
  
  //guardPosition.y = ballInWorldCoord.y / 2;
  
    //line from xPosOwnGroundline+guardLine   
  double deltaX = ballInWorldCoord.x - (xPosOwnGroundline+guardLine);
  double m = 0;
  if (deltaX > 0)
    m = ballInWorldCoord.y / deltaX;
  double n = ballInWorldCoord.y - m*ballInWorldCoord.x;
  
  guardPosition.y = m*goaliePose.translation.x + n;
  ARROW(goaliePositionField, xPosOwnGroundline+guardLine,0,ballInWorldCoord.x,ballInWorldCoord.y,6,0,Drawings::gray);
  Vector2<double> pointToGuard = ballInWorldCoord - Vector2<double>(xPosOwnGroundline,0);
  
  if (fabs(guardPosition.y) > cutY){
    guardPosition.y = min(cutY, guardPosition.y);
    guardPosition.y = max(-cutY, guardPosition.y);
    LINE(goaliePositionField,xPosOwnGroundline, guardPosition.y,-xPosOwnGroundline,guardPosition.y,6,0,Drawings::red);
  }

  if (xPosOwnGroundline-ballInWorldCoord.x > -guardDirectToGoal){
    guardPosition.x = xPosOwnGroundline+67;
    LINE (goaliePositionField,xPosOwnGroundline, 1000,xPosOwnGroundline+67, -1000,6,0,Drawings::red);
  }
  
  //guardPosition.normalize();
  
  CIRCLE(goaliePositionField, guardPosition.x, guardPosition.y, 150, 3, 0, Drawings::red);
  // end calculation guardPosition
  
  // begin calculation direction
  Vector2<double> direction = guardPosition - goaliePose.translation;
  Vector2<double> c1(cos(goaliePose.rotation), -sin(goaliePose.rotation)), 
    c2(sin(goaliePose.rotation), cos(goaliePose.rotation));
  Matrix2x2<double> rotate(c1, c2);
  
  direction = rotate*direction;
  if (direction.abs() > maxSpeed)
  {
    direction.normalize();
    direction *= maxSpeed;
  }
  
  double rotation = angleToBall;
  //end calculation direction
  
  // begin generation motionRequest with clipping
  // anti-zucking
  if( (fabs(rotation) < 0.2) && (abs(int(direction.x)) < minXTrans) && (abs(int(direction.y)) < minYTrans)){
    motionRequest.motionType = MotionRequest::stand;
  //  goaliePose = lastGoaliePose;
  }
  else 
  { 
    motionRequest.motionType = MotionRequest::walk;
    motionRequest.walkRequest.walkType = WalkRequest::normal;  
    if (!((abs(int(direction.x)) < minXTrans) && (abs(int(direction.y)) < minYTrans))){
      motionRequest.walkRequest.walkParams.translation.x = direction.x;
      motionRequest.walkRequest.walkParams.translation.y = direction.y;
    }
    if(!(fabs(rotation) < 0.2))
      motionRequest.walkRequest.walkParams.rotation = rotation; 
  }
  lastGoaliePose = goaliePose;
  lastOdometry = odometryData;
  lastRobotPose = robotPose;
  //end generation motion request
  DEBUG_DRAWING_FINISHED(goaliePositionField);  

}


void GT2005BasicBehaviorGoaliePositionReturn::execute()
{
  Vector2<double> destination(x,y);
  const Vector2<double>& self = robotPose.translation;
  
  double distanceToDestination = Geometry::distanceTo(self,destination);
  
  double angleDifference = normalize(fromDegrees(destinationAngle) - robotPose.rotation);
  
  if ((fabs(toDegrees(angleDifference))<25)&&(distanceToDestination<90))
  {
    motionRequest.motionType = MotionRequest::stand;
  }
  else
  {
    accelerationRestrictor.saveLastWalkParameters();
    
    if (maxSpeed==0) maxSpeed = 200;
    
    double maxTurnSpeed = fromDegrees(90);
    
    double angleToDestination = Geometry::angleTo(robotPose,destination);
    
    motionRequest.motionType = MotionRequest::walk;
    motionRequest.walkRequest.walkType = WalkRequest::normal;
    
    //this time does not necessarily include time for turning!:
    
    if (distanceToDestination > 200)
    {
      motionRequest.walkRequest.walkParams.translation.x = cos(angleToDestination) * maxSpeed;
      motionRequest.walkRequest.walkParams.translation.y = sin(angleToDestination) * maxSpeed;
    }
    else
    {
      motionRequest.walkRequest.walkParams.translation.x = 
        cos(angleToDestination) * maxSpeed*distanceToDestination/200;
      motionRequest.walkRequest.walkParams.translation.y = 
        sin(angleToDestination) * maxSpeed*distanceToDestination/200;
    }
    
    
    motionRequest.walkRequest.walkParams.rotation = angleDifference * 2;
    
    if (motionRequest.walkRequest.walkParams.rotation < -maxTurnSpeed)
      motionRequest.walkRequest.walkParams.rotation = -maxTurnSpeed;
    
    if (motionRequest.walkRequest.walkParams.rotation > maxTurnSpeed)
      motionRequest.walkRequest.walkParams.rotation = maxTurnSpeed;
    
    
    
    accelerationRestrictor.restrictAccelerations(150,150,100);
  }  
}


void GT2005BasicBehaviorGoalieReturnToGoal::execute()
{
  double angleToTurn;
  if(SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen) < 3000)
    //If the ball has been seen: Face the ball.
    angleToTurn = Geometry::angleTo(robotPose, ballModel.getKnownPosition(5000));
  else
    //Otherwise turn forward to the field:
    angleToTurn = -robotPose.rotation;
  
  // The destination to walk to:
  Vector2<double> destination(x,y);
  double angleToDestination(Geometry::angleTo(robotPose,destination));
  double distanceToDestination(Geometry::distanceTo(robotPose.translation,destination));

  // If this Basic Behavior is used in conjunction with GoaliePosition, this
  // condition will never be true. Anyway:
  if ((fabs(toDegrees(angleToTurn))<25)&&(distanceToDestination<90))
  {
    motionRequest.motionType = MotionRequest::stand;
  }
  else
  {
    accelerationRestrictor.saveLastWalkParameters();
    if (maxSpeed==0) maxSpeed = 250;
    
    // The robot has two goals: The destination and a certain distance to the groundline 
    // (to avoid collisions with the goal)
    Vector2<double> vecToDestination(cos(angleToDestination), sin(angleToDestination));
    Vector2<double> vecFromGroundLine(cos(-robotPose.rotation), sin(-robotPose.rotation));
    double importanceToAvoidGroundLine(0.0);
    if(robotPose.translation.x <= xPosOwnGroundline + 50)
      importanceToAvoidGroundLine = 1.0;
    else if(robotPose.translation.x <= xPosOwnGroundline+300)
      importanceToAvoidGroundLine = 1.0 - (fabs(double(xPosOwnGroundline - robotPose.translation.x + 50)) / 250.0);
    motionRequest.walkRequest.walkParams.translation = ((vecToDestination * (1 - importanceToAvoidGroundLine)) + 
                                                        (vecFromGroundLine * importanceToAvoidGroundLine)) * maxSpeed;

    //Slow down near destination (should not be true, if used in conjunction with GoaliePosition):
    if(distanceToDestination <= 200)
      motionRequest.walkRequest.walkParams.translation /= (distanceToDestination / 200.0);
       
    //Compute rotation:
    double maxTurnSpeed(fromDegrees(120));
    motionRequest.walkRequest.walkParams.rotation = angleToTurn * 2.0;
    if (motionRequest.walkRequest.walkParams.rotation < -maxTurnSpeed)
      motionRequest.walkRequest.walkParams.rotation = -maxTurnSpeed;
    else if (motionRequest.walkRequest.walkParams.rotation > maxTurnSpeed)
      motionRequest.walkRequest.walkParams.rotation = maxTurnSpeed;

    motionRequest.motionType = MotionRequest::walk;
    motionRequest.walkRequest.walkType = WalkRequest::normal;
    accelerationRestrictor.restrictAccelerations(250,250,120);
  }  
}


void GT2005BasicBehaviorTurnAroundPoint::execute()
{
  double maxTurnSpeed = fromDegrees(240);
  if (forwardComponent == 0) forwardComponent = 200.0;
  
  Vector2<double> destinationInWorldCoord(x,y);
  double angleToDestination = Geometry::angleTo(robotPose.getPose(), destinationInWorldCoord);
  double distance = Geometry::distanceTo(robotPose.getPose(), destinationInWorldCoord);
  
  // destination = ball position in robot 
  // coordintes plus 100mm in x direction
  Vector2<double> destination(
    distance * cos(angleToDestination), 
    distance * sin(angleToDestination));
  
  double factor = 
    Range<double>::Range(0, 1).
      limit((distance - radius) / (3*radius));

  destination *= factor;
  destination.y += (leftRight > 0 ? 1 : -1)*(200)*(1-factor);
  
  if (destination.x < forwardComponent)   
    destination.x = forwardComponent;
  
  double slowDownForMuchTurning = 
    Range<double>::Range(0.1, 1).limit
    ((pi-fabs(angleToDestination))/pi);

  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;  
  motionRequest.walkRequest.walkParams.translation.x = destination.x * slowDownForMuchTurning;
  motionRequest.walkRequest.walkParams.translation.y = destination.y * slowDownForMuchTurning;
  
  // set rotation speed in range
  motionRequest.walkRequest.walkParams.rotation = 
    Range<double>::Range(-maxTurnSpeed, maxTurnSpeed).
    limit(angleToDestination + .6*(leftRight > 0 ? -1 : 1)*(1-factor));
}

void GT2005BasicBehaviorTurnAroundPointFast::execute()
{
  double maxTurnSpeed = fromDegrees(200);
  if (forwardComponent == 0) forwardComponent = 200.0;
  
  Vector2<double> destinationInWorldCoord(x,y);
  double angleToDestination = Geometry::angleTo(robotPose.getPose(), destinationInWorldCoord);
  double distance = Geometry::distanceTo(robotPose.getPose(), destinationInWorldCoord);
  
  // destination = ball position in robot 
  // coordintes plus 100mm in x direction
  Vector2<double> destination(
    distance * cos(angleToDestination), 
    distance * sin(angleToDestination));
  
  double factor = 
    Range<double>::Range(0, 1).
      limit((distance - radius) / (3*radius));

  destination *= factor * 2;
  destination.y += (leftRight > 0 ? 1 : -1)*(450)*(1-factor);
  
  if (destination.x < forwardComponent)   
    destination.x = forwardComponent;
  
  double slowDownForMuchTurning = 
    Range<double>::Range(0.3, 1).limit
    ((pi-fabs(angleToDestination))/pi);

  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;  
  motionRequest.walkRequest.walkParams.translation.x = destination.x * slowDownForMuchTurning;
  motionRequest.walkRequest.walkParams.translation.y = destination.y * slowDownForMuchTurning;
  
  // set rotation speed in range
  motionRequest.walkRequest.walkParams.rotation = 
    Range<double>::Range(-maxTurnSpeed, maxTurnSpeed).
    limit(angleToDestination + .6*(leftRight > 0 ? -1 : 1)*(1-factor));
}

void GT2005BasicBehaviorTurnAroundPointWithRadius::execute()
{
  double maxTurnSpeed = fromDegrees(240);
  if (forwardComponent == 0) forwardComponent = 200.0;
  
  Vector2<double> destinationInWorldCoord(x,y);
  double angleToDestination = Geometry::angleTo(robotPose.getPose(), destinationInWorldCoord);
  double distance = Geometry::distanceTo(robotPose.getPose(), destinationInWorldCoord);
  
  // destination = ball position in robot 
  // coordintes plus 100mm in x direction
  Vector2<double> destination(
    distance * cos(angleToDestination), 
    distance * sin(angleToDestination));
  
  double factor = 
    Range<double>::Range(-1, 1).
      limit((distance - radius) / (radius));

  destination *= factor;
  destination.y += (leftRight > 0 ? 1 : -1)*(1000)*(1-factor);
  
  if (destination.x < forwardComponent)   
    destination.x = forwardComponent;
  
  double slowDownForMuchTurning = 
    Range<double>::Range(0.1, 1).limit
    ((pi-fabs(angleToDestination))/pi);

  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;  
  motionRequest.walkRequest.walkParams.translation.x = destination.x * slowDownForMuchTurning;
  motionRequest.walkRequest.walkParams.translation.y = destination.y * slowDownForMuchTurning;
  
  double angleFactor = 5;
  //MODIFY("angle factor", angleFactor); 

  // set rotation speed in range
  motionRequest.walkRequest.walkParams.rotation = 
    Range<double>::Range(-maxTurnSpeed, maxTurnSpeed).
    limit(angleToDestination*angleFactor + (leftRight > 0 ? -1 : 1)); //*(factor));
}


void GT2005BasicBehaviorGoForwardToPoint::execute()
{
  accelerationRestrictor.saveLastWalkParameters();
  
  Vector2<double> destination(x,y);
  
  if (maxSpeed == 0) maxSpeed = 350;
  
  double maxTurnSpeed = fromDegrees(120);
  
  double angleToDestination = Geometry::angleTo(robotPose,destination);
  
  
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = WalkRequest::normal;
  
  motionRequest.walkRequest.walkParams.translation.x = cos(angleToDestination) * maxSpeed;
  motionRequest.walkRequest.walkParams.translation.y = sin(angleToDestination) * maxSpeed;
  
  motionRequest.walkRequest.walkParams.rotation = angleToDestination*2;
  if (motionRequest.walkRequest.walkParams.rotation > maxTurnSpeed)
  {
    motionRequest.walkRequest.walkParams.rotation = maxTurnSpeed;
  }
  if (motionRequest.walkRequest.walkParams.rotation < -maxTurnSpeed) 
  {
    motionRequest.walkRequest.walkParams.rotation = -maxTurnSpeed;
  }
  
  accelerationRestrictor.restrictAccelerations(300,300,200);
  
}


void GT2005BasicBehaviorGoToPointAndAvoidObstacles::execute()
{
  double 
    obstacleAvoidanceDistance,
    distanceToDestination = Geometry::distanceTo(robotPose.getPose(),Vector2<double>(x,y)),
    angleToDestination = Geometry::angleTo(robotPose.getPose(),Vector2<double>(x,y)),
    targetSpeed, targetAngle, 
    freeSpaceInFront,
    widthOfCorridor,
    minSpeed,
    distanceBelowWhichObstaclesAreAvoided;
  
  // reset PIDs if last execution is long ago
  if (SystemCall::getTimeSince(lastTimeExecuted) > 500)
  {
    speedX.resetTo(100);
    speedY.resetTo(0);
    turnSpeed.resetTo(0); 
  }
  lastTimeExecuted = SystemCall::getCurrentSystemTime();
  
  if (maxSpeed == 0) maxSpeed = 600;
    
  // avoidance level:
  // 0 = rugby (very little avoidance)
  // 1 = football (medium...)
  // 2 = standard, stay clear of obstacles
  
//  switch((int)avoidanceLevel)
//  {
//  case 0: widthOfCorridor = 150; break;
//  case 1: widthOfCorridor = 200; break;
//  case 2: 
//  default: widthOfCorridor = 250; break;
//  }
  
  widthOfCorridor = 350;
  
  distanceBelowWhichObstaclesAreAvoided = 1000;
  minSpeed = -150;
  
  // perform clipping ...
  obstacleAvoidanceDistance = Range<double>::Range(0, distanceBelowWhichObstaclesAreAvoided).limit(distanceToDestination);
  
  // derive the forward speed of the robot
  // from the free range in front of the robot
  freeSpaceInFront = obstaclesModel.getDistanceInCorridor(0.0, widthOfCorridor);
  targetSpeed = Range<double>::Range(minSpeed, maxSpeed).limit(freeSpaceInFront - 300);
  
  // default: head to where there's more free space (necessary when close to obstacles!)
  double leftForward = obstaclesModel.getTotalFreeSpaceInSector(.7, 1.39, obstacleAvoidanceDistance);
  double rightForward = obstaclesModel.getTotalFreeSpaceInSector(-.7, 1.39, obstacleAvoidanceDistance);
  
  // this angle is a good angle to head towards free space
  targetAngle = (leftForward - rightForward) / (leftForward + rightForward);
      
  // determine the next free sector in the desired direction
  double nextFree = obstaclesModel.getAngleOfNextFreeSector(pi/4, angleToDestination, (int )obstacleAvoidanceDistance);
  
  // if there is enough space in the direction of "nextFree" 
  // (which is a good direction heading towards the goal...) then
  // head towards it
  if (obstaclesModel.getDistanceInCorridor(nextFree, widthOfCorridor) > obstacleAvoidanceDistance)
  {
    //if (fabs(nextFree) < .1)
      targetAngle = nextFree;
  } 
  // double check: if there's enough space in the direction of the 
  // destination, directly head there 
  else if (obstaclesModel.getDistanceInCorridor(angleToDestination, widthOfCorridor) > obstacleAvoidanceDistance)
  {
    targetAngle = angleToDestination;
  }
  // there is something close and obstacle avoidance should be performed
  else
  {
    double minTurnAngle = 0.3; // this angle is necessary for obstacles that are perpenticular in front of the robot and the target angle is therefore very small
    if ((targetAngle < minTurnAngle) && (targetAngle > 0))
      targetAngle = minTurnAngle;
    if ((targetAngle < 0) && (targetAngle > -minTurnAngle))
      targetAngle = -minTurnAngle;
  }


  
    
  // what is this good for?
  //if(obstaclesModel.getDistanceInCorridor((double)sgn(targetAngle) * pi/2, distanceBelowWhichObstaclesAreAvoided/2) < 100)
  //{
  //  targetSpeed = maxSpeed;
  //  targetAngle = 0;  
  //}
  
  // adjust forward speed: if a lot of turning is required, reduce the forward speed!
  if (targetSpeed == maxSpeed)
    targetSpeed *= Range<double>::Range(0.3, 1).limit( 
      (pi-fabs(targetAngle))/pi );

  if (targetSpeed < 150) targetSpeed = 150; 

  // if at destination, stop
  if (distanceToDestination < 150)
  {
    motionRequest.motionType = MotionRequest::stand;
  }
  else
  {
    motionRequest.motionType = MotionRequest::walk;
    motionRequest.walkRequest.walkType = WalkRequest::normal;
    motionRequest.walkRequest.walkParams.translation.x = speedX.approximateVal(targetSpeed);
    motionRequest.walkRequest.walkParams.translation.y = speedY.approximateVal(0);
    motionRequest.walkRequest.walkParams.rotation = turnSpeed.approximateVal(2*targetAngle);
  }
}

void GT2005BasicBehaviorGoToPoint::execute()
{
  accelerationRestrictor.saveLastWalkParameters();
  
  Vector2<double> destination(x,y);
  const Vector2<double>& self = robotPose.translation;
  
  if (maxSpeed==0) maxSpeed = 350;
  if (maxSpeedY==0) maxSpeedY = 350;
  
  double maxTurnSpeed = fromDegrees(40);
  
  double distanceToDestination = Geometry::distanceTo(self,destination);
  
  double angleDifference = normalize(fromDegrees(destinationAngle) - robotPose.rotation);
  
  double angleToDestination = Geometry::angleTo(robotPose,destination);
  
  
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
  
  //this time does not necessarily include time for turning!:
  double estimatedTimeToReachDestination;
  if (distanceToDestination > 200)
  {
    estimatedTimeToReachDestination = (distanceToDestination+200) / maxSpeed;
    
    motionRequest.walkRequest.walkParams.translation.x = cos(angleToDestination) * maxSpeed;
    motionRequest.walkRequest.walkParams.translation.y = sin(angleToDestination) * maxSpeed;
  }
  else
  {
    estimatedTimeToReachDestination = 2*distanceToDestination / maxSpeed + 2*fabs(angleDifference) / maxTurnSpeed;
//     OUTPUT (idText, text, "Time2ReachDestination previous: " << 
//      2*distanceToDestination / maxSpeed << "\nTime2ReachDestination now: " << estimatedTimeToReachDestination);
    
    if (distanceToDestination > 30)
    {
      motionRequest.walkRequest.walkParams.translation.x = 
        cos(angleToDestination) * maxSpeed * distanceToDestination/200;
      motionRequest.walkRequest.walkParams.translation.y = 
        sin(angleToDestination) * maxSpeed * distanceToDestination/200;
    }
    else
    {
      motionRequest.walkRequest.walkParams.translation.x = 0;
      motionRequest.walkRequest.walkParams.translation.y = 0;
    }
  }

  // If the estimated time is 0, position is already reached, so nothing has to be done anymore
  if (estimatedTimeToReachDestination != 0)
  {
  /* 
  {
    estimatedTimeToReachDestination = 0.001;
  }
  */
  
    if (fabs(toDegrees(angleDifference)) > 20)
    {
      motionRequest.walkRequest.walkParams.rotation =
        angleDifference / estimatedTimeToReachDestination;
      motionRequest.walkRequest.walkParams.rotation =
        min (maxTurnSpeed, motionRequest.walkRequest.walkParams.rotation);
      motionRequest.walkRequest.walkParams.rotation =
        max (-maxTurnSpeed, motionRequest.walkRequest.walkParams.rotation);
    }
    else
    {
      motionRequest.walkRequest.walkParams.rotation = 2*angleDifference;
    }
    
    /*
    if (motionRequest.walkRequest.walkParams.translation.y > maxSpeedY)
    motionRequest.walkRequest.walkParams.translation.y = maxSpeedY;
    */

    motionRequest.walkRequest.walkParams.translation.y =
      min (maxSpeedY, motionRequest.walkRequest.walkParams.translation.y);
    
    if ((fabs(toDegrees(angleDifference))<angleRemain)&&(distanceToDestination<distanceRemain))
    {
      motionRequest.walkRequest.walkParams.translation.x = 0;
      motionRequest.walkRequest.walkParams.translation.y = 0;
      motionRequest.walkRequest.walkParams.rotation = 0;
    }
    
    accelerationRestrictor.restrictAccelerations(150,150,100);
    
  } // if (estimatedTime != 0)
  else  //only to be sure the robot won't move if desired position is reached
  {
    motionRequest.walkRequest.walkParams.translation.x = 0;
    motionRequest.walkRequest.walkParams.translation.y = 0;
    motionRequest.walkRequest.walkParams.rotation = 0;
  }
}

void GT2005BasicBehaviorGoToRelativePoint::execute()
{
  if (!basicBehaviorWasActiveDuringLastExecutionOfEngine)
  {
    //save start positions for relative movement
    startX = robotPose.translation.x;
    startY = robotPose.translation.y;
    startRot = robotPose.rotation;
  }

  accelerationRestrictor.saveLastWalkParameters();
  
  Vector2<double> destination(x, y);

  Vector2<double> self = robotPose.translation;
  self.x -= startX;
  self.y -= startY;
  
  if (maxSpeed==0) maxSpeed = 450;
  if (maxSpeedY==0) maxSpeedY = 450;
  
  double maxTurnSpeed = fromDegrees(40);
  
  double distanceToDestination = Geometry::distanceTo(self, destination);
  
  double angleDifference = normalize(fromDegrees(destinationAngle) - (robotPose.rotation - startRot));
  
  double angleToDestination = Geometry::angleTo(Pose2D((robotPose.rotation - startRot), self.x, self.y), destination);
  
  
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
  
  //this time does not necessarily include time for turning!:
  double estimatedTimeToReachDestination;
  if (distanceToDestination > 200)
  {
    estimatedTimeToReachDestination = (distanceToDestination + 200) / maxSpeed;
    
    motionRequest.walkRequest.walkParams.translation.x = cos(angleToDestination) * maxSpeed;
    motionRequest.walkRequest.walkParams.translation.y = sin(angleToDestination) * maxSpeed;
  }
  else
  {
    estimatedTimeToReachDestination = 2 * distanceToDestination / maxSpeed + 2 * fabs(angleDifference) / maxTurnSpeed;
//     OUTPUT (idText, text, "Time2ReachDestination previous: " << 
//      2 * distanceToDestination / maxSpeed << "\nTime2ReachDestination now: " << estimatedTimeToReachDestination);
    
    if (distanceToDestination > 30)
    {
      motionRequest.walkRequest.walkParams.translation.x = cos(angleToDestination) * maxSpeed * distanceToDestination / 200;
      motionRequest.walkRequest.walkParams.translation.y = sin(angleToDestination) * maxSpeed * distanceToDestination / 200;
    }
    else
    {
      motionRequest.walkRequest.walkParams.translation.x = 0;
      motionRequest.walkRequest.walkParams.translation.y = 0;
    }
  }

  // If the estimated time is 0, position is already reached, so nothing has to be done anymore
  if (estimatedTimeToReachDestination != 0)
  { 
    if (fabs(toDegrees(angleDifference)) > 20)
    {
      motionRequest.walkRequest.walkParams.rotation = angleDifference / estimatedTimeToReachDestination;
      motionRequest.walkRequest.walkParams.rotation = min (maxTurnSpeed, motionRequest.walkRequest.walkParams.rotation);
      motionRequest.walkRequest.walkParams.rotation = max (-maxTurnSpeed, motionRequest.walkRequest.walkParams.rotation);
    }
    else
    {
      motionRequest.walkRequest.walkParams.rotation = 2 * angleDifference;
    }

    motionRequest.walkRequest.walkParams.translation.y = min(maxSpeedY, motionRequest.walkRequest.walkParams.translation.y);
    
    if ((fabs(toDegrees(angleDifference)) < angleRemain) && (distanceToDestination < distanceRemain))
    {
      motionRequest.walkRequest.walkParams.translation.x = 0;
      motionRequest.walkRequest.walkParams.translation.y = 0;
      motionRequest.walkRequest.walkParams.rotation = 0;
    }
  }
  else  //only to be sure the robot won't move if desired position is reached
  {
    motionRequest.walkRequest.walkParams.translation.x = 0;
    motionRequest.walkRequest.walkParams.translation.y = 0;
    motionRequest.walkRequest.walkParams.rotation = 0;
  }
}

void GT2005BasicBehaviorGoToPointFast::execute()
{
  //Gotopoint without accelerationRestrictor
  Vector2<double> destination(x,y);
  const Vector2<double>& self = robotPose.translation;
  
  if (maxSpeed==0) maxSpeed = 600;
  if (maxSpeedY==0) maxSpeedY = 600;
  
  double maxTurnSpeed = fromDegrees(40);
  double distanceToDestination = Geometry::distanceTo(self,destination);
  double angleDifference = normalize(fromDegrees(destinationAngle) - robotPose.rotation);
  double angleToDestination = Geometry::angleTo(robotPose,destination);
  
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType
    = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
  
  //this time does not necessarily include time for turning!:
  double estimatedTimeToReachDestination;
  if (distanceToDestination > 200)
  {
    estimatedTimeToReachDestination = (distanceToDestination+200) / maxSpeed;
    
    motionRequest.walkRequest.walkParams.translation.x = cos(angleToDestination) * maxSpeed;
    motionRequest.walkRequest.walkParams.translation.y = sin(angleToDestination) * maxSpeed;
  }
  else
  {
    estimatedTimeToReachDestination =   2*distanceToDestination
                                      / maxSpeed + 2*fabs(angleDifference) / maxTurnSpeed;
    
    if (distanceToDestination > 30)
    {
      motionRequest.walkRequest.walkParams.translation.x = 
        cos(angleToDestination) * maxSpeed * distanceToDestination/200;
      motionRequest.walkRequest.walkParams.translation.y = 
        sin(angleToDestination) * maxSpeed * distanceToDestination/200;
    }
    else
    {
      motionRequest.walkRequest.walkParams.translation.x = 0;
      motionRequest.walkRequest.walkParams.translation.y = 0;
    }
  }

  // If the estimated time is 0, position is already reached, so nothing has to be done anymore
  if (estimatedTimeToReachDestination != 0)
  {
    if (fabs(toDegrees(angleDifference)) > 20)
    {
      motionRequest.walkRequest.walkParams.rotation =
        angleDifference / estimatedTimeToReachDestination;
      motionRequest.walkRequest.walkParams.rotation =
        min (maxTurnSpeed, motionRequest.walkRequest.walkParams.rotation);
      motionRequest.walkRequest.walkParams.rotation =
        max (-maxTurnSpeed, motionRequest.walkRequest.walkParams.rotation);
    }
    else
    {
      motionRequest.walkRequest.walkParams.rotation = 2*angleDifference;
    }

    motionRequest.walkRequest.walkParams.translation.y =
      min (maxSpeedY, motionRequest.walkRequest.walkParams.translation.y);
    
    if ((fabs(toDegrees(angleDifference))<angleRemain)&&(distanceToDestination<distanceRemain))
    {
      motionRequest.walkRequest.walkParams.translation.x = 0;
      motionRequest.walkRequest.walkParams.translation.y = 0;
      motionRequest.walkRequest.walkParams.rotation = 0;
    }  
  } // if (estimatedTime != 0)
  else  //only to be sure the robot won't move if desired position is reached
  {
    motionRequest.walkRequest.walkParams.translation.x = 0;
    motionRequest.walkRequest.walkParams.translation.y = 0;
    motionRequest.walkRequest.walkParams.rotation = 0;
  }
}

void GT2005BasicBehaviorGoToInterceptionPoint::execute()
{
  if (maxSpeed == 0) maxSpeed = 600;
  if (maxSpeedY == 0) maxSpeedY = 600;

  double maxTurnSpeed;
  if (this->maxTurnSpeed == 0)
    maxTurnSpeed = fromDegrees(180);
  else
    maxTurnSpeed = fromDegrees(this->maxTurnSpeed);
  
  Vector2<double> destination(x, y);

  double angleToInterceptionPoint = Geometry::angleTo(Pose2D(0, 0, 0), destination);
  double distanceToInterceptionPoint = Geometry::distanceTo(Pose2D(0, 0, 0), destination);
  
  double factor;
  
  // adjust forward speed:
  // if a lot of turning is necessary, don't walk so fast!
  factor = (pi-fabs(angleToInterceptionPoint))/pi;
  if (factor > 1) factor = 1;
  if (factor < 0) factor = 0;

  destination.normalize();
  
  if (distanceToInterceptionPoint < slowDownDistance)
  {
    destination *= maxSpeed*((distanceToInterceptionPoint/2)/slowDownDistance);
  }
  else 
    destination *= (maxSpeed*factor);
  
  motionRequest.motionType = MotionRequest::walk;
  motionRequest.walkRequest.walkType   = static_cast<WalkRequest::WalkType>(static_cast<int>(walkType));
  motionRequest.walkRequest.walkParams.translation.x = destination.x;
  motionRequest.walkRequest.walkParams.translation.y = destination.y;
  
  if (motionRequest.walkRequest.walkParams.translation.y > maxSpeedY)
    motionRequest.walkRequest.walkParams.translation.y = maxSpeedY;
  
  if(angleFactor == 0)
    angleFactor = 1.0;

  factor = angleFactor;

  motionRequest.walkRequest.walkParams.rotation = angleToInterceptionPoint * factor; /* this should also be factored!!!! */
  
  // clip rotation speed
  motionRequest.walkRequest.walkParams.rotation = min(motionRequest.walkRequest.walkParams.rotation, maxTurnSpeed);
  motionRequest.walkRequest.walkParams.rotation = max(motionRequest.walkRequest.walkParams.rotation, -maxTurnSpeed);  
}

//void GT2005BasicBehaviorDogAsJoystick::execute()
//{
//  
///*  headControlMode.pidData.setValues(JointData::legFR1,0,0,0);
//pidData.setValues(JointData::legFR2,0,0,0);
//pidData.setValues(JointData::legFL1,0,0,0);
//pidData.setValues(JointData::legFL2,0,0,0);
//  */
//  double tmpr1 = toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::legFR1]));
//  double tmpr2 = toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::legFR2]));
//  //double tmpr3 = toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::legFR3]));
//  double tmpl1 = toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::legFL1]));
//  double tmpl2 = toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::legFL2]));
//  //double tmpl3 = toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::legFL3]));
//  
//  outgoingBehaviorTeamMessage.walkRequest.type = 0;
//  outgoingBehaviorTeamMessage.walkRequest.x = (tmpr1+tmpl1)*2.5;
//  outgoingBehaviorTeamMessage.walkRequest.y = (tmpr2-tmpl2)*2.5;
//  //  outgoingBehaviorTeamMessage.walkRequest.rotation = (tmpl1-tmpr1)/2  ;
//  outgoingBehaviorTeamMessage.walkRequest.rotation = (tmpl1-tmpr1)/20  ;
//}

void GT2005BasicBehaviorCalcWlanBearing::init()
{
  OUTPUT (idText, text, "wlanbearing:init" << endl);

  startTime = SystemCall::getCurrentSystemTime ();
  frames = 0;
  initExecuted = true;
  filterExecuted = false;

  for (int i = 0; i < WLANINFO; i++)
  {
    wlanInfo[i].link = 0.0;
    wlanInfo[i].measures = 0;
    wlanInfo[i].noise = 0.0;
    wlanInfo[i].signal = 0.0;
  }

  slamPercept.wlanBearing.inProgress = false;
  slamPercept.wlanBearing.finished = false;
  slamPercept.wlanBearing.direction = 0;

  rotationAngle = 0;
}

#define ROTATION_TIME 3750
//#define ROTATION_TIME 7900

#define TURNS 1

void GT2005BasicBehaviorCalcWlanBearing::execute()
{
  unsigned char _state = (unsigned char) state;

  headControlMode.headControlMode = headControlMode.lookStraightAhead;

  motionRequest.motionType = motionRequest.stand;

  motionRequest.walkRequest.walkParams.translation.x = 0;
  motionRequest.walkRequest.walkParams.translation.y = 0;

  Pose2D deltaOdometry = odometryData - lastOdometry;

  lastOdometry = odometryData;

  switch (_state)
  {
  case 0x00: 
    {
      //init

      if (!initExecuted)
        init ();

      motionRequest.walkRequest.walkParams.rotation = 0;

      return;
    };
  case 0x01:
    {
      rotationAngle += deltaOdometry.rotation;

      slamPercept.GPVal = rotationAngle;

//      OUTPUT (idText, text, "winkel : " << rotationAngle);

//      unsigned long timeStamp = SystemCall::getCurrentSystemTime ();

//      if ((timeStamp - startTime) < (ROTATION_TIME * TURNS))
      if (rotationAngle <= pi2)
      {
        slamPercept.wlanBearing.inProgress = true;

        motionRequest.walkRequest.walkParams.rotation = fromDegrees(45);

        motionRequest.motionType = motionRequest.walk;

    #ifdef APERIOS1_3_2
/*        unsigned long timeDiff = timeStamp - startTime;
        timeDiff %= ROTATION_TIME;

        double rotTime = (double)ROTATION_TIME;

        unsigned char angle = (unsigned char) (((double)timeDiff / rotTime) * (double)WLANINFO);
*/
        unsigned int angle = (unsigned int)((rotationAngle / pi2) * (double)WLANINFO);

//        OUTPUT (idText, text, "angle : " << angle);

        EtherDriverGetWLANStatisticsMsg myWLANStats;
        ERA201D1_GetWLANStatistics(&myWLANStats);

        wlanInfo[angle].measures++;
        wlanInfo[angle].link += myWLANStats.statistics.link;
        wlanInfo[angle].signal += myWLANStats.statistics.signal;
        wlanInfo[angle].noise += myWLANStats.statistics.noise;

        frames++;
    #endif
      }
      else
      {
        motionRequest.walkRequest.walkParams.rotation = 0;
        slamPercept.wlanBearing.finished = true;
        slamPercept.wlanBearing.inProgress = false;
      };

      break;
    };
  case 0x02:
    {
      if (!filterExecuted)
        Filter ();

      break;
    };
  };
}

void GT2005BasicBehaviorCalcWlanBearing::Filter ()
{
  OUTPUT (idText, text, "wlanbearing:filter" << endl);

  unsigned int i = 0;
/*
  FILE *file = fopen ("wlan.dat", "wt");

  for (i = 0; i < WLANINFO; i++)
  {
    fprintf (file, "raw data %u : %f\n", i, (float)(wlanInfo[i].signal / wlanInfo[i].measures));
  };

  fclose (file);
*/
  char fileName[256];

  sprintf (fileName, "/MS/wlan_%u.csv", numMeasure);

  OutTextFile stream (fileName);

  for (i = 0; i < WLANINFO; i++)
    if (wlanInfo[i].measures > 0)
      stream << "raw_data;" << i << ";" << (double)(wlanInfo[i].link / wlanInfo[i].measures) << ";" << (double)(wlanInfo[i].signal / wlanInfo[i].measures) << ";" << (double)(wlanInfo[i].noise / wlanInfo[i].measures) << ";" << "\n";  

//    OUTPUT (idText, text, "raw data " << i << " : " << wlanInfo[i].signal / wlanInfo[i].measures);

  double signalAverage = 0;
  
  unsigned int count = 0;

  //calculate signal average
  for (i = 0; i < WLANINFO; i++)
    if (wlanInfo[i].measures > 0)
    {
      signalAverage += wlanInfo[i].signal;

      count++;
    };

  signalAverage /= count;

  //calculate deltas
  for (i = 0; i < WLANINFO; i++)
    if (wlanInfo[i].measures > 0)
      wlanInfo[i].dSignal = wlanInfo[i].signal - signalAverage;

  double posSignalSum = 0;
  double negSignalSum = 0;

  for (i = 0; i < WLANINFO; i++)
    if (wlanInfo[i].measures > 0)
    {
      if (wlanInfo[i].dSignal >= 0)
        posSignalSum += wlanInfo[i].dSignal;
      else
        negSignalSum += wlanInfo[i].dSignal;
    };

  unsigned int position = WLANINFO / 2;

  double currentPosSignalSum = 0;

  while (currentPosSignalSum < posSignalSum)
  {
    if (wlanInfo[position % WLANINFO].measures > 0)
      if (wlanInfo[position % WLANINFO].dSignal >= 0)
        currentPosSignalSum += wlanInfo[position % WLANINFO].dSignal;

    position++;
  };

  unsigned int posPoint = position;

  slamPercept.wlanBearing.posCenter = position % WLANINFO;

  position = WLANINFO / 2;

  double currentNegSignalSum = 0;

  while (currentNegSignalSum > negSignalSum)
  {
    if (wlanInfo[position % WLANINFO].measures > 0)
      if (wlanInfo[position % WLANINFO].dSignal < 0)
        currentNegSignalSum += wlanInfo[position % WLANINFO].dSignal;

    position++;
  };

  unsigned int negPoint = position;

  slamPercept.wlanBearing.negCenter = position % WLANINFO;

  for (i = 0; i < 16; i++)
  {
    filteredInfo[i].measures = 0;
    filteredInfo[i].link = 0;
    filteredInfo[i].signal = 0;
    filteredInfo[i].noise = 0;
  };

  //filter
  for (i = 0; i < WLANINFO; i++)
  {
    //unsigned long timeDiff = wlanInfo[i].timeStamp - startTime;

    //int angle = (int) (((double)(timeDiff % (ROTATION_TIME / 4)) / (double)ROTATION_TIME / 4.0) * 16.0);

    unsigned int index = (unsigned int)(i / ((double) WLANINFO / (double)FILTEREDINFO));

    filteredInfo[index].link += wlanInfo[i].link;
    filteredInfo[index].noise += wlanInfo[i].noise;
    filteredInfo[index].signal += wlanInfo[i].signal;
    filteredInfo[index].measures += wlanInfo[i].measures;
  }

  for (i = 0; i < FILTEREDINFO; i++)
  {
    if (filteredInfo[i].measures > 0)
    {
      filteredInfo[i].link /= (double) filteredInfo[i].measures;
      filteredInfo[i].noise /= (double) filteredInfo[i].measures;
      filteredInfo[i].signal /= (double) filteredInfo[i].measures;
    };
  }

  sprintf (fileName, "/MS/wlanf_%u.csv", numMeasure);

  numMeasure++;

  OutTextFile filteredStream (fileName);

  for (i = 0; i < FILTEREDINFO; i++)
    if (filteredInfo[i].measures > 0)
      filteredStream  << "filtered_data;" << i << ";" << (double)(filteredInfo[i].link) << ";" << (double)(filteredInfo[i].signal) << ";" << (double)(filteredInfo[i].noise) << ";" << "\n";  

  int maxAngle = FILTEREDINFO + 1;
  double maxVal = 0.0;

  for (i = 0; i < FILTEREDINFO; i++)
  {
    if ((filteredInfo[i].measures > 0) && (filteredInfo[i].signal > maxVal))
    {
      maxVal = filteredInfo[i].signal;
      maxAngle = i;
    }
  };

  int minAngle = FILTEREDINFO + 1;
  double minVal = 200.0;

  for (i = 0; i < FILTEREDINFO; i++)
  {
    if ((filteredInfo[i].measures > 0) && (filteredInfo[i].signal < minVal))
    {
      minVal = filteredInfo[i].signal;
      minAngle = i;
    }
  };

  if (maxAngle < FILTEREDINFO/2)
  {
    std::cout << "facing yellow : " << maxAngle << ";" << minAngle << ";" << posPoint << ";" << negPoint << std::endl << std::flush;

    ledRequest.backRearRedLED = LEDRequest::llll;
    ledRequest.backFrontBlueLED = LEDRequest::oooo;
  }
  else
  {
    std::cout << "facing blue : " << maxAngle << ";" << minAngle << ";" << posPoint << ";" << negPoint << std::endl << std::flush;
  
    ledRequest.backFrontBlueLED = LEDRequest::llll;
    ledRequest.backRearRedLED = LEDRequest::oooo;
  }

  double bearing = (((double)maxAngle) / (double) FILTEREDINFO) * 360.0;

  bearing -= 180.0;

  slamPercept.wlanBearing.direction = bearing;

  OUTPUT (idText, text, "ap in " << bearing << " degrees" << endl);

  initExecuted = false;
  filterExecuted = true;
};

void GT2005BasicBehaviorLocateMaxGreen::Init ()
{
    slamPercept.greenLocation.greenLocationFinished = false;
    slamPercept.greenLocation.angleToMaxGreenValid = false;
    slamPercept.greenLocation.lengthOfMaxGreen = 0;
    slamPercept.greenLocation.angleToMaxGreen = 0;

    motionRequest.walkRequest.walkParams.rotation = 0;
    motionRequest.walkRequest.walkParams.translation.x = 0;
    motionRequest.walkRequest.walkParams.translation.y = 0;

    lastOdometry = odometryData;

    rotationAngle = 0;
};

void GT2005BasicBehaviorLocateMaxGreen::execute ()
{
  Pose2D deltaOdometry = odometryData - lastOdometry;

  lastOdometry = odometryData;

  headControlMode.headControlMode = headControlMode.lookStraightAhead;

  switch ((int)param)
  {
  case 0x00:
    {
      Init ();

      break;
    };
  case 0x01:
    {
      if (!slamPercept.greenLocation.greenLocationFinished)
      {
          rotationAngle += deltaOdometry.rotation;

          if (rotationAngle <= 2.0 * pi)
          {
            motionRequest.walkRequest.walkParams.rotation = fromDegrees(90);

            motionRequest.motionType = motionRequest.walk;

            if (slamPercept.greenLocation.impAngleToMaxGreenValid)
            {
              if (slamPercept.greenLocation.lengthOfMaxGreen < slamPercept.greenLocation.impLengthOfMaxGreen)
              {
                slamPercept.greenLocation.angleToMaxGreen = rotationAngle + slamPercept.greenLocation.impAngleToMaxGreen;

                while (slamPercept.greenLocation.angleToMaxGreen > pi)
                  slamPercept.greenLocation.angleToMaxGreen -= pi2;

                while (slamPercept.greenLocation.angleToMaxGreen < -pi)
                  slamPercept.greenLocation.angleToMaxGreen += pi2;

                slamPercept.greenLocation.angleToMaxGreen = (slamPercept.greenLocation.angleToMaxGreen / pi) * 180.0;

                slamPercept.greenLocation.lengthOfMaxGreen = slamPercept.greenLocation.impLengthOfMaxGreen;
              };
            }
          }
          else
          {
            motionRequest.walkRequest.walkParams.rotation = 0;
        
            slamPercept.greenLocation.greenLocationFinished = true;
            slamPercept.greenLocation.angleToMaxGreenValid = true;

            rotationAngle = 0;
          };
      }
    };
  };

  return;
};

void GT2005BasicBehaviorTurn::Init ()
{
    lastOdometry = odometryData;

    motionRequest.walkRequest.walkParams.rotation = 0;
    motionRequest.walkRequest.walkParams.translation.x = 0;
    motionRequest.walkRequest.walkParams.translation.y = 0;

    slamPercept.turnFinished = false;
    rotationAngle = 0;

    if (angle == 0.0)
      targetRotation = radian;
    else
      targetRotation = (angle / 180.0) * pi;
};

void GT2005BasicBehaviorTurn::execute ()
{
  Pose2D deltaOdometry = odometryData - lastOdometry;

  lastOdometry = odometryData;

  switch ((int)param)
  {
  case 0x00:
    {
      Init ();

      break;
    };
  case 0x01:
    {
      rotationAngle += deltaOdometry.rotation;

      if (fabs (targetRotation - rotationAngle) > pi / 18.0)
      {
        if (targetRotation > rotationAngle)
          motionRequest.walkRequest.walkParams.rotation = fromDegrees(90);
        else
          motionRequest.walkRequest.walkParams.rotation = -fromDegrees(90);

        motionRequest.motionType = motionRequest.walk;
      }
      else
      {
        motionRequest.walkRequest.walkParams.rotation = 0;
    
        rotationAngle = 0;

        slamPercept.turnFinished = true;
      };

      break;
    };
  };
  return;
};

---