sleek.cc

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

    LocomotionCommand lc;
    lc.Set(LocomotionCommand::TP_SINGLEWALK, 0.0, 0.0, 75.0, 0.0, 1.5);
    utils.RawCommand(lc);
    cout << "Emergency turn around triggered by vision! Shit.\n" << flush;
  }
}
Trick* Sleek::ConstructMoveToPointHead() {
  if (IsBallVisible()) {
    return new HeadTricks::FollowBallWithHeadSticky(10);
  }
  return new HeadTricks::HeadPanForBall();
}

bool Sleek::IsSelfWithinChaseBounds() {
  if ((wo_self_->x_ <= chaseUpperX) && (wo_self_->x_ >= chaseLowerX) && (wo_self_->y_ <= chaseUpperY) && (wo_self_->y_ >= chaseLowerY)) {

// change by CM - defenders roam free now :)
// 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 Sleek::IsBallWithinChaseBounds() {
  if ((wo_ball_->x_ <= chaseUpperX) && (wo_ball_->x_ >= chaseLowerX) && (wo_ball_->y_ <= chaseUpperY) && (wo_ball_->y_ >= chaseLowerY)) {
// change by CM - defenders roam free now :)
// Make sure the defenders don't go into the keeper area
//  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 Sleek::IsBallWithinEnlargedChaseBounds() {
  int chaseBonus = 0;
  if (myPosition == BL_GOALKEEPER) chaseBonus = 20;

  if ((wo_ball_->x_ <= chaseUpperX+chaseBonus) && (wo_ball_->x_ >= chaseLowerX-chaseBonus) && (wo_ball_->y_ <= chaseUpperY+chaseBonus) && (wo_ball_->y_ >= chaseLowerY-chaseBonus)) {
// change by CM - defenders roam free now :)
// Make sure the defenders don't go into the keeper area
//  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 Sleek::IsBallVisible() {
  return (vo_ball_ != NULL);
}
bool Sleek::IsBallVisibleTeam() {
  return (vo_ball_ != NULL || strcmp(headTrick->GetName(),"QuickPan")==0 || strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")==0 || ClosestToBallNotSelf() < 525.0);
}
bool Sleek::IsBallVisibleTeamNotSelf() {
  double smallestDistance = 10000.0;
  for (int i=0; i<4; i++) {
    if (othersNormal[i].distance < smallestDistance && i!=BOTID_-1) return true;
  }
  return false;
}
bool Sleek::AmIAllowedToChase(double distanceToBall) {
  return IsBallVisible() && IsBallWithinChaseBounds() && (distanceToBall < ClosestToBallNotSelf());
}
double Sleek::ClosestToBallNotSelf() {
  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 Sleek::WhoIsClosestToBall() {
  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;
}


void Sleek::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;
    }
  }

  //after ~4 seconds of no wireless from a robot make sure they don't stop us chasing
  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 Sleek::MovePlayer(int id, int newPosition) {
  robotPosition[id-1] = newPosition;
}

void Sleek::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 << "Sleek: 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 Sleek::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 = WhoIsClosestToBall(); // 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);
    }
//}
}


Sleek::Sleek() {
  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;
  }

  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");