Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/BehaviorControl/GT2005BehaviorControl/GT2005BehaviorControl.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005BehaviorControl.cpp
    00003 *
    00004 * Implementation of class GT2005BehaviorControl.
    00005 *
    00006 * @author Martin Lötzsch
    00007 * @author Matthias Jüngel
    00008 */
    00009
    00010 #include "GT2005BehaviorControl.h"
    00011 #include "Tools/Player.h"
    00012
    00013 #include "Representations/Perception/SlamPercept.h"
    00014
    00015 GT2005BehaviorControl::GT2005BehaviorControl(BehaviorControlInterfaces& interfaces)
    00016 : Xabsl2BehaviorControl(interfaces,SolutionRequest::gt2005),
    00017   calibrationSymbols(interfaces),
    00018   angleSymbols(interfaces),
    00019   ballSymbols(interfaces),
    00020   configurationSymbols(interfaces),
    00021   headAndTailSymbols(interfaces),
    00022   joystickSymbols(interfaces),
    00023   kickSelectionSymbols(interfaces,"GT2005.kst"),
    00024   ledAndSoundSymbols(interfaces),
    00025   gtCamSymbols(interfaces),
    00026   mathFunctions(interfaces),
    00027   motionRequestSymbols(interfaces),
    00028   obstaclesSymbols(interfaces),
    00029   roboCupGameManagerSymbols(interfaces),
    00030   robotPoseSymbols(interfaces),
    00031   robotStateSymbols(interfaces),
    00032   specialVisionSymbols(interfaces),
    00033   strategySymbols(interfaces),
    00034   openchallengeSymbols(interfaces),
    00035   challengeSymbols(interfaces),
    00036   fieldSymbols(interfaces),
    00037   kickLoggerSymbols(interfaces),
    00038   testSymbols(interfaces),
    00039   simpleBasicBehaviors(interfaces,errorHandler),
    00040   potentialFieldBasicBehaviors(interfaces,errorHandler),
    00041   commonBasicBehaviors(interfaces,errorHandler),
    00042   kickLogger(interfaces),
    00043   passSymbols(interfaces),
    00044   playersSymbols(interfaces),
    00045   gt2005WalkCalibrationBasicBehaviors(interfaces, errorHandler)
    00046 {
    00047   Xabsl2FileInputSource file("Xabsl2/gt05-ic.dat");
    00048   init(file);
    00049   ASSERT(pEngine);
    00050
    00051   quickStop = false;
    00052   timeSync = false;
    00053   syncTimestampSent = true;
    00054   initialBrightness = 0;
    00055   faceInfo = 0;
    00056   backFrontWasPressed = false;
    00057   timeSyncHeadControlMode = 0;
    00058   backBackWasPressed = false;
    00059   IP = 0;
    00060
    00061   #ifndef _WIN32
    00062   InConfigFile wlanconf("../../SYSTEM/CONF/wlanconf.txt");
    00063   ASSERT(wlanconf.exists());
    00064   char buf[50];
    00065   wlanconf >> buf >> buf;
    00066   int dots = 0;
    00067   int i = 0;
    00068   while (dots < 3)
    00069   {
    00070     if (buf[i]=='.') dots++;
    00071     i++;
    00072   }
    00073   IP = atoi(&buf[i]);
    00074   #endif
    00075
    00076 }
    00077
    00078 GT2005BehaviorControl::~GT2005BehaviorControl()
    00079 {
    00080 }
    00081
    00082 void GT2005BehaviorControl::registerSymbolsAndBasicBehaviors()
    00083 {
    00084   simpleBasicBehaviors.registerBasicBehaviors(*pEngine);
    00085   potentialFieldBasicBehaviors.registerBasicBehaviors(*pEngine);
    00086   commonBasicBehaviors.registerBasicBehaviors(*pEngine);
    00087   calibrationSymbols.registerSymbols(*pEngine);
    00088   angleSymbols.registerSymbols(*pEngine);
    00089   ballSymbols.registerSymbols(*pEngine);
    00090   gtCamSymbols.registerSymbols(*pEngine);
    00091   configurationSymbols.registerSymbols(*pEngine);
    00092   headAndTailSymbols.registerSymbols(*pEngine);
    00093   joystickSymbols.registerSymbols(*pEngine);
    00094   ledAndSoundSymbols.registerSymbols(*pEngine);
    00095   kickSelectionSymbols.registerSymbols(*pEngine);
    00096   mathFunctions.registerSymbols(*pEngine);
    00097   motionRequestSymbols.registerSymbols(*pEngine);
    00098   obstaclesSymbols.registerSymbols(*pEngine);
    00099   roboCupGameManagerSymbols.registerSymbols(*pEngine);
    00100   robotPoseSymbols.registerSymbols(*pEngine);
    00101   robotStateSymbols.registerSymbols(*pEngine);
    00102   specialVisionSymbols.registerSymbols(*pEngine);
    00103   strategySymbols.registerSymbols(*pEngine);
    00104   openchallengeSymbols.registerSymbols(*pEngine);
    00105   challengeSymbols.registerSymbols(*pEngine);
    00106   fieldSymbols.registerSymbols(*pEngine);
    00107   gt2005WalkCalibrationBasicBehaviors.registerBasicBehaviors(*pEngine);
    00108   kickLoggerSymbols.registerSymbols(*pEngine);
    00109   playersSymbols.registerSymbols(*pEngine);
    00110   passSymbols.registerSymbols(*pEngine);
    00111   testSymbols.registerSymbols(*pEngine);
    00112 }
    00113
    00114 void GT2005BehaviorControl::preExecute()
    00115 {
    00116   for(int i = 0; i < 14; i++) ledRequest.faceLED[i] = LEDRequest::oooo;
    00117   ledRequest.backFrontWhiteLED = LEDRequest::oooo;
    00118   ledRequest.backMiddleWhiteLED = LEDRequest::oooo;
    00119   ledRequest.backRearWhiteLED = LEDRequest::oooo;
    00120   ledRequest.backMiddleOrangeLED = LEDRequest::oooo;
    00121
    00122   ledRequest.headWhiteLED = LEDRequest::oooo;
    00123   ledRequest.headOrangeLED = LEDRequest::oooo;
    00124
    00125   // set the outgoing behavior team message to none
    00126   outgoingBehaviorTeamMessage.message = BehaviorTeamMessage::none;
    00127
    00128   // Set the tail request depending on whether the ball was seen
    00129   if (ballModel.ballWasSeen)
    00130   {
    00131     // ball seen
    00132     ledRequest.headWhiteLED = LEDRequest::llll;
    00133     motionRequest.tailRequest.tailRequestID = TailRequest::noTailWag;
    00134   }
    00135   else if (SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen)
    00136     > BallModel::behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted
    00137     && SystemCall::getTimeSince(ballModel.communicated.timeWhenLastObserved) < 3000)
    00138   {
    00139     // ball known
    00140     motionRequest.tailRequest.tailRequestID = TailRequest::wagHorizontalFast;
    00141   }
    00142   else
    00143   {
    00144     // ball not known
    00145     motionRequest.tailRequest.tailRequestID = TailRequest::noTailWag;
    00146   }
    00147
    00148   // set head control mode lookToStars to detect missing settings
    00149   headControlMode.headControlMode = HeadControlMode::lookToStars;
    00150
    00151   soundRequest.soundID = SoundRequest::none;
    00152
    00153   angleSymbols.angleShownByLEDs = AngleSymbols::undefined;
    00154 }
    00155
    00156 void GT2005BehaviorControl::execute()
    00157 {
    00158   angleSymbols.update();
    00159   ballSymbols.update();
    00160   configurationSymbols.update();
    00161   obstaclesSymbols.update();
    00162   roboCupGameManagerSymbols.update();
    00163   robotPoseSymbols.update();
    00164   strategySymbols.update();
    00165   openchallengeSymbols.update();
    00166   testSymbols.update();
    00167   potentialFieldBasicBehaviors.update();
    00168
    00169   kickLoggerSymbols.update();
    00170
    00171   // set some leds and and other variables
    00172   preExecute();
    00173
    00174   // execute the engine
    00175   executeEngine();
    00176
    00177   // play some sounds if necessary
    00178   postExecute();
    00179
    00180   drawOptions();
    00181   declareDrawingsForBasicBehaviors();
    00182
    00183   // execute the kick logger, but not during a game
    00184   kickLogger.execute();
    00185 }
    00186
    00187 void GT2005BehaviorControl::postExecute()
    00188 {
    00189   // handle fall down
    00190   if(outgoingBehaviorTeamMessage.gameState != BehaviorTeamMessage::penalized &&
    00191      ((robotState.getState() == RobotState::crashed)||
    00192      (robotState.getState() == RobotState::rollLeft)||
    00193      (robotState.getState() == RobotState::rollRight)))
    00194   {
    00195     if ((motionInfo.executedMotionRequest.motionType == MotionRequest::walk &&
    00196          motionInfo.executedMotionRequest.walkRequest.walkType != WalkRequest::upsideDown)||
    00197         (motionInfo.executedMotionRequest.motionType == MotionRequest::stand))
    00198     {
    00199       motionRequest.motionType = MotionRequest::getup;
    00200     }
    00201   }
    00202
    00203   // display team color
    00204   if (getPlayer().getTeamColor() == Player::blue)
    00205   {
    00206     ledRequest.backFrontBlueLED = LEDRequest::llll;
    00207     if(
    00208       outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::sleep ||
    00209       outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::initial ||
    00210       outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::ready ||
    00211       outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::set)
    00212     {
    00213       ledRequest.backFrontWhiteLED = LEDRequest::oooo;
    00214     }
    00215     ledRequest.backRearRedLED = LEDRequest::oooo;
    00216   }
    00217   else if (getPlayer().getTeamColor() == Player::red)
    00218   {
    00219     ledRequest.backFrontBlueLED = LEDRequest::oooo;
    00220     ledRequest.backRearRedLED = LEDRequest::llll;
    00221     if(
    00222       outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::sleep ||
    00223       outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::initial ||
    00224       outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::ready ||
    00225       outgoingBehaviorTeamMessage.gameState == BehaviorTeamMessage::set)
    00226     {
    00227       ledRequest.backRearWhiteLED = LEDRequest::oooo;
    00228     }
    00229   }
    00230   else
    00231   {
    00232     ledRequest.backFrontBlueLED = LEDRequest::lloo;
    00233     ledRequest.backRearRedLED = LEDRequest::lloo;
    00234   }
    00235
    00236   if(
    00237     outgoingBehaviorTeamMessage.gameState != BehaviorTeamMessage::sleep &&
    00238     outgoingBehaviorTeamMessage.gameState != BehaviorTeamMessage::initial)
    00239   {
    00240     // set the purple face leds depending on the dynamic role.
    00241     switch (strategySymbols.role)
    00242     {
    00243     case BehaviorTeamMessage::striker:
    00244       ledRequest.faceLED[13] = LEDRequest::llll;
    00245       break;
    00246     case BehaviorTeamMessage::offensiveSupporter:
    00247       ledRequest.faceLED[13] = LEDRequest::oooo;
    00248       ledRequest.faceLED[12] = LEDRequest::lloo;
    00249       break;
    00250     case BehaviorTeamMessage::defensiveSupporter:
    00251       ledRequest.faceLED[13] = LEDRequest::oooo;
    00252       break;
    00253     default:
    00254       ledRequest.faceLED[13] = LEDRequest::oooo;
    00255     }
    00256
    00257     if(getPlayer().getPlayerNumber() == Player::one)
    00258     {
    00259       if (ballModel.seen.ballRollsByLeft)
    00260         ledRequest.faceLED[1] = LEDRequest::llll;
    00261       if (ballModel.seen.ballRollsByRight)
    00262         ledRequest.faceLED[0] = LEDRequest::llll;
    00263       if (ballModel.seen.ballRollsFast)
    00264       {
    00265         ledRequest.faceLED[2] = LEDRequest::llll;
    00266         ledRequest.faceLED[3] = LEDRequest::llll;
    00267       }
    00268       if (ballModel.seen.ballRollsTowardsRobot)
    00269       {
    00270         ledRequest.faceLED[1] = LEDRequest::llll;
    00271         ledRequest.faceLED[0] = LEDRequest::llll;
    00272       }
    00273     }
    00274
    00275     if(angleSymbols.angleShownByLEDs == AngleSymbols::undefined)
    00276     {
    00277       ledRequest.faceLED[10] = LEDRequest::lolo;
    00278     }
    00279     else
    00280     {
    00281       // visualize the angle the robot wants to play to with the top white leds.
    00282       double bestAngle;
    00283       bestAngle = toDegrees(normalize(angleSymbols.getAngle(angleSymbols.angleShownByLEDs)));
    00284
    00285       if     (bestAngle >  75) ledRequest.faceLED[ 7] = LEDRequest::lolo; //?_____
    00286       else if(bestAngle >  45) ledRequest.faceLED[ 7] = LEDRequest::llll; // x____
    00287       else if(bestAngle >  15) ledRequest.faceLED[ 9] = LEDRequest::llll; // _x___
    00288       else if(bestAngle > -15) ledRequest.faceLED[10] = LEDRequest::llll; // __x__
    00289       else if(bestAngle > -45) ledRequest.faceLED[ 8] = LEDRequest::llll; // ___x_
    00290       else if(bestAngle > -75) ledRequest.faceLED[ 6] = LEDRequest::llll; // ____x
    00291       else                     ledRequest.faceLED[ 6] = LEDRequest::lolo; // _____?
    00292     }
    00293
    00294     /*
    00295     // display stuck/obstacles
    00296     if (obstaclesSymbols.robotIsStuck)
    00297     {
    00298       ledRequest.faceLED[4] = LEDRequest::llll;
    00299       ledRequest.faceLED[5] = LEDRequest::llll;
    00300     }
    00301     else if (obstaclesSymbols.obstaclesAreClose)
    00302     {
    00303       ledRequest.faceLED[11] = LEDRequest::llll;
    00304     }
    00305     */
    00306
    00307     // display angle used for perfect angle
    00308     /*
    00309     if (angleSymbols.angleUsedForPerfectAngle == AngleSymbols::angleToFreePartOfOpponentGoal)
    00310     {
    00311       ledRequest.faceLED[4] = LEDRequest::llll;
    00312       ledRequest.faceLED[5] = LEDRequest::llll;
    00313     }
    00314     else */
    00315     if (angleSymbols.angleUsedForPerfectAngle == AngleSymbols::seenAngleToOpponentGoal)
    00316     {
    00317       ledRequest.faceLED[4] = LEDRequest::llll;
    00318       ledRequest.faceLED[5] = LEDRequest::llll;
    00319     }
    00320     else
    00321     {
    00322       ledRequest.faceLED[4] = LEDRequest::oooo;
    00323       ledRequest.faceLED[5] = LEDRequest::oooo;
    00324     }
    00325   }
    00326
    00327   // if the head control mode was not set, play a warning sound
    00328   if (headControlMode.headControlMode == HeadControlMode::lookToStars)
    00329   {
    00330     soundRequest.soundID = SoundRequest::rob101;
    00331   }
    00332
    00333   if(sensorDataBuffer.frame[0].data[SensorData::chin] == 1)
    00334     soundRequest.soundID = SoundRequest::bark2;
    00335
    00336
    00337   if(sensorDataBuffer.lastFrame().data[SensorData::chin] == 1)
    00338   {
    00339     if(
    00340       robotState.getButtonPressed(BodyPercept::backBack) &&
    00341       robotState.getButtonDuration(BodyPercept::backBack) > 1000 &&
    00342       profiler.profilerCollectMode == GTXabsl2Profiler::collectProfiles
    00343       )
    00344     {
    00345       profiler.profilerCollectMode = GTXabsl2Profiler::dontCollectProfiles;
    00346       profiler.profilerWriteMode = GTXabsl2Profiler::writeCompleteProfiles;
    00347       soundRequest.soundID = SoundRequest::bing01;
    00348     }
    00349
    00350     if(
    00351       robotState.getButtonPressed(BodyPercept::backFront) &&
    00352       robotState.getButtonDuration(BodyPercept::backFront) > 1000
    00353       )
    00354     {
    00355       profiler.profilerCollectMode = GTXabsl2Profiler::collectProfiles;
    00356       soundRequest.soundID = SoundRequest::bing03;
    00357     }
    00358   }
    00359
    00360   DEBUG_RESPONSE("automated requests:enable quickstop", quickStop = true;);
    00361   DEBUG_RESPONSE("automated requests:disable quickstop", quickStop = false;);
    00362   if(!quickStop)
    00363   {
    00364     if(
    00365       robotState.getButtonPressed(BodyPercept::chin) &&
    00366       robotState.getButtonDuration(BodyPercept::chin) > 500 &&
    00367
    00368       robotState.getButtonPressed(BodyPercept::backMiddle) &&
    00369       robotState.getButtonDuration(BodyPercept::backMiddle) > 500
    00370       )
    00371     {
    00372       quickStop = true;
    00373       timeSync = false;
    00374     }
    00375   }
    00376   else
    00377   {
    00378     if(
    00379       robotState.getButtonPressed(BodyPercept::head) &&
    00380       robotState.getButtonDuration(BodyPercept::head) > 100 &&
    00381       robotState.getButtonDuration(BodyPercept::head) < 700
    00382       )
    00383       quickStop = false;
    00384   }
    00385
    00386   if(!timeSync)
    00387   {
    00388     if(
    00389       robotState.getButtonPressed(BodyPercept::chin) &&
    00390       robotState.getButtonDuration(BodyPercept::chin) > 500 &&
    00391
    00392       robotState.getButtonPressed(BodyPercept::backFront) &&
    00393       robotState.getButtonDuration(BodyPercept::backFront) > 500
    00394       )
    00395     {
    00396     // entering time-sync mode
    00397       timeSync = true;
    00398       syncTimestampSent = true;
    00399       backFrontWasPressed = true;
    00400       faceInfo = 0;
    00401       backBackWasPressed = false;
    00402       timeSyncHeadControlMode = 0;
    00403       quickStop = false;
    00404     }
    00405   }
    00406   else
    00407   {
    00408     if(
    00409       robotState.getButtonPressed(BodyPercept::head) &&
    00410       robotState.getButtonDuration(BodyPercept::head) > 100 &&
    00411       robotState.getButtonDuration(BodyPercept::head) < 700
    00412       )
    00413     // leaving timesync-mode
    00414       timeSync = false;
    00415   }
    00416
    00417   if(quickStop)
    00418   {
    00419     if(robotState.getButtonPressed(BodyPercept::backFront)) debugHeadControlMode.headControlMode = HeadControlMode::none;
    00420     if(robotState.getButtonPressed(BodyPercept::backMiddle)) debugHeadControlMode.headControlMode = HeadControlMode::searchForBall;
    00421     if(robotState.getButtonPressed(BodyPercept::backBack)) debugHeadControlMode.headControlMode = HeadControlMode::stayAsForced;
    00422
    00423     if(debugHeadControlMode.headControlMode != HeadControlMode::none)
    00424     {
    00425       headControlMode = debugHeadControlMode;
    00426     }
    00427
    00428     motionRequest.motionType = MotionRequest::stand;
    00429     ledRequest.backFrontWhiteLED = LEDRequest::oooo;
    00430     ledRequest.backMiddleWhiteLED = LEDRequest::oooo;
    00431     ledRequest.backRearWhiteLED = LEDRequest::oooo;
    00432
    00433     ledRequest.backFrontBlueLED = LEDRequest::llll;
    00434     ledRequest.backMiddleOrangeLED = LEDRequest::llll;
    00435     ledRequest.backRearRedLED = LEDRequest::llll;
    00436
    00437     if(debugHeadControlMode.headControlMode == HeadControlMode::none) ledRequest.backFrontWhiteLED = LEDRequest::llll;
    00438     if(debugHeadControlMode.headControlMode == HeadControlMode::searchForBall) ledRequest.backMiddleWhiteLED = LEDRequest::llll;
    00439     if(debugHeadControlMode.headControlMode == HeadControlMode::stayAsForced) ledRequest.backRearWhiteLED = LEDRequest::llll;
    00440   }
    00441
    00442   if(timeSync)
    00443   {
    00444     // clear all back LEDs
    00445     ledRequest.backMiddleWhiteLED=LEDRequest::oooo;
    00446     ledRequest.backFrontWhiteLED=LEDRequest::oooo;
    00447     ledRequest.backRearWhiteLED=LEDRequest::oooo;
    00448     ledRequest.backFrontBlueLED=LEDRequest::oooo;
    00449     ledRequest.backMiddleOrangeLED=LEDRequest::oooo;
    00450     ledRequest.backRearRedLED=LEDRequest::oooo;
    00451
    00452   // don't move
    00453     motionRequest.motionType = MotionRequest::stand;
    00454
    00455   // switch through headcontrol modes with back back button
    00456   /////////////////////////////////////////////////////////
    00457     if (robotState.getButtonPressed(BodyPercept::backBack))
    00458     {
    00459       if (!backBackWasPressed)
    00460       {
    00461         timeSyncHeadControlMode++;
    00462         backBackWasPressed = true;
    00463       }
    00464     }
    00465     else
    00466     {
    00467       backBackWasPressed = false;
    00468     }
    00469
    00470     switch (timeSyncHeadControlMode)
    00471     {
    00472     case 0:
    00473       ledRequest.backRearWhiteLED=LEDRequest::oooo;
    00474       headControlMode.headControlMode = HeadControlMode::lookStraightAhead;
    00475       break;
    00476     case 1:
    00477       ledRequest.backRearWhiteLED=LEDRequest::looo;
    00478       headControlMode.headControlMode = HeadControlMode::searchForBall;
    00479       break;
    00480     case 2:
    00481       ledRequest.backRearWhiteLED=LEDRequest::lolo;
    00482       headControlMode.headControlMode = HeadControlMode::searchAuto;
    00483       break;
    00484     case 3:
    00485       ledRequest.backRearWhiteLED=LEDRequest::lllo;
    00486       headControlMode.headControlMode = HeadControlMode::stayAsForced;
    00487       break;
    00488     case 4:
    00489       ledRequest.backRearWhiteLED=LEDRequest::llll;
    00490       break;
    00491     default:
    00492       timeSyncHeadControlMode = 0;
    00493       headControlMode.headControlMode = HeadControlMode::lookStraightAhead;
    00494       break;
    00495     }
    00496
    00497   // do time-sync
    00498   ///////////////
    00499     if (specialVisionRequest.specialVisionRequest != SpecialVisionRequest::imageBrightness)
    00500     {
    00501       specialVisionRequest.specialVisionRequest = SpecialVisionRequest::imageBrightness;
    00502     }
    00503     else
    00504     {
    00505       unsigned char estimatedBrightness=specialPercept.estimatedImageBrightness;
    00506
    00507       ledRequest.backMiddleOrangeLED=LEDRequest::lolo;
    00508
    00509     // reset initial brightness on back middle button
    00510       if (robotState.getButtonPressed(BodyPercept::backMiddle)) {
    00511         initialBrightness=estimatedBrightness;
    00512         syncTimestampSent = false;
    00513         ledRequest.backMiddleOrangeLED=LEDRequest::llll;
    00514       }
    00515
    00516       double degreeOfBrightness=(double)(estimatedBrightness-initialBrightness)/(220-initialBrightness);
    00517
    00518       if (degreeOfBrightness<-0.2) ledRequest.backFrontBlueLED=LEDRequest::looo;
    00519       if (degreeOfBrightness<-0.3) ledRequest.backFrontBlueLED=LEDRequest::lolo;
    00520       if (degreeOfBrightness<-0.6) ledRequest.backFrontBlueLED=LEDRequest::loll;
    00521       if (degreeOfBrightness<-0.8) ledRequest.backFrontBlueLED=LEDRequest::llll;
    00522
    00523       if (degreeOfBrightness>0.2) ledRequest.backRearRedLED=LEDRequest::looo;
    00524       if (degreeOfBrightness>0.3)
    00525       {
    00526         ledRequest.backRearRedLED=LEDRequest::lolo;
    00527     // send sync timestamp
    00528         if (!syncTimestampSent) OUTPUT(idSyncTimestampRobot, bin, SystemCall::getCurrentSystemTime());
    00529         syncTimestampSent = true;
    00530       }
    00531       if (degreeOfBrightness>0.6) ledRequest.backRearRedLED=LEDRequest::loll;
    00532       if (degreeOfBrightness>0.8) ledRequest.backRearRedLED=LEDRequest::llll;
    00533
    00534       if (syncTimestampSent) ledRequest.backMiddleOrangeLED=LEDRequest::llll;
    00535     }
    00536
    00537     //display different infos on face-LEDs depending on back front button
    00538   //////////////////////////////////////
    00539     if (robotState.getButtonPressed(BodyPercept::backFront))
    00540     {
    00541       if (!backFrontWasPressed)
    00542       {
    00543         faceInfo++;
    00544         backFrontWasPressed = true;
    00545       }
    00546     }
    00547     else
    00548     {
    00549       backFrontWasPressed = false;
    00550     }
    00551
    00552     unsigned char power;
    00553
    00554     switch (faceInfo)
    00555     {
    00556     case 0:
    00557     // display remaining power
    00558       ledRequest.backFrontWhiteLED=LEDRequest::oooo;
    00559       power = SystemCall::getRemainingPower();
    00560       break;
    00561     case 1:
    00562     // display seen ball distance
    00563       ledRequest.backFrontWhiteLED=LEDRequest::looo;
    00564       if (ballModel.ballWasSeen)
    00565       {
    00566         power = (unsigned char) min(Geometry::fieldCoord2Relative(robotPose, ballModel.seen.positionField).abs()/10.0, 255.0);
    00567       }
    00568       else
    00569       {
    00570         power = 0;
    00571       }
    00572       break;
    00573     case 2:
    00574     // display validity of selflocalization
    00575       ledRequest.backFrontWhiteLED=LEDRequest::lolo;
    00576       power = (unsigned char) (robotPose.getValidity()*100);
    00577       break;
    00578     case 3:
    00579     // display running time in minutes
    00580       ledRequest.backFrontWhiteLED=LEDRequest::lllo;
    00581       power = (unsigned char) (SystemCall::getCurrentSystemTime()/60000);
    00582       break;
    00583     case 4:
    00584     // display IP
    00585       ledRequest.backFrontWhiteLED=LEDRequest::llll;
    00586       power = IP;
    00587       break;
    00588     default:
    00589       faceInfo = 0;
    00590       power = 0;
    00591       break;
    00592     }
    00593
    00594   // display hundreds with LEDs 2 and 3
    00595     switch (power/100)
    00596     {
    00597     case 0:
    00598       ledRequest.faceLED[2]=LEDRequest::oooo;
    00599       ledRequest.faceLED[3]=LEDRequest::oooo;
    00600       break;
    00601     case 1:
    00602       ledRequest.faceLED[2]=LEDRequest::llll;
    00603       ledRequest.faceLED[3]=LEDRequest::oooo;
    00604       break;
    00605     case 2:
    00606       ledRequest.faceLED[2]=LEDRequest::llll;
    00607       ledRequest.faceLED[3]=LEDRequest::llll;
    00608       break;
    00609     default:
    00610       ledRequest.faceLED[2]=LEDRequest::oooo;
    00611       ledRequest.faceLED[3]=LEDRequest::oooo;
    00612       break;
    00613     }
    00614
    00615   // display tens with LEDs 6, 8 10, 9, 7, 4, 11, 5, 0, 1
    00616   // blinking stands for five
    00617     power%=100;
    00618
    00619     if (power>=5)
    00620     {
    00621       if (power>=10)
    00622       {
    00623         ledRequest.faceLED[6]=LEDRequest::llll;
    00624       }
    00625       else
    00626       {
    00627         ledRequest.faceLED[6]=LEDRequest::lolo;
    00628       }
    00629     }
    00630     else
    00631     {
    00632       ledRequest.faceLED[6]=LEDRequest::oooo;
    00633     }
    00634
    00635     if (power>=15)
    00636     {
    00637       if (power>=20)
    00638       {
    00639         ledRequest.faceLED[8]=LEDRequest::llll;
    00640       }
    00641       else
    00642       {
    00643         ledRequest.faceLED[8]=LEDRequest::lolo;
    00644       }
    00645     }
    00646     else
    00647     {
    00648       ledRequest.faceLED[8]=LEDRequest::oooo;
    00649     }
    00650
    00651     if (power>=25)
    00652     {
    00653       if (power>=30)
    00654       {
    00655         ledRequest.faceLED[10]=LEDRequest::llll;
    00656       }
    00657       else
    00658       {
    00659         ledRequest.faceLED[10]=LEDRequest::lolo;
    00660       }
    00661     }
    00662     else
    00663     {
    00664       ledRequest.faceLED[10]=LEDRequest::oooo;
    00665     }
    00666
    00667     if (power>=35)
    00668     {
    00669       if (power>=40)
    00670       {
    00671         ledRequest.faceLED[9]=LEDRequest::llll;
    00672       }
    00673       else
    00674       {
    00675         ledRequest.faceLED[9]=LEDRequest::lolo;
    00676       }
    00677     }
    00678     else
    00679     {
    00680       ledRequest.faceLED[9]=LEDRequest::oooo;
    00681     }
    00682
    00683
    00684     if (power>=45)
    00685     {
    00686       if (power>=50)
    00687       {
    00688         ledRequest.faceLED[7]=LEDRequest::llll;
    00689       }
    00690       else
    00691       {
    00692         ledRequest.faceLED[7]=LEDRequest::lolo;
    00693       }
    00694     }
    00695     else
    00696     {
    00697       ledRequest.faceLED[7]=LEDRequest::oooo;
    00698     }
    00699
    00700
    00701     if (power>=55)
    00702     {
    00703       if (power>=60)
    00704       {
    00705         ledRequest.faceLED[4]=LEDRequest::llll;
    00706       }
    00707       else
    00708       {
    00709         ledRequest.faceLED[4]=LEDRequest::lolo;
    00710       }
    00711     }
    00712     else
    00713     {
    00714       ledRequest.faceLED[4]=LEDRequest::oooo;
    00715     }
    00716
    00717
    00718     if (power>=65)
    00719     {
    00720       if (power>=70)
    00721       {
    00722         ledRequest.faceLED[11]=LEDRequest::llll;
    00723       }
    00724       else
    00725       {
    00726         ledRequest.faceLED[11]=LEDRequest::lolo;
    00727       }
    00728     }
    00729     else
    00730     {
    00731       ledRequest.faceLED[11]=LEDRequest::oooo;
    00732     }
    00733
    00734
    00735     if (power>=75)
    00736     {
    00737       if (power>=80)
    00738       {
    00739         ledRequest.faceLED[5]=LEDRequest::llll;
    00740       }
    00741       else
    00742       {
    00743         ledRequest.faceLED[5]=LEDRequest::lolo;
    00744       }
    00745     }
    00746     else
    00747     {
    00748       ledRequest.faceLED[5]=LEDRequest::oooo;
    00749     }
    00750
    00751
    00752     if (power>=85)
    00753     {
    00754       if (power>=90)
    00755       {
    00756         ledRequest.faceLED[0]=LEDRequest::llll;
    00757       }
    00758       else
    00759       {
    00760         ledRequest.faceLED[0]=LEDRequest::lolo;
    00761       }
    00762     }
    00763     else
    00764     {
    00765       ledRequest.faceLED[0]=LEDRequest::oooo;
    00766     }
    00767
    00768
    00769     if (power>=95)
    00770     {
    00771       if (power>=100)
    00772       {
    00773         ledRequest.faceLED[1]=LEDRequest::llll;
    00774       }
    00775       else
    00776       {
    00777         ledRequest.faceLED[1]=LEDRequest::lolo;
    00778       }
    00779     }
    00780     else
    00781     {
    00782       ledRequest.faceLED[1]=LEDRequest::oooo;
    00783     }
    00784
    00785   // display single units with the LED-pairs 12, 13
    00786   // a lighted pair stands for two and a blinking pair for one
    00787     switch (power%5)
    00788     {
    00789     case 0:
    00790       ledRequest.faceLED[12]=LEDRequest::oooo;
    00791       ledRequest.faceLED[13]=LEDRequest::oooo;
    00792       break;
    00793     case 1:
    00794       ledRequest.faceLED[12]=LEDRequest::oooo;
    00795       ledRequest.faceLED[13]=LEDRequest::lolo;
    00796       break;
    00797     case 2:
    00798       ledRequest.faceLED[12]=LEDRequest::oooo;
    00799       ledRequest.faceLED[13]=LEDRequest::llll;
    00800       break;
    00801     case 3:
    00802       ledRequest.faceLED[12]=LEDRequest::lolo;
    00803       ledRequest.faceLED[13]=LEDRequest::llll;
    00804       break;
    00805     case 4:
    00806       ledRequest.faceLED[12]=LEDRequest::llll;
    00807       ledRequest.faceLED[13]=LEDRequest::llll;
    00808       break;
    00809     default:
    00810       ledRequest.faceLED[12]=LEDRequest::lolo;
    00811       ledRequest.faceLED[13]=LEDRequest::lolo;
    00812       break;
    00813     }
    00814
    00815   }
    00816 }
    00817
    00818 void GT2005BehaviorControl::drawOptions()
    00819 {
    00820   NDECLARE_DEBUGDRAWING("behavior:active-zone", "drawingOnField", "highlights the current zone");
    00821   NDECLARE_DEBUGDRAWING("behavior:slam-path", "drawingOnField", "shows slam path");
    00822
    00823   const Xabsl2Option* option;
    00824   option = pEngine->getRootOption();
    00825   while (option!=0)
    00826   {
    00827     if(!strcmp(option->n, "handle-ball") )
    00828     {
    00829       NCOMPLEX_DRAWING("behavior:active-zone",
    00830
    00831         if(!strcmp(option->activeState->n, "ball-in-center-of-field") )
    00832         {
    00833           NRECTANGLE("behavior:active-zone",
    00834             xPosOwnPenaltyArea + 200 ,yPosLeftSideline - 300,
    00835             xPosOpponentGroundline - 650, yPosRightSideline + 300,
    00836             25,Drawings::ps_solid, Drawings::light_gray);
    00837         }
    00838         if(!strcmp(option->activeState->n, "ball-near-own-goal") )
    00839         {
    00840           NRECTANGLE("behavior:active-zone",
    00841             xPosOwnPenaltyArea + 200 ,yPosLeftSideline - 300,
    00842             xPosOwnGroundline, yPosRightSideline + 300,
    00843             25,Drawings::ps_solid, Drawings::light_gray);
    00844         }
    00845         if(!strcmp(option->activeState->n, "ball-at-left-border") )
    00846         {
    00847           NRECTANGLE("behavior:active-zone",
    00848             xPosOpponentGroundline,yPosLeftSideline - 300,
    00849             xPosOwnGroundline, yPosLeftFieldBorder,
    00850             25,Drawings::ps_solid, Drawings::light_gray);
    00851         }
    00852         if(!strcmp(option->activeState->n, "ball-at-right-border") )
    00853         {
    00854           NRECTANGLE("behavior:active-zone",
    00855             xPosOpponentGroundline, yPosRightFieldBorder,
    00856             xPosOwnPenaltyArea, yPosRightSideline + 300,
    00857             25,Drawings::ps_solid, Drawings::light_gray);
    00858         }
    00859         if(!strcmp(option->activeState->n, "ball-at-left-opponent-border") )
    00860         {
    00861           NRECTANGLE("behavior:active-zone",
    00862             xPosOpponentGroundline, yPosLeftFieldBorder,
    00863             xPosOpponentGroundline - 650, yPosLeftPenaltyArea,
    00864             25,Drawings::ps_solid, Drawings::light_gray);
    00865         }
    00866         if(!strcmp(option->activeState->n, "ball-at-right-opponent-border") )
    00867         {
    00868           NRECTANGLE("behavior:active-zone",
    00869             xPosOpponentGroundline, yPosRightFieldBorder,
    00870             xPosOpponentGroundline - 650, yPosRightPenaltyArea,
    00871             25,Drawings::ps_solid, Drawings::light_gray);
    00872         }
    00873         if(!strcmp(option->activeState->n, "at-opponent-goal") )
    00874         {
    00875           NCIRCLE("behavior:active-zone", xPosOpponentGroundline,yPosCenterGoal,600,25,Drawings::ps_solid, Drawings::light_gray);
    00876         }
    00877       ); //NCOMPLEX_DRAWING("behavior:active-zone",
    00878
    00879     }// "handle ball"
    00880
    00881     //draw slam info
    00882     if(!strcmp(option->n, "slam-control") )
    00883     {
    00884       NCOMPLEX_DRAWING("behavior:slam-path",
    00885
    00886         double x1 = robotPose.getPose().translation.x;
    00887         double y1 = robotPose.getPose().translation.y;
    00888
    00889         for (int i = 0; i < 5; i++)
    00890         {
    00891           double x2 = challengeSymbols.challenge2TargetPositionX (i);
    00892           double y2 = challengeSymbols.challenge2TargetPositionY (i);
    00893
    00894           NLINE("behavior:slam-path", x1, y1, x2, y2, 1, Drawings::ps_solid, Drawings::dark_gray);
    00895
    00896           x1 = x2;
    00897           y1 = y2;
    00898         };
    00899       ); //NCOMPLEX_DRAWING("behavior:slam-path",
    00900
    00901     }
    00902
    00903     option = option->activeState->subsequentOption;
    00904   }
    00905 }
    00906
    00907
    00908
    00909 void GT2005BehaviorControl::declareDrawingsForBasicBehaviors()
    00910 {
    00911   NDECLARE_DEBUGDRAWING( "GT2005WalkCalibration:walkpath",
    00912                          "drawingOnField",
    00913                          "Draws estimated path of walk calibration.");
    00914 }
    00915
    00916 bool GT2005BehaviorControl::handleMessage(InMessage& message)
    00917 {
    00918   return Xabsl2BehaviorControl::handleMessage(message) || kickSelectionSymbols.handleMessage(message);
    00919 }
    00920