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

# Modules/BehaviorControl/CommonXabsl2Symbols/RobotStateSymbols.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file RobotStateSymbols.cpp
    00003 *
    00004 * Implementation of class RobotStateSymbols.
    00005 *
    00006 * @author Matthias Jüngel
    00007 */
    00008
    00009 #include "RobotStateSymbols.h"
    00010
    00011 #ifdef APERIOS1_3_2
    00012   #include <ERA201D1.h>
    00013 #endif
    00014
    00015
    00016 RobotStateSymbols::RobotStateSymbols(const BehaviorControlInterfaces& interfaces)
    00017 : BehaviorControlInterfaces(interfaces)
    00018 {
    00019 #ifdef APERIOS1_3_2
    00020 #ifndef NDEBUG
    00021   if( false )
    00022   {
    00023     char* t =_etherStatusStr[0];
    00024     t[0] = t[1];
    00025   }
    00026 #endif
    00027 #endif
    00028 }
    00029
    00030
    00031 void RobotStateSymbols::registerSymbols(Xabsl2Engine& engine)
    00032 {
    00033   // robot-state.buttons
    00034   engine.registerBooleanInputSymbol("robot-state.any-back-button-pressed-for-one-second",this,
    00035     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getAnyBackButtonPressedForOneSecond);
    00036
    00037   engine.registerBooleanInputSymbol("robot-state.any-back-button-pressed-short-and-released",this,
    00038     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getAnyBackButtonPressedShortAndReleased);
    00039
    00040   engine.registerBooleanInputSymbol("robot-state.fore-back-button-pressed-short-and-released",this,
    00041     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getForeBackButtonPressedShortAndReleased);
    00042
    00043   engine.registerBooleanInputSymbol("robot-state.middle-back-button-pressed-short-and-released",this,
    00044     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getMiddleBackButtonPressedShortAndReleased);
    00045
    00046   engine.registerBooleanInputSymbol("robot-state.hind-back-button-pressed-short-and-released",this,
    00047     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getHindBackButtonPressedShortAndReleased);
    00048
    00049   engine.registerBooleanInputSymbol("robot-state.fore-back-button-pressed-short",this,
    00050     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getForeBackButtonPressedShort);
    00051
    00052   engine.registerBooleanInputSymbol("robot-state.middle-back-button-pressed-short",this,
    00053     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getMiddleBackButtonPressedShort);
    00054
    00055   engine.registerBooleanInputSymbol("robot-state.hind-back-button-pressed-short",this,
    00056     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getHindBackButtonPressedShort);
    00057
    00058   engine.registerBooleanInputSymbol("robot-state.head-button-pressed-for-three-seconds",this,
    00059     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getHeadButtonPressedForThreeSeconds);
    00060
    00061   engine.registerBooleanInputSymbol("robot-state.head-button-pressed-short",this,
    00062     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getHeadButtonPressedShort);
    00063
    00064   engine.registerBooleanInputSymbol("robot-state.head-button-pressed-short-and-released",this,
    00065     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getHeadButtonPressedShortAndReleased);
    00066
    00067   engine.registerBooleanInputSymbol("robot-state.chin-button-pressed-for-three-seconds",this,
    00068   (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getChinButtonPressedForThreeSeconds);
    00069
    00070   engine.registerBooleanInputSymbol("robot-state.connected-to-wlan",this,
    00071     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getConnectedToWLAN);
    00072
    00073
    00074   // "robot-state.mouth-state"
    00075   engine.registerEnumeratedInputSymbol("robot-state.mouth-state",this,
    00076     (int (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getMouthState);
    00077   engine.registerEnumeratedInputSymbolEnumElement("robot-state.mouth-state","mouth-open",RobotState::mouthOpen);
    00078   engine.registerEnumeratedInputSymbolEnumElement("robot-state.mouth-state","mouth-closed",RobotState::mouthClosed);
    00079
    00080   engine.registerBooleanInputSymbol("robot-state.ball-under-head",this,
    00081     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getBallUnderHead);
    00082
    00083   engine.registerBooleanInputSymbol("robot-state.something-in-front-of-chest",this,
    00084     (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getSomethingInFrontOfChest);
    00085
    00086   engine.registerDecimalInputSymbol("robot-state.time-since-something-was-in-front-of-chest",this,
    00087     (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getTimeSinceSomethingWasInFrontOfChest);
    00088
    00089   engine.registerDecimalInputSymbol("robot-state.distance-to-something-in-front-of-chest",this,
    00090     (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getDistanceToSIFOC);
    00091
    00092   // "robot-state.system-time"
    00093   engine.registerDecimalInputSymbol("robot-state.system-time",this,
    00094     (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getCurrentSystemTime);
    00095   // "robot-state.remaining-capacity"
    00096   engine.registerDecimalInputSymbol("robot-state.remaining-capacity",this,
    00097     (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getRemainingCapacity);
    00098   // "robot-state.wlan-signal"
    00099   engine.registerDecimalInputSymbol("robot-state.wlan-link",this,
    00100     (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getWLANLink);
    00101   // "robot-state.wlan-strength"
    00102   engine.registerDecimalInputSymbol("robot-state.wlan-signal",this,
    00103     (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getWLANSignal);
    00104   // "robot-state.wlan-noise"
    00105   engine.registerDecimalInputSymbol("robot-state.wlan-noise",this,
    00106     (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getWLANNoise);
    00107
    00108   // "robot-state.wlan-signal"
    00109   engine.registerDecimalInputSymbol("robot-state.wlan-filtered-link",this,
    00110     (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getFilteredWLANLink);
    00111   // "robot-state.wlan-strength"
    00112   engine.registerDecimalInputSymbol("robot-state.wlan-filtered-signal",this,
    00113     (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getFilteredWLANSignal);
    00114   // "robot-state.wlan-noise"
    00115   engine.registerDecimalInputSymbol("robot-state.wlan-filtered-noise",this,
    00116     (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getFilteredWLANNoise);
    00117
    00118   engine.registerEnumeratedOutputSymbol("robot-state.show-wlan-status",(int*)&WLANSignalStrenghShownByFaceLEDs);
    00119   engine.registerEnumeratedOutputSymbolEnumElement("robot-state.show-wlan-status","robot-state.show-wlan-status.wlanHide",wlanHide);
    00120   engine.registerEnumeratedOutputSymbolEnumElement("robot-state.show-wlan-status","robot-state.show-wlan-status.wlanShow",wlanShow);
    00121 }
    00122
    00123 void RobotStateSymbols::update()
    00124 {
    00125 }
    00126
    00127 int RobotStateSymbols::getMouthState()
    00128 {
    00129   return robotState.getMouthState();
    00130 }
    00131
    00132 double RobotStateSymbols::getCurrentSystemTime()
    00133 {
    00134   return SystemCall::getCurrentSystemTime();
    00135 }
    00136
    00137 double RobotStateSymbols::getRemainingCapacity()
    00138 {
    00139   return SystemCall::getRemainingPower();
    00140 }
    00141
    00142 bool RobotStateSymbols::getBallUnderHead()
    00143 {
    00144 //  cout << sensorDataBuffer.lastFrame().data[SensorData::neckTilt]<<"|"<<sensorDataBuffer.lastFrame().data[SensorData::neckTilt]<<"\n";
    00145 //  cout << flush;
    00146   return sensorDataBuffer.lastFrame().data[SensorData::headTilt]>270000;
    00147 }
    00148
    00149 bool RobotStateSymbols::getSomethingInFrontOfChest()
    00150 {
    00151   return robotState.getSomethingInFrontOfChest();
    00152 }
    00153
    00154 double RobotStateSymbols::getTimeSinceSomethingWasInFrontOfChest()
    00155 {
    00156   return SystemCall::getTimeSince(robotState.getTimeWhenSomethingWasInFrontOfChestLast());
    00157 }
    00158
    00159 double RobotStateSymbols::getDistanceToSIFOC()
    00160 {
    00161   return robotState.getDistanceToSIFOC();
    00162 }
    00163
    00164
    00165 bool RobotStateSymbols::getAnyBackButtonPressedForOneSecond()
    00166 {
    00167   return (
    00168     (
    00169     robotState.getAnyBackButtonPressed() &&
    00170     robotState.getAnyBackButtonDuration() > 1000
    00171     )
    00172     ||
    00173     (
    00174     !robotState.getAnyBackButtonPressed() &&
    00175     robotState.getAnyBackButtonPressedTime() > 1000 &&
    00176     SystemCall::getTimeSince(robotState.getAnyBackButtonTime()) < 700
    00177     )
    00178     );
    00179 }
    00180
    00181 bool RobotStateSymbols::getAnyBackButtonPressedShortAndReleased()
    00182 {
    00183   return (
    00184     !robotState.getAnyBackButtonPressed() &&
    00185     robotState.getAnyBackButtonPressedTime() < 1000 &&
    00186     SystemCall::getTimeSince(robotState.getAnyBackButtonTime()) < 700
    00187     );
    00188 }
    00189
    00190 bool RobotStateSymbols::getForeBackButtonPressedShort()
    00191 {
    00192   return (
    00193     (
    00194     robotState.getButtonPressed(BodyPercept::backFront) &&
    00195     robotState.getButtonDuration(BodyPercept::backFront) < 800
    00196     )
    00197     );
    00198 }
    00199
    00200 bool RobotStateSymbols::getForeBackButtonPressedShortAndReleased()
    00201 {
    00202   return (
    00203     !robotState.getButtonPressed(BodyPercept::backFront) &&
    00204     robotState.getButtonPressedTime(BodyPercept::backFront) < 1000 &&
    00205     SystemCall::getTimeSince(robotState.getButtonTime(BodyPercept::backFront)) < 700
    00206     );
    00207 }
    00208
    00209 bool RobotStateSymbols::getMiddleBackButtonPressedShort()
    00210 {
    00211   return (
    00212     (
    00213     robotState.getButtonPressed(BodyPercept::backMiddle) &&
    00214     robotState.getButtonDuration(BodyPercept::backMiddle) < 800
    00215     )
    00216     );
    00217 }
    00218
    00219 bool RobotStateSymbols::getMiddleBackButtonPressedShortAndReleased()
    00220 {
    00221   return (
    00222     !robotState.getButtonPressed(BodyPercept::backMiddle) &&
    00223     robotState.getButtonPressedTime(BodyPercept::backMiddle) < 1000 &&
    00224     SystemCall::getTimeSince(robotState.getButtonTime(BodyPercept::backMiddle)) < 700
    00225     );
    00226 }
    00227
    00228 bool RobotStateSymbols::getHindBackButtonPressedShort()
    00229 {
    00230   return (
    00231     (
    00232     robotState.getButtonPressed(BodyPercept::backBack) &&
    00233     robotState.getButtonDuration(BodyPercept::backBack) < 800
    00234     )
    00235     );
    00236 }
    00237
    00238 bool RobotStateSymbols::getHindBackButtonPressedShortAndReleased()
    00239 {
    00240   return (
    00241     !robotState.getButtonPressed(BodyPercept::backBack) &&
    00242     robotState.getButtonPressedTime(BodyPercept::backBack) < 1000 &&
    00243     SystemCall::getTimeSince(robotState.getButtonTime(BodyPercept::backBack)) < 700
    00244     );
    00245 }
    00246
    00247 bool RobotStateSymbols::getHeadButtonPressedForThreeSeconds()
    00248 {
    00249   return (
    00250     (
    00251     robotState.getButtonPressed(BodyPercept::head) &&
    00252     robotState.getButtonDuration(BodyPercept::head) > 3000
    00253     )
    00254     ||
    00255     (
    00256     !robotState.getButtonPressed(BodyPercept::head) &&
    00257     robotState.getButtonPressedTime(BodyPercept::head) > 3000 &&
    00258     SystemCall::getTimeSince(robotState.getButtonTime(BodyPercept::head)) < 500
    00259     )
    00260     );
    00261 }
    00262
    00263 bool RobotStateSymbols::getHeadButtonPressedShort()
    00264 {
    00265   return (
    00266     (
    00267     robotState.getButtonPressed(BodyPercept::head) &&
    00268 //    robotState.getButtonDuration(BodyPercept::head) > 100 &&
    00269     robotState.getButtonDuration(BodyPercept::head) < 800
    00270     )
    00271     );
    00272 }
    00273
    00274 bool RobotStateSymbols::getHeadButtonPressedShortAndReleased()
    00275 {
    00276   return (
    00277     !robotState.getButtonPressed(BodyPercept::head) &&
    00278 //    robotState.getButtonPressedTime(BodyPercept::head) > 100 &&
    00279     robotState.getButtonPressedTime(BodyPercept::head) < 1000 &&
    00280     SystemCall::getTimeSince(robotState.getButtonTime(BodyPercept::head)) < 700
    00281     );
    00282 }
    00283
    00284 bool RobotStateSymbols::getConnectedToWLAN() {
    00285 #ifdef APERIOS1_3_2
    00286   EtherDriverGetWLANStatisticsMsg myWLANStats;
    00287   ERA201D1_GetWLANStatistics(&myWLANStats);
    00288   if ( myWLANStats.statistics.link > 5) {
    00289     return true;
    00290   } else {
    00291     return false;
    00292   }
    00293 #ifndef NDEBUG
    00294   char* t =_etherStatusStr[0];
    00295   t[0] = t[1];
    00296 #endif
    00297 #else
    00298   return true;
    00299 #endif
    00300 }
    00301
    00302 double RobotStateSymbols::getWLANLink() {
    00303 #ifdef APERIOS1_3_2
    00304   EtherDriverGetWLANStatisticsMsg myWLANStats;
    00305   ERA201D1_GetWLANStatistics(&myWLANStats);
    00306
    00307   return myWLANStats.statistics.link;
    00308 #else
    00309   return 0;
    00310 #endif
    00311 }
    00312
    00313 double RobotStateSymbols::getWLANSignal() {
    00314 #ifdef APERIOS1_3_2
    00315   EtherDriverGetWLANStatisticsMsg myWLANStats;
    00316   ERA201D1_GetWLANStatistics(&myWLANStats);
    00317
    00318   return myWLANStats.statistics.signal;
    00319 #else
    00320   return 0;
    00321 #endif
    00322 }
    00323
    00324 double RobotStateSymbols::getWLANNoise() {
    00325 #ifdef APERIOS1_3_2
    00326   EtherDriverGetWLANStatisticsMsg myWLANStats;
    00327   ERA201D1_GetWLANStatistics(&myWLANStats);
    00328
    00329   return myWLANStats.statistics.noise;
    00330 #else
    00331   return 0;
    00332 #endif
    00333 }
    00334
    00335 double alpha = 0.95;
    00336
    00337 double RobotStateSymbols::getFilteredWLANLink() {
    00338 #ifdef APERIOS1_3_2
    00339   EtherDriverGetWLANStatisticsMsg myWLANStats;
    00340   ERA201D1_GetWLANStatistics(&myWLANStats);
    00341
    00342   filteredWLANLink *= alpha;
    00343   filteredWLANLink += (1.0 - alpha) * myWLANStats.statistics.link;
    00344   return filteredWLANLink;
    00345 #else
    00346   return 0;
    00347 #endif
    00348 }
    00349
    00350 double RobotStateSymbols::getFilteredWLANSignal() {
    00351 #ifdef APERIOS1_3_2
    00352   EtherDriverGetWLANStatisticsMsg myWLANStats;
    00353   ERA201D1_GetWLANStatistics(&myWLANStats);
    00354
    00355   filteredWLANSignal *= alpha;
    00356   filteredWLANSignal += (1.0 - alpha) * myWLANStats.statistics.signal;
    00357   return filteredWLANSignal;
    00358 #else
    00359   return 0;
    00360 #endif
    00361 }
    00362
    00363 double RobotStateSymbols::getFilteredWLANNoise() {
    00364 #ifdef APERIOS1_3_2
    00365   EtherDriverGetWLANStatisticsMsg myWLANStats;
    00366   ERA201D1_GetWLANStatistics(&myWLANStats);
    00367
    00368   filteredWLANNoise *= alpha;
    00369   filteredWLANNoise += (1.0 - alpha) * myWLANStats.statistics.noise;
    00370   return filteredWLANNoise;
    00371 #else
    00372   return 0;
    00373 #endif
    00374 }
    00375
    00376 bool RobotStateSymbols::getChinButtonPressedForThreeSeconds()
    00377 {
    00378   return (
    00379     (
    00380     robotState.getButtonPressed(BodyPercept::chin) &&
    00381     robotState.getButtonDuration(BodyPercept::chin) > 3000
    00382     )
    00383     ||
    00384     (
    00385     !robotState.getButtonPressed(BodyPercept::chin) &&
    00386     robotState.getButtonPressedTime(BodyPercept::chin) > 3000 &&
    00387     SystemCall::getTimeSince(robotState.getButtonTime(BodyPercept::chin)) < 500
    00388     )
    00389     );
    00390 }