Modules/BehaviorControl/KickLogger.cpp

Go to the documentation of this file.

/**
* @file KickLogger.cpp
* 
* Implementation of class KickLogger.
*
* @author Martin Lötzsch
*/

#include "KickLogger.h"
#include "Platform/SystemCall.h"
#include "Platform/GTAssert.h"
#include "Tools/Math/Geometry.h"

KickLogger::KickLogger(BehaviorControlInterfaces& interfaces)
: timeOfLastStateChange(SystemCall::getCurrentSystemTime()),
BehaviorControlInterfaces(interfaces), state(initial)
{
}

KickLogger::~KickLogger()
{
}

void KickLogger::execute()
{
  switch (state)
  {
  case initial:
    if (motionRequest.motionType == MotionRequest::specialAction
      && SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen) < 500
      && Geometry::distanceTo(robotPose.getPose(),ballModel.seen.positionField) < 350)
    {
      kick.ballXR = getBallX(robotPose);
      kick.ballYR = getBallY(robotPose);
      kick.ballTR = SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen);
      kickID = motionRequest.specialActionRequest.specialActionType;
      odometryDataWhenKickWasStarted = odometryData;
      changeState(kickRequested);
    }
    break;

  case kickRequested:
    if (motionInfo.executedMotionRequest.motionType == MotionRequest::specialAction)
    {
      Pose2D odometryDiff = odometryData- odometryDataWhenKickWasStarted;
      Pose2D relativePoseWhenStarted = robotPose.minusDiff(odometryDiff);

      kick.ballXS = getBallX(relativePoseWhenStarted);
      kick.ballYS = getBallY(relativePoseWhenStarted);
      kick.ballTS = SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen);
      changeState(kickStarted);
    }
    break;

  case kickStarted:
    if (motionInfo.executedMotionRequest.motionType != MotionRequest::specialAction)
    {
      changeState(kickFinished);
    }
    break;

  case kickFinished:
    if (motionInfo.executedMotionRequest.motionType == MotionRequest::specialAction)
    {
      changeState(wait);
    }
    if (SystemCall::getTimeSince(timeOfLastStateChange) > 2000)
    {
      changeState(waitingForBall);
    }
    break;

  case waitingForBall:
    if (motionInfo.executedMotionRequest.motionType == MotionRequest::specialAction)
    {
      changeState(wait);
    }
    if (SystemCall::getTimeSince(ballModel.seen.timeWhenLastSeen) < 100)
    {
#ifdef USE_CEILING_CAM
      kick.ballXA = gtCamWorldState.getBallModel().seen.positionRobot.x;
      kick.ballYA = gtCamWorldState.getBallModel().seen.positionRobot.x;
#else
      Pose2D odometryDiff = odometryData - odometryDataWhenKickWasStarted;
      Pose2D relativePoseWhenStarted = robotPose.minusDiff(odometryDiff);
      kick.ballXA = getBallX(relativePoseWhenStarted);
      kick.ballYA = getBallY(relativePoseWhenStarted);
#endif
      kick.tA = SystemCall::getTimeSince(timeOfLastStateChange) + 2000;
      record();
      changeState(wait);
    }
    if (SystemCall::getTimeSince(timeOfLastStateChange) > 4000)
    {
      changeState(wait);
    }
    break;

  case wait:
    if (SystemCall::getTimeSince(timeOfLastStateChange) > 3000
      && (motionInfo.executedMotionRequest.motionType == MotionRequest::walk
      || motionInfo.executedMotionRequest.motionType == MotionRequest::stand))
    {
      changeState(initial);
    }
    break;

  default:
    ASSERT(false);
  }
}

double KickLogger::getBallX(Pose2D from)
{
  return Geometry::distanceTo(from,ballModel.seen.positionField) 
    * cos(Geometry::angleTo(from,ballModel.seen.positionField));
}

double KickLogger::getBallY(Pose2D from)
{
  return Geometry::distanceTo(from,ballModel.seen.positionField) 
    * sin(Geometry::angleTo(from,ballModel.seen.positionField));
}

void KickLogger::changeState(State newState)
{
  timeOfLastStateChange = SystemCall::getCurrentSystemTime();
  state = newState;
//  OUTPUT(idText,text,"KickLogger: changed state to " << state << ".");
}

void KickLogger::record()
{
  INFO(sendKickRecords, idKickRecord, bin, SpecialActionRequest::getSpecialActionIDName(kickID) << kick);
}

---