---

AccelerateBallToVelocity.cc

Go to the documentation of this file.

/* -*- Mode: C++ -*-
 * $Id: AccelerateBallToVelocity.cc,v 1.4 2005/09/08 12:30:31 frehe Exp $
 *
 *
 * COPYRIGHT INFORMATION
 *
 * This file is part of RoboSoc created by Fredrik Heintz <frehe@ida.liu.se>
 * Copyright (C) 1999, 2000 Fredrik Heintz, Linköping University, Sweden
 *
 * You are allowed to modify and use this code as long as you retain this
 * notice. If you make any changes or have any comments I would appreciate
 * it if you send me a message. For more information, please see
 * http://www.ida.liu.se/~frehe/RoboCup/RoboSoc/
 *
 *
 * IDENTIFICATION
 *
 * Filename: AccelerateBallToVelocity.cc
 * Unitname: Framework/skill
 * $Revision: 1.4 $
 * Created by: Peter Andersson 2003-08-29
 * Last modified by $Author: frehe $ $Date: 2005/09/08 12:30:31 $
 *
 *
 * HISTORY
 *
 * $Log: AccelerateBallToVelocity.cc,v $
 * Revision 1.4  2005/09/08 12:30:31  frehe
 * Check the applicability in generateStep.
 *
 * Revision 1.3  2005/09/08 08:58:35  frehe
 * Implemented the reset method.
 *
 * Revision 1.2  2004/08/13 15:39:27  petan
 * Moved implementation to generateStep
 *
 * Revision 1.1  2003/09/03 14:08:43  aip19
 * Updated and documented primitive skills
 *
 */

#include "AccelerateBallToVelocity.h"
#include "view_constants.h"
#include "KickCommand.h"
#include "rs_error.h"


RS_BEGIN_NAMESPACE


// --- CONSTRUCTORS ---

AccelerateBallToVelocity::AccelerateBallToVelocity(const Vector& vec)
  : SkillWithVector(vec),
    mAgentView(NULL),
    mBallView(NULL),
    mKickBallView(NULL)
{
  getView(mAgentView, AGENT_VIEW_ID);
  getView(mBallView, BALL_VIEW_ID);
  getView(mKickBallView, KICKBALL_VIEW_ID);
}


// --- DESTRUCTOR ---

AccelerateBallToVelocity::~AccelerateBallToVelocity()
{
  viewManager->releaseView(AGENT_VIEW_ID);
  viewManager->releaseView(BALL_VIEW_ID);
  viewManager->releaseView(KICKBALL_VIEW_ID);
}


// --- METHODS ---

void AccelerateBallToVelocity::reset()
{
}

Plan AccelerateBallToVelocity::generatePlan()
{
  return Plan(generateStep());
}

AgentStep AccelerateBallToVelocity::generateStep()
{
  KickCommand *command;
  AgentStep step;
  
  if ( !applicable() ) {
    RS_WARNING("Tried to call AccelerateBallToVelocity which isn't applicable");
    return step;
  }
  
  Vector wantedBallVector = vector - mBallView->getSpeedVector().getVector();
  
  const WorldFacts* facts = WorldModelInterface::instance()->getWorldFacts();

  if (wantedBallVector.getLength() <= facts->SP_maxpower) {
    FloatUE power = mKickBallView->getKickPowerForSpeed(wantedBallVector.getLength());
    AngleDeg angle = wantedBallVector.getAngle() - mAgentView->getBodyDirection().getAngle();

    command = new KickCommand(power.getValue(), angle);
  }
  else {
    // Recompute vector to give ball maximum speed in desired angle
    wantedBallVector = mBallView->getSpeedVector().getVector();
    wantedBallVector.rotate(-vector.getAngle());
    Point point(wantedBallVector);

    Float length = facts->SP_maxpower * mKickBallView->kickPowerRate().getValue();
    AngleDeg angle = vector.getAngle() - asin(point.getY()/length);
    angle -= mAgentView->getBodyDirection().getAngle();

    command = new KickCommand(facts->SP_maxpower, angle);
  }

  step.setBodyCommand(command);
  return step;
}

FuzzyBool AccelerateBallToVelocity::persistent()
{
  // Never persistent.
  return FuzzyBool(0.0);
}

FuzzyBool AccelerateBallToVelocity::applicable()
{
  return mAgentView->getBodyDirection().isKnown() &&
    mBallView->getSpeedVector().isKnown() &&
    mBallView->isBallKickable();
}

FuzzyBool AccelerateBallToVelocity::succeed()
{
  // Has not succeeded yet
  return FuzzyBool(0.0);
}

FuzzyBool AccelerateBallToVelocity::failed()
{
  // Has not failed yet
  return FuzzyBool(0.0);
}



RS_END_NAMESPACE

---