sleek.cc

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

#include "Sleek.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"

// Sleek is elite. 2004 high level behaviour code.
void Sleek::CalculateHomePosition() {
  actualHomeX = homeX;
  actualHomeY = 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;
    actualHomeX = homeX+homeRadius*sin(theta)*2.0;
    actualHomeY = homeY+homeRadius*cos(theta)*0.30;
    if (actualHomeY < -210) actualHomeY = -210;
    if (actualHomeY > -200) actualHomeY = -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) {
      actualHomeY = -210;
      actualHomeX = 35;            
    }
    else if (wo_ball_->y_ < - 160 && wo_ball_->x_ < -65 ) {
      actualHomeY = -210;
      actualHomeX = -35;
    } else { // else its far away or in the middle - use normal code 
      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;
      actualHomeX = homeX+homeRadius*sin(theta)*2.0;
      actualHomeY = homeY+homeRadius*cos(theta)*0.30;
      if (actualHomeY < -210) actualHomeY = -210;
    }
  } else { // not keeper !
    if (myPosition == BL_CENTREDEFENDER /*|| myPosition == BL_CENTRESTRIKER (MQ 5/7)*/) {
      actualHomeX = (wo_ball_->x_ + homeX)/2.0;
    } else if (myPosition == BL_LEFTSTRIKER || myPosition == BL_RIGHTSTRIKER) {
      actualHomeY = (wo_ball_->y_ + homeY)/2.0;
      if (actualHomeY<(homeY+10)) actualHomeY = homeY;
    } 
  }

  // extra bounds on position if not the keeper ..
  if (myPosition != BL_GOALKEEPER) {
    if (actualHomeY > chaseUpperY-boundOffset) actualHomeY = chaseUpperY-boundOffset;
    if (actualHomeY < chaseLowerY+boundOffset) actualHomeY = chaseLowerY+boundOffset;
    if (actualHomeY < -150) actualHomeY = -150;

    if (actualHomeX > chaseUpperX-boundOffset) actualHomeX = chaseUpperX-boundOffset;
    if (actualHomeX < chaseLowerX+boundOffset) actualHomeX = chaseLowerX+boundOffset;
  }
}


void Sleek::NewPlayingModel() {
  CheckWireless();
  CalculateHomePosition();

  if (!inPlaying) {
    if (myPosition != BL_GOALKEEPER && oneInchPunch && (frame_ - lastReadyFrame_) < 30 && robotState_.GetKickOff() == RobotState::KO_OPPONENT) {
      lcq_.Clear();
      ChangeTrick(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()));
      ChangeTrick(headTrick, new HeadTricks::LookAt(-10.0,0.0,0.0,35));
   //   ChangeTrick(currentTrick, new ComplexTricks::AdvancedChaseWithKick());
   //   ChangeTrick(headTrick, new BasicTricks::NullHead());
    } else {
      ChangeTrick(currentTrick, new BasicTricks::NullBody());
      ChangeTrick(headTrick, new BasicTricks::NullHead());
    }
  }
  inPlaying=true;
  inGoHome=false;

  int hC = headTrick->Continue();
  if (hC < 1) {
    if (currentTrick->IsUsingHead()) {
      ChangeTrick(headTrick, new BasicTricks::NullHead());
    } else {
      ChangeTrick(headTrick, ConstructMoveToPointHead());
    }
  }
  // watching the ball and moving home. count!
  // flick look code

  watchingBallCounter++; // counter since we last did headpan !
  if (strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")==0 && strcmp(currentTrick->GetName(),"MoveToPoint")==0) {
    if (vo_ball_ == NULL || ABS(vo_ball_->distance_) > 40.0) {
      if (watchingBallCounter > 120 && (myPosition == BL_GOALKEEPER && wo_self_->uncertX_+wo_self_->uncertY_ > 60)) {
        ChangeTrick(headTrick, new HeadTricks::QuickPan(30,!lastLookedLeft));
        lastLookedLeft = !lastLookedLeft;
        watchingBallCounter = 0;
      }
    }
  } 
  
  if (!IsBallVisibleTeamNotSelf() && strcmp(headTrick->GetName(),"QuickPan")==0 && strcmp(currentTrick->GetName(),"MoveToPoint")==0) {
    if (vo_ball_ == NULL || ABS(vo_ball_->distance_) > 40.0) {
      if (wo_self_->uncertX_+wo_self_->uncertY_ < 20) { // pretty certain of possible. get out !
        ChangeTrick(headTrick, ConstructMoveToPointHead());
      }
    }
  }

  currentTrickResult = currentTrick->Continue();
  double ballDistance = CalculateBallDistance();

  //executing normally. we may wish to interrupt.
  if (currentTrickResult >= 1) {
    // we're searching but somebody else has seen the ball .. maybe we don't need to search anymore
    if (SearchForBall()) {
       // If somebody can see it and its not close to me then i should just look where they say it is
       if (IsBallVisibleTeam() && (pow(wo_self_->x_ - wo_ball_->x_, 2)+pow(wo_self_->y_ - wo_ball_->y_, 2) > 30*30) && !nonStopSearch) {
         ChangeTrick(currentTrick, new MovementTricks::MoveToPoint(actualHomeX,actualHomeY,0.0,MovementTricks::MoveToPoint::MTP_DYNAMIC_FACE_BALL));
         if (myPosition == BL_GOALKEEPER) ((MovementTricks::MoveToPoint*)(currentTrick))->SetKeeper(true);
       }
    // we're chasing, but we're allowed to interrupt. Also don't interrupt if we are close to the ball.
    // if we're doing postchasesearchforball w/i advancedchasewithkick, we cannot force it to exit out here.
    } else if (InterruptibleChase() && !PostChaseSearchForBall()) {
      // NB - don't necessarily need to be able to see the ball to be allowed to continue chasing (!)
      // therefore CANNOT use AmIAllowedToChase() here !!
      if (IsBallWithinEnlargedChaseBounds() == false || ClosestToBallNotSelf() < ballDistance) {
        if (debugOutput)
          cout << "Sleek: Aborted AdvancedChaseWithKick (chase bound exceeded)." << endl << flush;
        // change to move home and look around accordingly
        ChangeTrick(currentTrick, new MovementTricks::MoveToPoint(actualHomeX,actualHomeY,0.0,MovementTricks::MoveToPoint::MTP_DYNAMIC_FACE_BALL));
        ChangeTrick(headTrick, ConstructMoveToPointHead());
        if (myPosition == BL_GOALKEEPER) ((MovementTricks::MoveToPoint*)(currentTrick))->SetKeeper(true);
      }
    // currently moving home or searching.
    } else if (MoveToPoint()) {
      // ball is within our chase area !
      if (AmIAllowedToChase(ballDistance)) {
        if (debugOutput)
          cout << "Sleek: Ball within chase area. Aborting MoveToPoint and chasing." << endl << flush;
    
        ChangeTrick(currentTrick, new ComplexTricks::AdvancedChaseWithKick());
        ChangeTrick(headTrick, new BasicTricks::NullHead());

      } 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);
             } */
            if (!strutSystem_.isKicking && lcq_.IsWalkOnly()) {
              utils.Kick(LocomotionCommand::TP_GOALIE_BLOCK, false);
              ballPrediction_.Clear();
            }
        }
      } else if (!IsBallVisible() && (pow(wo_self_->x_ - wo_ball_->x_, 2)+pow(wo_self_->y_ - wo_ball_->y_, 2) < 30*30) && strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")!=0) {
        nonStopSearch = true;
//        if (pow(wo_self_->x_ - wo_ball_->x_, 2)+pow(wo_self_->y_ - wo_ball_->y_, 2) < 40*40) nonStopSearch = true;
        ChangeTrick(currentTrick, new MovementTricks::SearchForBall(180.0));
        ChangeTrick(headTrick, new BasicTricks::NullHead());
      } else {
        // not chasing, not blocking. reposition instead.
        ((MovementTricks::MoveToPoint*)(currentTrick))->ResetHome((int)actualHomeX,(int)actualHomeY);    

      }
    }
    // shit finished. decide what to do !
  } else if (currentTrickResult < 1) {
    if (AmIAllowedToChase(ballDistance)) {
      if (debugOutput)
        cout << "Sleek: Ball within chase area, previous trick has ended. Chasing." << endl << flush;

      ChangeTrick(currentTrick, new ComplexTricks::AdvancedChaseWithKick());
      ChangeTrick(headTrick, new BasicTricks::NullHead());
    } 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 (!SearchForBall() && (strcmp(currentTrick->GetName(), "PostChaseSearchForBall")!=0) && (IsBallVisible() == false) && (IsSelfWithinChaseBounds()) && strcmp(headTrick->GetName(),"FollowBallWithHeadSticky")!=0 && strcmp(headTrick->GetName(),"QuickPan")!=0) {
        if (debugOutput)
          cout << "Sleek: Searching for ball." << endl << flush;

        nonStopSearch = false;
        if (pow(wo_self_->x_ - wo_ball_->x_, 2)+pow(wo_self_->y_ - wo_ball_->y_, 2) < 40*40) nonStopSearch = true;

        ChangeTrick(currentTrick, new MovementTricks::SearchForBall(360.0));
        ChangeTrick(headTrick, new BasicTricks::NullHead());

      } 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!
        ChangeTrick(currentTrick, new MovementTricks::MoveToPoint(actualHomeX,actualHomeY,0.0,MovementTricks::MoveToPoint::MTP_DYNAMIC_FACE_BALL));
        ChangeTrick(headTrick, ConstructMoveToPointHead());  
        if (myPosition == BL_GOALKEEPER) ((MovementTricks::MoveToPoint*)(currentTrick))->SetKeeper(true);    
      }
    }
  }

  if (isCaptain) {
    RunCaptain();
  }

  // Send wireless behaviour info (1 if doing a chase otherwise 0) 
  bool isChasing = false;
  if (Chase() && !PostChaseSearchForBall()) {
    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 very rapidly. output ! check for oscillatory crap happening..
/*if (strcmp(currentTrick->GetName(),previousTrick)!=0) {
    if (frame_-previousTrickFrame == 1) {
      cout << "Switched from " << previousTrick << " to " << currentTrick->GetName() << endl << flush;
    }
    previousTrick = currentTrick->GetName();
    previousTrickFrame = frame_;
  }*/

  if (emergencyTurnAround_ && lcq_.IsWalkOnly()) {