www.pudn.com > Product_Submit2004.rar > LOCWMChal.cc, change:2004-10-12,size:7341b


#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]; 
  } 
}