brucelee.cc

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

#include "BruceLee.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"
#include "../TOOLS/KeeperTricks.h"


// 2003 behaviour.
// "In this art, efficiency is anything that scores."
//    -Bruce Lee
BruceLee::BruceLee() {
  configuration_.ParseFile("/ms/open-r/mw/data/regplay.cfg");
  configuration_.ParseFile("/ms/open-r/mw/data/captain.cfg");
  timeOfLastChase = frame_;
  debugOutput = false;
  homeRadius = configuration_.GetAsInt("HomeRadius");
  homeErrorDistance = configuration_.GetAsInt("HomeErrorDistance");
  currentTrick = new BasicTricks::NullBody();
  headTrick = new BasicTricks::NullHead();
  inPlaying = false;
  inGoHome=false;

  lastBallDistance = 20000;

  keeperRoaming=false;

  myPosition = -1;
  for (int i=0; i<4; i++) {
    lastReceivedTime[i] = frame_;

    othersNormal[i].chasing = false;
    othersNormal[i].distance = 20000.0;
    othersCaptain.chasing=false;
    othersCaptain.distance = 20000.0;
    othersCaptain.position[i] = -1;
    othersCaptain.upperX[i] = 0;
    othersCaptain.upperY[i] = 0;
    othersCaptain.lowerX[i] = 0;
    othersCaptain.lowerY[i] = 0;
    othersCaptain.homeX[i] = 0;
    othersCaptain.homeY[i] = 0;
  }
  for (int i = 0; i < NUM_POSITIONS; i++) {
    robotRegions[i].upperX = 0;
    robotRegions[i].upperY = 0;
    robotRegions[i].lowerX = 0;
    robotRegions[i].lowerY = 0;
    robotRegions[i].homeX = 0;
    robotRegions[i].homeY = 0;
  }

  isTeamOnOffense = false;
  lastSwitchTime = 0;
  if (BOTID_ == configuration_.GetAsInt("CaptainID")) isCaptain=true; else isCaptain=false;

  if (isCaptain) cout << "CAPTAIN !" << endl << flush;

  if (configuration_.GetAsInt("WP_GOALKEEPER") != -1) {
    robotPosition[configuration_.GetAsInt("WP_GOALKEEPER")-1] = BL_GOALKEEPER;
  }
  if (configuration_.GetAsInt("WP_CENTREDEFENDER") != -1) {
    robotPosition[configuration_.GetAsInt("WP_CENTREDEFENDER")-1] = BL_CENTREDEFENDER;
  }
  if (configuration_.GetAsInt("WP_LEFTDEFENDER") != -1) {
    robotPosition[configuration_.GetAsInt("WP_LEFTDEFENDER")-1] = BL_LEFTDEFENDER;
  }
  if (configuration_.GetAsInt("WP_RIGHTDEFENDER") != -1) {
    robotPosition[configuration_.GetAsInt("WP_RIGHTDEFENDER")-1] = BL_RIGHTDEFENDER;
  }
  if (configuration_.GetAsInt("WP_CENTRESTRIKER") != -1) {
    robotPosition[configuration_.GetAsInt("WP_CENTRESTRIKER")-1] = BL_CENTRESTRIKER;
  }
  if (configuration_.GetAsInt("WP_LEFTSTRIKER") != -1) {
    robotPosition[configuration_.GetAsInt("WP_LEFTSTRIKER")-1] = BL_LEFTSTRIKER;
  }
  if (configuration_.GetAsInt("WP_RIGHTSTRIKER") != -1) {
    robotPosition[configuration_.GetAsInt("WP_RIGHTSTRIKER")-1] = BL_RIGHTSTRIKER;
  }

  // this blows chunks.
  robotRegions[BL_GOALKEEPER].upperX = configuration_.GetAsInt("GoalKeeper_UpperX");
  robotRegions[BL_GOALKEEPER].upperY = configuration_.GetAsInt("GoalKeeper_UpperY");
  robotRegions[BL_GOALKEEPER].lowerX = configuration_.GetAsInt("GoalKeeper_LowerX");
  robotRegions[BL_GOALKEEPER].lowerY = configuration_.GetAsInt("GoalKeeper_LowerY");
  robotRegions[BL_GOALKEEPER].homeX = configuration_.GetAsInt("GoalKeeper_HomeX");
  robotRegions[BL_GOALKEEPER].homeY = configuration_.GetAsInt("GoalKeeper_HomeY");

  robotRegions[BL_LEFTSTRIKER].upperX = configuration_.GetAsInt("LeftStriker_UpperX");
  robotRegions[BL_LEFTSTRIKER].upperY = configuration_.GetAsInt("LeftStriker_UpperY");
  robotRegions[BL_LEFTSTRIKER].lowerX = configuration_.GetAsInt("LeftStriker_LowerX");
  robotRegions[BL_LEFTSTRIKER].lowerY = configuration_.GetAsInt("LeftStriker_LowerY");
  robotRegions[BL_LEFTSTRIKER].homeX = configuration_.GetAsInt("LeftStriker_HomeX");
  robotRegions[BL_LEFTSTRIKER].homeY = configuration_.GetAsInt("LeftStriker_HomeY");

  robotRegions[BL_RIGHTSTRIKER].upperX = configuration_.GetAsInt("RightStriker_UpperX");
  robotRegions[BL_RIGHTSTRIKER].upperY = configuration_.GetAsInt("RightStriker_UpperY");
  robotRegions[BL_RIGHTSTRIKER].lowerX = configuration_.GetAsInt("RightStriker_LowerX");
  robotRegions[BL_RIGHTSTRIKER].lowerY = configuration_.GetAsInt("RightStriker_LowerY");
  robotRegions[BL_RIGHTSTRIKER].homeX = configuration_.GetAsInt("RightStriker_HomeX");
  robotRegions[BL_RIGHTSTRIKER].homeY = configuration_.GetAsInt("RightStriker_HomeY");

  robotRegions[BL_CENTRESTRIKER].upperX = configuration_.GetAsInt("CentreStriker_UpperX");
  robotRegions[BL_CENTRESTRIKER].upperY = configuration_.GetAsInt("CentreStriker_UpperY");
  robotRegions[BL_CENTRESTRIKER].lowerX = configuration_.GetAsInt("CentreStriker_LowerX");
  robotRegions[BL_CENTRESTRIKER].lowerY = configuration_.GetAsInt("CentreStriker_LowerY");
  robotRegions[BL_CENTRESTRIKER].homeX = configuration_.GetAsInt("CentreStriker_HomeX");
  robotRegions[BL_CENTRESTRIKER].homeY = configuration_.GetAsInt("CentreStriker_HomeY");

  robotRegions[BL_LEFTDEFENDER].upperX = configuration_.GetAsInt("LeftDefender_UpperX");
  robotRegions[BL_LEFTDEFENDER].upperY = configuration_.GetAsInt("LeftDefender_UpperY");
  robotRegions[BL_LEFTDEFENDER].lowerX = configuration_.GetAsInt("LeftDefender_LowerX");
  robotRegions[BL_LEFTDEFENDER].lowerY = configuration_.GetAsInt("LeftDefender_LowerY");
  robotRegions[BL_LEFTDEFENDER].homeX = configuration_.GetAsInt("LeftDefender_HomeX");
  robotRegions[BL_LEFTDEFENDER].homeY = configuration_.GetAsInt("LeftDefender_HomeY");

  robotRegions[BL_RIGHTDEFENDER].upperX = configuration_.GetAsInt("RightDefender_UpperX");
  robotRegions[BL_RIGHTDEFENDER].upperY = configuration_.GetAsInt("RightDefender_UpperY");
  robotRegions[BL_RIGHTDEFENDER].lowerX = configuration_.GetAsInt("RightDefender_LowerX");
  robotRegions[BL_RIGHTDEFENDER].lowerY = configuration_.GetAsInt("RightDefender_LowerY");
  robotRegions[BL_RIGHTDEFENDER].homeX = configuration_.GetAsInt("RightDefender_HomeX");
  robotRegions[BL_RIGHTDEFENDER].homeY = configuration_.GetAsInt("RightDefender_HomeY");

  robotRegions[BL_CENTREDEFENDER].upperX = configuration_.GetAsInt("CentreDefender_UpperX");
  robotRegions[BL_CENTREDEFENDER].upperY = configuration_.GetAsInt("CentreDefender_UpperY");
  robotRegions[BL_CENTREDEFENDER].lowerX = configuration_.GetAsInt("CentreDefender_LowerX");
  robotRegions[BL_CENTREDEFENDER].lowerY = configuration_.GetAsInt("CentreDefender_LowerY");
  robotRegions[BL_CENTREDEFENDER].homeX = configuration_.GetAsInt("CentreDefender_HomeX");
  robotRegions[BL_CENTREDEFENDER].homeY = configuration_.GetAsInt("CentreDefender_HomeY");

  offensiveSwitchY = configuration_.GetAsInt("OffensiveSwitchY");
  defensiveSwitchY = configuration_.GetAsInt("DefensiveSwitchY");

  boundOffset = configuration_.GetAsInt("BoundOffset");
  oneInchPunch = configuration_.GetAsBool("OneInchPunch");

  myPosition = robotPosition[BOTID_-1];

  chaseUpperX = robotRegions[myPosition].upperX;
  chaseUpperY = robotRegions[myPosition].upperY;
  chaseLowerX = robotRegions[myPosition].lowerX;
  chaseLowerY = robotRegions[myPosition].lowerY;
  homeX = robotRegions[myPosition].homeX;
  homeY = robotRegions[myPosition].homeY;

  actualHomeX = homeX;
  actualHomeY = homeY;

  previousTrick = "nothing";
  previousTrickFrame = frame_;


  watchingBallCounter=0;
  lastLookedLeft=true; // no matter really.
}
BruceLee::~BruceLee() {
  delete currentTrick;
  delete headTrick;
}

void BruceLee::NewGoHomeModel() {
  double xPosition = 0.0;
  double yPosition = 0.0;
  double headingDeg = 0.0;
  int moveType = MovementTricks::MoveToPoint::MTP_DYNAMIC;

  if (robotState_.GetKickOff() == RobotState::KO_OWN) {
    if (BOTID_ == 1) {
      xPosition = 0.0;
      yPosition = -210.0;
      headingDeg = 0.0;
    } else if (BOTID_ == 2) {
      xPosition = 20.0;
      yPosition = -30.0;
      headingDeg = 30.0;
//    moveType = MovementTricks::MoveToPoint::MTP_DYNAMIC_FACE_BALL;
    } else if (BOTID_ == 3) {
      xPosition = -100.0;
      yPosition = -20.0;
      headingDeg = 0.0;
    } else if (BOTID_ == 4) {
      xPosition = 60.0;
      yPosition = -140.0;
      headingDeg = 0.0;
    }

  } else if (robotState_.GetKickOff() == RobotState::KO_OPPONENT) {
    if (BOTID_ == 1) {
      xPosition = 10.0;
      yPosition = -210.0;
      headingDeg = 0.0;
    } else if (BOTID_ == 2) {
      xPosition = 80.0;
      yPosition = -120.0;
      headingDeg = 0.0;
    } else if (BOTID_ == 3) {
      xPosition = -10.0;
      yPosition = -120.0;
      headingDeg = 0.0;
    } else if (BOTID_ == 4) {
      xPosition = -80.0;
      yPosition = -120.0;
      headingDeg = 0.0;
    }
  }

  if (!inGoHome) {
    delete headTrick;
    headTrick = new HeadTricks::HeadPan(93,-93,3,50);   // MQ 19/6 .. changed from (70,-70,0,12)
    headTrick->Start();
    delete currentTrick;                         //x,y,h,type
    currentTrick = new MovementTricks::MoveToPoint(xPosition,yPosition,headingDeg,moveType);
    currentTrick->Start();
  }
  inGoHome=true;
  currentTrick->Continue();
  headTrick->Continue();
}

void BruceLee::NewReadyModel() {
  CheckWireless();
  if (inPlaying) {
    delete headTrick;
    headTrick = new HeadTricks::HeadPanForBall();
    headTrick->Start();
    delete currentTrick;
    currentTrick = new BasicTricks::NullBody();
    currentTrick->Start();
  }

  int hC=headTrick->Continue();
//char* hMsg = headTrick->GetErrorMsg(hC);
  if (!currentTrick->IsUsingHead() && (hC < 1)) {
    delete headTrick;
    headTrick = new HeadTricks::HeadPanForBall();
    headTrick->Start();
  } else if (vo_ball_ != NULL && strcmp(headTrick->GetName(),"HeadPan")==0) {
    delete headTrick;
    headTrick = new HeadTricks::FollowBallWithHeadSticky(5);
    headTrick->Start();
  }

  inPlaying = false;
  inGoHome = false;
  timeOfLastChase = frame_;
}
void BruceLee::NewPlayingModel() {
  CheckWireless();

  if (!inPlaying) {
    delete currentTrick;
    delete headTrick;
    if (myPosition != BL_GOALKEEPER && oneInchPunch && (frame_ - lastReadyFrame_) < 30 && robotState_.GetKickOff() == RobotState::KO_OPPONENT) {
      lcq_.Clear();
      #ifdef ERS_210
      currentTrick = new BasicTricks::MultiTrick(4, new BasicTricks::Step(LocomotionCommand::TP_SINGLEWALK,60,60,0,0,1.6), new BasicTricks::Step(LocomotionCommand::TP_SINGLEWALK,98,98,0,0,1.6), new BasicTricks::Step(LocomotionCommand::TP_SINGLEWALK,95,95,0,0,1.6), new BallTricks::WaitForKick());
      currentTrick->Start();
      #endif
      #ifdef ERS_7
      currentTrick = new BasicTricks::MultiTrick(4, new BasicTricks::Step(LocomotionCommand::TP_SINGLEWALK,180,200,0,0,1.82), new BasicTricks::Step(LocomotionCommand::TP_SINGLEWALK,180,200,0,0,1.82), new BasicTricks::Step(LocomotionCommand::TP_SINGLEWALK,180,200,0,0,1.82), new BallTricks::WaitForKick());
      currentTrick->Start();
      #endif

      headTrick = new HeadTricks::LookAt(-10.0,0.0,0.0,35);
      headTrick->Start();
    } else {
      currentTrick = new BasicTricks::NullBody();
      currentTrick->Start();
      headTrick = new BasicTricks::NullHead();
      headTrick->Start();
    }
  }
  inPlaying=true;
  inGoHome=false;

  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(10);
      } else {
        headTrick->Abort();
        delete headTrick;
        headTrick = new HeadTricks::HeadPanForBall();
      }
    }
    headTrick->Start();
  }
  // watching the ball and moving home. count!
  // flick look code
/*
  if (strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")==0 && strcmp(currentTrick->GetName(),"MoveToPoint")==0) {
    if (vo_ball_ == NULL || ABS(vo_ball_->distance_) > 40.0) {
      watchingBallCounter++;
      if (watchingBallCounter > 90) {
        headTrick->Abort();
        delete headTrick;
        headTrick = new HeadTricks::QuickPan(30,!lastLookedLeft);
        headTrick->Start();
        lastLookedLeft = !lastLookedLeft;
      }
    }
  } else {
    watchingBallCounter = 0;
  }