calibrater.cc

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

#include "Calibrater.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"
#include "../TOOLS/PenaltyTricks.h"
#include "../TOOLS/Utilities.h"
#include "../Globals.h"

// Does auto odometry calculation
Calibrater::Calibrater() {
  currentIndex = 0;

  lcq_.SetTailWagging(false);
  currentTrick = new BasicTricks::NullBody();
  headTrick = new BasicTricks::NullHead();
  inPlaying = false;
  currentSize = 10.0;

  forwardComplete = true;
  backwardComplete = true;
  strafeComplete = true;
  turnComplete = false;

  turnMultiplier = configuration_.GetAsDouble("TurnMultiplier");
  turnMultiplierWithoutFront = configuration_.GetAsDouble("TurnMultiplierWithoutFront");
  forwardMultiplier = configuration_.GetAsDouble("ForwardMultiplier");
  backwardMultiplier = configuration_.GetAsDouble("BackMultiplier");
  strafeMultiplier = configuration_.GetAsDouble("StrafeMultiplier");
  OpenFile();
  
  currentMult = forwardMultipliers[((int)currentSize)/20 - 1];
 }

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

void Calibrater::NewReadyModel() {
  if (inPlaying) {
    delete headTrick;
    headTrick = new HeadTricks::HeadPan(85,-85,3,50);
    headTrick->Start();
    delete currentTrick;
    currentTrick = new BasicTricks::NullBody();
    currentTrick->Start();
    lcq_.SetTailWagging(false);
  }

  int hC=headTrick->Continue();
  if (!currentTrick->IsUsingHead() && (hC < 1)) {
    headTrick = new HeadTricks::HeadPan(85,-85,3,50);
    headTrick->Start();
  } 

  inPlaying = false;
}

void Calibrater::NewPlayingModel() {
  if (!inPlaying) {  // Make sure we have localised
    currentIndex = 0;
    currentSubIndex = 0;
    delete currentTrick;
    currentTrick = new BasicTricks::NullBodyTime(150);
    currentTrick->Start();
    lcq_.SetTailWagging(false);
  }
  inPlaying = true;

  if (currentIndex == C_GOTOSTART && currentSubIndex == 1 && ABS(wo_self_->x_ - startX) < 3 && ABS(wo_self_->y_ - startY) < 3 && ABS(RAD_TO_DEG(wo_self_->heading_)-startH) < 5) {
    cout << "At starting spot\n";
    delete currentTrick;
    currentTrick = new BasicTricks::NullBodyTime(300);
    currentTrick->Start();
    currentSubIndex = 2;
  }

  int cC = currentTrick->Continue();
  if (cC < 1) {    
    if (currentIndex == 0) currentIndex = C_GOTOSTART;
    if (currentIndex == C_GOTOSTART) {
      if (currentSubIndex == 0) { 
        delete currentTrick;
        if (!forwardComplete) {
          startX = 50.0;
          startY = -20.0 - (currentSize / 2.0);
          startH = 0.0;
        } else if (!backwardComplete) {
          startX = 50.0;
          startY = 20 + (currentSize / 2.0 );
          startH = 0.0;
        } else if (!strafeComplete) {
          startX = (currentSize / 2.0);
          startY = -50.0;
          startH = 0.0;
        } else if (!turnComplete) {
          startX = 0.0;
          startY = -50.0;
          startH = 0.0;
        }
        currentTrick = new MovementTricks::MoveToPoint(startX,startY,startH,MovementTricks::MoveToPoint::MTP_DYNAMIC);
        currentTrick->Start();
        currentSubIndex = 1;
      } else if (currentSubIndex == 2) {
        if (!forwardComplete) currentIndex = C_FORWARD;
        else if (!backwardComplete) currentIndex = C_BACKWARD;
        else if (!strafeComplete) currentIndex = C_STRAFE;
        else if (!turnComplete) currentIndex = C_TURN;
        currentSubIndex = 0;
      } 
    }
    if (currentIndex == C_FORWARD) {
      if (currentSubIndex == 0) {
        delete currentTrick;
        int type = LocomotionCommand::TP_SINGLEWALK;
        double fsl = currentSize*currentMult; double bsl = currentSize*currentMult;
        double turn = 0.0;
        double strafe = 0.0;
        double stepfrequency = 1.8;
        currentTrick = new BasicTricks::MultiTrick(13, new BasicTricks::Kick(LocomotionCommand::TP_GETUP,true), new BallTricks::WaitForKick(), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BallTricks::WaitForKick());
        currentTrick->Start();
        prevX = wo_self_->x_;
        prevY = wo_self_->y_;
        prevH = wo_self_->heading_;
        currentSubIndex=1;
      } else if (currentSubIndex == 1) {
        cout << "Checking forward\n";
        delete currentTrick;
        currentTrick = new BasicTricks::NullBodyTime(300);
        currentTrick->Start();
        currentSubIndex = 2;
      } else if (currentSubIndex == 2) {
        double d = sqrt((double)((wo_self_->x_ - prevX)*(wo_self_->x_ - prevX)+(wo_self_->y_ - prevY)*(wo_self_->y_ - prevY)));
        cout << "CurrentSize = " << currentSize << "   Distance = " << d << "   Old multiplier = " << currentMult;
        if (ABS(currentSize - d) < 3) {
          forwardComplete=true;
          forwardMultiplier = currentMult;
          cout << "\n";
        } else {
          double suggested = currentMult * (currentSize/(d*10.0/10.0));
          currentMult = (currentMult + suggested) / 2;
          cout << "  New multiplier = " << currentMult << endl << flush; 
        }
        delete currentTrick;
        currentTrick = new BasicTricks::NullBody();
        currentTrick->Start();
        currentSubIndex = 3; 
      } else if (currentSubIndex == 3) {
        delete currentTrick;
        currentTrick = new BasicTricks::NullBody();
        currentTrick->Start();
        int maxForward = 100;
        #ifdef ERS_7
          maxForward = 120;
        #endif
        if (!forwardComplete) {
          currentIndex = C_GOTOSTART;
          currentSubIndex = 0;
        } else if (currentSize < maxForward) {
          forwardComplete = false;
          currentIndex = C_GOTOSTART;
          currentSubIndex = 0;
          forwardMultipliers[((int)currentSize)/20 - 1] = forwardMultiplier;
          cout << "***** Forward CurrentSize = " << currentSize << "   Multiplier =" << forwardMultiplier << endl << flush;
          WriteConfigFile();
          currentSize+=20.00;
          currentMult = forwardMultipliers[((int)currentSize)/20 - 1];
        } else { // would move to backwards now
          forwardComplete = true;
          currentIndex = C_GOTOSTART;
          currentSubIndex = 0;
          forwardMultipliers[((int)currentSize)/20 - 1] = forwardMultiplier;
          cout << "***** Forward CurrentSize = " << currentSize << "   Multiplier =" << forwardMultiplier << endl << flush; 
          WriteConfigFile();
          currentSize = 20;
          currentMult = backwardMultipliers[((int)currentSize)/20 - 1];
          
        }
      }
    }
    if (currentIndex == C_BACKWARD) {
      if (currentSubIndex == 0) {
        delete currentTrick;
        int type = LocomotionCommand::TP_SINGLEWALK;
        double fsl = -1.0*currentSize*currentMult; double bsl = -1.0*currentSize*currentMult;
        double turn = 0.0;
        double strafe = 0.0;
        double stepfrequency = 1.1;
        LocomotionCommand lc;
        lc.SetDefaults(70.0,5.0,60.0,2.0,100.0,20.0,-47.0,2.0,LocomotionCommand::L_TRAPEZOID);
        currentTrick = new BasicTricks::MultiTrick(13, new BasicTricks::Kick(LocomotionCommand::TP_GETUP,true), new BallTricks::WaitForKick(), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BasicTricks::Step(type,fsl,bsl,turn,strafe,stepfrequency), new BallTricks::WaitForKick());
        currentTrick->Start();
        prevX = wo_self_->x_;
        prevY = wo_self_->y_;
        prevH = wo_self_->heading_;
        currentSubIndex=1;
      } else if (currentSubIndex == 1) {
        cout << "Checking backward\n";
        delete currentTrick;
        currentTrick = new BasicTricks::NullBodyTime(300);
        currentTrick->Start();
        currentSubIndex = 2;
      } else if (currentSubIndex == 2) {
        double d = sqrt((double)((wo_self_->x_ - prevX)*(wo_self_->x_ - prevX)+(wo_self_->y_ - prevY)*(wo_self_->y_ - prevY)));
        cout << "CurrentSize = " << currentSize << "   Distance = " << d << "   Old multiplier = " << currentMult;
        if (ABS(currentSize - d) < 3) {
          backwardComplete=true;
          backwardMultiplier = currentMult;
          cout << "\n";
        } else {
          double suggested = currentMult * (currentSize/(d*10.0/10.0));
          currentMult = (currentMult + suggested) / 2;
          cout << "  New multiplier = " << currentMult << endl << flush; 
        }
        delete currentTrick;
        currentTrick = new BasicTricks::NullBody();
        currentTrick->Start();
        currentSubIndex = 3; 
      } else if (currentSubIndex == 3) {
        delete currentTrick;
        currentTrick = new BasicTricks::NullBody();