penaltytricks.cc

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

// Bag of primarily Keeper tricks

#include <math.h>

#include "PenaltyTricks.h"
#include "MovementTricks.h"
#include "BasicTricks.h"
#include "HeadTricks.h"
#include "ComplexTricks.h"
#include "BallTricks.h"
#include "Utilities.h"
#include "../Common/Common.h"
#include "../Globals.h"

void PenaltyTricks::ChaseWithKick::Init(double _chaseSlowDownRate) {
  currentTrick = new BasicTricks::NullBody();
  headTrick = new BasicTricks::NullHead();
  justKicked = false;
  interruptible = true;
  chaseSlowDownRate = _chaseSlowDownRate;
}

PenaltyTricks::ChaseWithKick::~ChaseWithKick() {
  delete currentTrick;
  delete headTrick;
}
int PenaltyTricks::ChaseWithKick::Start() {
  return Continue();
}
int PenaltyTricks::ChaseWithKick::Abort() {
  currentTrick->Abort();
  headTrick->Abort();
  return 0;
}
int PenaltyTricks::ChaseWithKick::Continue() {
  headTrick->Continue();
  int cC = currentTrick->Continue();
  char* cMsg = currentTrick->GetErrorMsg(cC);

  if (cC < 1) {
    delete currentTrick;
    if (strcmp(cMsg,"BALL_UNDER_CHIN") == 0) {
      // we want to clear the current step so the damn thing stops moving ASAP !
      utils.Step(LocomotionCommand::TP_WALK,0.0,0.0,0.0,0.0,1.0);
      if (wo_self_->y_ < 0) {
        currentTrick = ks.NormalSelecta(wo_self_->x_, wo_self_->y_, wo_self_->heading_);
      } else {
        currentTrick = ks.PenaltySelecta(wo_self_->x_, wo_self_->y_, wo_self_->heading_);
      }
      interruptible = false;
      justKicked = true;
    } else if (strcmp(cMsg,"SEE_BALL_AFTER_KICK") == 0 || vo_ball_ != NULL) {
      interruptible = true;
      currentTrick = new PenaltyTricks::ChaseBall(chaseSlowDownRate);
    } else {
      interruptible = true;
      currentTrick = new BasicTricks::NullBody();
      return 0;
    }
    currentTrick->Start();
  } else if (strcmp(cMsg,"GRAB_FAIL") == 0) {
    // if grab failed, we want to totally abort.
    // this generally means that a WaitForGrab ended and we could still see the ball (!!)
      cout << "ChaseWithKick: Failed grab." << endl << flush;
      currentTrick->Abort();
      interruptible = true;
      delete currentTrick;
      currentTrick = new PenaltyTricks::ChaseBall(chaseSlowDownRate);
      currentTrick->Start();
  }
  // if we've just lost the ball (CLOSE_SEARCH), we're not interruptible for this frame!
  if (interruptible && (strcmp(cMsg,"CLOSE_SEARCH")!=0) ) return 1;
  return 2;
}
bool PenaltyTricks::ChaseWithKick::IsUsingHead() {
  return true;
}
char* PenaltyTricks::ChaseWithKick::GetErrorMsg(int c) {
  if (c == 0) return "CWK_DONE";
  else if (c == 1) return "INTERRUPTIBLE";
  else if (c == 2) return "NOT_INTERRUPTIBLE";
  return "CWK:NO_MESSAGE";
}
char* PenaltyTricks::ChaseWithKick::GetName() {
  return "ChaseWithKick";
}
// END ChaseWithkick
//////////////////////////////////////////////////////////////////////
/*
void PenaltyTricks::ChaseBall::Init(double sdr) {
  underChinAngle = configuration_.GetAsDouble("UnderChinAngle");
  maxWalkAroundDistance = configuration_.GetAsDouble("MaxWalkAroundDistance");
  slowDownRate = sdr;
}


PenaltyTricks::ChaseBall::~ChaseBall() {
  delete headTrick;
}

int PenaltyTricks::ChaseBall::Start() { 
  headTrick = new HeadTricks::FollowBallWithHeadSticky(5);
  headTrick->Start();
  timeBallUnderChin = 0;
  return Continue();
}
int PenaltyTricks::ChaseBall::Abort() {
  return 0;
}

int PenaltyTricks::ChaseBall::Continue() {
  int hC = headTrick->Continue();
  char* hMsg = headTrick->GetErrorMsg(hC);
  
  // If we are searching close don't step!
  if ((strcmp(hMsg,"LOST_BALL_TEMP") == 0 )) {
    return 2;    
  }

  // head trick ended... ie, we lost the ball !
  if (hC < 1) {
    return -1;
  }
  double HeadPan = sensorValues_[S_HEAD_PAN];
  double HeadTilt = sensorValues_[S_HEAD_TILT];

  bool hardOn = false;

  // check ball under chin !
  if ((HeadTilt <= DEG_TO_MICRO(underChinAngle)) && (HeadPan  <= DEG_TO_MICRO(15)) && (HeadPan  >= DEG_TO_MICRO(-15)) ) {
    if (timeBallUnderChin > 3) {
      timeBallUnderChin = 0;
      return 0;
    } else {
      timeBallUnderChin++;
      LocomotionCommand lc;
      lc.Set(LocomotionCommand::TP_WALK,10,11,0,0,1.6);
      lc.frontSideOffset = 14.0;
      lc.frontForwardOffset = 70.0;
      utils.RawCommand(lc);

    }
  } else {
    timeBallUnderChin = 0;
  }

  if (vo_ball_ == NULL) return 1;

  double forward= 0;
  double turn = 0;
  double strafe = 0;


  double ballHeading = RAD_TO_DEG(vo_ball_->heading_);
  double ballDistance = ABS(vo_ball_->distance_);
  double absBallHeading = ABS(ballHeading);

  // calc walk params
	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*10.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.6;
  double fso = 2.0;
  double ffo = 60.0;
  double originalForward = forward;
  double originalFso = 2.0;
  double originalFfo = ffo;
  // we slow down here if required...
  if (hardOn == false) {
    if (ABS(HeadPan) <= 450000) {
      if (HeadTilt < DEG_TO_MICRO(-20)) { forward = forward*0.6; fso = 14.0; ffo = 65; aimAtMiddleOfBall_ = true;}
      else if (HeadTilt < DEG_TO_MICRO(-10)) { forward = forward*0.7; fso = 14.0; ffo = 65;aimAtMiddleOfBall_ = true;}
      else if (HeadTilt < DEG_TO_MICRO(0)) { forward = forward*0.8; 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_.Size() == 0) && (lcq_.IsTheStrutIdle() == true)) || (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.3;
    strafe = (-100.0);
    turn += 10.0;
    LocomotionCommand lc;
    lc.Set(LocomotionCommand::TP_SINGLEWALK,forward,forward,turn,strafe,speed);
    lc.frontSideOffset = fso;
    lc.frontForwardOffset = ffo;
    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.3;
    strafe = (100.0);
    turn -= 10.0;
    LocomotionCommand lc;
    lc.Set(LocomotionCommand::TP_SINGLEWALK,forward,forward,turn,strafe,speed);
    lc.frontSideOffset = fso;
    lc.frontForwardOffset = ffo;
    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";
}
*/


void PenaltyTricks::ChaseBall::Init(double sdr) {
  underChinAngle = configuration_.GetAsDouble("UnderChinAngle");
  maxWalkAroundDistance = configuration_.GetAsDouble("MaxWalkAroundDistance");
  slowDownRate = sdr;
}


PenaltyTricks::ChaseBall::~ChaseBall() {
  delete headTrick;
}

int PenaltyTricks::ChaseBall::Start() { 
  headTrick = new HeadTricks::FollowBallWithHeadSticky(8);
  headTrick->Start();
  timeBallUnderChin = 0;
  return Continue();
}
int PenaltyTricks::ChaseBall::Abort() {
  return 0;
}

int PenaltyTricks::ChaseBall::Continue() {
  int hC = headTrick->Continue();
  char* hMsg = headTrick->GetErrorMsg(hC);
  
  // If we are searching close don't step!
  if ((strcmp(hMsg,"LOST_BALL_TEMP") == 0 )) {
    return 2;    
  }

  // head trick ended... ie, we lost the ball !
  if (hC < 1) {
    return -1;
  }
  double HeadPan = sensorValues_[S_HEAD_PAN];
  #ifdef ERS_7
    double HeadTilt = sensorValues_[S_HEAD_TILT1];
  #endif
  #ifdef ERS_210
    double HeadTilt = sensorValues_[S_HEAD_TILT];
  #endif

  bool hardOn = false;
  // do we want to keep running hard on to the ball ? FIX for zoid walk, we are to accurate now and it is almost useless
/*  if ((ABS(wo_self_->heading_) < DEG_TO_RAD(35.0)) && (wo_self_->y_ < 100 && wo_self_->y_ > -125) && (frame_-lastReadyFrame_ > 75)) {
    hardOn = true;
    // relax grab condition when we're moving this fast ... make sure ball has been there for 5 frame before grabbing
    if ((HeadTilt <= DEG_TO_MICRO(underChinAngle)) && (HeadPan  <= DEG_TO_MICRO(15)) && (HeadPan  >= DEG_TO_MICRO(-15)) ) {
      if (timeBallUnderChin > 2) {
        timeBallUnderChin = 0;
        return 0;
      }
      timeBallUnderChin++;
      LocomotionCommand lc;
      lc.Set(LocomotionCommand::TP_WALK,10,11,0,0,1.6);
      lc.frontSideOffset = 14.0;
      lc.frontForwardOffset = 70.0;
      utils.RawCommand(lc);
      return 1;
    } else {
      timeBallUnderChin=0; 
    }
  }
*/
  // check ball under chin !
  if ((HeadTilt <= DEG_TO_MICRO(underChinAngle)) && (HeadPan  <= DEG_TO_MICRO(15)) && (HeadPan  >= DEG_TO_MICRO(-15)) ) {
    if (timeBallUnderChin > 3) { // changed from 3 by CM (03/07/2003)
      timeBallUnderChin = 0;
      return 0;
    } else {
      timeBallUnderChin++;
      LocomotionCommand lc;
      lc.Set(LocomotionCommand::TP_WALK,10,11,0,0,1.6);
      lc.frontSideOffset = 14.0;
      lc.frontForwardOffset = 70.0;
      utils.RawCommand(lc);

    }
  } else {
    timeBallUnderChin = 0;
  }

  if (vo_ball_ == NULL) return 1;

  double forward= 0;
  double turn = 0;
  double strafe = 0;


  double ballHeading = RAD_TO_DEG(vo_ball_->heading_);
  double ballDistance = ABS(vo_ball_->distance_);
  double absBallHeading = ABS(ballHeading);

  // calc walk params
/*
	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*10.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);
	}
*/

	if (absBallHeading > 55) {
    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, 55.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 = 28.0 - sqrt(forward*forward/100.0 + strafe*strafe/100.0); // 28 was 18