complextricks.cc

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

// Bag of complex tricks
#include <math.h>
#include <iostream>
#include <stdarg.h>

using namespace std;

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

ComplexTricks::AdvancedChaseWithKick::AdvancedChaseWithKick() {
  currentTrick = new BasicTricks::NullBody();
  headTrick = new BasicTricks::NullHead();
}

ComplexTricks::AdvancedChaseWithKick::~AdvancedChaseWithKick() {
  delete currentTrick;
}
int ComplexTricks::AdvancedChaseWithKick::Start() {
  return Continue();
}
int ComplexTricks::AdvancedChaseWithKick::Abort() {
  currentTrick->Abort();
  headTrick->Abort();
  return 0;
}

int ComplexTricks::AdvancedChaseWithKick::ChaseSelecta() {
  
   // we are kicking off, we just kicked off, and we're the second bot (kicker!)
  // => RAM override.
  // i also has to overwrite this is movements tricks (in 2 places) for some dumb reason ... so change it there aswell !!!
  if (robotState_.GetState() != RobotState::ST_PLAYING) {
    return MovementTricks::ChaseBall::CB_RAM;
  }
  if (robotState_.GetKickOff() == RobotState::KO_OWN && (frame_-lastReadyFrame_) < 90 && BOTID_ != 1) {
    return MovementTricks::ChaseBall::CB_RAM;
  }
  /// ------


  double goalHeadingDeg = utils.GetHeadingToGoal();
  double absGoalHeadingDeg = ABS(goalHeadingDeg);
  double ramAngle = 5.0; 
 // if (wo_self_->y_ < 20) ramAngle = 10.0;
  if ((absGoalHeadingDeg < ramAngle && ABS(wo_self_->heading_) < DEG_TO_RAD(70)) || (ABS(RAD_TO_DEG(wo_self_->heading_)) < ramAngle*2.0 && wo_self_->y_ < 75 && wo_self_->y_ > -125)) {
    return MovementTricks::ChaseBall::CB_RAM;
    // run into it
  } else if ((vo_ball_!= NULL && vo_ball_->elevation_ > -30) && wo_self_->x_ < 110 && ((absGoalHeadingDeg > 70.0 && absGoalHeadingDeg < 130.0 && ABS(wo_self_->y_) < 170) || (absGoalHeadingDeg>80 && absGoalHeadingDeg<90))) {
    return MovementTricks::ChaseBall::CB_SLAP;
    // slap it
  } else {
    return MovementTricks::ChaseBall::CB_GRAB;
    // grab it.
  }
  return 0;
}

int ComplexTricks::AdvancedChaseWithKick::Continue() {
  headTrick->Continue();

  int cC = currentTrick->Continue();
  char* cMsg = currentTrick->GetErrorMsg(cC);
  if (cC >= 1) {
    if (strcmp(currentTrick->GetName(),"ChaseBall")==0) {
      MovementTricks::ChaseBall* chaseTrick = (MovementTricks::ChaseBall*)currentTrick;
      chaseTrick->SetChaseMode(ChaseSelecta());
      // can always interrupt straight out chase ..
      return 1;
    } else if (strcmp(currentTrick->GetName(),"PostChaseSearchForBall")==0) {
      return 1;
    } else { // we're not chasing => we're not interruptible ! (ie, doing a kick or something)
      return 2;
    }
  } else if (cC < 1) {
    if (strcmp(cMsg,"BALL_UNDER_CHIN") == 0) {
      // must be chase trick ..
      int chaseMode = ((MovementTricks::ChaseBall*)currentTrick)->GetChaseMode();
      delete currentTrick;
      // if kicking off, don't turn !!
/*    if (frame_-lastReadyFrame_ <= 75 ) {
        currentTrick = ks.FastSelecta(wo_self_->x_, wo_self_->y_, wo_self_->heading_);
      } else {
        currentTrick = ks.NormalSelecta(wo_self_->x_, wo_self_->y_, wo_self_->heading_);
      }*/
      lcq_.Clear(); // make sure no turn steps on queue... we're kicking damnit !
#ifdef ERS_210
      if (chaseMode == 0) { // no slow down ..
        currentTrick = ks.FastSelecta(wo_self_->x_, wo_self_->y_, wo_self_->heading_);
      } else {
        currentTrick = ks.NormalSelecta(wo_self_->x_, wo_self_->y_, wo_self_->heading_);
      }
#endif
#ifdef ERS_7
      if (chaseMode == MovementTricks::ChaseBall::CB_SLAP) {
        //currentTrick = new BasicTricks::MultiTrick(2,currentTrick = new BallTricks::SlapTrick(LocomotionCommand::TP_SLAP_LEFT);,new BasicTrick::WaitForKick());//
        currentTrick = ks.FastSelecta(wo_self_->x_, wo_self_->y_, wo_self_->heading_, 1);
      } else {
        currentTrick = ks.NormalSelecta(wo_self_->x_, wo_self_->y_, wo_self_->heading_);

//        currentTrick = new BasicTricks::MultiTrick(6, new BasicTricks::Kick(LocomotionCommand::TP_GRAB,true), new BallTricks::WaitForKick(), new BasicTricks::Kick(LocomotionCommand::TP_PAW_KICK,true), new BallTricks::WaitForKick(),new BasicTricks::Kick(LocomotionCommand::TP_GETUP,true), new BallTricks::WaitForKick()); 
      }
#endif
      currentTrick->Start();
      return 1;
    } else if (vo_ball_ != NULL) {
      delete currentTrick;
      currentTrick = new MovementTricks::ChaseBall(true);
      currentTrick->Start();
      return 1;
    } else if (strcmp(currentTrick->GetName(),"PostChaseSearchForBall") != 0) {
      delete currentTrick;
      if (frame_ - lastKickFrame_ < 120) currentTrick = new ComplexTricks::PostChaseSearchForBall(true,0);
      else currentTrick = new ComplexTricks::PostChaseSearchForBall(false,0);
      currentTrick->Start();
      return 1;
    } else {
      delete currentTrick;
      currentTrick = new BasicTricks::NullBody();
      currentTrick->Start();
      return 0;
    }
  }
  return 1;
}
bool ComplexTricks::AdvancedChaseWithKick::IsUsingHead() {
  return true;
}
char* ComplexTricks::AdvancedChaseWithKick::GetErrorMsg(int c) {
  if (c == -1) return "LOST_BALL";
  else if (c == 0) return "ACWK_DONE";
  else if (c == 1) return "INTERRUPTIBLE";
  else if (c == 2) return "NOT_INTERRUPTIBLE";
  return "ACWK:NO_MESSAGE";
}
char* ComplexTricks::AdvancedChaseWithKick::GetName() {
  return "AdvancedChaseWithKick";
}


ComplexTricks::PostChaseSearchForBall::PostChaseSearchForBall(bool k, double deg) {
  currentTrick = NULL;

  lookRight = false;
  kicked = k;
  maxTurn = deg;
  numFrames=0;
}
ComplexTricks::PostChaseSearchForBall::~PostChaseSearchForBall() {
  if (currentTrick != NULL)  {
    delete currentTrick;
  }
  if (lastKickType_ == LocomotionCommand::TP_BACK_KICK_RIGHT || lastKickType_ == LocomotionCommand::TP_BACK_KICK_LEFT)
    lastKickType_ = LocomotionCommand::TP_CHEST_PUSH;
}
int ComplexTricks::PostChaseSearchForBall::Abort() {
  return 0;
}
int ComplexTricks::PostChaseSearchForBall::Start() {
  utils.Kick(LocomotionCommand::TP_GETUP,false);
  double fieldAngleToBall = Normalise_PI((atan2(wo_ball_->y_-wo_self_->y_, wo_ball_->x_-wo_self_->x_) - (PI / 2.0)) - wo_self_->heading_);
  if (fieldAngleToBall > 0) lookRight = false;
  else lookRight = true;
  if (kicked) {
    if (lastKickType_ == LocomotionCommand::TP_SLAP_LEFT) lookRight = false;
    else if (lastKickType_ == LocomotionCommand::TP_SLAP_RIGHT) lookRight = true;
    else if (lastKickType_ == LocomotionCommand::TP_TURN_LEFT) lookRight = false;
    else if (lastKickType_ == LocomotionCommand::TP_TURN_RIGHT) lookRight = true;
    else if (lastKickType_ == LocomotionCommand::TP_BACK_RIGHT_150 || lastKickType_ == LocomotionCommand::TP_BACK_RIGHT_110) {
      lookRight = false;
    } else if (lastKickType_ == LocomotionCommand::TP_BACK_LEFT_150 || lastKickType_ == LocomotionCommand::TP_BACK_LEFT_150) {
      lookRight = true;
    }
  } 
  utils.Head_PanForBall(true, lookRight, 50);
  return Continue();
}
int ComplexTricks::PostChaseSearchForBall::Continue() {
  if (vo_ball_ != NULL) {
    lcq_.SetEmergencyCancel(true);
    lcq_.Clear();
    if (lastKickType_ == LocomotionCommand::TP_BACK_KICK_RIGHT || lastKickType_ == LocomotionCommand::TP_BACK_KICK_LEFT)
      lastKickType_ = LocomotionCommand::TP_CHEST_PUSH;
    return 0;
  }
  
  if (kicked) {
     if (lastKickType_ == LocomotionCommand::TP_BACK_RIGHT_150 || lastKickType_ == LocomotionCommand::TP_BACK_RIGHT_110) {
      lookRight = false;
      utils.Step(LocomotionCommand::TP_WALK, 0.0,0.0,-40,0,1.6);
    } else if (lastKickType_ == LocomotionCommand::TP_BACK_LEFT_150 || lastKickType_ == LocomotionCommand::TP_BACK_LEFT_150) {
      lookRight = true;
      utils.Step(LocomotionCommand::TP_WALK, 0.0,0.0,40,0,1.6);
    }
  }

  if (numFrames > 50) {   
  if (lastKickType_ == LocomotionCommand::TP_BACK_RIGHT_150 || lastKickType_ == LocomotionCommand::TP_BACK_RIGHT_110 || lastKickType_ == LocomotionCommand::TP_BACK_LEFT_150 || lastKickType_ == LocomotionCommand::TP_BACK_LEFT_150)
    lastKickType_ = LocomotionCommand::TP_CHEST_PUSH;
    return -1;
  }

  numFrames++;

  if (currentTrick == NULL) {
    currentTrick = new BasicTricks::NullBody();;
    currentTrick->Start();
  }