kickselecta.cc

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

      minTurnAmount = maxTurnAmount;
      maxTurnAmount = maxTurnAmount;
    }
  } else {
    minTurnAmount = MAX(30.0,ABS(oppGoalHeading)+20.0);
    maxTurnAmount = 90.0; // 90

    if (wo_self_->y_ < 20.0  && ABS(oppGoalHeading) < 40) maxTurnAmount = 0.0;
   
    if (oppGoalHeading > 120) {
      minTurnAmount = maxTurnAmount;
      maxTurnAmount = maxTurnAmount;
    } else if (oppGoalHeading > 70 && oppGoalHeading < 120) {
      minTurnAmount = 0.0;
      maxTurnAmount = 0.0;
    } else if (oppGoalHeading >= (0)) {      
      minTurnAmount = minTurnAmount;
      maxTurnAmount = maxTurnAmount;
  //--------------------------------------------------------------
    } else if (oppGoalHeading < -120) {
      minTurnAmount = maxTurnAmount;
      maxTurnAmount = maxTurnAmount;
    } else if (oppGoalHeading < -70 && oppGoalHeading > -120) {
      minTurnAmount = 0.0;
      maxTurnAmount = 0.0;
    } else if (oppGoalHeading < 0) {
      minTurnAmount = -minTurnAmount;
      maxTurnAmount = -maxTurnAmount;
    }
  }

  // complexKick = new BasicTricks::MultiTrick(2, new BallTricks::GrabTrick(), new BasicTricks::SelectaTrick(2)); 
 
  int maxSteps = 1;
 /* if (wo_self_->x_ < 30 && wo_self_->y_ < -180 && ABS(minTurnAmount) > 60) { 
    minTurnAmount*=2;  // needs testing
    maxTurnAmount*=2;
    maxSteps = 2;
  }*/
  if (complexKick == NULL) {
    if (ABS(maxTurnAmount) < 0.01 && ABS(oppGoalHeading) < 100)
       complexKick = new BasicTricks::MultiTrick(1, new BasicTricks::SelectaTrick(2)); // 2 means no grab by default and also no turn
    else {
       complexKick = new BasicTricks::MultiTrick(3, new BallTricks::GrabTrick(), new BallTricks::TurnWithBallUntilGoal(minTurnAmount,maxTurnAmount, maxSteps), new BasicTricks::SelectaTrick(0)); 
    }
  }
  return new BasicTricks::MultiTrick(true, 1, complexKick);

/*
  if (complexKick==NULL) {
    cout << "NormalSelecta: Logic flaw! Returning default kick." << endl << flush;
    return new BasicTricks::MultiTrick(2, new BasicTricks::Kick(16), new BallTricks::WaitForKick());
  }*/

#endif
}

Trick* KickSelecta::FastSelecta210(double currx, double curry, double currheading, int type) {
#ifdef ERS_210
    double goalHeading = utils.GetHeadingToGoal();

  // make sure globals are clear
  lastKickedLeft_ = -100;
  lastKickedRight_ = -100;

  Trick* kick = NULL;
  // goal is practically lined up. hammer it in!
  if (ABS(goalHeading) < 30.0) {
    if (curry <= 150) {
      kick = new BasicTricks::MultiTrick(3, new BasicTricks::Kick(LocomotionCommand::TP_GRAB),
                                            new BasicTricks::Kick(LocomotionCommand::TP_PAW_KICK),
                                            new BasicTricks::Kick(LocomotionCommand::TP_GETUP));
    } else {
      kick = new BasicTricks::Kick(LocomotionCommand::TP_CHEST_PUSH);
    }
  // goal is not far off being lined up. angled paw kick should get it in
  } /*else if (ABS(goalHeading) < 50.0) {
    if (curry <= 150) {
      if (goalHeading > 0) {
        kick = new BasicTricks::MultiTrick(2, new BasicTricks::Kick(LocomotionCommand::TP_PAW_KICK_LEFT),
                                              new BasicTricks::Kick(LocomotionCommand::TP_GETUP));
      } else {
        kick = new BasicTricks::MultiTrick(2, new BasicTricks::Kick(LocomotionCommand::TP_PAW_KICK_RIGHT),
                                              new BasicTricks::Kick(LocomotionCommand::TP_GETUP));
      }
    } else {
      kick = new BasicTricks::Kick(LocomotionCommand::TP_CHEST_PUSH);
    }
  // ok, we're still nowhere near the opposition goal :). spin !
  } */
  else {
    if (goalHeading > 0) {
      // we're in the back left corner, and we're about to spin kick left across our goal. don't allow this. spin kick right instead !
      if (goalHeading > (100) && curry < -150 && currx < -60) {
        kick = new BasicTricks::Kick(LocomotionCommand::TP_SLAP_RIGHT);
        lastKickedRight_ = frame_;
      } else if (goalHeading < 90  && curry >160 && ABS(currx) > 30) {
        kick = new BasicTricks::Kick(LocomotionCommand::TP_HEAD_KICK_LEFT);
        lastKickedLeft_ = frame_;
      } else {
        kick = new BasicTricks::Kick(LocomotionCommand::TP_SLAP_LEFT);
        lastKickedLeft_ = frame_;
      }
    } else {
      // we're in the back right corner, and we're about to spin kick right across our goal. don't allow this. spin left instead.
      if (goalHeading < (-100) && curry < -150 && currx > 60) {
        kick = new BasicTricks::Kick(LocomotionCommand::TP_SLAP_LEFT);
        lastKickedLeft_ = frame_;
      } else if (goalHeading > -90 && curry >160 && ABS(currx) > 30) {
        kick = new BasicTricks::Kick(LocomotionCommand::TP_HEAD_KICK_RIGHT);
        lastKickedRight_ = frame_;
      } else {
        kick = new BasicTricks::Kick(LocomotionCommand::TP_SLAP_RIGHT);
        lastKickedRight_ = frame_;
      }
    }
  }
  if (kick==NULL) {
    cout << "FastSelecta: Logic flaw! Returning default kick." << endl << flush;
    return new BasicTricks::MultiTrick(2, new BasicTricks::Kick(16), new BallTricks::WaitForKick());
  } else {
    return new BasicTricks::MultiTrick(2, kick, new BallTricks::WaitForKick());
  }
#endif
  return NULL;
}

Trick* KickSelecta::NormalSelecta210(double currx, double curry, double currheading) {
#ifdef ERS_210
 double goalHeading = utils.GetHeadingToGoal();

  double zeroHeading = 0.0-RAD_TO_DEG(currheading);
  //if (wo_self_->uncertHeading_ < 0.2 && ABS(RAD_TO_DEG(currheading)) < 40 && curry < 0) {
  //  goalHeading = 0.0;
  //} else {
  //  if (wo_self_->uncertHeading_ < 0.25 && ABS(RAD_TO_DEG(currheading)) < 20 && curry < 0) {
  //    goalHeading = 0.0;
  //  }
  //}

  // if in defensive half
  if (curry < 0) {
    if (ABS(currx) >= 0) { // always !!
      if (ABS(goalHeading) > ABS(zeroHeading)) {
        goalHeading = zeroHeading;
      }
    }
  }

  double minTurnAmount = MIN(35.0,ABS(goalHeading));
  if (minTurnAmount < 25.0 && wo_self_->uncertHeading_ > 0.2) minTurnAmount = 25.0;
  double maxTurnAmount = 90.0;

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

  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) || wo_self_->uncertHeading_ > 0.2 ) {
      complexKick = new ComplexTricks::ComplexKick(-minTurnAmount,-maxTurnAmount); // turn then spin kick RIGHT
    } else {
      complexKick = new ComplexTricks::ComplexKick(NULL);

    }
  } else if (goalHeading > (160)) {
      if (curry <= 50)
        complexKick = new ComplexTricks::ComplexKick(minTurnAmount,maxTurnAmount, 50); // turn then spin kick LEFT
      else
        complexKick = new ComplexTricks::ComplexKick(minTurnAmount,45.0, goalHeading-50.0);

  } else if (goalHeading > (120)) {
    complexKick = new ComplexTricks::ComplexKick(minTurnAmount,maxTurnAmount, 65); // turn then spin kick LEFT
  } else if (goalHeading > (80)) {
    // If we are uncertain of our heading then we should do a turn and hope to pick up a landmark 
    // and NOT kick in the wrong directon. This really pisses me off if it does it infront of the goal !!
    if (wo_self_->uncertHeading_ < 0.2)
      complexKick = new ComplexTricks::ComplexKick(NULL); // turn then spin kick LEFT
    else
      complexKick = new ComplexTricks::ComplexKick(minTurnAmount,maxTurnAmount); 
  } 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) || wo_self_->uncertHeading_ > 0.2) {
      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 < (-160)) {
    if (curry <= 50)
        complexKick = new ComplexTricks::ComplexKick(-minTurnAmount,-maxTurnAmount, -50); // turn then spin kick RIGHT
      else
        complexKick = new ComplexTricks::ComplexKick(-minTurnAmount,-45.0, goalHeading + 50);
  } else if (goalHeading < (-120)) {
    complexKick = new ComplexTricks::ComplexKick(-minTurnAmount,-maxTurnAmount, -65); // turn then spin kick RIGHT
  } else if (goalHeading < (-80)) {
    // If we are uncertain of our heading then we should do a turn and hope to pick up a landmark 
    // and NOT kick in the wrong directon. This really pisses me off if it does it infront of the goal !!
    if (wo_self_->uncertHeading_ < 0.2)
      complexKick = new ComplexTricks::ComplexKick(NULL); // turn then spin kick LEFT
    else
      complexKick = new ComplexTricks::ComplexKick(-minTurnAmount,-maxTurnAmount);
  } else if (goalHeading < (0)) {
    complexKick = new ComplexTricks::ComplexKick(-minTurnAmount,-maxTurnAmount); // turn then pawkick/chest push
  }

  if (complexKick==NULL) {
    cout << "NormalSelecta: Logic flaw! Returning default kick." << endl << flush;
    return new BasicTricks::MultiTrick(2, new BasicTricks::Kick(16), 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);
*/
  return new BasicTricks::MultiTrick(true, 1, complexKick);
#endif
  return NULL;
}


double KickSelecta::GetDistanceToGoal(double currx, double curry, double currheading) {
  // enemy goal coords
  double x_ = 0;
  double y_ = 215;
  if (curry>210) curry=210;

  // heading to goal based on WM.
  double goalDistance = sqrt( (currx-x_)*(currx-x_) + (curry-y_)*(curry-y_) );

  // we can see opposition goal ! Use vision instead of WM
  if (vo_oppositionGoal_ != NULL) {
    goalDistance = vo_oppositionGoal_->distance_;
  }
   
  return goalDistance;
}