Modules/BehaviorControl/GT2004BehaviorControl/GT2004BasicBehaviors/GT2004SimpleBasicBehaviors.cpp

Go to the documentation of this file.

/** 
* @file GT2004SimpleBasicBehaviors.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
*/

#include "Tools/Math/PIDsmoothedValue.h"
#include "GT2004SimpleBasicBehaviors.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Math/Matrix2x2.h"
#include "Tools/Math/Common.h"

#include "Tools/Debugging/DebugDrawings.h"
#include "Tools/Debugging/Debugging.h"

//needed for dribbleInCenterOfField and getBehindBall2
#include "Tools/FieldDimensions.h"

void GT2004BasicBehaviorGoToBall::execute()
{
  //accelerationRestrictor.saveLastWalkParameters();
  
  double targetAngle;
  
  if (maxSpeed == 0) maxSpeed = 350;
  if (maxSpeedY == 0) maxSpeed = 350;
  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;
  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;
  
  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 GT2004BasicBehaviorGoToBallWithoutTurning::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 GT2004BasicBehaviorGoaliePosition::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 GT2004BasicBehaviorGoaliePositionReturn::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 GT2004BasicBehaviorTurnAroundPoint::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 GT2004BasicBehaviorGoForwardToPoint::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 GT2004BasicBehaviorGoToPointAndAvoidObstacles::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 = 350;
    
  // 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 = 250;
  
  distanceBelowWhichObstaclesAreAvoided = 500;
  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);
  targetAngle = (leftForward - rightForward) / (leftForward + rightForward);
      
  // determine the next free sector in the desired direction
  double nextFree = obstaclesModel.getAngleOfNextFreeSector(pi/4, angleToDestination, (int )obstacleAvoidanceDistance);
  
  // check if there is enough space in the direction of "nextFree"
  if (obstaclesModel.getDistanceInCorridor(nextFree, widthOfCorridor) > obstacleAvoidanceDistance)
  {
    if (fabs(nextFree) < .8)
      targetAngle = nextFree;
  } 
  
  // double check: if there's enough space in the direction of the destination, directly head there 
  if (obstaclesModel.getDistanceInCorridor(angleToDestination, widthOfCorridor) > obstacleAvoidanceDistance)
    targetAngle = angleToDestination;
    
  // 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.1, 1).limit( 
      (pi-fabs(targetAngle))/pi );

  // 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 GT2004BasicBehaviorGoToPoint::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 GT2004BasicBehaviorMoveTheBridge::execute()
{
  outgoingBehaviorTeamMessage.walkRequest.type = type;
  outgoingBehaviorTeamMessage.walkRequest.x = x;
  if (outgoingBehaviorTeamMessage.walkRequest.x > 50)
    outgoingBehaviorTeamMessage.walkRequest.x = 50;
  if (outgoingBehaviorTeamMessage.walkRequest.x < -50)
    outgoingBehaviorTeamMessage.walkRequest.x = -50;
  
  outgoingBehaviorTeamMessage.walkRequest.y = y;
  if (outgoingBehaviorTeamMessage.walkRequest.y > 50)
    outgoingBehaviorTeamMessage.walkRequest.y = 50;
  if (outgoingBehaviorTeamMessage.walkRequest.y < -50)
    outgoingBehaviorTeamMessage.walkRequest.y = -50;
  
  outgoingBehaviorTeamMessage.walkRequest.rotation = rotation;
  if (outgoingBehaviorTeamMessage.walkRequest.rotation > 2)
    outgoingBehaviorTeamMessage.walkRequest.rotation = 2;
  if (outgoingBehaviorTeamMessage.walkRequest.rotation < -2)
    outgoingBehaviorTeamMessage.walkRequest.rotation = -2;
}

void GT2004BasicBehaviorDogAsJoystick::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  ;
}

/*
* Change Log
* 
* $Log: GT2004SimpleBasicBehaviors.cpp,v $
* Revision 1.32  2004/07/20 21:56:45  spranger
* reintroduced martin loetzschs goalie-position for the return states in playing-goalie
*
* Revision 1.31  2004/07/20 14:16:28  spranger
* removed intercept-at-x and intercept-test-batchman
*
* Revision 1.30  2004/07/16 15:30:35  goetzke
* converted "goalie-position-benjamin" in "goalie-position"
*
* Revision 1.29  2004/07/01 09:08:46  jhoffman
* - parameter tweaking in turn around point
*
* Revision 1.28  2004/06/30 16:16:15  jhoffman
* - state removed from "get-to-position-and-avoid-obstacles" because basic behavior now does the turning
* - basic behavior "go-to-point-and-avoide-obstacles" improved (faster, stops when destination is reached)
*
* Revision 1.27  2004/06/30 10:42:28  spranger
* renamed goalie-clear-ball to tun-without-ball
*
* Revision 1.26  2004/06/29 18:13:08  altmeyer
* adjust parameters in GoaliePosititonBenjamin
*
* Revision 1.25  2004/06/29 17:51:41  schmitt
* goToPoint
* - rotation time is now considered in estimatedTime
* - simplified some calculations
*
* Revision 1.24  2004/06/29 10:10:53  altmeyer
* adjust paramaters goalie-benjamin
*
* Revision 1.23  2004/06/28 19:51:29  altmeyer
* adjust goalie-position-benjamin parameters
*
* Revision 1.22  2004/06/27 17:00:07  altmeyer
* fixed warning
*
* Revision 1.21  2004/06/27 16:36:01  altmeyer
* improved GoaliePositionBenjamin
*
* Revision 1.20  2004/06/27 14:59:23  goehring
* goToPoint has parameter distanceRemain and angleRemain
*
* Revision 1.19  2004/06/27 14:53:51  altmeyer
* new GoaliePositionBenjamin and new debug key send_goaliePositionField
*
* Revision 1.18  2004/06/23 20:59:04  juengel
* Added basicBehaviorGoalieClearBall.
*
* Revision 1.17  2004/06/23 16:19:27  loetzsch
* reactivated and improved old ath goalie
*
* Revision 1.16  2004/06/18 10:56:33  altmeyer
* improved goalie-position
*
* Revision 1.15  2004/06/18 00:32:18  risler
* added walk-type to go-to-point
* added max-speed.y to go-to-ball and go-to-point
*
* Revision 1.14  2004/06/17 15:54:04  altmeyer
* fixed warning
*
* Revision 1.13  2004/06/17 15:30:04  altmeyer
* new GoaliePosition (Parameters, Motion-Glätting ;-)
*
* Revision 1.12  2004/06/16 17:35:04  altmeyer
* added GoaliePosition (author: Jan Hoffmann)
*
* Revision 1.11  2004/06/15 16:15:40  risler
* added go-to-ball parameters max-turn-speed and walk-type
*
* Revision 1.10  2004/06/02 17:18:23  spranger
* MotionRequest cleanup
*
* Revision 1.9  2004/05/27 18:49:17  kerdels
* added a small 5 frames sliding average for the relative ballspeed,
* added new ballState Representation and adjusted the apropriate files
*
* Revision 1.8  2004/05/26 19:19:29  spranger
* added yOffset to go-to-ball basicbehavior
*
* Revision 1.7  2004/05/25 14:19:19  kerdels
* adjusted intercept-at-x
*
* Revision 1.6  2004/05/25 09:58:20  huelsbusch
* intercept added
*
* Revision 1.5  2004/05/25 09:35:50  kerdels
* intercept at x ...
*
* Revision 1.4  2004/05/24 16:58:25  kerdels
* intercept at x added
*
* Revision 1.3  2004/05/24 16:31:24  loetzsch
* added basic behavior intercept-batch-man
*
* Revision 1.2  2004/05/22 20:44:16  juengel
* Renamed ballP_osition to ballModel.
*
* Revision 1.1.1.1  2004/05/22 17:18:01  cvsadm
* created new repository GT2004_WM
*
* Revision 1.5  2004/05/14 22:53:07  roefer
* Warnings removed
*
* Revision 1.4  2004/05/13 22:11:38  hamerla
* OpenChallenge use a dog as joystick for the bridgemove
*
* Revision 1.3  2004/05/08 16:18:13  hamerla
* Open Challenge
*
* Revision 1.2  2004/05/04 10:48:58  loetzsch
* replaced all enums
* xxxBehaviorControlTimeAfterWhichCommunicatedBallsAreAccepted
* by
* behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted
* (this mechanism was neither fully implemented nor used)
*
* Revision 1.1  2004/05/02 13:24:39  juengel
* Added GT2004BehaviorControl.
*
*/

---