bosnich.cc

该文件是包含了机器人足球比赛中的整个系统的代码

#include "Bosnich.h"
#include "../Common/Common.h"
#include "../TOOLS/BallTricks.h"
#include "../TOOLS/BasicTricks.h"
#include "../TOOLS/KeeperTricks.h"
#include "../TOOLS/HeadTricks.h"
#include "../TOOLS/ComplexTricks.h"
#include "../TOOLS/MovementTricks.h"
#include "../Globals.h"
#include "../Common/LocomotionCommand.h"

// Keeper test dude.
Bosnich::Bosnich() {
  configuration_.ParseFile("/ms/open-r/mw/data/bosnich.cfg");

  debugOutput = configuration_.GetAsBool("BOS_DebugOutput");

  chaseUpperX = configuration_.GetAsInt("BOS_ChaseUpperX");
  chaseUpperY = configuration_.GetAsInt("BOS_ChaseUpperY");
  chaseLowerX = configuration_.GetAsInt("BOS_ChaseLowerX");
  chaseLowerY = configuration_.GetAsInt("BOS_ChaseLowerY");

  homeX = configuration_.GetAsInt("BOS_HomeX");
  homeY = configuration_.GetAsInt("BOS_HomeY");
  homeRadius = configuration_.GetAsInt("BOS_HomeRadius");
  homeErrorDistance = configuration_.GetAsInt("BOS_HomeErrorDistance");

  currentTrick = new BasicTricks::NullBody();
  headTrick = new BasicTricks::NullHead();
  inPlaying = false;
}

Bosnich::~Bosnich() {
  delete currentTrick;
  delete headTrick;
}

void Bosnich::NewReadyModel() {
  if (inPlaying) {
    delete headTrick;
    headTrick = new HeadTricks::HeadPan(93,-93,3,50);
    headTrick->Start();

    delete currentTrick;
    currentTrick = new BasicTricks::NullBody();
    currentTrick->Start();
  }

  int hC = headTrick->Continue();
  if (hC < 1) {
    delete headTrick;
    headTrick = new HeadTricks::HeadPan(93,-93,3,50);
    headTrick->Start();
  }

  inPlaying = false;
}

void Bosnich::NewPlayingModel() {
  if (!inPlaying) {
    delete headTrick;
    headTrick = new BasicTricks::NullHead();
    headTrick->Start();

    delete currentTrick;
    currentTrick = new BasicTricks::NullBody();
    currentTrick->Start();
  }
  inPlaying=true;

  int hC = headTrick->Continue();
  if (hC < 1) {
    if (currentTrick->IsUsingHead()) {
      delete headTrick;
      headTrick = new BasicTricks::NullHead();
    } else {
      if (IsBallVisible()) {
          headTrick->Abort();
          delete headTrick;
          headTrick = new HeadTricks::FollowBallWithHeadSticky(25);
        } else {
          headTrick->Abort();
          delete headTrick;
          headTrick = new HeadTricks::HeadPan(93,-93,3,50);
        }
    }
    headTrick->Start();
  }

  int cC = currentTrick->Continue();
  //executing normally. we may wish to interrupt.
  if (cC >= 1) {
    // we're chasing, but we're allowed to interrupt.
    if ((strcmp(currentTrick->GetName(),"AdvancedChaseWithKick")==0) && (strcmp(currentTrick->GetErrorMsg(cC),"INTERRUPTIBLE")==0)) {
      // exceeded chase bounds ! back off !!! (note- Chase will terminate if ball is not visible - so this is ok)
      if (IsBallWithinChaseBounds() == false) {
        if (debugOutput)
          cout << "Bosnich: Aborted ChaseWithKick (chase bound exceeded)." << endl << flush;
        // change to move home
        currentTrick->Abort();
        delete currentTrick;
        currentTrick = new MovementTricks::MoveToPoint(homeX,homeY,0,MovementTricks::MoveToPoint::MTP_DYNAMIC);

        // look around for beacons !
        if (IsBallVisible()) {
          headTrick->Abort();
          delete headTrick;
          headTrick = new HeadTricks::FollowBallWithHeadSticky(25);
        } else {
          headTrick->Abort();
          delete headTrick;
          headTrick = new HeadTricks::HeadPan(93,-93,3,50);
        }
        headTrick->Start();
      }
    // currently moving home or searching.
    } else if (strcmp(currentTrick->GetName(),"MoveToPoint")==0) {
      // we can see the ball !!
      if (vo_ball_ != NULL) {
        // ball is within our chase area !
        if (IsBallVisible() && IsBallWithinChaseBounds()) {
          if (debugOutput)
            cout << "Bosnich: Ball within chase area. Aborting SearchForBall/MoveToPoint and chasing." << endl << flush;
          currentTrick->Abort();  
          delete currentTrick;
          currentTrick = new ComplexTricks::AdvancedChaseWithKick();
          currentTrick->Start();

          headTrick->Abort();
          delete headTrick;
          headTrick = new BasicTricks::NullHead();
          headTrick->Start();
        } else {
          double destX;
          double destY;
          if (wo_ball_->y_ > chaseUpperY) {
            double theta = Normalise_PI((atan2(wo_ball_->y_-homeY, homeX-wo_ball_->x_) - (PI / 2.0)));
            destX = homeX+homeRadius*sin(theta)*2.4;
            destY = homeY+homeRadius*cos(theta)*0.30;
            destX = CROP(destX,-25,25);
            destY = CROP(destY,-210,-195);
          } else {
            destX = CROP(wo_ball_->x_,-25,25);
            destY = -210; // homeY+homeRadius*sin(theta);
          }
          ((MovementTricks::MoveToPoint*)(currentTrick))->ResetHome((int)destX,(int)destY);    
//        cout << "DX: " << destX << ", DY: " << destY << endl << flush;
          // we're executing MoveToPoint and we can see the ball - but we're not allowed to chase it.
          // need to change MoveToPoint so we move to a different point (eg, face ball style)
          // look around for beacons !
        if (IsBallVisible()) {
          headTrick->Abort();
          delete headTrick;
          headTrick = new HeadTricks::FollowBallWithHeadSticky(25);
        } else {
          headTrick->Abort();
          delete headTrick;
          headTrick = new HeadTricks::HeadPan(93,-93,3,50);
        }
        headTrick->Start();
        }
      }
    }
  // move to point exited. start moving again ?
  } else if (cC < 1) {
    if (IsBallVisible() && IsBallWithinChaseBounds()) {
      if (debugOutput)
        cout << "Bosnich: Ball within chase area, previous trick has ended. Chasing." << endl << flush;
      delete currentTrick;
      currentTrick = new ComplexTricks::AdvancedChaseWithKick();
      currentTrick->Start();

      headTrick->Abort();
      delete headTrick;
      headTrick = new BasicTricks::NullHead();
      headTrick->Start();
    } else {
      // if self is within chase bounds, and we did NOT just search, and the ball is not visible .. spin around !
      if ( (strcmp(currentTrick->GetName(), "SearchForBall")!=0) && (IsBallVisible() == false) && (IsSelfWithinChaseBounds()) ) {
        if (debugOutput)
          cout << "Bosnich: Searching for ball." << endl << flush;
        delete currentTrick;

        bool turnRight = true;
        if (lastBallHeading_ > 0) {
          turnRight = false;
        }
        currentTrick = new MovementTricks::SearchForBall(turnRight, 360.0);
        currentTrick->Start();

        delete headTrick;
        headTrick = new BasicTricks::NullHead();
        headTrick->Start();
      } else {
        // if either a) we've just searched, b) the ball is visible but out of bounds or c) WE are out of bounds, then go home!
        delete currentTrick;
        currentTrick = new MovementTricks::MoveToPoint(homeX,homeY,0,MovementTricks::MoveToPoint::MTP_DYNAMIC);
        currentTrick->Start();
        // look around for beacons !
     
        if (IsBallVisible()) {
          headTrick->Abort();
          delete headTrick;
          headTrick = new HeadTricks::FollowBallWithHeadSticky(25);
        } else {
          headTrick->Abort();
          delete headTrick;
          headTrick = new HeadTricks::HeadPan(93,-93,3,50);
        }
        headTrick->Start();
      }
    }
  }
}

bool Bosnich::IsSelfWithinChaseBounds() {
  if ((wo_self_->x_ <= chaseUpperX) && (wo_self_->x_ >= chaseLowerX) && (wo_self_->y_ <= chaseUpperY) && (wo_self_->y_ >= chaseLowerY)) {
    return true;
  }
  return false;
}

bool Bosnich::IsBallWithinChaseBounds() {
  if ((wo_ball_->x_ <= chaseUpperX) && (wo_ball_->x_ >= chaseLowerX) && (wo_ball_->y_ <= chaseUpperY) && (wo_ball_->y_ >= chaseLowerY)) {
    return true;
  }
  return false;
}

bool Bosnich::IsBallVisible() {
  return (vo_ball_ != NULL);
}