Modules/BehaviorControl/CommonXabsl2Symbols/RobotStateSymbols.cpp

Go to the documentation of this file.

/** 
* @file RobotStateSymbols.cpp
*
* Implementation of class RobotStateSymbols.
*
* @author Matthias Jüngel
*/

#include "RobotStateSymbols.h"

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


RobotStateSymbols::RobotStateSymbols(const BehaviorControlInterfaces& interfaces)
: BehaviorControlInterfaces(interfaces)
{
#ifdef APERIOS1_3_2
#ifndef NDEBUG
  if( false )
  {
    char* t =_etherStatusStr[0];
    t[0] = t[1];
  }
#endif
#endif
}


void RobotStateSymbols::registerSymbols(Xabsl2Engine& engine)
{
  // robot-state.buttons
  engine.registerBooleanInputSymbol("robot-state.any-back-button-pressed-for-one-second",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getAnyBackButtonPressedForOneSecond);

  engine.registerBooleanInputSymbol("robot-state.any-back-button-pressed-short-and-released",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getAnyBackButtonPressedShortAndReleased);

  engine.registerBooleanInputSymbol("robot-state.fore-back-button-pressed-short-and-released",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getForeBackButtonPressedShortAndReleased);

  engine.registerBooleanInputSymbol("robot-state.middle-back-button-pressed-short-and-released",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getMiddleBackButtonPressedShortAndReleased);

  engine.registerBooleanInputSymbol("robot-state.hind-back-button-pressed-short-and-released",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getHindBackButtonPressedShortAndReleased);

  engine.registerBooleanInputSymbol("robot-state.fore-back-button-pressed-short",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getForeBackButtonPressedShort);

  engine.registerBooleanInputSymbol("robot-state.middle-back-button-pressed-short",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getMiddleBackButtonPressedShort);

  engine.registerBooleanInputSymbol("robot-state.hind-back-button-pressed-short",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getHindBackButtonPressedShort);

  engine.registerBooleanInputSymbol("robot-state.head-button-pressed-for-three-seconds",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getHeadButtonPressedForThreeSeconds);

  engine.registerBooleanInputSymbol("robot-state.head-button-pressed-short",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getHeadButtonPressedShort);

  engine.registerBooleanInputSymbol("robot-state.head-button-pressed-short-and-released",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getHeadButtonPressedShortAndReleased);

  engine.registerBooleanInputSymbol("robot-state.chin-button-pressed-for-three-seconds",this,
  (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getChinButtonPressedForThreeSeconds);

  engine.registerBooleanInputSymbol("robot-state.connected-to-wlan",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getConnectedToWLAN);


  // "robot-state.mouth-state"
  engine.registerEnumeratedInputSymbol("robot-state.mouth-state",this,
    (int (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getMouthState);
  engine.registerEnumeratedInputSymbolEnumElement("robot-state.mouth-state","mouth-open",RobotState::mouthOpen);
  engine.registerEnumeratedInputSymbolEnumElement("robot-state.mouth-state","mouth-closed",RobotState::mouthClosed);

  engine.registerBooleanInputSymbol("robot-state.ball-under-head",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getBallUnderHead);

  engine.registerBooleanInputSymbol("robot-state.something-in-front-of-chest",this,
    (bool (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getSomethingInFrontOfChest);

  engine.registerDecimalInputSymbol("robot-state.time-since-something-was-in-front-of-chest",this,
    (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getTimeSinceSomethingWasInFrontOfChest);

  engine.registerDecimalInputSymbol("robot-state.distance-to-something-in-front-of-chest",this,
    (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getDistanceToSIFOC);

  // "robot-state.system-time"
  engine.registerDecimalInputSymbol("robot-state.system-time",this,
    (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getCurrentSystemTime);
  // "robot-state.remaining-capacity"
  engine.registerDecimalInputSymbol("robot-state.remaining-capacity",this,
    (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getRemainingCapacity);
  // "robot-state.wlan-signal"
  engine.registerDecimalInputSymbol("robot-state.wlan-link",this,
    (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getWLANLink);
  // "robot-state.wlan-strength"
  engine.registerDecimalInputSymbol("robot-state.wlan-signal",this,
    (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getWLANSignal);
  // "robot-state.wlan-noise"
  engine.registerDecimalInputSymbol("robot-state.wlan-noise",this,
    (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getWLANNoise);

  // "robot-state.wlan-signal"
  engine.registerDecimalInputSymbol("robot-state.wlan-filtered-link",this,
    (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getFilteredWLANLink);
  // "robot-state.wlan-strength"
  engine.registerDecimalInputSymbol("robot-state.wlan-filtered-signal",this,
    (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getFilteredWLANSignal);
  // "robot-state.wlan-noise"
  engine.registerDecimalInputSymbol("robot-state.wlan-filtered-noise",this,
    (double (Xabsl2FunctionProvider::*)())&RobotStateSymbols::getFilteredWLANNoise);

  engine.registerEnumeratedOutputSymbol("robot-state.show-wlan-status",(int*)&WLANSignalStrenghShownByFaceLEDs);
  engine.registerEnumeratedOutputSymbolEnumElement("robot-state.show-wlan-status","robot-state.show-wlan-status.wlanHide",wlanHide);
  engine.registerEnumeratedOutputSymbolEnumElement("robot-state.show-wlan-status","robot-state.show-wlan-status.wlanShow",wlanShow);
}

void RobotStateSymbols::update()
{
}

int RobotStateSymbols::getMouthState()
{
  return robotState.getMouthState();
}

double RobotStateSymbols::getCurrentSystemTime()
{
  return SystemCall::getCurrentSystemTime();
}

double RobotStateSymbols::getRemainingCapacity()
{
  return SystemCall::getRemainingPower();
}

bool RobotStateSymbols::getBallUnderHead()
{
//  cout << sensorDataBuffer.lastFrame().data[SensorData::neckTilt]<<"|"<<sensorDataBuffer.lastFrame().data[SensorData::neckTilt]<<"\n";
//  cout << flush;
  return sensorDataBuffer.lastFrame().data[SensorData::headTilt]>270000;
}

bool RobotStateSymbols::getSomethingInFrontOfChest()
{
  return robotState.getSomethingInFrontOfChest();
}

double RobotStateSymbols::getTimeSinceSomethingWasInFrontOfChest()
{
  return SystemCall::getTimeSince(robotState.getTimeWhenSomethingWasInFrontOfChestLast());
}

double RobotStateSymbols::getDistanceToSIFOC()
{
  return robotState.getDistanceToSIFOC();
}


bool RobotStateSymbols::getAnyBackButtonPressedForOneSecond()
{
  return (
    (
    robotState.getAnyBackButtonPressed() &&
    robotState.getAnyBackButtonDuration() > 1000
    )
    ||
    (
    !robotState.getAnyBackButtonPressed() &&
    robotState.getAnyBackButtonPressedTime() > 1000 &&
    SystemCall::getTimeSince(robotState.getAnyBackButtonTime()) < 700
    )
    );
}

bool RobotStateSymbols::getAnyBackButtonPressedShortAndReleased()
{
  return (
    !robotState.getAnyBackButtonPressed() &&
    robotState.getAnyBackButtonPressedTime() < 1000 &&
    SystemCall::getTimeSince(robotState.getAnyBackButtonTime()) < 700
    );
}

bool RobotStateSymbols::getForeBackButtonPressedShort()
{
  return (
    (
    robotState.getButtonPressed(BodyPercept::backFront) &&
    robotState.getButtonDuration(BodyPercept::backFront) < 800
    )
    );
}

bool RobotStateSymbols::getForeBackButtonPressedShortAndReleased()
{
  return (
    !robotState.getButtonPressed(BodyPercept::backFront) &&
    robotState.getButtonPressedTime(BodyPercept::backFront) < 1000 &&
    SystemCall::getTimeSince(robotState.getButtonTime(BodyPercept::backFront)) < 700
    );
}

bool RobotStateSymbols::getMiddleBackButtonPressedShort()
{
  return (
    (
    robotState.getButtonPressed(BodyPercept::backMiddle) &&
    robotState.getButtonDuration(BodyPercept::backMiddle) < 800
    )
    );
}

bool RobotStateSymbols::getMiddleBackButtonPressedShortAndReleased()
{
  return (
    !robotState.getButtonPressed(BodyPercept::backMiddle) &&
    robotState.getButtonPressedTime(BodyPercept::backMiddle) < 1000 &&
    SystemCall::getTimeSince(robotState.getButtonTime(BodyPercept::backMiddle)) < 700
    );
}

bool RobotStateSymbols::getHindBackButtonPressedShort()
{
  return (
    (
    robotState.getButtonPressed(BodyPercept::backBack) &&
    robotState.getButtonDuration(BodyPercept::backBack) < 800
    )
    );
}

bool RobotStateSymbols::getHindBackButtonPressedShortAndReleased()
{
  return (
    !robotState.getButtonPressed(BodyPercept::backBack) &&
    robotState.getButtonPressedTime(BodyPercept::backBack) < 1000 &&
    SystemCall::getTimeSince(robotState.getButtonTime(BodyPercept::backBack)) < 700
    );
}

bool RobotStateSymbols::getHeadButtonPressedForThreeSeconds()
{
  return (
    (
    robotState.getButtonPressed(BodyPercept::head) &&
    robotState.getButtonDuration(BodyPercept::head) > 3000
    )
    ||
    (
    !robotState.getButtonPressed(BodyPercept::head) &&
    robotState.getButtonPressedTime(BodyPercept::head) > 3000 &&
    SystemCall::getTimeSince(robotState.getButtonTime(BodyPercept::head)) < 500
    )
    );
}

bool RobotStateSymbols::getHeadButtonPressedShort()
{
  return (
    (
    robotState.getButtonPressed(BodyPercept::head) &&
//    robotState.getButtonDuration(BodyPercept::head) > 100 &&
    robotState.getButtonDuration(BodyPercept::head) < 800
    )
    );
}

bool RobotStateSymbols::getHeadButtonPressedShortAndReleased()
{
  return (
    !robotState.getButtonPressed(BodyPercept::head) &&
//    robotState.getButtonPressedTime(BodyPercept::head) > 100 &&
    robotState.getButtonPressedTime(BodyPercept::head) < 1000 &&
    SystemCall::getTimeSince(robotState.getButtonTime(BodyPercept::head)) < 700
    );
}

bool RobotStateSymbols::getConnectedToWLAN() {
#ifdef APERIOS1_3_2
  EtherDriverGetWLANStatisticsMsg myWLANStats;
  ERA201D1_GetWLANStatistics(&myWLANStats);
  if ( myWLANStats.statistics.link > 5) {
    return true;
  } else {
    return false;
  }
#ifndef NDEBUG
  char* t =_etherStatusStr[0];
  t[0] = t[1];
#endif
#else
  return true;
#endif
}

double RobotStateSymbols::getWLANLink() {
#ifdef APERIOS1_3_2
  EtherDriverGetWLANStatisticsMsg myWLANStats;
  ERA201D1_GetWLANStatistics(&myWLANStats);

  return myWLANStats.statistics.link;
#else
  return 0;
#endif
}

double RobotStateSymbols::getWLANSignal() {
#ifdef APERIOS1_3_2
  EtherDriverGetWLANStatisticsMsg myWLANStats;
  ERA201D1_GetWLANStatistics(&myWLANStats);

  return myWLANStats.statistics.signal;
#else
  return 0;
#endif
}

double RobotStateSymbols::getWLANNoise() {
#ifdef APERIOS1_3_2
  EtherDriverGetWLANStatisticsMsg myWLANStats;
  ERA201D1_GetWLANStatistics(&myWLANStats);

  return myWLANStats.statistics.noise;
#else
  return 0;
#endif
}

double alpha = 0.95;

double RobotStateSymbols::getFilteredWLANLink() {
#ifdef APERIOS1_3_2
  EtherDriverGetWLANStatisticsMsg myWLANStats;
  ERA201D1_GetWLANStatistics(&myWLANStats);

  filteredWLANLink *= alpha;
  filteredWLANLink += (1.0 - alpha) * myWLANStats.statistics.link;
  return filteredWLANLink;
#else
  return 0;
#endif
}

double RobotStateSymbols::getFilteredWLANSignal() {
#ifdef APERIOS1_3_2
  EtherDriverGetWLANStatisticsMsg myWLANStats;
  ERA201D1_GetWLANStatistics(&myWLANStats);

  filteredWLANSignal *= alpha;
  filteredWLANSignal += (1.0 - alpha) * myWLANStats.statistics.signal;
  return filteredWLANSignal;
#else
  return 0;
#endif
}

double RobotStateSymbols::getFilteredWLANNoise() {
#ifdef APERIOS1_3_2
  EtherDriverGetWLANStatisticsMsg myWLANStats;
  ERA201D1_GetWLANStatistics(&myWLANStats);

  filteredWLANNoise *= alpha;
  filteredWLANNoise += (1.0 - alpha) * myWLANStats.statistics.noise;
  return filteredWLANNoise;
#else
  return 0;
#endif
}

bool RobotStateSymbols::getChinButtonPressedForThreeSeconds()
{
  return (
    (
    robotState.getButtonPressed(BodyPercept::chin) &&
    robotState.getButtonDuration(BodyPercept::chin) > 3000
    )
    ||
    (
    !robotState.getButtonPressed(BodyPercept::chin) &&
    robotState.getButtonPressedTime(BodyPercept::chin) > 3000 &&
    SystemCall::getTimeSince(robotState.getButtonTime(BodyPercept::chin)) < 500
    )
    );
}

---