Modules/BehaviorControl/GT2005BehaviorControl/GT2005BehaviorControl.cpp

Go to the documentation of this file.

/**
* @file GT2005BehaviorControl.cpp
* 
* Implementation of class GT2005BehaviorControl.
*
* @author Martin Lötzsch
* @author Matthias Jüngel
*/

#include "GT2005BehaviorControl.h"
#include "Tools/Player.h"

#include "Representations/Perception/SlamPercept.h"

GT2005BehaviorControl::GT2005BehaviorControl(BehaviorControlInterfaces& interfaces)
: Xabsl2BehaviorControl(interfaces,SolutionRequest::gt2005),
  calibrationSymbols(interfaces),
  angleSymbols(interfaces),
  ballSymbols(interfaces),
  configurationSymbols(interfaces),
  headAndTailSymbols(interfaces),
  joystickSymbols(interfaces),
  kickSelectionSymbols(interfaces,"GT2005.kst"),
  ledAndSoundSymbols(interfaces),
  gtCamSymbols(interfaces),
  mathFunctions(interfaces),
  motionRequestSymbols(interfaces),
  obstaclesSymbols(interfaces),
  roboCupGameManagerSymbols(interfaces),
  robotPoseSymbols(interfaces),
  robotStateSymbols(interfaces),
  specialVisionSymbols(interfaces),
  strategySymbols(interfaces),
  openchallengeSymbols(interfaces),
  challengeSymbols(interfaces),
  fieldSymbols(interfaces),
  kickLoggerSymbols(interfaces),
  testSymbols(interfaces),
  simpleBasicBehaviors(interfaces,errorHandler),
  potentialFieldBasicBehaviors(interfaces,errorHandler),
  commonBasicBehaviors(interfaces,errorHandler),
  kickLogger(interfaces),
  passSymbols(interfaces),
  playersSymbols(interfaces),
  gt2005WalkCalibrationBasicBehaviors(interfaces, errorHandler)
{
  Xabsl2FileInputSource file("Xabsl2/gt05-ic.dat");
  init(file);
  ASSERT(pEngine);

  quickStop = false;
  timeSync = false;
  syncTimestampSent = true;
  initialBrightness = 0;
  faceInfo = 0;
  backFrontWasPressed = false;
  timeSyncHeadControlMode = 0;
  backBackWasPressed = false;
  IP = 0;

  #ifndef _WIN32
  InConfigFile wlanconf("../../SYSTEM/CONF/wlanconf.txt");
  ASSERT(wlanconf.exists());
  char buf[50];
  wlanconf >> buf >> buf;
  int dots = 0;
  int i = 0;
  while (dots < 3)
  {
    if (buf[i]=='.') dots++;
    i++;
  }
  IP = atoi(&buf[i]);
  #endif

}

GT2005BehaviorControl::~GT2005BehaviorControl()
{
}

void GT2005BehaviorControl::registerSymbolsAndBasicBehaviors()
{
  simpleBasicBehaviors.registerBasicBehaviors(*pEngine);
  potentialFieldBasicBehaviors.registerBasicBehaviors(*pEngine);
  commonBasicBehaviors.registerBasicBehaviors(*pEngine);
  calibrationSymbols.registerSymbols(*pEngine);
  angleSymbols.registerSymbols(*pEngine);
  ballSymbols.registerSymbols(*pEngine);
  gtCamSymbols.registerSymbols(*pEngine);
  configurationSymbols.registerSymbols(*pEngine);
  headAndTailSymbols.registerSymbols(*pEngine);
  joystickSymbols.registerSymbols(*pEngine);
  ledAndSoundSymbols.registerSymbols(*pEngine);
  kickSelectionSymbols.registerSymbols(*pEngine);
  mathFunctions.registerSymbols(*pEngine);
  motionRequestSymbols.registerSymbols(*pEngine);
  obstaclesSymbols.registerSymbols(*pEngine);
  roboCupGameManagerSymbols.registerSymbols(*pEngine);
  robotPoseSymbols.registerSymbols(*pEngine);
  robotStateSymbols.registerSymbols(*pEngine);
  specialVisionSymbols.registerSymbols(*pEngine);
  strategySymbols.registerSymbols(*pEngine);
  openchallengeSymbols.registerSymbols(*pEngine);
  challengeSymbols.registerSymbols(*pEngine);
  fieldSymbols.registerSymbols(*pEngine);
  gt2005WalkCalibrationBasicBehaviors.registerBasicBehaviors(*pEngine);
  kickLoggerSymbols.registerSymbols(*pEngine);
  playersSymbols.registerSymbols(*pEngine);
  passSymbols.registerSymbols(*pEngine);
  testSymbols.registerSymbols(*pEngine);
}

void GT2005BehaviorControl::preExecute()
{
  for(int i = 0; i < 14; i++) ledRequest.faceLED[i] = LEDRequest::oooo;
  ledRequest.backFrontWhiteLED = LEDRequest::oooo;
  ledRequest.backMiddleWhiteLED = LEDRequest::oooo;
  ledRequest.backRearWhiteLED = LEDRequest::oooo;
  ledRequest.backMiddleOrangeLED = LEDRequest::oooo;

  ledRequest.headWhiteLED = LEDRequest::oooo;
  ledRequest.headOrangeLED = LEDRequest::oooo;

  // set the outgoing behavior team message to none
  outgoingBehaviorTeamMessage.message = BehaviorTeamMessage::none;
  
  // Set the tail request depending on whether the ball was seen
  if (ballModel.ballWasSeen)
  {
    // ball seen
    ledRequest.headWhiteLED = LEDRequest::llll;
    motionRequest.tailRequest.tailRequestID = TailRequest::noTailWag;
  }
  else if (SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen) 
    > BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted
    && SystemCall::getTimeSince(ballModel.communicated.timeWhenLastObserved) < 3000)
  {
    // ball known
    motionRequest.tailRequest.tailRequestID = TailRequest::wagHorizontalFast;
  }
  else
  {
    // ball not known
    motionRequest.tailRequest.tailRequestID = TailRequest::noTailWag;
  }
  
  // set head control mode lookToStars to detect missing settings
  headControlMode.headControlMode = HeadControlMode::lookToStars;

  soundRequest.soundID = SoundRequest::none;

  angleSymbols.angleShownByLEDs = AngleSymbols::undefined;
}

void GT2005BehaviorControl::execute()
{
  angleSymbols.update();
  ballSymbols.update();
  configurationSymbols.update();
  obstaclesSymbols.update();
  roboCupGameManagerSymbols.update();
  robotPoseSymbols.update();
  strategySymbols.update();
  openchallengeSymbols.update();
  testSymbols.update();
  potentialFieldBasicBehaviors.update();
  
  kickLoggerSymbols.update();
 
  // set some leds and and other variables
  preExecute();

  // execute the engine
  executeEngine();

  // play some sounds if necessary
  postExecute();

  drawOptions();
  declareDrawingsForBasicBehaviors();

  // execute the kick logger, but not during a game
  kickLogger.execute();
}

void GT2005BehaviorControl::postExecute()
{
  // handle fall down
  if(outgoingBehaviorTeamMessage.gameState != BehaviorTeamMessage::penalized &&
     ((robotState.getState() == RobotState::crashed)||
     (robotState.getState() == RobotState::rollLeft)||
     (robotState.getState() == RobotState::rollRight)))
  {
    if ((motionInfo.executedMotionRequest.motionType == MotionRequest::walk &&
         motionInfo.executedMotionRequest.walkRequest.walkType != WalkRequest::upsideDown)||
        (motionInfo.executedMotionRequest.motionType == MotionRequest::stand))
    {
      motionRequest.motionType = MotionRequest::getup;
    }
  }

  // display team color
  if (getPlayer().getTeamColor() == Player::blue)
  {
    ledRequest.backFrontBlueLED = LEDRequest::llll;
    if(
      outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::sleep ||
      outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::initial ||
      outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::ready ||
      outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::set)
    {
      ledRequest.backFrontWhiteLED = LEDRequest::oooo;
    }
    ledRequest.backRearRedLED = LEDRequest::oooo;
  } 
  else if (getPlayer().getTeamColor() == Player::red) 
  {
    ledRequest.backFrontBlueLED = LEDRequest::oooo;
    ledRequest.backRearRedLED = LEDRequest::llll;
    if(
      outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::sleep ||
      outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::initial ||
      outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::ready ||
      outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::set)
    {
      ledRequest.backRearWhiteLED = LEDRequest::oooo;
    }
  } 
  else 
  {
    ledRequest.backFrontBlueLED = LEDRequest::lloo;
    ledRequest.backRearRedLED = LEDRequest::lloo;
  }

  if(
    outgoingBehaviorTeamMessage.gameState != BehaviorTeamMessage::sleep &&
    outgoingBehaviorTeamMessage.gameState != BehaviorTeamMessage::initial)
  {
    // set the purple face leds depending on the dynamic role.
    switch (strategySymbols.role)
    {
    case BehaviorTeamMessage::striker:
      ledRequest.faceLED[13] = LEDRequest::llll;
      break;
    case BehaviorTeamMessage::offensiveSupporter:
      ledRequest.faceLED[13] = LEDRequest::oooo;
      ledRequest.faceLED[12] = LEDRequest::lloo;
      break;
    case BehaviorTeamMessage::defensiveSupporter:
      ledRequest.faceLED[13] = LEDRequest::oooo;
      break;
    default:
      ledRequest.faceLED[13] = LEDRequest::oooo;
    }

    if(getPlayer().getPlayerNumber() == Player::one)
    {
      if (ballModel.seen.ballRollsByLeft)
        ledRequest.faceLED[1] = LEDRequest::llll; 
      if (ballModel.seen.ballRollsByRight)
        ledRequest.faceLED[0] = LEDRequest::llll; 
      if (ballModel.seen.ballRollsFast)
      {
        ledRequest.faceLED[2] = LEDRequest::llll; 
        ledRequest.faceLED[3] = LEDRequest::llll; 
      }
      if (ballModel.seen.ballRollsTowardsRobot)
      {
        ledRequest.faceLED[1] = LEDRequest::llll; 
        ledRequest.faceLED[0] = LEDRequest::llll; 
      }
    }

    if(angleSymbols.angleShownByLEDs == AngleSymbols::undefined)
    {
      ledRequest.faceLED[10] = LEDRequest::lolo;
    }
    else
    {
      // visualize the angle the robot wants to play to with the top white leds.
      double bestAngle;
      bestAngle = toDegrees(normalize(angleSymbols.getAngle(angleSymbols.angleShownByLEDs)));

      if     (bestAngle >  75) ledRequest.faceLED[ 7] = LEDRequest::lolo; //?_____   
      else if(bestAngle >  45) ledRequest.faceLED[ 7] = LEDRequest::llll; // x____
      else if(bestAngle >  15) ledRequest.faceLED[ 9] = LEDRequest::llll; // _x___      
      else if(bestAngle > -15) ledRequest.faceLED[10] = LEDRequest::llll; // __x__
      else if(bestAngle > -45) ledRequest.faceLED[ 8] = LEDRequest::llll; // ___x_
      else if(bestAngle > -75) ledRequest.faceLED[ 6] = LEDRequest::llll; // ____x
      else                     ledRequest.faceLED[ 6] = LEDRequest::lolo; // _____?
    }

    /*
    // display stuck/obstacles
    if (obstaclesSymbols.robotIsStuck)
    {
      ledRequest.faceLED[4] = LEDRequest::llll;
      ledRequest.faceLED[5] = LEDRequest::llll;
    }
    else if (obstaclesSymbols.obstaclesAreClose)
    {
      ledRequest.faceLED[11] = LEDRequest::llll;
    }
    */

    // display angle used for perfect angle
    /*
    if (angleSymbols.angleUsedForPerfectAngle == AngleSymbols::angleToFreePartOfOpponentGoal)
    {
      ledRequest.faceLED[4] = LEDRequest::llll;
      ledRequest.faceLED[5] = LEDRequest::llll;
    }
    else */
    if (angleSymbols.angleUsedForPerfectAngle == AngleSymbols::seenAngleToOpponentGoal)
    {
      ledRequest.faceLED[4] = LEDRequest::llll;
      ledRequest.faceLED[5] = LEDRequest::llll;
    }
    else
    {
      ledRequest.faceLED[4] = LEDRequest::oooo;
      ledRequest.faceLED[5] = LEDRequest::oooo;
    }
  }

  // if the head control mode was not set, play a warning sound
  if (headControlMode.headControlMode == HeadControlMode::lookToStars)
  {
    soundRequest.soundID = SoundRequest::rob101;
  }

  if(sensorDataBuffer.frame[0].data[SensorData::chin] == 1)
    soundRequest.soundID = SoundRequest::bark2;


  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;
      soundRequest.soundID = SoundRequest::bing01;
    }

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

  DEBUG_RESPONSE("automated requests:enable quickstop", quickStop = true;);
  DEBUG_RESPONSE("automated requests:disable quickstop", quickStop = false;);
  if(!quickStop)
  {
    if(
      robotState.getButtonPressed(BodyPercept::chin) &&
      robotState.getButtonDuration(BodyPercept::chin) > 500 &&
      
      robotState.getButtonPressed(BodyPercept::backMiddle) &&
      robotState.getButtonDuration(BodyPercept::backMiddle) > 500
      )
    {
      quickStop = true;
      timeSync = false;
    }
  }
  else
  {
    if(
      robotState.getButtonPressed(BodyPercept::head) &&
      robotState.getButtonDuration(BodyPercept::head) > 100 &&
      robotState.getButtonDuration(BodyPercept::head) < 700
      )
      quickStop = false;
  }

  if(!timeSync)
  {
    if(
      robotState.getButtonPressed(BodyPercept::chin) &&
      robotState.getButtonDuration(BodyPercept::chin) > 500 &&
      
      robotState.getButtonPressed(BodyPercept::backFront) &&
      robotState.getButtonDuration(BodyPercept::backFront) > 500
      )
    {
    // entering time-sync mode
      timeSync = true;
      syncTimestampSent = true;
      backFrontWasPressed = true;
      faceInfo = 0;
      backBackWasPressed = false;
      timeSyncHeadControlMode = 0;
      quickStop = false;
    }
  }
  else
  {
    if(
      robotState.getButtonPressed(BodyPercept::head) &&
      robotState.getButtonDuration(BodyPercept::head) > 100 &&
      robotState.getButtonDuration(BodyPercept::head) < 700
      )
    // leaving timesync-mode
      timeSync = false;
  }

  if(quickStop)
  {
    if(robotState.getButtonPressed(BodyPercept::backFront)) debugHeadControlMode.headControlMode = HeadControlMode::none;
    if(robotState.getButtonPressed(BodyPercept::backMiddle)) debugHeadControlMode.headControlMode = HeadControlMode::searchForBall;
    if(robotState.getButtonPressed(BodyPercept::backBack)) debugHeadControlMode.headControlMode = HeadControlMode::stayAsForced;

    if(debugHeadControlMode.headControlMode != HeadControlMode::none)
    {
      headControlMode = debugHeadControlMode;
    }

    motionRequest.motionType = MotionRequest::stand;
    ledRequest.backFrontWhiteLED = LEDRequest::oooo;
    ledRequest.backMiddleWhiteLED = LEDRequest::oooo;
    ledRequest.backRearWhiteLED = LEDRequest::oooo;

    ledRequest.backFrontBlueLED = LEDRequest::llll;
    ledRequest.backMiddleOrangeLED = LEDRequest::llll;
    ledRequest.backRearRedLED = LEDRequest::llll;

    if(debugHeadControlMode.headControlMode == HeadControlMode::none) ledRequest.backFrontWhiteLED = LEDRequest::llll;
    if(debugHeadControlMode.headControlMode == HeadControlMode::searchForBall) ledRequest.backMiddleWhiteLED = LEDRequest::llll;
    if(debugHeadControlMode.headControlMode == HeadControlMode::stayAsForced) ledRequest.backRearWhiteLED = LEDRequest::llll;
  }

  if(timeSync)
  {
    // clear all back LEDs
    ledRequest.backMiddleWhiteLED=LEDRequest::oooo;
    ledRequest.backFrontWhiteLED=LEDRequest::oooo;
    ledRequest.backRearWhiteLED=LEDRequest::oooo;
    ledRequest.backFrontBlueLED=LEDRequest::oooo;
    ledRequest.backMiddleOrangeLED=LEDRequest::oooo;
    ledRequest.backRearRedLED=LEDRequest::oooo;    
    
  // don't move
    motionRequest.motionType = MotionRequest::stand;
    
  // switch through headcontrol modes with back back button
  /////////////////////////////////////////////////////////
    if (robotState.getButtonPressed(BodyPercept::backBack))
    {
      if (!backBackWasPressed)
      {
        timeSyncHeadControlMode++;
        backBackWasPressed = true;
      }
    }
    else
    {
      backBackWasPressed = false;
    }

    switch (timeSyncHeadControlMode)
    {
    case 0:
      ledRequest.backRearWhiteLED=LEDRequest::oooo;
      headControlMode.headControlMode = HeadControlMode::lookStraightAhead;
      break;
    case 1:
      ledRequest.backRearWhiteLED=LEDRequest::looo;
      headControlMode.headControlMode = HeadControlMode::searchForBall;
      break;
    case 2:
      ledRequest.backRearWhiteLED=LEDRequest::lolo;
      headControlMode.headControlMode = HeadControlMode::searchAuto;
      break;
    case 3:
      ledRequest.backRearWhiteLED=LEDRequest::lllo;
      headControlMode.headControlMode = HeadControlMode::stayAsForced;
      break;
    case 4:
      ledRequest.backRearWhiteLED=LEDRequest::llll;
      break;
    default:
      timeSyncHeadControlMode = 0;
      headControlMode.headControlMode = HeadControlMode::lookStraightAhead;
      break;
    }
 
  // do time-sync
  ///////////////
    if (specialVisionRequest.specialVisionRequest != SpecialVisionRequest::imageBrightness)
    {
      specialVisionRequest.specialVisionRequest = SpecialVisionRequest::imageBrightness;
    }
    else
    {
      unsigned char estimatedBrightness=specialPercept.estimatedImageBrightness;
      
      ledRequest.backMiddleOrangeLED=LEDRequest::lolo;
      
    // reset initial brightness on back middle button
      if (robotState.getButtonPressed(BodyPercept::backMiddle)) {
        initialBrightness=estimatedBrightness;
        syncTimestampSent = false;
        ledRequest.backMiddleOrangeLED=LEDRequest::llll;
      }
      
      double degreeOfBrightness=(double)(estimatedBrightness-initialBrightness)/(220-initialBrightness);
            
      if (degreeOfBrightness<-0.2) ledRequest.backFrontBlueLED=LEDRequest::looo;
      if (degreeOfBrightness<-0.3) ledRequest.backFrontBlueLED=LEDRequest::lolo;
      if (degreeOfBrightness<-0.6) ledRequest.backFrontBlueLED=LEDRequest::loll;
      if (degreeOfBrightness<-0.8) ledRequest.backFrontBlueLED=LEDRequest::llll;
      
      if (degreeOfBrightness>0.2) ledRequest.backRearRedLED=LEDRequest::looo;
      if (degreeOfBrightness>0.3) 
      {
        ledRequest.backRearRedLED=LEDRequest::lolo;
    // send sync timestamp
        if (!syncTimestampSent) OUTPUT(idSyncTimestampRobot, bin, SystemCall::getCurrentSystemTime());
        syncTimestampSent = true;
      }
      if (degreeOfBrightness>0.6) ledRequest.backRearRedLED=LEDRequest::loll;
      if (degreeOfBrightness>0.8) ledRequest.backRearRedLED=LEDRequest::llll;
      
      if (syncTimestampSent) ledRequest.backMiddleOrangeLED=LEDRequest::llll;    
    }

    //display different infos on face-LEDs depending on back front button
  //////////////////////////////////////
    if (robotState.getButtonPressed(BodyPercept::backFront))
    {
      if (!backFrontWasPressed)
      {
        faceInfo++;
        backFrontWasPressed = true;
      }
    }
    else
    {
      backFrontWasPressed = false;
    }

    unsigned char power;

    switch (faceInfo)
    {
    case 0:
    // display remaining power
      ledRequest.backFrontWhiteLED=LEDRequest::oooo;
      power = SystemCall::getRemainingPower();
      break;
    case 1:
    // display seen ball distance
      ledRequest.backFrontWhiteLED=LEDRequest::looo;
      if (ballModel.ballWasSeen)
      {
        power = (unsigned char) min(Geometry::fieldCoord2Relative(robotPose, ballModel.seen.positionField).abs()/10.0, 255.0);
      }
      else
      {
        power = 0;
      }
      break;
    case 2:
    // display validity of selflocalization
      ledRequest.backFrontWhiteLED=LEDRequest::lolo;
      power = (unsigned char) (robotPose.getValidity()*100);
      break;
    case 3:
    // display running time in minutes
      ledRequest.backFrontWhiteLED=LEDRequest::lllo;
      power = (unsigned char) (SystemCall::getCurrentSystemTime()/60000);
      break;
    case 4:
    // display IP
      ledRequest.backFrontWhiteLED=LEDRequest::llll;
      power = IP;
      break;
    default:
      faceInfo = 0;
      power = 0;
      break;
    }

  // display hundreds with LEDs 2 and 3
    switch (power/100)
    {
    case 0:
      ledRequest.faceLED[2]=LEDRequest::oooo;
      ledRequest.faceLED[3]=LEDRequest::oooo;
      break;
    case 1:
      ledRequest.faceLED[2]=LEDRequest::llll;
      ledRequest.faceLED[3]=LEDRequest::oooo;
      break;
    case 2:
      ledRequest.faceLED[2]=LEDRequest::llll;
      ledRequest.faceLED[3]=LEDRequest::llll;
      break;
    default:
      ledRequest.faceLED[2]=LEDRequest::oooo;
      ledRequest.faceLED[3]=LEDRequest::oooo;
      break;
    }

  // display tens with LEDs 6, 8 10, 9, 7, 4, 11, 5, 0, 1
  // blinking stands for five
    power%=100;

    if (power>=5)
    {
      if (power>=10)
      {
        ledRequest.faceLED[6]=LEDRequest::llll;
      }
      else
      {
        ledRequest.faceLED[6]=LEDRequest::lolo;
      }
    }
    else
    {
      ledRequest.faceLED[6]=LEDRequest::oooo;
    }
    
    if (power>=15)
    {
      if (power>=20)
      {
        ledRequest.faceLED[8]=LEDRequest::llll;
      }
      else
      {
        ledRequest.faceLED[8]=LEDRequest::lolo;
      }
    }
    else
    {
      ledRequest.faceLED[8]=LEDRequest::oooo;
    }
    
    if (power>=25)
    {
      if (power>=30)
      {
        ledRequest.faceLED[10]=LEDRequest::llll;
      }
      else
      {
        ledRequest.faceLED[10]=LEDRequest::lolo;
      }
    }
    else
    {
      ledRequest.faceLED[10]=LEDRequest::oooo;
    }
    
    if (power>=35)
    {
      if (power>=40)
      {
        ledRequest.faceLED[9]=LEDRequest::llll;
      }
      else
      {
        ledRequest.faceLED[9]=LEDRequest::lolo;
      }
    }
    else
    {
      ledRequest.faceLED[9]=LEDRequest::oooo;
    }

    
    if (power>=45)
    {
      if (power>=50)
      {
        ledRequest.faceLED[7]=LEDRequest::llll;
      }
      else
      {
        ledRequest.faceLED[7]=LEDRequest::lolo;
      }
    }
    else
    {
      ledRequest.faceLED[7]=LEDRequest::oooo;
    }

    
    if (power>=55)
    {
      if (power>=60)
      {
        ledRequest.faceLED[4]=LEDRequest::llll;
      }
      else
      {
        ledRequest.faceLED[4]=LEDRequest::lolo;
      }
    }
    else
    {
      ledRequest.faceLED[4]=LEDRequest::oooo;
    }

    
    if (power>=65)
    {
      if (power>=70)
      {
        ledRequest.faceLED[11]=LEDRequest::llll;
      }
      else
      {
        ledRequest.faceLED[11]=LEDRequest::lolo;
      }
    }
    else
    {
      ledRequest.faceLED[11]=LEDRequest::oooo;
    }

    
    if (power>=75)
    {
      if (power>=80)
      {
        ledRequest.faceLED[5]=LEDRequest::llll;
      }
      else
      {
        ledRequest.faceLED[5]=LEDRequest::lolo;
      }
    }
    else
    {
      ledRequest.faceLED[5]=LEDRequest::oooo;
    }

    
    if (power>=85)
    {
      if (power>=90)
      {
        ledRequest.faceLED[0]=LEDRequest::llll;
      }
      else
      {
        ledRequest.faceLED[0]=LEDRequest::lolo;
      }
    }
    else
    {
      ledRequest.faceLED[0]=LEDRequest::oooo;
    }

    
    if (power>=95)
    {
      if (power>=100)
      {
        ledRequest.faceLED[1]=LEDRequest::llll;
      }
      else
      {
        ledRequest.faceLED[1]=LEDRequest::lolo;
      }
    }
    else
    {
      ledRequest.faceLED[1]=LEDRequest::oooo;
    }

  // display single units with the LED-pairs 12, 13
  // a lighted pair stands for two and a blinking pair for one
    switch (power%5)
    {
    case 0:
      ledRequest.faceLED[12]=LEDRequest::oooo;
      ledRequest.faceLED[13]=LEDRequest::oooo;
      break;
    case 1:
      ledRequest.faceLED[12]=LEDRequest::oooo;
      ledRequest.faceLED[13]=LEDRequest::lolo;
      break;
    case 2:
      ledRequest.faceLED[12]=LEDRequest::oooo;
      ledRequest.faceLED[13]=LEDRequest::llll;
      break;
    case 3:
      ledRequest.faceLED[12]=LEDRequest::lolo;
      ledRequest.faceLED[13]=LEDRequest::llll;
      break;
    case 4:
      ledRequest.faceLED[12]=LEDRequest::llll;
      ledRequest.faceLED[13]=LEDRequest::llll;
      break;
    default:
      ledRequest.faceLED[12]=LEDRequest::lolo;
      ledRequest.faceLED[13]=LEDRequest::lolo;
      break;
    }
    
  }
}

void GT2005BehaviorControl::drawOptions()
{
  NDECLARE_DEBUGDRAWING("behavior:active-zone", "drawingOnField", "highlights the current zone");
  NDECLARE_DEBUGDRAWING("behavior:slam-path", "drawingOnField", "shows slam path");

  const Xabsl2Option* option;
  option = pEngine->getRootOption();
  while (option!=0) 
  {
    if(!strcmp(option->n, "handle-ball") )
    {
      NCOMPLEX_DRAWING("behavior:active-zone",

        if(!strcmp(option->activeState->n, "ball-in-center-of-field") )
        {
          NRECTANGLE("behavior:active-zone",
            xPosOwnPenaltyArea + 200 ,yPosLeftSideline - 300,
            xPosOpponentGroundline - 650, yPosRightSideline + 300,
            25,Drawings::ps_solid, Drawings::light_gray);
        }
        if(!strcmp(option->activeState->n, "ball-near-own-goal") )
        {
          NRECTANGLE("behavior:active-zone",
            xPosOwnPenaltyArea + 200 ,yPosLeftSideline - 300,
            xPosOwnGroundline, yPosRightSideline + 300,
            25,Drawings::ps_solid, Drawings::light_gray);
        }
        if(!strcmp(option->activeState->n, "ball-at-left-border") )
        {
          NRECTANGLE("behavior:active-zone",
            xPosOpponentGroundline,yPosLeftSideline - 300,
            xPosOwnGroundline, yPosLeftFieldBorder,
            25,Drawings::ps_solid, Drawings::light_gray);
        }
        if(!strcmp(option->activeState->n, "ball-at-right-border") )
        {
          NRECTANGLE("behavior:active-zone",
            xPosOpponentGroundline, yPosRightFieldBorder,
            xPosOwnPenaltyArea, yPosRightSideline + 300,
            25,Drawings::ps_solid, Drawings::light_gray);
        }
        if(!strcmp(option->activeState->n, "ball-at-left-opponent-border") )
        {
          NRECTANGLE("behavior:active-zone",
            xPosOpponentGroundline, yPosLeftFieldBorder,
            xPosOpponentGroundline - 650, yPosLeftPenaltyArea,
            25,Drawings::ps_solid, Drawings::light_gray);
        }
        if(!strcmp(option->activeState->n, "ball-at-right-opponent-border") )
        {
          NRECTANGLE("behavior:active-zone",
            xPosOpponentGroundline, yPosRightFieldBorder,
            xPosOpponentGroundline - 650, yPosRightPenaltyArea,
            25,Drawings::ps_solid, Drawings::light_gray);
        }
        if(!strcmp(option->activeState->n, "at-opponent-goal") )
        {
          NCIRCLE("behavior:active-zone", xPosOpponentGroundline,yPosCenterGoal,600,25,Drawings::ps_solid, Drawings::light_gray);
        }
      ); //NCOMPLEX_DRAWING("behavior:active-zone",

    }// "handle ball"

    //draw slam info
    if(!strcmp(option->n, "slam-control") )
    {
      NCOMPLEX_DRAWING("behavior:slam-path",

        double x1 = robotPose.getPose().translation.x;
        double y1 = robotPose.getPose().translation.y;

        for (int i = 0; i < 5; i++)
        {
          double x2 = challengeSymbols.challenge2TargetPositionX (i);
          double y2 = challengeSymbols.challenge2TargetPositionY (i);

          NLINE("behavior:slam-path", x1, y1, x2, y2, 1, Drawings::ps_solid, Drawings::dark_gray);

          x1 = x2;
          y1 = y2;
        };
      ); //NCOMPLEX_DRAWING("behavior:slam-path",

    }

    option = option->activeState->subsequentOption;
  }
}



void GT2005BehaviorControl::declareDrawingsForBasicBehaviors()
{
  NDECLARE_DEBUGDRAWING( "GT2005WalkCalibration:walkpath",
                         "drawingOnField",
                         "Draws estimated path of walk calibration.");
}

bool GT2005BehaviorControl::handleMessage(InMessage& message)
{
  return Xabsl2BehaviorControl::handleMessage(message) || kickSelectionSymbols.handleMessage(message);
}

---