brucelee.cc

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

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