www.pudn.com > Product_Submit2004.rar > BruceLee.cc, change:2004-10-12,size:37867b


#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; 
  } 
*/ 
 
  int cC = currentTrick->Continue(); 
  char* cMsg = currentTrick->GetErrorMsg(cC); 
 
  keeperRoaming = false; 
  double ballDistance = 20000.0; 
  if (vo_ball_ != NULL) { 
    ballDistance = ABS(vo_ball_->distance_); 
      // enemy goal coords 
  double currx = wo_self_->x_; 
  double curry = wo_self_->y_; 
  double currheading = wo_self_->heading_; 
  double x_ = 0; 
  double y_ = 215; 
  if (curry>210) curry=210; 
  double theta = (atan2(y_-curry, x_-currx) - (PI / 2.0)) - currheading; 
 
  // heading to goal based on WM. 
  double goalHeading = RAD_TO_DEG(theta); 
 
  // we can see opposition goal ! Use vision instead of WM 
  if (vo_oppositionGoal_ != NULL) { 
    goalHeading = RAD_TO_DEG(vo_oppositionGoal_->heading_); 
    if (goalHeadingLeft_ < DEG_TO_RAD(10) && goalHeadingRight_ > DEG_TO_RAD(-10)) { 
      goalHeading = 0.0; 
    } else { 
      double temp = RAD_TO_DEG(goalHeadingLeft_)-10.0; 
      if (ABS(temp) < ABS(goalHeading)) { 
        goalHeading = temp; 
      }  
      temp = RAD_TO_DEG(goalHeadingRight_)+10.0; 
      if (ABS(temp) < ABS(goalHeading)) { 
        goalHeading = temp; 
      }  
    } 
    
  // we can see our own goal ! Use vision instead of WM. 
  } else if (vo_ownGoal_ != NULL) { 
    goalHeading = 180.0 - RAD_TO_DEG(vo_ownGoal_->heading_); 
    //cout << "ownGoal->heading = " << RAD_TO_DEG(vo_ownGoal_->heading_) << ", goalHeading: " << goalHeading << endl << flush; 
  } 
 
  while (goalHeading > 180.0) goalHeading -= 360.0; 
  while (goalHeading <= -180.0) goalHeading += 360.0; 
   
  ballDistance+=(ABS(goalHeading)/180.0*ballDistance); 
 
#ifdef ERS_7 
    if ((MICRO_TO_DEG(sensorValues_[S_HEAD_TILT1])+MICRO_TO_DEG(sensorValues_[S_HEAD_TILT2]) < -25.0) && (ABS(MICRO_TO_DEG(sensorValues_[S_HEAD_PAN])) < 20.0)) { 
      ballDistance = 1.25; 
    } 
#endif 
#ifdef ERS_210 
    if ((MICRO_TO_DEG(sensorValues_[S_HEAD_TILT]) < -25.0) && (ABS(MICRO_TO_DEG(sensorValues_[S_HEAD_PAN])) < 20.0)) { 
      ballDistance = 1.25; 
    } 
#endif 
 
    // NOTE! keeper roaming is always OFF here. so we don't REALLY need to call the NonEnlarged version ... 
    // keeper cheats in bruce lee distance negotiation stuff. 
    if (myPosition == BL_GOALKEEPER && IsBallWithinNonEnlargedChaseBounds()) { 
      ballDistance = 1.5; 
    } else if (myPosition == BL_GOALKEEPER && (!IsBallWithinNonEnlargedChaseBounds())) { 
      // if no one else has chased for over 4 seconds and we're close to the ball, enable keeperRoaming ! 
      // this essentially just enlarges the keeper's chase bounds. 
 
      if (frame_-timeOfLastChase > 100 && ballDistance < 120.0) { 
        keeperRoaming = true; 
      } else { 
        ballDistance = 20000.0; 
      } 
    } 
  } 
  //doing a chase and not interruptible AND we're too fucking close (head tilt measure), FUCK OFF AWAY!! 
  if (strcmp(currentTrick->GetName(),"AdvancedChaseWithKick")==0 && strcmp(cMsg,"INTERRUPTIBLE")!=0) { 
#ifdef ERS_7 
    if ((MICRO_TO_DEG(sensorValues_[S_HEAD_TILT1])+MICRO_TO_DEG(sensorValues_[S_HEAD_TILT2]) < -18.0) && (ABS(MICRO_TO_DEG(sensorValues_[S_HEAD_PAN])) < 20.0)) { 
      ballDistance = 1.25; 
    } 
#endif 
  } 
 
  // this babble is so we don't let other chase when we're doing turnwithballuntilgoal 
  Trick* cT = currentTrick; 
  while (cT != NULL) { 
    if (cT->GetCurrentTrick() != NULL) { 
      cT = cT->GetCurrentTrick(); 
    } else break; 
  } 
  if (cT != NULL) { 
    if (strcmp(cT->GetName(),"TurnWithBallUntilGoal")==0 || strcmp(cT->GetName(),"WaitForKick") == 0 || strcmp(cT->GetName(),"Kick") == 0 ) { 
      ballDistance = 1.0; 
    }                                                                                                              
    if (strcmp(currentTrick->GetName(),"AdvancedChaseWithKick")==0 && ballDistance > 600 && strcmp(cT->GetName(),"PostChaseSearchForBall")!=0) ballDistance = lastBallDistance; 
 
  } 
 
  // if kicking, make sure others DO NOT CHASE !!!! 
  // note that postchasesearchforball does a getup, but this DOES NOT COUNT AS SHIT 
  if (lcq_.Size() >= 0 && strcmp(cT->GetName(),"PostChaseSearchForBall")!=0) { 
    LocomotionCommand currentLocomotionCommand = lcq_.GetCurrentLocomotionCommand(); 
    int motionType = currentLocomotionCommand.motionType; 
    if ((motionType > 4 && motionType < 50) || (motionType > 100) || (motionType == LocomotionCommand::TP_SINGLEWALKWITHOUTFRONT) || (motionType == 99)) { 
      if (motionType != 6) { 
        ballDistance = 1.0; 
      } 
    } 
  } 
 
   
  lastBallDistance = ballDistance; 
 
//cout << "ct: " << cT->GetName() << ", balldistance: " << ballDistance << endl << flush; 
 
  //executing normally. we may wish to interrupt. 
  if (cC >= 1) { 
    // we're searching but somebody else has seen the ball .. maybe we don't need to search anymore 
    if (strcmp(currentTrick->GetName(),"SearchForBall")==0) { 
       // If somebody can see it and its not close to me then i should just look where they say it is 
       if (ClosestToBall() < 525.0 && (pow(wo_self_->x_ - wo_ball_->x_, 2)+pow(wo_self_->y_ - wo_ball_->y_, 2) > 900)) { 
          currentTrick->Abort(); 
          delete currentTrick; 
          currentTrick = new MovementTricks::MoveToPoint(homeX,homeY,0.0,MovementTricks::MoveToPoint::MTP_DYNAMIC_FACE_BALL); 
          currentTrick->Start(); 
       } 
    // we're chasing, but we're allowed to interrupt. Also don't interrupt if we are close to the ball. 
    #ifdef ERS_7 
    // if we're doing postchasesearchforball w/i advancedchasewithkick, we cannot force it to exit out here. 
    } else if ((strcmp(currentTrick->GetName(),"AdvancedChaseWithKick")==0) && strcmp(cMsg,"INTERRUPTIBLE")==0 && strcmp(cT->GetName(),"PostChaseSearchForBall")!=0/*&& MICRO_TO_DEG(sensorValues_[S_HEAD_TILT1])+MICRO_TO_DEG(sensorValues_[S_HEAD_TILT2]) > -15*/) { 
    #endif 
    #ifdef ERS_210 
    } else if ((strcmp(currentTrick->GetName(),"ChaseWithKick")==0) && strcmp(cMsg,"INTERRUPTIBLE")==0 && MICRO_TO_DEG(sensorValues_[S_HEAD_TILT]) > -15) { 
    #endif 
      // exceeded chase bounds ! back off !!! (note- Chase will terminate if ball is not visible - so this is ok) 
      //int whoIs = WhoIsChasing(); 
      //WhoIsChasing(); 
      if (IsBallWithinChaseBounds() == false || ClosestToBall() < ballDistance) { 
        if (debugOutput) 
          cout << "BruceLee: Aborted AdvancedChaseWithKick (chase bound exceeded)." << endl << flush; 
        // change to move home 
        currentTrick->Abort(); 
        delete currentTrick; 
        currentTrick = new MovementTricks::MoveToPoint(homeX,homeY,0.0,MovementTricks::MoveToPoint::MTP_DYNAMIC_FACE_BALL); 
        currentTrick->Start(); 
        // look around for beacons ! 
        if (IsBallVisible()) { 
          headTrick->Abort(); 
          delete headTrick; 
          headTrick = new HeadTricks::FollowBallWithHeadSticky(10); 
        } else { 
          headTrick->Abort(); 
          delete headTrick; 
          headTrick = new HeadTricks::HeadPanForBall(); 
        } 
        headTrick->Start(); 
      } 
    // currently moving home or searching. 
    } else if (strcmp(currentTrick->GetName(),"MoveToPoint")==0) { 
      // ball is within our chase area ! 
      if (AmIAllowedToChase(ballDistance)) { 
        if (debugOutput) 
          cout << "BruceLee: Ball within chase area. Aborting SearchForBall/MoveToPoint and chasing." << endl << flush; 
        currentTrick->Abort();   
        delete currentTrick; 
        // If we are the keeper chase a bit slower so we don't make a mistake 
        if (myPosition == BL_GOALKEEPER) currentTrick = new ComplexTricks::AdvancedChaseWithKick(); // Chase a bit slower so we don't make a mistake 
        else currentTrick = new ComplexTricks::AdvancedChaseWithKick(); 
        cC = currentTrick->Start(); 
        headTrick->Abort(); 
        delete headTrick; 
        headTrick = new BasicTricks::NullHead(); 
        headTrick->Start(); 
      } else if (ballPrediction_.ShouldIDive()) { 
        // if we're the goalie, or we're facing basically towards opposition goal ... 
        if (myPosition == BL_GOALKEEPER || ABS(RAD_TO_DEG(wo_self_->heading_)) < 80) { 
            /* int direction = ballPrediction_.DiveDirection(); 
             if (lcq_.Front().motionType < 4) {   
               if (direction == BallPrediction::BP_LEFT) utils.Kick(LocomotionCommand::TP_GOALIE_LEFT,false); 
               else if (direction == BallPrediction::BP_RIGHT) utils.Kick(LocomotionCommand::TP_GOALIE_RIGHT,false); 
               else utils.Kick(LocomotionCommand::TP_GOALIE_BLOCK, false); 
             } */ 
            utils.Kick(LocomotionCommand::TP_GOALIE_BLOCK, false); 
        } 
      } else { 
        // not chasing, not blocking. reposition instead. 
        double destX = homeX; 
        double destY = homeY; 
 
        if (myPosition == BL_GOALKEEPER) { 
          double theta = (atan2(wo_ball_->y_-homeY, homeX-wo_ball_->x_) - (PI / 2.0)); 
          while (theta > PI) theta -= 2*PI; 
          while (theta < -PI) theta += 2*PI; 
          destX = homeX+homeRadius*sin(theta)*2.4; 
          destY = homeY+homeRadius*cos(theta)*0.30; 
          if (destY < -210) destY = -210; 
          if (destY > -200) destY = -200; 
 
          // if the ball is in the corners bring the circle focus back 
          // so the ball wont sneak down the walls 
          if (wo_ball_->y_ < - 160 && wo_ball_->x_ > 65) { 
            destY = -202; 
            destX = 35;             
          } 
          else if (wo_ball_->y_ < - 160 && wo_ball_->x_ < -65 ) { 
            destY = -202; 
            destX = -35; 
          } 
          // else its far away or in the middle - use normal code  
          else { 
            double theta = (atan2(wo_ball_->y_-homeY, homeX-wo_ball_->x_) - (PI / 2.0)); 
            while (theta > PI) theta -= 2*PI; 
            while (theta < -PI) theta += 2*PI; 
            destX = homeX+homeRadius*sin(theta)*2.4; 
            destY = homeY+homeRadius*cos(theta)*0.30; 
            if (destY < -210) destY = -210; 
          } 
/* 
          // if ball is on other half of field and we dont need to move much (5cm) 
          // relocalise 
          if (wo_ball_->y_ > 0 && ( (destX - wo_self_->x_) * (destX - wo_self_->x_) + (destY - wo_self_->y_) * (destY - wo_self_->y_) < 25 )) 
          { 
            delete headTrick; 
            headTrick = new HeadTricks::HeadPan(85,-85,3,50);    
            headTrick->Start(); 
            //delete currentTrick; 
            //currentTrick = new BasicTricks::NullBody(); 
            //currentTrick->Start(); 
          } 
*/ 
        } else { 
          if (myPosition == BL_CENTREDEFENDER /*|| myPosition == BL_CENTRESTRIKER (MQ 5/7)*/) { 
            destX = (wo_ball_->x_ + homeX)/2.0; 
          } else if (myPosition == BL_LEFTSTRIKER || myPosition == BL_RIGHTSTRIKER) { 
            /* 
            if (homeX > 0) { 
              destX = 135+wo_ball_->x_; 
            } else { 
              destX = wo_ball_->x_-135; 
            } 
            if (ABS(destX) > ABS(homeX)) destX = homeX; 
            */ 
 
            destY = (wo_ball_->y_ + homeY)/2.0; 
            if (destY<homeY) destY = homeY; 
          }  
        } 
        if (myPosition != BL_GOALKEEPER) { 
          if (destY > chaseUpperY-boundOffset) destY = chaseUpperY-boundOffset; 
          if (destY < chaseLowerY+boundOffset) destY = chaseLowerY+boundOffset; 
          if (destY < -150) destY = -150; 
 
          if (destX > chaseUpperX-boundOffset) destX = chaseUpperX-boundOffset; 
          if (destX < chaseLowerX+boundOffset) destX = chaseLowerX+boundOffset; 
        } 
        ((MovementTricks::MoveToPoint*)(currentTrick))->ResetHome((int)destX,(int)destY);     
        actualHomeX = destX; 
        actualHomeY = destY; 
 
        if (strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")!=0 && IsBallVisible() == false && !(ClosestToBall() < 525.0) /*&& (pow(wo_self_->x_ - actualHomeX, 2)+pow(wo_self_->y_ - actualHomeY, 2) < homeRadius*homeRadius)*/) { 
          // we're moving to point, and we can't see the ball. 
          // if we're practically home AND no one else is chasing, we'd better look for the ball !! 
 
          currentTrick->Abort(); 
          delete currentTrick; 
          currentTrick = new MovementTricks::SearchForBall(360.0); 
          currentTrick->Start(); 
          delete headTrick; 
          headTrick = new BasicTricks::NullHead(); 
          headTrick->Start(); 
        } 
      } 
    } 
  // move to point exited. start moving again ? 
  } else if (cC < 1) { 
    if (AmIAllowedToChase(ballDistance)) { 
      if (debugOutput) 
        cout << "BruceLee: Ball within chase area, previous trick has ended. Chasing." << endl << flush; 
      delete currentTrick; 
      // If we are the keeper chase a bit slower so we don't make a mistake 
      if (myPosition == BL_GOALKEEPER) currentTrick = new ComplexTricks::AdvancedChaseWithKick();  
      else currentTrick = new ComplexTricks::AdvancedChaseWithKick(); 
      cC = 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 ! 
      // note - don't do this if in sticky search. 
      if ( (strcmp(currentTrick->GetName(), "SearchForBall")!=0) && (IsBallVisible() == false) && (IsSelfWithinChaseBounds()) && strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")!=0 && strcmp(headTrick->GetName(),"QuickPan")!=0) { 
        if (debugOutput) 
          cout << "BruceLee: Searching for ball." << endl << flush; 
        delete currentTrick; 
/* 
        if (lastBallHeading_ > 0) { 
          turnRight = false; 
        } 
        // if we kicked left very recently, we probably should turn that way... 
        if (frame_-lastKickedLeft_ < 25) turnRight = false; 
        // same deal for right 
        if (frame_-lastKickedRight_ < 25) turnRight = true;*/ 
 
        currentTrick = new MovementTricks::SearchForBall(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.0,MovementTricks::MoveToPoint::MTP_DYNAMIC_FACE_BALL); 
        currentTrick->Start(); 
        // look around for beacons ! 
         if (IsBallVisible()) { 
          headTrick->Abort(); 
          delete headTrick; 
          headTrick = new HeadTricks::FollowBallWithHeadSticky(10); 
        } else { 
          headTrick->Abort(); 
          delete headTrick; 
          headTrick = new HeadTricks::HeadPanForBall(); 
        } 
        headTrick->Start(); 
      } 
    } 
  } 
 
  if (isCaptain) { 
    RunCaptain(); 
  } 
 
  // Send wireless behaviour info (1 if doing a chase otherwise 0)  
  bool isChasing = false; 
  if (vo_ball_ != NULL) { 
    if (strcmp(currentTrick->GetName(), "AdvancedChaseWithKick")==0) { 
      isChasing = true; 
    } 
  } 
  if (isCaptain) { 
    CaptainMessage sndMsg; 
    sndMsg.chasing = isChasing; 
    sndMsg.distance = ballDistance; 
 
    othersNormal[BOTID_-1].distance = ballDistance; 
 
    // load up sendmsg with desired positions for everyone 
    for (int i = 0; i < 4; i++) { 
      int position = robotPosition[i]; 
      sndMsg.upperX[i] = robotRegions[position].upperX; 
      sndMsg.upperY[i] = robotRegions[position].upperY; 
      sndMsg.lowerX[i] = robotRegions[position].lowerX; 
      sndMsg.lowerY[i] = robotRegions[position].lowerY; 
      sndMsg.homeX[i] = robotRegions[position].homeX; 
      sndMsg.homeY[i] = robotRegions[position].homeY; 
      sndMsg.position[i] = position; 
 
      // captain needs to interpret his own position here (since he won't receive messages from himself) 
      if (i == BOTID_-1) { 
        chaseUpperX = robotRegions[position].upperX; 
        chaseUpperY = robotRegions[position].upperY; 
        chaseLowerX = robotRegions[position].lowerX; 
        chaseLowerY = robotRegions[position].lowerY; 
        homeX = robotRegions[position].homeX; 
        homeY = robotRegions[position].homeY; 
        myPosition = position; 
      } 
    } 
    outMessageSize_ = sizeof(CaptainMessage); 
    memcpy(outMessage_,&sndMsg,outMessageSize_);  
  } else { 
    NormalMessage sndMsg; 
    sndMsg.chasing = isChasing; 
    sndMsg.distance = ballDistance; 
    outMessageSize_ = sizeof(NormalMessage); 
    memcpy(outMessage_,&sndMsg,outMessageSize_);  
  } 
 
  //ledOn_[0] = false; 
  //ledOn_[2] = false; 
  //ledOn_[3] = false; 
  //ledOn_[5] = false; 
  if (myPosition == BL_GOALKEEPER) { 
 
  } else if (myPosition == BL_CENTREDEFENDER) { 
    //ledOn_[2] = true; // back right 
    //ledOn_[5] = true; // back left 
  } else if (myPosition == BL_LEFTDEFENDER) { 
    //ledOn_[2] = true; // back left 
  } else if (myPosition == BL_RIGHTDEFENDER) { 
    //ledOn_[5] = true; // back right 
  } else if (myPosition == BL_CENTRESTRIKER) { 
    //ledOn_[0] = true; // front right 
    //ledOn_[3] = true; // front left 
  } else if (myPosition == BL_LEFTSTRIKER) { 
    //ledOn_[0] = true; // front left 
  } else if (myPosition == BL_RIGHTSTRIKER) { 
    //ledOn_[3] = true; // front right 
  } 
 
  // trick has changed. output ! 
  if (strcmp(currentTrick->GetName(),previousTrick)!=0) { 
    if (frame_-previousTrickFrame == 1) { 
      cout << "Switched from " << previousTrick << " to " << currentTrick->GetName() << endl << flush; 
    } 
    previousTrick = currentTrick->GetName(); 
    previousTrickFrame = frame_; 
  } 
} 
 
bool BruceLee::IsSelfWithinChaseBounds() { 
  if ((wo_self_->x_ <= chaseUpperX) && (wo_self_->x_ >= chaseLowerX) && (wo_self_->y_ <= chaseUpperY) && (wo_self_->y_ >= chaseLowerY)) { 
 
// change by CM - only the keeper has bounds now, and these are handled by configus. 
    // Make sure the defenders don't go into the keeper area 
//  if ((myPosition != BL_GOALKEEPER) && (wo_self_->x_ <= 50) && (wo_self_->x_ >= -50) && (wo_self_->y_ <= -160) && (wo_self_->y_ >= -300)) 
//     return false; 
    return true; 
  } 
  return false; 
} 
 
bool BruceLee::IsBallWithinNonEnlargedChaseBounds() { 
  if ((wo_ball_->x_ <= chaseUpperX) && (wo_ball_->x_ >= chaseLowerX) && (wo_ball_->y_ <= chaseUpperY) && (wo_ball_->y_ >= chaseLowerY)) { 
    // Make sure the defenders don't go into the keeper area 
// change by CM - only keepers have bounds now. 
//  if ((myPosition != BL_GOALKEEPER) && (wo_ball_->x_ <= 50) && (wo_ball_->x_ >= -50) && (wo_ball_->y_ <= -160) && (wo_ball_->y_ >= -300)) 
//    return false; 
    return true; 
  } 
  return false; 
} 
 
bool BruceLee::IsBallWithinChaseBounds() { 
  bool ret = IsBallWithinNonEnlargedChaseBounds(); 
  int roamingRegionIncrease = 8; 
  if (keeperRoaming && myPosition == BL_GOALKEEPER) { 
    if ((wo_ball_->x_ <= chaseUpperX+roamingRegionIncrease) && (wo_ball_->x_ >= chaseLowerX-roamingRegionIncrease) && (wo_ball_->y_ <= chaseUpperY+roamingRegionIncrease) && (wo_ball_->y_ >= chaseLowerY-roamingRegionIncrease)) { 
      return true; 
    } 
  } 
  return ret; 
} 
 
bool BruceLee::IsBallVisible() { 
  return (vo_ball_ != NULL); 
} 
 
bool BruceLee::AmIAllowedToChase(double distanceToBall) { 
  return IsBallVisible() && IsBallWithinChaseBounds() && (distanceToBall < ClosestToBall()); 
 
} 
 
double BruceLee::ClosestToBall() { 
  double smallestDistance = 20000.0; 
  for (int i=0; i<4; i++) { 
    if (othersNormal[i].distance < smallestDistance && i!=BOTID_-1) smallestDistance = othersNormal[i].distance; 
  } 
  return smallestDistance; 
} 
 
int BruceLee::WhoIsClosestToBall() { 
  double smallestDistance = 10000.0; 
  int closest = -2; 
  for (int i=0; i<4; i++) { 
    if (othersNormal[i].distance < smallestDistance && i!=BOTID_-1) { 
      smallestDistance = othersNormal[i].distance; 
      closest = i; 
    } 
  } 
  return closest+1; 
} 
 
int BruceLee::WhoIsClosestToBallIncludingMe() { 
  double smallestDistance = 10000.0; 
  int closest = -2; 
  for (int i=0; i<4; i++) { 
    if (othersNormal[i].distance < smallestDistance) { 
      smallestDistance = othersNormal[i].distance; 
      closest = i; 
    } 
  } 
  return closest+1; 
} 
 
 
bool BruceLee::IsAnybodyChasing() { 
  for (int i=0; i<4; i++) { 
    if (othersNormal[i].chasing && i!=BOTID_-1) return true; 
  } 
  return false; 
} 
 
int BruceLee::WhoIsChasing() { 
  for (int i=0; i<4; i++) { 
    if (othersNormal[i].chasing && i!=BOTID_-1) return i+1; 
  } 
  return -1; 
} 
 
void BruceLee::CheckWireless() { 
  for (int i = 0; i < BVR_BUFFERSIZE; i++) { 
    ToBVRMessage* msg = &(toBVRMessages[i]); 
    // do we have BVR data ? 
    if (msg->freeSlot==false) { 
      memcpy(&othersNormal[(msg->sourceBotID)-1],msg->data,sizeof(NormalMessage)); 
      lastReceivedTime[(msg->sourceBotID)-1] = frame_; 
      if (othersNormal[(msg->sourceBotID)-1].chasing) { 
        timeOfLastChase = frame_; 
      } 
      // actually have a captain message here. 
      if (msg->size == sizeof(CaptainMessage)) { 
        memcpy(&othersCaptain, msg->data, sizeof(CaptainMessage)); 
        chaseUpperX = othersCaptain.upperX[BOTID_-1]; 
        chaseUpperY = othersCaptain.upperY[BOTID_-1]; 
        chaseLowerX = othersCaptain.lowerX[BOTID_-1]; 
        chaseLowerY = othersCaptain.lowerY[BOTID_-1]; 
        homeX = othersCaptain.homeX[BOTID_-1]; 
        homeY = othersCaptain.homeY[BOTID_-1]; 
 
        myPosition = othersCaptain.position[BOTID_-1]; 
        // homeX,homeY etc all get checked in the positioning code next time NewPlayingModel gets called. 
        // ie, no need to do anything here 
      } 
      // message interpreted. we can free the slot now ! 
      msg->freeSlot=true; 
    } 
  } 
 
  for (int i = 0; i < 4; i++) { 
    if (i != BOTID_-1) { 
      if (frame_-lastReceivedTime[i] > 100) { 
        othersNormal[i].chasing = false; 
        othersNormal[i].distance = 20000.0; 
      } 
    } 
  } 
} 
 
void BruceLee::MovePlayer(int id, int newPosition) { 
  robotPosition[id-1] = newPosition; 
} 
 
void BruceLee::AllocatePositions(int notThisId, int position1, int position2) { 
  int id1 = -1; int id2 = -1; 
  for (int i = 1; i < 5; i++) { 
    if (i == notThisId || robotPosition[i-1] == BL_GOALKEEPER) continue; 
    if (id1 == -1) id1 = i; 
    else if (id2 == -1) id2 = i; 
  } 
  if (id1 == -1 || id2 == -1) { 
    cout << "BruceLee: AllocatePositions is screwed" << endl << flush; 
  } 
 
  double x1 = worldObjectsBVR_[id1-1].x_; 
  double y1 = worldObjectsBVR_[id1-1].y_; 
  double x2 = worldObjectsBVR_[id2-1].x_; 
  double y2 = worldObjectsBVR_[id2-1].y_; 
 
  double distance1 = (pow(x1-robotRegions[position1].homeX, 2) + pow(y1-robotRegions[position1].homeY,2))+(pow(x2-robotRegions[position2].homeX, 2) + pow(y2-robotRegions[position2].homeY,2)); 
  double distance2 = (pow(x2-robotRegions[position1].homeX, 2) + pow(y2-robotRegions[position1].homeY,2))+(pow(x1-robotRegions[position2].homeX, 2) + pow(y1-robotRegions[position2].homeY,2)); 
  if (distance1 < distance2) { 
    MovePlayer(id1, position1); 
    MovePlayer(id2, position2); 
  } else { 
    MovePlayer(id1, position2); 
    MovePlayer(id2, position1); 
  } 
} 
 
void BruceLee::RunCaptain() { 
  // can't change positions more than once a second. Unsure of this bit, hence commented out .. 
//if (/*(frame_ - lastSwitchTime) > 0*/true) { 
 
    lastSwitchTime = frame_; 
    int closestId = WhoIsClosestToBallIncludingMe(); // ids are 1 through 4 inclusive 
    if (closestId < 1 || closestId > 4) { 
      if (closestId > 4 || closestId < -1 || closestId == 0) cout << "ClosestId Failed! "<< closestId << endl << flush; 
      return; 
    } 
    if (robotPosition[closestId-1] == BL_GOALKEEPER) { 
      // goalkeeper is closest robot... don't do any swapping. 
      return; 
    } 
 
    int bestPosition = -1; 
    double bestDistance = 1000000.0; 
    double x = worldObjectsBVR_[closestId-1].x_; 
    double y = worldObjectsBVR_[closestId-1].y_; 
    for (int i = 1; i < 5; i++) { 
      double d = pow(x-robotRegions[i].homeX, 2) + pow(y-robotRegions[i].homeY,2); 
      if (d < bestDistance) { 
        bestDistance = d; 
        bestPosition = i; 
      } 
    } 
    int oldPosition = robotPosition[closestId-1]; 
    if (oldPosition == bestPosition) return; 
//  cout << "Chose bestPosition: " << bestPosition << " for " << closestId << endl << flush; 
    MovePlayer(closestId,bestPosition); 
     
    if (bestPosition == BL_LEFTSTRIKER) { 
      AllocatePositions(closestId, BL_RIGHTSTRIKER, BL_CENTREDEFENDER); 
 
    } else if (bestPosition == BL_RIGHTSTRIKER) { 
      AllocatePositions(closestId, BL_LEFTSTRIKER, BL_CENTREDEFENDER); 
 
    } else if (bestPosition == BL_LEFTDEFENDER) { 
      AllocatePositions(closestId, BL_RIGHTDEFENDER, BL_CENTRESTRIKER); 
 
    } else if (bestPosition == BL_RIGHTDEFENDER) { 
      AllocatePositions(closestId, BL_LEFTDEFENDER, BL_CENTRESTRIKER); 
    } 
//} 
} 
 
void BruceLee::OneInchPunch() { 
  lcq_.Clear(); 
  utils.Step(LocomotionCommand::TP_SINGLEWALK,60,60,0,0,1.6); 
  utils.Step(LocomotionCommand::TP_SINGLEWALK,98,98,0,0,1.6); 
  utils.Step(LocomotionCommand::TP_SINGLEWALK,95,95,0,0,1.6); 
}