keeperselecta.cc

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

#include "KickSelecta.h"
#include "../Common/Common.h"
#include "Trick.h"
#include "BallTricks.h"
#include "BasicTricks.h"
#include "ComplexTricks.h"


Trick* KickSelecta::KeeperSelecta(double currx, double curry, double currheading) {
  // enemy goal coords
  double x_ = 0;
  double y_ = 215;

  double theta = (atan2(y_-curry, x_-currx) - (PI / 2.0)) - currheading;

  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(5) && goalHeadingRight_ > DEG_TO_RAD(-5)) {
      goalHeading = 0.0;
    } else {
      double temp = RAD_TO_DEG(goalHeadingLeft_)-5.0;
      if (ABS(temp) < ABS(goalHeading)) {
        goalHeading = temp;
      } 
      temp = RAD_TO_DEG(goalHeadingRight_)+5.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_);
  }

  while (goalHeading > 180.0) goalHeading -= 360.0;
  while (goalHeading <= -180.0) goalHeading += 360.0;

  double zeroHeading = 0.0-RAD_TO_DEG(currheading);
  // if in defensive half
  if (curry < 0) {
    if (ABS(currx) > 90) {
      if (ABS(goalHeading) > ABS(zeroHeading)) {
        goalHeading = zeroHeading;
      }
    }
  }

  // start moving ye olde head up ASAP
  HeadCommand hc;
  hc.Set(-15.0,0.0,0.0,true);
  lcq_.SetHeadCommand(hc);

  double minTurnAmount = MIN(45.0,ABS(goalHeading));
  double maxTurnAmount = 90.0;
//double turnIncrement = 45.0;

  Trick* complexKick = NULL;
  // we're in the back left corner and we're likely to spin LEFT across our own goal.
  // can't allow this. spin RIGHT instead.
  if (goalHeading > (100) && curry < -150 && currx < 0) {
    if (goalHeading > (120)) {
      complexKick = new ComplexTricks::ComplexKick(-minTurnAmount,-maxTurnAmount); // turn then spin kick RIGHT
    } else {
      complexKick = new ComplexTricks::ComplexKick(NULL); //straight out spin kick RIGHT
    }
  } else /*if (goalHeading > (150)) {
    complexKick = new ComplexTricks::ComplexKick(new BallTricks::TurnWithBallHeadDown(135.0));
  } else */if (goalHeading > (120)) {
    complexKick = new ComplexTricks::ComplexKick(minTurnAmount,maxTurnAmount); // turn then spin kick LEFT
  } else if (goalHeading > (80)) {
    complexKick = new ComplexTricks::ComplexKick(NULL); //straight out spin kick LEFT
  } else if (goalHeading >= (0)) {
    complexKick = new ComplexTricks::ComplexKick(minTurnAmount,maxTurnAmount); // turn then pawkick/chest push
  }

  // we're in the back right corner and we're likely to spin RIGHT across our own goal.
  // can't allow this. spin LEFT instead.
if (goalHeading < (-100) && curry < -150 && currx > 0) {
    if (goalHeading < (-120)) {
      complexKick = new ComplexTricks::ComplexKick(minTurnAmount,maxTurnAmount); // turn then spin kick LEFT
    } else {
      complexKick = new ComplexTricks::ComplexKick(NULL); //straight out spin kick LEFT
    }
  } else /*if (goalHeading < (-150)) {
    complexKick = new ComplexTricks::ComplexKick(new BallTricks::TurnWithBallHeadDown(-135.0));
  } else*/ if (goalHeading < (-120)) {
    complexKick = new ComplexTricks::ComplexKick(-minTurnAmount,-maxTurnAmount); // turn then spin kick RIGHT
  } else if (goalHeading < (-80)) {
    complexKick = new ComplexTricks::ComplexKick(NULL); // straight out spin kick RIGHT
  } else if (goalHeading < (0)) {
    complexKick = new ComplexTricks::ComplexKick(-minTurnAmount,-maxTurnAmount); // turn then pawkick/chest push
  }

  if (complexKick==NULL) {
    cout << "KeeperSelecta: Logic flaw! Returning default kick." << endl << flush;
    return new BasicTricks::MultiTrick(3, new BasicTricks::Kick(LocomotionCommand::TP_PAW_KICK), new BasicTricks::Kick(LocomotionCommand::TP_GETUP), new BallTricks::WaitForKick());
  }
  // NOTE: We time this multitrick :). Should give us an idea of how long we're taking with the ball. It's not quite 
  //   accurate, as the ball has already left us when the multitrick completes. Under ~85 frames is probably legal ( < 135
  //   for the keeper 

  return new BasicTricks::MultiTrick(true, 3, new BasicTricks::Kick(LocomotionCommand::TP_GRAB),
                                              new BallTricks::WaitForGrab(),
                                              complexKick);

}