sleek.cc

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

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


  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.

  // going home
  xPosition = 0.0;
  yPosition = 0.0;  
  headingDeg = 0.0;
  kickLeft = true;

  nonStopSearch = false;
}

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


void Sleek::NewGoHomeModel() {

  int moveType = MovementTricks::MoveToPoint::MTP_FORWARDS; 

  if (!inGoHome) {
    delete headTrick;
    headTrick = new HeadTricks::HeadPan(93,-93,3,20); 
    headTrick->Start();
    if (currentTrick != NULL) delete currentTrick;                         //x,y,h,type
    currentTrick = new MovementTricks::CheckLocalisation(270.0,30);
    currentTrick->Start();
   
    xPosition = 0.0;
    yPosition = 0.0;  
    headingDeg = 0.0;
  } 

  if (robotState_.GetKickOff() == RobotState::KO_OWN && strcmp(currentTrick->GetName(),"CheckLocalisation")!=0) {
    if (BOTID_ == 1) {
      xPosition = 0.0;
      yPosition = -210.0;
      headingDeg = 0.0;
    } else if (BOTID_ == 2) {  // Walk well away from the ball then approach it
      if (headingDeg != 30.0) {
        yPosition = -75.0;
        if (kickLeft) xPosition = 60.0; else xPosition = -60.0; 
        headingDeg = 0.0;
      } 
      if (wo_self_->y_ < -40 && headingDeg < 0.1) { 
        headingDeg = 30.0;
      }
      if (headingDeg > 0.1) {
        if (vo_ball_ == NULL) {
          if (!MoveToPoint()) {
            ChangeTrick(currentTrick, new MovementTricks::MoveToPoint(xPosition,yPosition,headingDeg,moveType));
            ChangeTrick(headTrick, ConstructMoveToPointHead());
          }
        } else { 
          //if (vo_ball_->heading_ < DEG_TO_RAD(20)) {
            if (ABS(vo_ball_->distance_) > 32 && strcmp(currentTrick->GetName(), "AdvancedChaseWithKick") != 0) {
              ChangeTrick(currentTrick, new ComplexTricks::AdvancedChaseWithKick());
              ChangeTrick(headTrick, new BasicTricks::NullHead());
            } else if (ABS(vo_ball_->distance_) <= 32) {
              ChangeTrick(currentTrick, new BasicTricks::NullBody());
              ChangeTrick(headTrick, new HeadTricks::FollowBallWithHeadSticky(15));
            } 
          /*} else {
            bool dir = true;
            if (vo_ball_->heading_ > 0) dir = false;
            ChangeTrick(currentTrick, new MovementTricks::FaceBeacon(&vo_ball_,dir));
            ChangeTrick(headTrick, ConstructMoveToPointHead());
          }*/
        }
      }
    } else if (BOTID_ == 3) {
      xPosition = -100.0;
      yPosition = -20.0;
      headingDeg = 0.0;
    } else if (BOTID_ == 4) {
      xPosition = 60.0;
      yPosition = -105.0;
      headingDeg = 0.0;
    }

  } else if (robotState_.GetKickOff() == RobotState::KO_OPPONENT && strcmp(currentTrick->GetName(),"CheckLocalisation")!=0) {
    if (BOTID_ == 1) {
      xPosition = 10.0;
      yPosition = -210.0;
      headingDeg = 0.0;
    } else if (BOTID_ == 2) {
      xPosition = 60.0;
      yPosition = -107.0;
      headingDeg = 0.0;
    } else if (BOTID_ == 3) {
      xPosition = -10.0;
      yPosition = -107.0;
      headingDeg = 0.0;
    } else if (BOTID_ == 4) {
      xPosition = -60.0;
      yPosition = -107.0;
      headingDeg = 0.0;
    }
  }
  
  if (MoveToPoint()) {
    ((MovementTricks::MoveToPoint*)(currentTrick))->ResetHome(xPosition,yPosition);
    double currx = wo_self_->x_;
    double curry = wo_self_->y_;
    double d = sqrt(((xPosition-currx)*(xPosition-currx)+(yPosition-curry)*(yPosition-curry)));
    if (inGoHome && d < 20.0 && moveType == MovementTricks::MoveToPoint::MTP_FORWARDS) {
      ((MovementTricks::MoveToPoint*)(currentTrick))->SetType(MovementTricks::MoveToPoint::MTP_DYNAMIC);
    } 
  }
  
  inGoHome=true;
  int cC = currentTrick->Continue();

  if (cC < 1 && strcmp(currentTrick->GetName(),"CheckLocalisation")==0) {
     delete currentTrick;                         //x,y,h,type
    currentTrick = new MovementTricks::MoveToPoint(xPosition,yPosition,headingDeg,moveType);
    currentTrick->Start();
    if (wo_self_->x_ < 0) kickLeft = false; else kickLeft = true;
  }

  int hC = 1;
  if (!currentTrick->IsUsingHead()) hC=headTrick->Continue();
  if (hC < 1) {
    if (currentTrick->IsUsingHead()) {
      ChangeTrick(headTrick, new BasicTricks::NullHead());
    } else {
      if (BOTID_ == 2 && headingDeg > 0.1) ChangeTrick(headTrick,ConstructMoveToPointHead());
      else ChangeTrick(headTrick, new HeadTricks::HeadPan(93,-93,3,30));
    }
  }
}


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

  if (vo_ball_ != NULL && strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")!=0) {
    delete headTrick;
    headTrick = new HeadTricks::FollowBallWithHeadSticky(10);
    headTrick->Start();
  } else {
    if (strcmp(headTrick->GetName(),"HeadPanForBall")!=0) {
      delete headTrick;
      headTrick = new HeadTricks::HeadPanForBall();
      headTrick->Start();
    }
  } 
  headTrick->Continue();
 

  inPlaying = false;
  inGoHome = false;
  timeOfLastChase = frame_;
}

double Sleek::CalculateBallDistance() {
  keeperRoaming = false;
  double ballDistance = 20000.0;
  if (vo_ball_ != NULL) {
    ballDistance = ABS(vo_ball_->distance_);
    if (ballDistance > 520.0) ballDistance = 520.0;

    double goalHeading = utils.GetHeadingToGoal();
    ballDistance+=(ABS(goalHeading)/180.0*ballDistance);

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

    // keeper cheats in bruce lee distance negotiation stuff... if the ball is within his bounds,
    // he will always chase. however, keeper bounds depend on exactly what he is doing.
    // this calculation takes this all into account.

    // what bounds should we be using ?
    bool useEnlargedBounds = (InterruptibleChase() && !PostChaseSearchForBall());
    bool withinBounds = false;
    // use bounds that we want
    if (useEnlargedBounds) {
      withinBounds = IsBallWithinEnlargedChaseBounds();
    } else {
      withinBounds = IsBallWithinChaseBounds();
    }

    if (myPosition == BL_GOALKEEPER && withinBounds) {
      ballDistance = 1.5;
    } else if (myPosition == BL_GOALKEEPER && !withinBounds) {
      // 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;
        ballDistance = 524.0;
      } else {
        ballDistance = 524.0;
      }
    }
  }
  //doing a chase and not interruptible AND we're too fucking close (head tilt measure), FUCK OFF AWAY FROM US!!
  if (NonInterruptibleChase()) {
    ballDistance = 1.0;
  } else if ((Chase() && !PostChaseSearchForBall() && !IsBallVisible()) || (MoveToPoint() && strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")==0 && vo_ball_ == NULL)) {
    // chase or MTP has momentarily lost the ball. that's ok. don't fuck him up !!                                                                                 
    ballDistance = lastBallDistance;
  }
 
  lastBallDistance = ballDistance;
//  cout << ballDistance << " " << endl;
  return ballDistance;
}