locwmchal.cc

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

#include "LOCWMChal.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"

// This is the dude for the localisation without colored landmarks challenge
// I am looking for a better name ?? !
LOCWMChal::LOCWMChal() {
  //FILE* f = fopen("/ms/points.cfg","rb");
  FILE* f = fopen("/ms/open-r/mw/data/points.cfg","rb");
  cout << "*** LOCWMChal ***\n";
  if (f != NULL) {
    for (int i=0; i<NUMPOINTS; i++) {
      posX[i]=0;
      posY[i]=0;
      fscanf(f,"%d %d\n",&posX[i],&posY[i]);
      cout << "Loaded ("<< posX[i] << "," << posY[i] << ")\n";
      // Convert these points to our co-ordinate system (don't need to do this for our test)
/*      cout << "Converting ("<< posX[i] << "," << posY[i] << ")  to  ";
      int tempX = posX[i];
      posX[i] = -1 * posY[i];
      posY[i] = tempX;
      cout << "("<< posX[i] << "," << posY[i] << ")\n";*/
    }
    fclose(f);
  } else {
    cout << "LOCWMChal: unable to open points.cfg" << endl << flush;
  }

  currentIndex = 0;

  errorDistance = 3.0;
  maxSpins = 1;
  numSpins = 0;

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

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

void LOCWMChal::NewReadyModel() {
  if (inPlaying) {
    delete headTrick;
    headTrick = new HeadTricks::HeadPan(85,-85,3,50);
    headTrick->Start();
    delete currentTrick;
    currentTrick = new BasicTricks::NullBody();
    currentIndex = 0;
    errorDistance = 11.0;
    maxSpins = 3;
    numSpins = 0;

    currentTrick->Start();
    lcq_.SetTailWagging(false);
  }

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

  inPlaying = false;
}

void LOCWMChal::NewPlayingModel() {
  int moveToPointType = MovementTricks::MoveToPoint::MTP_DYNAMIC_NO_HEADING;


  if (!inPlaying) {
    // From current location calculate optimal route - NOT NEEDED ATM
    // CalculateOptimalPath((int)wo_self_->x_,(int)wo_self_->y_);
    delete currentTrick;
    currentTrick = new MovementTricks::MoveToPoint(posX[0],posY[0],0.0,moveToPointType);
    currentTrick->Start();
    lcq_.SetTailWagging(false);
  }
  inPlaying = true;
 
  // Set the Ball Position somewhere, so you look in the correct directon (I was thinking maybe the next point in the array)
  wo_ball_->y_ = 0;
  wo_ball_->x_ = 0;
  
  if (ABS(wo_self_->x_ - posX[currentIndex]) < errorDistance &&  ABS(wo_self_->y_ - posY[currentIndex]) < errorDistance && strcmp(currentTrick->GetName(),"NullBodyTime")!=0) {
    // Now Spin and double check your location ...
    if (strcmp(currentTrick->GetName(),"MoveToPoint")==0) {
      if (numSpins < maxSpins) {
        delete currentTrick;
        currentTrick = new MovementTricks::CheckLocalisation(360.0,30);
        currentTrick->Start();
        numSpins++;
        errorDistance-=2;
      } else {
        if (currentIndex < (NUMPOINTS - 1)) {
          currentIndex++;
          errorDistance=11;
          numSpins = 0;
          delete currentTrick;
          currentTrick = new BasicTricks::NullBodyTime(75);
          currentTrick->Start();       
        } else {
          lcq_.SetTailWagging(true);
          delete currentTrick;
          currentTrick = new BasicTricks::NullBody();
          currentTrick->Start(); 
        }
      }
    } else if (strcmp(currentTrick->GetName(),"CheckLocalisation")==0) {
      int cc = currentTrick->Continue();
      if (cc == 0) {
        if (currentIndex < (NUMPOINTS - 1)) {
          currentIndex++;
          errorDistance=11;
          numSpins = 0;
          delete currentTrick;
          currentTrick = new BasicTricks::NullBodyTime(75);
          currentTrick->Start();       
        } else {
          lcq_.SetTailWagging(true);
          delete currentTrick;
          currentTrick = new BasicTricks::NullBody();
          currentTrick->Start(); 
        }
      }
    }
  } else if (strcmp(currentTrick->GetName(),"CheckLocalisation")==0) {
    int cc = currentTrick->Continue();
    if (cc == 0) {
      delete currentTrick;
      currentTrick = new MovementTricks::MoveToPoint(posX[currentIndex],posY[currentIndex],0.0,moveToPointType);
      currentTrick->Start();
      lcq_.SetTailWagging(false);
    }
  } else if (strcmp(currentTrick->GetName(),"NullBodyTime")==0) {
    int cc = currentTrick->Continue();
    if (cc == 0) {
      delete currentTrick;
      currentTrick = new MovementTricks::MoveToPoint(posX[currentIndex],posY[currentIndex],0.0,moveToPointType);
      currentTrick->Start();
      lcq_.SetTailWagging(false);
    }
  }
  // If move to point quits (I don't know how this should happen !!! & I hope it never does)
  if (strcmp(currentTrick->GetName(),"MoveToPoint")==0) {
    if (currentTrick->Continue() < 1) {
      delete currentTrick;
      currentTrick = new MovementTricks::MoveToPoint(posX[currentIndex],posY[currentIndex],0.0,moveToPointType);
      currentTrick->Start();
      lcq_.SetTailWagging(false);
    } 
  }

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

bool LOCWMChal::IsBallVisible() {
  return (vo_ball_ != NULL);
}

double LOCWMChal::GetDistance(int x, int y, int x2, int y2) {
  return sqrt((double)((x-x2)*(x-x2)+(y-y2)*(y-y2)));
}

void LOCWMChal::CalculateOptimalPath(int x, int y) {
  cout << "Calculating Optimal Path" << endl;
  double minDistance = 5000000;
  double distance = 0;
  int newX[NUMPOINTS];
  int newY[NUMPOINTS];
  for (int i=0; i<NUMPOINTS; i++) {
    for (int j=0; j<NUMPOINTS; j++) {
      if (j==i) continue;
      for (int k=0; k<NUMPOINTS; k++) {
        if (k==i || k==j) continue;
        for (int l=0; l<NUMPOINTS; l++) {
          if (l==i || l==j || l==k) continue;
          for (int m=0; m<NUMPOINTS; m++) {
            if (m==i || m==j || m==k || m==l) continue;
            distance = GetDistance(x,y,posX[i],posY[i])+GetDistance(posX[i],posY[i],posX[j],posY[j])+GetDistance(posX[j],posY[j],posX[k],posY[k])+GetDistance(posX[k],posY[k],posX[l],posY[l])+GetDistance(posX[l],posY[l],posX[m],posY[m]);
            if (distance < minDistance) {
              newX[0]=posX[i]; newY[0]=posY[i];
              newX[1]=posX[j]; newY[1]=posY[j];
              newX[2]=posX[k]; newY[2]=posY[k];                
              newX[3]=posX[l]; newY[3]=posY[l];
              newX[4]=posX[m]; newY[4]=posY[m];
              minDistance = distance;
            }
          }
        }
      }
    }
  }
  cout << "shortest path:" << endl << flush;
  for (int n=0; n<NUMPOINTS; n++) {
    cout << "  x: " << newX[n] << ", y: " << newY[n] << endl << flush;
    posX[n] = newX[n];
    posY[n] = newY[n];
  }
}