regionplayer.cc

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

#include "RegionPlayer.h"
#include "../Common/Common.h"
#include "../TOOLS/BallTricks.h"
#include "../TOOLS/BasicTricks.h"
#include "../TOOLS/HeadTricks.h"
#include "../TOOLS/ComplexTricks.h"
#include "../TOOLS/MovementTricks.h"

// 2002 behaviour (essentially, anyway)
RegionPlayer::RegionPlayer() {
  configuration_.ParseFile("/ms/open-r/mw/data/regplay.cfg");

  debugOutput = configuration_.GetAsBool("DebugOutput");

  chaseUpperX = configuration_.GetAsInt("ChaseUpperX");
  chaseUpperY = configuration_.GetAsInt("ChaseUpperY");
  chaseLowerX = configuration_.GetAsInt("ChaseLowerX");
  chaseLowerY = configuration_.GetAsInt("ChaseLowerY");

  homeX = configuration_.GetAsInt("HomeX");
  homeY = configuration_.GetAsInt("HomeY");
  homeRadius = configuration_.GetAsInt("HomeRadius");
  homeErrorDistance = configuration_.GetAsInt("HomeErrorDistance");

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

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

void RegionPlayer::NewReadyModel() {
  if (inPlaying) {
    delete headTrick;
    headTrick = new HeadTricks::HeadPan(85,-85,3,50);   // MQ 19/6 .. changed from (70,-70,0,12)
    headTrick->Start();
    delete currentTrick;
    currentTrick = new BasicTricks::NullBody();
    currentTrick->Start();
  }

  int hC=headTrick->Continue();
//char* hMsg = headTrick->GetErrorMsg(hC);
  if (!currentTrick->IsUsingHead() && (hC < 1)) {
    headTrick = new HeadTricks::HeadPan(85,-85,3,50);   // MQ 19/6 .. changed from (70,-70,0,12)
    headTrick->Start();
  } else if (vo_ball_ != NULL && strcmp(headTrick->GetName(),"HeadPan")==0) {
    delete headTrick;
    headTrick = new HeadTricks::FollowBallWithHeadSticky(10);
    headTrick->Start();
  }

  inPlaying = false;
}

void RegionPlayer::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) {
    delete headTrick;
    headTrick = new BasicTricks::NullHead();
    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(),"ChaseWithKick")==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 << "RegionPlayer: 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 !
        headTrick->Abort();
        delete headTrick;
        headTrick = new HeadTricks::HeadPan(70,-70,0,12);
      }
    // 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 << "RegionPlayer: Ball within chase area. Aborting SearchForBall/MoveToPoint and chasing." << endl << flush;
          currentTrick->Abort();  
          delete currentTrick;
          currentTrick = new ComplexTricks::ChaseWithKick();
          currentTrick->Start();

          headTrick->Abort();
          delete headTrick;
          headTrick = new BasicTricks::NullHead();
          headTrick->Start();
        } else {
    
/*
          //FIXME !! this is r00ted. use brucelee.

          double theta = (atan2(wo_ball_->y_-homeY, wo_ball_->x_-homeX) - (PI / 2.0));
          double thetaOrig = theta;
          while (theta > PI) theta -= 2*PI;
          while (theta < -PI) theta += 2*PI;
          double destX = homeX+homeRadius*cos(theta);
          double destY = homeY+homeRadius*sin(theta);*/
         // should reset movetopoint so it moves to these coords

          //        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) [done]
        }
      } else if ( (pow(wo_self_->x_ - homeX, 2)+pow(wo_self_->y_ - homeY, 2)) < homeRadius*homeRadius) {
        currentTrick->Abort();
        delete currentTrick;
        currentTrick = new MovementTricks::SearchForBall(true, 360.0);
        currentTrick->Start();
      }
    }
  // move to point exited. start moving again ?
  } else if (cC < 1) {
    if (IsBallVisible() && IsBallWithinChaseBounds()) {
      if (debugOutput)
        cout << "RegionPlayer: Ball within chase area, previous trick has ended. Chasing." << endl << flush;
      delete currentTrick;
      currentTrick = new ComplexTricks::ChaseWithKick();
      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 << "RegionPlayer: 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();
        delete headTrick;
        headTrick = new HeadTricks::HeadPan(70,-70,0,12);;
        headTrick->Start();
      }
    }
  }

}

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

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