testkicks.cc

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

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

// Simple chaser hardcoded to doing just one type of kick.
TestKicks::TestKicks() {
  configuration_.ParseFile("/ms/open-r/mw/data/testkick.cfg");
  
  kickAngle = 0;
  debugOutput = configuration_.GetAsBool("DebugOutput");

  currentTrick = new BasicTricks::NullBody();
  headTrick = new BasicTricks::NullHead();
  inPlaying = false;
}

TestKicks::~TestKicks() {
  delete currentTrick;
  delete headTrick;
}

void TestKicks::NewReadyModel() {
  if (inPlaying) {
    delete headTrick;
    headTrick = new HeadTricks::HeadPan(85,-85,3,50);   // MQ 19/6 .. changed from (70,-70,0,12)

    delete currentTrick;
    currentTrick = new BasicTricks::NullBody();
    currentTrick->Start();
  }

  int hC = headTrick->Continue();
  if (hC < 1) {
    delete headTrick;
    headTrick = new HeadTricks::HeadPan(85,-85,3,50);   // MQ 19/6 .. changed from (70,-70,0,12)
    headTrick->Start();
  }
  inPlaying = false;
}

void TestKicks::NewPlayingModel() {
  if (!inPlaying) {
    delete headTrick;
    headTrick = new BasicTricks::NullHead();
    headTrick->Start();

    delete currentTrick;
    currentTrick = new BasicTricks::NullBody();
    currentTrick->Start();
  }
  inPlaying=true;

  int hC = headTrick->Continue();
  if (hC < 1) {
    delete headTrick;
    headTrick = new BasicTricks::NullHead();
    headTrick->Start();
  }

  int cC = currentTrick->Continue();
  char* cMsg = currentTrick->GetErrorMsg(cC);
  if (cC < 1) {
    delete currentTrick;
    if (strcmp(cMsg,"BALL_UNDER_CHIN") == 0) {
      //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()); 
      double angle = utils.GetHeadingToGoal();//o_self_->x_, wo_self_->y_, wo_self_->heading_);
      if (angle < 0) angle = -360.0;
      else angle = 360.0;

     //currentTrick = new BasicTricks::MultiTrick(5, new BasicTricks::Kick(LocomotionCommand::TP_GRAB,true), new BallTricks::WaitForKick(), new BallTricks::TurnWithBallUntilGoal(angle,angle,10), new BasicTricks::Kick(LocomotionCommand::TP_CHEST_PUSH,true), new BallTricks::WaitForKick());
       currentTrick = new BasicTricks::MultiTrick(6, new BasicTricks::Kick(LocomotionCommand::TP_GRAB,true), new BallTricks::WaitForKick(), new BasicTricks::Kick(LocomotionCommand::TP_BACK_LEFT_150), new BallTricks::WaitForKick(), new BasicTricks::Kick(LocomotionCommand::TP_GETUP), new BallTricks::WaitForKick());
      //currentTrick = new BasicTricks::Step(LocomotionCommand::TP_SINGLEWALK,30.0,15.0,70.0,0.0,1.6);
      //currentTrick = new BallTricks::TurnKick(kickAngle,false,false);
    //  Trick* temp = NULL;
    //  temp = ks.FastSelecta(wo_self_->x_, wo_self_->y_,wo_self_->heading_,0);
   //   currentTrick = new BasicTricks::MultiTrick(2, new BallTricks::GrabTrick(), temp);
      //currentTrick = new BallTricks::SlapTrick(LocomotionCommand::TP_SLAP_LEFT);
      currentTrick->Start();

      // relinquish head control
      headTrick->Abort();
      delete headTrick;
      headTrick = new BasicTricks::NullHead();
      headTrick->Start();

    } else if (vo_ball_ != NULL) {
      if (debugOutput)
        cout << "TestKicks: Ball visible. Chase !" << endl << flush;
      currentTrick = new MovementTricks::ChaseBall();
      currentTrick->Start();

      // relinquish head control
      headTrick->Abort();
      delete headTrick;
      headTrick = new BasicTricks::NullHead();
      headTrick->Start();
    } else {
        if (debugOutput)
          cout << "TestKicks: Ball not visible. Search !" << endl << flush;

        bool turnRight = true;
        if (lastBallHeading_ > 0) {
          turnRight = false;
        }
        currentTrick = new MovementTricks::SearchForBall(turnRight, 360.0);
        currentTrick->Start();

        delete headTrick;
        headTrick = new BasicTricks::NullHead();
        headTrick->Start();
    }
  }
}