getcapability.cc

2007年机器人足球世界杯3D仿真组亚军于07年底的源程序。该队还于2008年获得伊朗公开赛的亚军。

#include "cankickright.ih"

Behavior::ConfidenceInterval CanKickRight::getCapability(rf<Behavior::State> s, rf<Behavior::Goal> g)
{
  WorldModel& wm = WorldModel::getInstance();
  Vector3D ballPos = wm.getObjectPosition(Types::BALL).getMu();
  
  if (ballPos.getX() > 0 && ballPos.getX() < 1.3 && ballPos.getY() < 0 && ballPos.getY() > -0.7)
    return ConfidenceInterval(0.5, 0.2);
    
  return ConfidenceInterval(-1.0, 0.0);

  /*_debugLevel4("WalkStraightTo::getCapability");
  
  rf<StateVarNode> gameStateNode = rf_cast<StateVarNode>(s->findDeep("GameState"));
  if (!gameStateNode)
  {
    _debugLevel1("WalkStraightTo doesn't understand!");
    return ConfidenceInterval(-1.0, 0.0);
  }
    
  StateVar gameState = gameStateNode->getVar();
  
  WorldModel& wm = WorldModel::getInstance();
  NormalDistribution3D BallInfo = wm.getObjectPosition(Types::BALL);
  Vector3D ballPosCar = BallInfo.getMu();
  double ballX = ballPosCar.getX();
  double ballY = ballPosCar.getY();
  // Distance from foot to ball (not from camera)
  double distToBall = sqrt(ballX * ballX + ballY * ballY);
  Vector3D ballPosPol = wm.get(WorldModel::iVisionBall);
  double angleToBall = ballPosPol.getY() / M_PI * 180;
  double angle = 20.0;
  
  if (distToBall > 1.5 || angleToBall > angle || angleToBall < 0)
  {
  	_debugLevel3("Too far to kick.");
    return ConfidenceInterval(-1.0, 0.0);
  }
 	_debugLevel4("Capable");
  return ConfidenceInterval(0.5, 0.2);*/
}