Modules/BehaviorControl/GT2004BehaviorControl/GT2004Symbols/OpenChallengeSymbols.cpp

Go to the documentation of this file.

/** 
* @file OpenChallengeSymbols.cpp
*
* Implementation of class OpenChallengeSymbols.
*
* @author 
*/

#include "OpenChallengeSymbols.h"
#include "Tools/Math/Geometry.h"
#include "Tools/FieldDimensions.h"

// this will be changed until release...
OCSetStatus2 ocstate(dtt_initial);
bool g_changetorip(false);//hack for swich fast from GT2004 ImgeProcessor to RIP

OpenChallengeSymbols::OpenChallengeSymbols(const BehaviorControlInterfaces& interfaces)
: BehaviorControlInterfaces(interfaces)
{
  ocStatus = initial;

  m_walkRequest.type = 0;
  m_walkRequest.x = 0;
  m_walkRequest.y = 0;
  m_walkRequest.rotation = 0;

  int i;
  for (i = 0; i < BitePoint::numOfPositions; ++i)
    bitePointReady[i] = false;

  // mapping player numbers to bitePoints
  // change mapping here, if u want to
  bitePointToUse[Player::one]   = BitePoint::frontleft;
  bitePointToUse[Player::two]   = BitePoint::frontright;
  bitePointToUse[Player::three] = BitePoint::behindleft;
  bitePointToUse[Player::four]  = BitePoint::behindright;
  // masterdog doesnt use this... just for integrity
  bitePointToUse[Player::five]  = BitePoint::master;
  masterDog = Player::five;
  bitePointReady[BitePoint::master] = true;

  learnAcc = 0;
  groundVal.reset(50);
  ambVal = 0;

}


void OpenChallengeSymbols::registerSymbols(Xabsl2Engine& engine)
{
  engine.registerDecimalInputSymbol("open-challenge.walk-request.type",&(m_walkRequest.type));
  engine.registerDecimalInputSymbol("open-challenge.walk-request.x",&(m_walkRequest.x));
  engine.registerDecimalInputSymbol("open-challenge.walk-request.y",&(m_walkRequest.y));
  engine.registerDecimalInputSymbol("open-challenge.walk-request.rotation",&(m_walkRequest.rotation));

  engine.registerDecimalInputSymbol("open-challenge.headrot",&(m_headrot));

  // set dtt stati
  engine.registerEnumeratedOutputSymbol("open-challenge.set-status-dtt",this, (void (Xabsl2FunctionProvider::*)(int))&OpenChallengeSymbols::setOCStatusDtt);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status-dtt","open-challenge.set-dtt-initial", dtt_initial);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status-dtt","open-challenge.set-dtt-start",dtt_start);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status-dtt","open-challenge.set-dtt-finishedIntro",dtt_finishedIntro);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status-dtt","open-challenge.set-dtt-finishedGoToBridge",dtt_finishedGoToBridge);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status-dtt","open-challenge.set-dtt-finishedClimbBridge",dtt_finishedClimbBridge);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status-dtt","open-challenge.set-dtt-finishedWalkWithBridge",dtt_finishedWalkWithBridge);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status-dtt","open-challenge.set-dtt-finishedGoToBitePoint",dtt_finishedGoToBitePoint);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status-dtt","open-challenge.set-dtt-finishedBite",dtt_finishedBite);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status-dtt","open-challenge.set-dtt-LoseBitePoints",dtt_loseBitePoints);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status-dtt","open-challenge.set-dtt-finishedExtro",dtt_finishedExtro);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status-dtt","open-challenge.set-dtt-finished",dtt_finished);

  // bridge.last-seen-side 
  engine.registerEnumeratedInputSymbol("open-challenge.bridge.last-seen-side", this,
    (int (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getLastSeenSide);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.bridge.last-seen-side", "open-challenge.bridge.seen-none",  OCBridge::none);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.bridge.last-seen-side", "open-challenge.bridge.seen-left",  OCBridge::left);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.bridge.last-seen-side", "open-challenge.bridge.seen-right", OCBridge::right);

  // bridge.seen.distance
  engine.registerDecimalInputSymbol("open-challenge.bridge.seen.distance",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getSeenDistanceToBridge);

  // bridge.seen.angle
  engine.registerDecimalInputSymbol("open-challenge.bridge.seen.angle",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getSeenAngleToBridge);

  // bridge.time-since-last-seen
  engine.registerDecimalInputSymbol("open-challenge.bridge.time-since-last-seen",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getTimeSinceLastSeenBridge);

  // bite-point.position 
  engine.registerEnumeratedInputSymbol("open-challenge.bite-point.position", this,
    (int (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getBitePointPosition);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.bite-point.position", "open-challenge.bite-point.frontleft",   BitePoint::frontleft);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.bite-point.position", "open-challenge.bite-point.frontright",  BitePoint::frontright);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.bite-point.position", "open-challenge.bite-point.behindleft",  BitePoint::behindleft);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.bite-point.position", "open-challenge.bite-point.behindright", BitePoint::behindright);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.bite-point.position", "open-challenge.bite-point.master", BitePoint::master);

  // bite-point.seen.distance
  engine.registerDecimalInputSymbol("open-challenge.bite-point.seen.distance",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getSeenDistanceToBitePoint);

  // bite-point.seen.angle
  engine.registerDecimalInputSymbol("open-challenge.bite-point.seen.angle",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getSeenAngleToBitePoint);

  // bite-point.precise-shift
  engine.registerDecimalInputSymbol("open-challenge.bite-point.precise-shift",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getPreciseShiftToBitePoint);

  // bite-point.time-since-last-seen
  engine.registerDecimalInputSymbol("open-challenge.bite-point.time-since-last-seen",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getTimeSinceLastSeenBitePoint);

  // open-challenge.status 
  engine.registerEnumeratedInputSymbol("open-challenge.status",(int*)&ocStatus);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.status", "open-challenge.initial",            initial);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.status", "open-challenge.intro",              intro);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.status", "open-challenge.goToBridge",         goToBridge);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.status", "open-challenge.goToBitePoint",      goToBitePoint);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.status", "open-challenge.allBitePointsReady", allBitePointsReady);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.status", "open-challenge.walkWithBridge",     walkWithBridge);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.status", "open-challenge.extro",              extro);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.status", "open-challenge.finished",           finished);

  // "open-challenge.set-status"
  engine.registerEnumeratedOutputSymbol("open-challenge.set-status",this,
    (void (Xabsl2FunctionProvider::*)(int))&OpenChallengeSymbols::setOCStatus);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status","open-challenge.set-initial",            setInitial);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status","open-challenge.set-intro",              setIntro);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status","open-challenge.set-goToBridge",         setGoToBridge);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status","open-challenge.set-goToBitePoint",      setGoToBitePoint);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status","open-challenge.set-myBitePointIsReady", setMyBitePointIsReady);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status","open-challenge.set-lostMyBitePoint",    setLostMyBitePoint);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status","open-challenge.set-walkWithBridge",     setWalkWithBridge);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status","open-challenge.set-extro",              setExtro);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-status","open-challenge.set-finished",           setFinished);

  // RedLine
  engine.registerDecimalInputSymbol("open-challenge.time-since-red-line-last-seen",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getTimeSinceLastSeenRedLine);
  engine.registerDecimalInputSymbol("open-challenge.red-line-angle-in-image", this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::redLineInImageAngle);
  engine.registerDecimalInputSymbol("open-challenge.red-line-angle", this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::redLineAngle);
  engine.registerDecimalInputSymbol("open-challenge.ramp.lowest-line-point.distance", this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getSeenDistanceToRedLine);
  
  // psd data
  engine.registerDecimalInputSymbol("open-challenge.headpsd.near",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getSensorPSDNear);
  engine.registerDecimalInputSymbol("open-challenge.headpsd.far",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getSensorPSDFar);
  engine.registerDecimalInputSymbol("open-challenge.distance-to-sifoc",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getDistanceToSIFOC);
  
  // acceleration data
  engine.registerDecimalInputSymbol("open-challenge.accelerator.x",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getSensorAccelerationX);
  engine.registerDecimalInputSymbol("open-challenge.accelerator.y",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getSensorAccelerationY);
  engine.registerDecimalInputSymbol("open-challenge.accelerator.z",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getSensorAccelerationZ);

  // learning acceleration data
  engine.registerEnumeratedOutputSymbol("open-challenge.learning", (int*)&learnAcc);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.learning","open-challenge.learn-on",   1);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.learning","open-challenge.learn-none", 0);

  // enable disable RIP
  engine.registerEnumeratedOutputSymbol("open-challenge.set-image",this, (void (Xabsl2FunctionProvider::*)(int))&OpenChallengeSymbols::setRIP);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-image","open-challenge.disable-RIP", unchanged);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-image","open-challenge.enable-RIP",rip);

  // ramp
  engine.registerDecimalInputSymbol("open-challenge.on-ramp",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getAiboOnRamp);
  engine.registerDecimalInputSymbol("ramp.angle-of-line",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getRampAngleOfRedLine);
  engine.registerDecimalInputSymbol("ramp.lowest-line-point.x",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getRampLowestLinePointX);
  engine.registerDecimalInputSymbol("open-challenge.ramp.lowest-line-point.y",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getRampLowestLinePointY);
  engine.registerDecimalInputSymbol("ramp.highest-line-point.x",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getRampHighestLinePointX);
  engine.registerDecimalInputSymbol("ramp.highest-line-point.y",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getRampHighestLinePointY);
  engine.registerDecimalInputSymbol("ramp.lowest-line-point.distance",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getRampDistanceToStartPoint);
  engine.registerDecimalInputSymbol("open-challenge.ramp.time-since-last-seen",this,
    (double (Xabsl2FunctionProvider::*)())&OpenChallengeSymbols::getRampTimeSinceLastSeen);

  // set-already-moved-bridge
  engine.registerEnumeratedOutputSymbol("open-challenge.set-already-moved-bridge", (int*)&ambVal);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-already-moved-bridge","open-challenge.set-amb-yes",   1);
  engine.registerEnumeratedOutputSymbolEnumElement("open-challenge.set-already-moved-bridge","open-challenge.set-amb-no" ,   0);

  // get already-moved-bridge
  engine.registerEnumeratedInputSymbol("open-challenge.already-moved-bridge",(int*)&ambVal);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.already-moved-bridge", "open-challenge.amb-yes", 1);
  engine.registerEnumeratedInputSymbolEnumElement("open-challenge.already-moved-bridge", "open-challenge.amb-no",  0);



}

void OpenChallengeSymbols::update()
{
  //verdrehung des aibos
   m_headrot = toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::headPan]));
  //lernen für die gyros...
  if (learnAcc == 1)
  {
    groundVal.add(xspeed.getAverage());
    groundVal.trigger();
  }

  double phi(0);
  double x,y,r;
//  double l(35),b(25);
  double l(300),b(350);
  // Vorzeichen l,b  hängt vom Hund ab
  
  int playnum = getPlayer().getPlayerNumber();
  bool left = (bitePointToUse[playnum] == BitePoint::frontleft)
            | (bitePointToUse[playnum] == BitePoint::behindleft);
  bool front = (bitePointToUse[playnum] == BitePoint::frontleft)
            | (bitePointToUse[playnum] == BitePoint::frontright);
  
  Vector2<double> pos, nexpos;

  if (!left)
    b *= -1;  
  if (!front)
    l *= -1;

  int ii = 0;
  for (int j = 0; j< Player::numOfPlayerNumbers; j++)
  {
    if ( teamMessageCollection[j].playerNumberOfSender == masterDog)
      ii = j;
  }

  m_walkRequest.type 
    = teamMessageCollection[ii].behaviorTeamMessage.walkRequest.type;
  x = teamMessageCollection[ii].behaviorTeamMessage.walkRequest.x;
  y = teamMessageCollection[ii].behaviorTeamMessage.walkRequest.y;
  r = -(teamMessageCollection[ii].behaviorTeamMessage.walkRequest.rotation);

  r = fromDegrees(r);

  Vector2<double> s(0,0),srtmp(1,0), sr(1,0);
  sr.normalize();

  phi = sr.angle();
  pos = s+(sr*l+sr.rotateLeft()*b);
  sr.rotateRight();
//  sr.x = 1;sr.y = 0;

  s.x = x;
  s.y = y;
  srtmp.x = sr.x * cos(r)+sr.y*sin(r);
  srtmp.y = sr.y * cos(r)-sr.x*sin(r);
  sr = srtmp;
  nexpos = s+(sr*l+sr.rotateLeft()*b);
  sr.rotateRight();

  pos = nexpos-pos;
  
  if (left)
    pos.rotateLeft();
  else
    pos.rotateRight();
    
  r = sr.angle()-phi;

  r = toDegrees(r);

  m_walkRequest.x = pos.x;
  m_walkRequest.y = pos.y;
  m_walkRequest.rotation = r;


  // send ocStatus
  {
  for (int i = 0; i < Player::numOfPlayerNumbers-1; ++i)
    if ((teamMessageCollection[i].isActual()) &&
        (teamMessageCollection[i].playerNumberOfSender != Player::undefinedPlayerNumber))
    {
      if (teamMessageCollection[i].playerNumberOfSender == masterDog)
        switch (teamMessageCollection[i].behaviorTeamMessage.ocStatus)
        {
          case setInitial        : ocStatus = initial;        break;
          case setIntro          : ocStatus = intro;          break;
          case setGoToBridge     : ocStatus = goToBridge;     break;
          case setGoToBitePoint  : ocStatus = goToBitePoint;  break;
          case setWalkWithBridge : ocStatus = walkWithBridge; break;
          case setExtro          : ocStatus = extro;          break;
          case setFinished       : ocStatus = finished;       break;
        }

      if (teamMessageCollection[i].behaviorTeamMessage.ocStatus == setMyBitePointIsReady)
        bitePointReady[bitePointToUse[teamMessageCollection[i].playerNumberOfSender]] = true;
      if (teamMessageCollection[i].behaviorTeamMessage.ocStatus == setLostMyBitePoint)
        bitePointReady[bitePointToUse[teamMessageCollection[i].playerNumberOfSender]] = false;
    }
  }

  int allReady = 1;
  int k;
  for (k = 0; k < BitePoint::numOfPositions; ++k)
    allReady *= bitePointReady[k];
  if ((allReady) && 
      (ocStatus == goToBitePoint))
    ocStatus = allBitePointsReady;
  if ((!allReady) &&
      ((ocStatus == allBitePointsReady) || (ocStatus == walkWithBridge)))
    ocStatus = goToBitePoint;

  /** normally the following stuff should be placed inside an BridgeLocator module.. 
      so this is near to a hack ;-) */

  SpecialPercept& sp = const_cast<SpecialPercept&>(specialPercept);
  if (sp.ocBridge.fresh) {
    sp.ocBridge.fresh = false;
  } else {
    sp.ocBridge.relPos =
      (sp.ocBridge.lastOdometry + Pose2D(sp.ocBridge.relPos) - odometryData).translation;
    sp.ocBridge.distanceTo = sp.ocBridge.relPos.abs();
    sp.ocBridge.angleTo = 
      (sgn(sp.ocBridge.relPos.y) * asin(fabs(sp.ocBridge.relPos.y)/(fabs(sp.ocBridge.distanceTo)+1))) +
      (odometryData.rotation - sp.ocBridge.lastOdometry.rotation);
  }
  sp.ocBridge.lastOdometry = odometryData;
  for (k = 0; k < BitePoint::numOfPositions; ++k)
  {
    if (sp.ocBridge.bitePoint[k].fresh) {
      sp.ocBridge.bitePoint[k].fresh = false;
    } else {
      sp.ocBridge.bitePoint[k].relPos =
        (sp.ocBridge.bitePoint[k].lastOdometry + Pose2D(sp.ocBridge.bitePoint[k].relPos) - odometryData).translation;  
      sp.ocBridge.bitePoint[k].distanceTo = sp.ocBridge.bitePoint[k].relPos.abs();
      sp.ocBridge.bitePoint[k].angleTo =
        (sgn(sp.ocBridge.bitePoint[k].relPos.y) * asin(fabs(sp.ocBridge.bitePoint[k].relPos.y)/(fabs(sp.ocBridge.bitePoint[k].distanceTo)+1))) +
        (odometryData.rotation - sp.ocBridge.bitePoint[k].lastOdometry.rotation);
                                         
    }
    sp.ocBridge.bitePoint[k].lastOdometry = odometryData;
  }
/*
 if(ocStatus == initial)
 {
    //just for fun
    int ii = 0;
    for (int j = 0; j< Player::numOfPlayerNumbers; j++)
    {
      if ( teamMessageCollection[j].playerNumberOfSender == masterDog)
        ii = j;
    }
    m_walkRequest.type 
      = teamMessageCollection[ii].behaviorTeamMessage.walkRequest.type;
    m_walkRequest.x = teamMessageCollection[ii].behaviorTeamMessage.walkRequest.x;
    m_walkRequest.y = teamMessageCollection[ii].behaviorTeamMessage.walkRequest.y;
    m_walkRequest.rotation = teamMessageCollection[ii].behaviorTeamMessage.walkRequest.rotation;
 }
*/
}

OCBridge::Side OpenChallengeSymbols::getLastSeenSide()    
{
  return specialPercept.ocBridge.lastSeenSide;
}

double OpenChallengeSymbols::getSeenDistanceToBridge()
{
  return specialPercept.ocBridge.distanceTo;
}    

double OpenChallengeSymbols::getSeenAngleToBridge()
{
  return toDegrees(specialPercept.ocBridge.angleTo);
}

double OpenChallengeSymbols::getTimeSinceLastSeenBridge() 
{
  return SystemCall::getTimeSince(specialPercept.ocBridge.timeWhenLastSeen);
}

BitePoint::Position OpenChallengeSymbols::getBitePointPosition() 
{
  return specialPercept.ocBridge.bitePoint[bitePointToUse[getPlayer().getPlayerNumber()]].position;
}

double OpenChallengeSymbols::getSeenDistanceToBitePoint()        
{
  return specialPercept.ocBridge.bitePoint[bitePointToUse[getPlayer().getPlayerNumber()]].distanceTo;
}

double OpenChallengeSymbols::getSeenAngleToBitePoint()           
{
  return toDegrees(specialPercept.ocBridge.bitePoint[bitePointToUse[getPlayer().getPlayerNumber()]].angleTo);
}

double OpenChallengeSymbols::getPreciseShiftToBitePoint()        
{
  return specialPercept.ocBridge.bitePoint[bitePointToUse[getPlayer().getPlayerNumber()]].preciseShift;
}

double OpenChallengeSymbols::getTimeSinceLastSeenBitePoint()     
{
  return SystemCall::getTimeSince(specialPercept.ocBridge.bitePoint[bitePointToUse[getPlayer().getPlayerNumber()]].timeWhenLastSeen);
}

void OpenChallengeSymbols::setOCStatus(int aStatus)
{
  outgoingBehaviorTeamMessage.ocStatus = aStatus;
  if (getPlayer().getPlayerNumber() == masterDog)
    switch (aStatus)
    {
      case setInitial        : ocStatus = initial;        break;
      case setIntro          : ocStatus = intro;          break;
      case setGoToBridge     : ocStatus = goToBridge;     break;
      case setGoToBitePoint  : ocStatus = goToBitePoint;  break;
      case setWalkWithBridge : ocStatus = walkWithBridge; break;
      case setExtro          : ocStatus = extro;          break;
      case setFinished       : ocStatus = finished;       break;
    }
  else
    switch (aStatus)
    {
      case setMyBitePointIsReady : bitePointReady[bitePointToUse[getPlayer().getPlayerNumber()]] = true;  break;
      case setLostMyBitePoint    : bitePointReady[bitePointToUse[getPlayer().getPlayerNumber()]] = false; break;
    }
}

// PSD
//----------------------------------------------------------------------------
double OpenChallengeSymbols::getSensorPSDNear()
{
  double tmpdata = 0;
  for (int i = 0; i < sensorDataBuffer.numOfFrames; ++i)
    tmpdata += sensorDataBuffer.frame[i].data[SensorData::headPsdNear];
  if (sensorDataBuffer.numOfFrames !=0) 
    tmpdata /= sensorDataBuffer.numOfFrames;
  else
    tmpdata=0;
  return tmpdata;
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::getSensorPSDFar()
{
  double tmpdata = 0;
  for (int i = 0; i < sensorDataBuffer.numOfFrames; ++i)
    tmpdata += sensorDataBuffer.frame[i].data[SensorData::headPsdFar];
  if (sensorDataBuffer.numOfFrames !=0) 
    tmpdata /= sensorDataBuffer.numOfFrames;
  else
    tmpdata=0;
  return tmpdata;
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::getDistanceToSIFOC()
{
  double value = 50;
  double psdval = 0;
  for (int i = 0; i < psdPercept.numOfPercepts; ++i)
    psdval += psdPercept[i].body;
  psdval /= (double)(psdPercept.numOfPercepts);
  if (psdval >= 170)
    psdval = 170;

  // from 160 to 170 equals from 1cm to 0cm
  if (psdval >= 160)
    value = 10-(psdval-160);
  else
  // from 145 to 160 equals from 2cm to 1cm
  if (psdval >= 145)
    value = ((15-(psdval-145))/1.5)+10;
  else
  // from 130 to 145 equals from 4cm to 2cm
  if (psdval >= 130)
    value = ((15-(psdval-130))/0.75)+20;

  return value;
}

// acceleration
//----------------------------------------------------------------------------
double OpenChallengeSymbols::getSensorAccelerationX()
{
  double tmpdata = 0;
  for (int i = 0; i < sensorDataBuffer.numOfFrames; ++i)
    tmpdata += toDegrees(fromMicroRad(sensorDataBuffer.frame[i].data[SensorData::accelerationX]));
  if (sensorDataBuffer.numOfFrames !=0) 
    tmpdata /= sensorDataBuffer.numOfFrames;
  else
    tmpdata=0;
  return tmpdata;
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::getSensorAccelerationY()
{
  double tmpdata = 0;
  for (int i = 0; i < sensorDataBuffer.numOfFrames; ++i)
    tmpdata += toDegrees(fromMicroRad(sensorDataBuffer.frame[i].data[SensorData::accelerationY]));
 if (sensorDataBuffer.numOfFrames !=0) 
   tmpdata /= sensorDataBuffer.numOfFrames;
 else
   tmpdata=0;
  return tmpdata;
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::getSensorAccelerationZ()
{
  double tmpdata = 0;
  for (int i = 0; i < sensorDataBuffer.numOfFrames; ++i)
    tmpdata += toDegrees(fromMicroRad(sensorDataBuffer.frame[i].data[SensorData::accelerationZ]));
  if (sensorDataBuffer.numOfFrames !=0) 
    tmpdata /= sensorDataBuffer.numOfFrames;
  else
    tmpdata=0;
  return tmpdata;
  return toDegrees(fromMicroRad(sensorDataBuffer.lastFrame().data[SensorData::accelerationZ]));
}

// IMAGE PROCESSSING INPUTS
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
double OpenChallengeSymbols::redLineInImageAngle()
{ 
  return specialPercept.ocRedLine.angleInImage-90;
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::redLineAngle()
{ 
  return toDegrees(specialPercept.ocRedLine.lineStart.angle());
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::getTimeSinceLastSeenRedLine()
{ 
  return SystemCall::getCurrentSystemTime()-specialPercept.ocRedLine.timeWhenLastSeen;
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::getSeenDistanceToRedLine()
{ 
  return specialPercept.ocRedLine.lineStart.abs();
}

// IMAGE PROCESSSING OUTPUTS
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
void OpenChallengeSymbols::setRIP(int t)
{
    switch(t)
    {
        case rip:
            g_changetorip = true;
            break;
        default:
            g_changetorip = false;
    }
}

// ramp functions
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
double OpenChallengeSymbols::getAiboOnRamp()
{
  xspeed.add(int(getSensorAccelerationX()));
  xspeed.trigger();
//  if (xspeed.getAverage()>(groundVal.getAverage()))
  if (xspeed.getAverage() > 40 )
    return 1;
  return 0;
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::getRampAngleOfRedLine()
{
  return specialPercept.ocRedLine.angleInImage;
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::getRampLowestLinePointX()
{
  return specialPercept.ocRedLine.lineStart.x;
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::getRampLowestLinePointY()
{
  return specialPercept.ocRedLine.lineStart.y;
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::getRampHighestLinePointX()
{
  return specialPercept.ocRedLine.lineEnd.x;
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::getRampHighestLinePointY()
{
  return specialPercept.ocRedLine.lineEnd.y;
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::getRampDistanceToStartPoint()
{
  return specialPercept.ocRedLine.lineStart.abs();
}

//----------------------------------------------------------------------------
double OpenChallengeSymbols::getRampTimeSinceLastSeen()
{
  return SystemCall::getTimeSince(specialPercept.ocRedLine.timeWhenLastSeen);
}

// dtt stati
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
void OpenChallengeSymbols::setOCStatusDtt(int aStatus)
{
  ocstate = (OCSetStatus2)aStatus;
}





/*
* Change Log
* 
* $Log: OpenChallengeSymbols.cpp,v $
* Revision 1.5  2004/08/09 13:51:27  hamerla
* import OpenChallen Behavior to WM
*
* Revision 1.4  2004/07/22 22:38:20  kerdels
* added DTT used by Open Challenge, RIP and Xabsl-Options will follow
*
* Revision 1.3  2004/06/16 14:42:24  risler
* renamed some open challenge symbols
*
* Revision 1.2  2004/06/01 13:58:14  schumann
* special percept type redLine added,
* part 1 of open challenge added
*
* Revision 1.1.1.1  2004/05/22 17:18:04  cvsadm
* created new repository GT2004_WM
*
* Revision 1.7  2004/05/21 16:32:43  hamerla
* no message
*
* Revision 1.6  2004/05/20 16:30:03  kerdels
* further work on open challenge
*
* Revision 1.5  2004/05/17 18:20:00  kerdels
* adjusted symbols for open challenge to enable free mapping of players to bitePoints- and controllerpositions
*
* Revision 1.4  2004/05/17 17:17:50  kerdels
* added symbols for the sequence control of the open challenge,
* fixed a bug concerning 5-dog-mode and distribution of option-ratings,
* set one = 0 in enum Player::playerNumber --> just to be sure ;-)
*
* Revision 1.3  2004/05/17 01:56:15  kerdels
* prepared some symbols for the open challenge bridge detection
*
* Revision 1.2  2004/05/13 22:22:41  hamerla
* OpenChallenge walk with bridge
*
* Revision 1.1  2004/05/08 16:18:14  hamerla
* Open Challenge
*
*
*/

---