penaltytricks.cc

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

    double absMaximumTurn = ABS(maximumTurn);
    turn = MIN(absBallHeading, absMaximumTurn);
	}

/* experiment! don't use
	if (absBallHeading > 30) {
    forward = MIN(ballDistance*10.0,20.0)*cos(vo_ball_->heading_);
    strafe = MIN(ballDistance*10.0,20.0)*sin(vo_ball_->heading_);
    turn = MIN(absBallHeading, 30.0);
	}	else {
    forward = MIN(ballDistance*5.0,110.0)*cos(vo_ball_->heading_);
    strafe = MIN(ballDistance*10.0,15.0)*sin(vo_ball_->heading_);
    forward = MIN(forward, 95.0);
    double maximumTurn = 18.0 - sqrt(forward*forward/100.0 + strafe*strafe/100.0);
    double absMaximumTurn = ABS(maximumTurn);
    turn = MIN(absBallHeading, absMaximumTurn);
	}
*/
	// set turn direction - negative value means turn clockwise (RIGHT)
	if (ballHeading < 0) {
    turn = -turn;
  }

  double speed = 1.54;
  double fso = 9.0;
  double ffo = 60.0;
/*double originalForward = forward;
  double originalFso = 2.0;
  double originalFfo = ffo;*/
  // we slow down here if required...
  hardOn=false;
  if (hardOn == false) {
    if (ABS(HeadPan) <= 450000) {
      // THESE FSOs are NOT USED!!!!
if (HeadTilt < DEG_TO_MICRO(8)) { aimAtMiddleOfBall_ = true;}
 /*     if (HeadTilt < DEG_TO_MICRO(-20)) { forward = forward*0.9; fso = 14.0; ffo = 60; aimAtMiddleOfBall_ = true;}
      else if (HeadTilt < DEG_TO_MICRO(-10)) { forward = forward*0.8; fso = 14.0; ffo = 60;aimAtMiddleOfBall_ = true;}
      else if (HeadTilt < DEG_TO_MICRO(0)) { forward = forward*0.9; fso = 8.0; ffo = 65;aimAtMiddleOfBall_ = true;}
      else if (HeadTilt < DEG_TO_MICRO(8)) { aimAtMiddleOfBall_ = true;} */
    }
  }
//fso=9.0;
/*
  if (hardOn == false) {
    if (ABS(HeadPan) <= 450000) {
      if (HeadTilt < DEG_TO_MICRO(-20)) { forward = forward*0.8; fso = 14.0; ffo = 65; aimAtMiddleOfBall_ = true;}
      else if (HeadTilt < DEG_TO_MICRO(-10)) {  fso = 14.0; ffo = 65;aimAtMiddleOfBall_ = true;}
      else if (HeadTilt < DEG_TO_MICRO(0)) {  fso = 8.0; ffo = 65;aimAtMiddleOfBall_ = true;}
      else if (HeadTilt < DEG_TO_MICRO(8)) { aimAtMiddleOfBall_ = true;}
    }
  }
*/
  // new walk around ball code... should be able to get it off the back walls as well.
  bool walkLeftAroundBall = false;
  bool walkRightAroundBall = false;
  if (vo_ball_->distance_ < maxWalkAroundDistance && vo_ball_->distance_ > 0) {
    // make sure we're not already walking around the ball
    if ( (lcq_.IsTheStrutReady()) || (lcq_.IsWalkOnly()) ) {
      // check if ball is close to side and we're facing wrong way
      if (ABS(wo_ball_->x_) > 70 && ABS(wo_self_->heading_) > DEG_TO_RAD(140)) {
        if (wo_ball_->x_ < 0 && wallOnRight_) {
          walkLeftAroundBall=true;
        }
        if (wo_ball_->x_ > 0 && wallOnLeft_) {
          walkRightAroundBall=true;
        }
      }
      // check if ball is on home wall
      if (wo_ball_->y_ < -165) {
        double h = RAD_TO_DEG(wo_self_->heading_);
        double minHeading = -130;
        double maxHeading = -80;
        // check if we're on the left side of our own goal and kind of facing towards it, with wall to the right of the ball
        if (wo_self_->x_ < 0 && wo_ball_->x_ < 0 && h < maxHeading && h > minHeading && wallOnRight_) {
          walkLeftAroundBall=true;
        }
        minHeading = 80;
        maxHeading = 130;
        // check if we're on the right side of our own goal and kind of facing towards it, with wall to the left of the ball
        if (wo_self_->x_ > 0 && wo_ball_->x_ > 0 && h < maxHeading && h > minHeading && wallOnLeft_) {
          walkRightAroundBall=true;
        }
      }

      // check if ball on opposition wall
      if (wo_ball_->y_ > 165) {
        double h = RAD_TO_DEG(wo_self_->heading_);
        double minHeading = 50;
        double maxHeading = 115;
        // check if we're on the left side of the opposition goal (using our normal coords) and kind of facing away from it, with the ball on our right
        if (wo_self_->x_ < 0 && wo_ball_->x_ < 0 && h < maxHeading && h > minHeading && wallOnRight_) {
          walkLeftAroundBall=true;
        }
        minHeading = -115;
        maxHeading = -50;
        // check if we're on the right side of the opposition goal and kind of facing away from it, with wall to the left of the ball
        if (wo_self_->x_ > 0 && wo_ball_->x_ > 0 && h < maxHeading && h > minHeading && wallOnLeft_) {
          walkRightAroundBall=true;
        }
      }
    }
  }
  if (walkRightAroundBall && !walkLeftAroundBall) {
    // we want to step right a bit to try and get around the ball
    forward = forward*0.2;
    strafe = (-80.0);
    turn += 3.0;
    if (turn > 8.0) turn = 8.0;
    LocomotionCommand lc;
    lc.Set(LocomotionCommand::TP_SINGLEWALK,forward,forward,turn,strafe,1.45);
    lc.frontSideOffset = fso;
    lc.frontForwardOffset = ffo;
    utils.RawCommand(lc);
    utils.RawCommand(lc);
    utils.RawCommand(lc);
    return 1;
  } else if (walkLeftAroundBall && !walkRightAroundBall) {
    // we want to step left a bit to try and get around the ball
    forward = forward*0.2;
    strafe = (80.0);
    turn -= 3.0;
    if (turn < -8.0) turn = -8.0;
    LocomotionCommand lc;
    lc.Set(LocomotionCommand::TP_SINGLEWALK,forward,forward,turn,strafe,1.45);
    lc.frontSideOffset = fso;
    lc.frontForwardOffset = ffo;
    utils.RawCommand(lc);
    utils.RawCommand(lc);
    utils.RawCommand(lc);
    return 1;
  } else if (walkLeftAroundBall && walkRightAroundBall) {
    // umm what ??
  }

  // can't use a normal step here... have to bypass the trick AND part of the utils abstraction layer
  // in order to set the front side offset how we want it.
  LocomotionCommand lc;
  speed = speed*slowDownRate;
  lc.Set(LocomotionCommand::TP_WALK,forward,forward,turn,strafe,speed);
//  lc.frontSideOffset = fso;
//  lc.frontForwardOffset = ffo;
  utils.RawCommand(lc);
  return 1;
}
bool PenaltyTricks::ChaseBall::IsUsingHead() {
  return true;
}
char* PenaltyTricks::ChaseBall::GetErrorMsg(int msg) {
  if (msg == 2) return "CLOSE_SEARCH";
  else if (msg == 1) return "CHASING_BALL";
  else if (msg == 0) return "BALL_UNDER_CHIN";
  else if (msg == -1) return "LOST_BALL";
  else if (msg == -2) return "BLOCKED";
  return "CB:NO_MESSAGE";
}
char* PenaltyTricks::ChaseBall::GetName() {
  return "ChaseBall";
}


// END CHASEBALL
////////////////////////////////////////////////////////////////

Trick* KickSelecta::PenaltySelecta(double currx, double curry, double currheading) {

// FIXME this might be a good idea still
//if (ABS(currx) > 80) return NormalSelecta(currx,curry,currheading);

  // enemy goal coords
  double x_ = 0;
  double y_ = 215;
  if (curry > 210) curry = 210;
  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(10) && goalHeadingRight_ > DEG_TO_RAD(-10)) {
      goalHeading = 0.0;
    } else {
      double temp = RAD_TO_DEG(goalHeadingLeft_)-10.0;
      if (ABS(temp) < ABS(goalHeading)) {
        goalHeading = temp;
      } 
      temp = RAD_TO_DEG(goalHeadingRight_)+10.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;
      }
    }
  }
  double minTurnAmount = MIN(135.0,ABS(goalHeading));
  double maxTurnAmount = 135.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 > (120)) {
    complexKick = new PenaltyTricks::ComplexKick(30,turnIncrement,goalHeading-50.0); // turn then spin kick LEFT
  } else if (goalHeading > (80)) {
    complexKick = new PenaltyTricks::ComplexKick((minTurnAmount-turnIncrement),maxTurnAmount,65); //straight out spin kick LEFT
  } else if (goalHeading >= (0)) {
    complexKick = new PenaltyTricks::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 < (-120)) {
    complexKick = new PenaltyTricks::ComplexKick(-30,-turnIncrement,goalHeading+50.0); // turn then spin kick RIGHT
  } else if (goalHeading < (-80)) {
    complexKick = new PenaltyTricks::ComplexKick(-(minTurnAmount-turnIncrement),-maxTurnAmount,-65); // straight out spin kick RIGHT
  } else if (goalHeading < (0)) {
    complexKick = new PenaltyTricks::ComplexKick(-minTurnAmount,-maxTurnAmount); // turn then pawkick/chest push
  }

  if (complexKick==NULL) {
    cout << "PenaltySelecta: 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());
  }
  return new BasicTricks::MultiTrick(true, 1, complexKick);
}

Trick* KickSelecta::FastPenaltySelecta(double currx, double curry, double currheading) {
  // enemy goal coords

  double x_ = 0;
  double y_ = 215;
  if (curry > 210) curry = 210;
  double theta = (atan2(y_-curry, x_-currx) - (PI / 2.0)) - currheading;

  // heading to goal based on WM.
  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(10) && goalHeadingRight_ > DEG_TO_RAD(-10)) {
      goalHeading = 0.0;
    } else {
      double temp = RAD_TO_DEG(goalHeadingLeft_)-10.0;
      if (ABS(temp) < ABS(goalHeading)) {
        goalHeading = temp;
      } 
      temp = RAD_TO_DEG(goalHeadingRight_)+10.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_);
    //cout << "ownGoal->heading = " << RAD_TO_DEG(vo_ownGoal_->heading_) << ", goalHeading: " << goalHeading << endl << flush;
  }


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

  // make sure globals are clear
  Trick* kick = NULL;
  // goal is practically lined up. hammer it in!
  if (ABS(goalHeading) < 25.0) {
    if (curry <= 130) {
      kick = new BasicTricks::MultiTrick(2, 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 {
    HeadCommand hc;
    hc.Set(-15.0,0.0,0.0,true);
    lcq_.SetHeadCommand(hc);
    kick = new BasicTricks::Step(LocomotionCommand::TP_SINGLEWALK,-60,-60,0.0,0.0,1.2);
  }
  if (kick==NULL) {
    cout << "FastPenaltySelecta: 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());
  }
}

//////////////////////////////////////////////////////////////////////
// BEGIN ComplexKick

//Worst documentation ever!!

//ComplexKick will turn the specified 'normal' amount (first parameter). If it sees the goal
//  at any angle up to its 'maximum' amount (second parameter), it'll try to line it up.
//Notice that if the first two parameters are both 0.0, we need not do a turn at all.

//Once all turning has been completed, we make a kick decision by calling FastSelecta.
//  FastSelecta can only do kicks- it's not allowed to turn. The idea is, we delay the
//  actual kick decision for as long as possible.

//A handy side effect is that all kick decisions are back in KickSelecta now.

PenaltyTricks::ComplexKick::ComplexKick(double _normalTurnAmount, double _maxTurnAmount, double _extraFastTurn) {
  currentTrick = NULL;
  turnTrick = NULL;
  // we aren't meant to do a turn. so don't. just choose a kick and start doing it.
  if (_normalTurnAmount == 0.0 && _maxTurnAmount == 0.0) {
    KickSelecta ks;
    currentTrick = ks.FastSelecta(wo_self_->x_, wo_self_->y_, wo_self_->heading_);
  // go the turn !
  } else {
    //currentTrick = new BallTricks::TurnWithBallUntilGoal(_normalTurnAmount,_maxTurnAmount);
    if (ABS(_extraFastTurn) > 65) {
      currentTrick = new BasicTricks::MultiTrick(2, new BallTricks::TurnWithBallHeadDown(_extraFastTurn), new BallTricks::TurnWithBallUntilGoal(_normalTurnAmount,_maxTurnAmount, 1));
    } else {
      currentTrick = new BasicTricks::MultiTrick(2, new BallTricks::TurnWithBallHeadDown(_extraFastTurn), new BallTricks::TurnWithBallUntilGoal(_normalTurnAmount,_maxTurnAmount));
    }
    turnTrick = currentTrick;
  }
}

PenaltyTricks::ComplexKick::ComplexKick(double _normalTurnAmount, double _maxTurnAmount) {
  currentTrick = NULL;
  turnTrick = NULL;
  // we aren't meant to do a turn. so don't. just choose a kick and start doing it.
  if (_normalTurnAmount == 0.0 && _maxTurnAmount == 0.0) {
    KickSelecta ks;
    if (wo_self_->y_ < 0) {
      currentTrick = ks.FastSelecta(wo_self_->x_, wo_self_->y_, wo_self_->heading_);
    } else {
      currentTrick = ks.FastPenaltySelecta(wo_self_->x_, wo_self_->y_, wo_self_->heading_);
    }
  // go the turn !
  } else {
    currentTrick = new BallTricks::TurnWithBallUntilGoal(_normalTurnAmount,_maxTurnAmount);
    turnTrick = currentTrick;
  }
}

PenaltyTricks::ComplexKick::ComplexKick(Trick* _turnTrick) {
  currentTrick = NULL;
  // we aren't meant to do a turn. so don't. just choose a kick and start doing it.
  if (_turnTrick == NULL) {
    KickSelecta ks;
    currentTrick = ks.FastPenaltySelecta(wo_self_->x_, wo_self_->y_, wo_self_->heading_);
  // go the turn !
  } else {
    currentTrick = _turnTrick;
//  currentTrick = new BallTricks::TurnWithBallUntilGoal(_normalTurnAmount,_maxTurnAmount);
  }
  turnTrick = _turnTrick;
}
PenaltyTricks::ComplexKick::~ComplexKick() {
  delete currentTrick;
}
int PenaltyTricks::ComplexKick::Start() {
  return currentTrick->Start(); 
}
int PenaltyTricks::ComplexKick::Abort() {
  return currentTrick->Abort();
}
int PenaltyTricks::ComplexKick::Continue() {
  int cC = currentTrick->Continue();
  // current trick completed.
  if (cC < 1) {
//  char* cMsg = currentTrick->GetErrorMsg(cC);

    // finished our turn with ball trick. evaluate kick !
//  if (strcmp(currentTrick->GetName(),"TurnWithBallUntilGoal")==0) {
    if (turnTrick == currentTrick) {
      turnTrick = NULL;
      delete currentTrick;

      KickSelecta ks;
      // evaluate kick. we may have more information now than we did before sooo :)
      currentTrick = ks.FastPenaltySelecta(wo_self_->x_, wo_self_->y_, wo_self_->heading_);
      currentTrick->Start();

    // otherwise we've completed our kick ! get out !
    } else {
      return cC;
    }
  }
  return 40;
}
bool PenaltyTricks::ComplexKick::IsUsingHead() {
  return true;
}
char* PenaltyTricks::ComplexKick::GetErrorMsg(int m) {
  if (m == 40) {
    return "COMPLEXKICK_IN_PROGRESS";
  } else {
    return currentTrick->GetErrorMsg(m);
  }
}
char* PenaltyTricks::ComplexKick::GetName() {
  return "ComplexKick";
}
// END ComplexKick
//////////////////////////////////////////////////////////////////////