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


#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(); 
        currentTrick->Start(); 
        if (!backwardComplete) { 
          currentIndex = C_GOTOSTART; 
          currentSubIndex = 0; 
        } else if (currentSize < 80.0) { 
          backwardComplete = false; 
          currentIndex = C_GOTOSTART; 
          currentSubIndex = 0; 
          backwardMultipliers[((int)currentSize)/20 - 1] = backwardMultiplier; 
          cout << "***** Backward CurrentSize = " << currentSize << "   Multiplier =" << backwardMultiplier << endl << flush; 
          WriteConfigFile(); 
          currentSize+=20.00; 
          currentMult = backwardMultipliers[((int)currentSize)/20 - 1]; 
        } else {  
          backwardComplete = true; 
          currentIndex = C_GOTOSTART; 
          currentSubIndex = 0; 
          backwardMultipliers[((int)currentSize)/20 - 1] = backwardMultiplier; 
          cout << "***** Backward CurrentSize = " << currentSize << "   Multiplier =" << backwardMultiplier << endl << flush;  
          WriteConfigFile(); 
          currentSize = 20; 
          currentMult = strafeMultipliers[((int)currentSize)/20 - 1]; 
          } 
      } 
    } 
    if (currentIndex == C_STRAFE) { 
      if (currentSubIndex == 0) { 
        delete currentTrick; 
        int type = LocomotionCommand::TP_SINGLEWALK; 
        double fsl = 0; 
        double bsl = 0; 
        double turn = 0.0; 
        double strafe = currentSize*currentMult; 
        double stepfrequency = 1.6; 
        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 strafe\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) { 
          strafeComplete=true; 
          strafeMultiplier = 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(); 
        if (!strafeComplete) { 
          currentIndex = C_GOTOSTART; 
          currentSubIndex = 0; 
        } else if (currentSize < 80.0) { 
          strafeComplete = false; 
          currentIndex = C_GOTOSTART; 
          currentSubIndex = 0; 
          strafeMultipliers[((int)currentSize)/20 - 1] = strafeMultiplier; 
          cout << "***** Strafe CurrentSize = " << currentSize << "   Multiplier =" << strafeMultiplier << endl << flush; 
          WriteConfigFile(); 
          currentSize+=20.00; 
          currentMult = strafeMultipliers[((int)currentSize)/20 - 1]; 
        } else {  
          strafeComplete = true; 
          currentIndex = C_GOTOSTART; 
          currentSubIndex = 0; 
          strafeMultipliers[((int)currentSize)/20 - 1] = strafeMultiplier; 
          cout << "***** Strafe CurrentSize = " << currentSize << "   Multiplier =" << strafeMultiplier << endl << flush;  
          WriteConfigFile(); 
          currentMult = turnMultipliers[((int)currentSize)/5 - 1]; 
          } 
      } 
    } 
    if (currentIndex == C_TURN) { 
      if (currentSubIndex == 0) { 
        delete currentTrick; 
        int type = LocomotionCommand::TP_SINGLEWALK; 
        double fsl = 0; 
        double bsl = 0; 
        double turn = currentSize*currentMult; 
        double strafe = 0.0; 
        double stepfrequency = 1.8; 
        currentTrick = new BasicTricks::MultiTrick(5, 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 BallTricks::WaitForKick()); 
        currentTrick->Start(); 
        prevX = wo_self_->x_; 
        prevY = wo_self_->y_; 
        prevH = wo_self_->heading_; 
        currentSubIndex=1; 
      } else if (currentSubIndex == 1) { 
        cout << "Checking turn\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)));; 
       double a = Normalise_PI(wo_self_->heading_ - prevH); 
        cout << "CurrentSize = " << currentSize << "   Angle = " << RAD_TO_DEG(a) << "   Old multiplier = " << currentMult; 
        if (ABS(2.0*currentSize - RAD_TO_DEG(a)) < currentSize/10.0) { 
          turnComplete=true; 
          turnMultiplier = currentMult; 
          cout << "\n"; 
        } else { 
          double suggested = currentMult * (2.0*currentSize/RAD_TO_DEG(a)); 
          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(); 
        if (!turnComplete) { 
          currentIndex = C_GOTOSTART; 
          currentSubIndex = 0; 
        } else if (currentSize < 80.0) { 
          turnComplete = false; 
          currentIndex = C_GOTOSTART; 
          currentSubIndex = 0; 
          turnMultipliers[((int)currentSize)/5 - 1] = turnMultiplier; 
          cout << "***** Turn CurrentSize = " << currentSize << "   Multiplier =" << turnMultiplier << endl << flush; 
          WriteConfigFile(); 
          currentSize+=5.00; 
          currentMult = turnMultipliers[((int)currentSize)/5 - 1]; 
        } else {  
          turnComplete = true; 
          currentIndex = C_GOTOSTART; 
          currentSubIndex = 0; 
          turnMultipliers[((int)currentSize)/5 - 1] = turnMultiplier; 
          cout << "***** Turn CurrentSize = " << currentSize << "   Multiplier =" << turnMultiplier << endl << flush;  
          WriteConfigFile(); 
          } 
      } 
    } 
  } 
 
  int hC=headTrick->Continue(); 
  if (hC < 1) { 
    delete headTrick; 
    headTrick = new HeadTricks::HeadPan(85,-85,3,50); 
    headTrick->Start(); 
  }  
} 
 
void Calibrater::WriteConfigFile() { 
  /*FILE* cfFile = fopen("/ms/open-r/mw/data/odometry.cfg","wb"); 
	 
  fprintf(cfFile,"TurnMultiplier = %f\n",turnMultiplier); 
  fprintf(cfFile,"TurnMultiplierWithoutFront = %f\n",turnMultiplierWithoutFront); 
  fprintf(cfFile,"ForwardMultiplier = %f\n",forwardMultiplier); 
  fprintf(cfFile,"BackMultiplier = %f\n",backwardMultiplier); 
  fprintf(cfFile,"StafeMultiplier = %f\n",strafeMultiplier); 
   
  fclose(cfFile);*/ 
   
  FILE* cfFile = fopen("/ms/open-r/mw/data/calib.dat","wb"); 
  if (cfFile==NULL) { 
    cout << "Can't open for write calib.dat\n"; 
  } else { 
    cout << "Writing calib.dat\n"; 
    for (int i=0; i<ASIZE-1; i++) fprintf(cfFile,"%lf,",forwardMultipliers[i]);  
    fprintf(cfFile,"%lf\n",forwardMultipliers[ASIZE]);  
    for (int i=0; i<ASIZE-1; i++) fprintf(cfFile,"%lf,",backwardMultipliers[i]);  
    fprintf(cfFile,"%lf\n",backwardMultipliers[ASIZE]);  
    for (int i=0; i<ASIZE-1; i++) fprintf(cfFile,"%lf,",strafeMultipliers[i]);  
    fprintf(cfFile,"%lf\n",strafeMultipliers[ASIZE]); 
    for (int i=0; i<ASIZE-1; i++) fprintf(cfFile,"%lf,",turnMultipliers[i]);  
    fprintf(cfFile,"%lf\n",turnMultipliers[ASIZE]);   
    fclose(cfFile); 
  } 
} 
 
void Calibrater::OpenFile() { 
  for (int i=0; i<ASIZE; i++) {  
    forwardMultipliers[i]=1; 
    backwardMultipliers[i]=1; 
    strafeMultipliers[i]=1; 
    turnMultipliers[i]=1; 
  } 
   
  FILE* cfFile = fopen("/ms/open-r/mw/data/calib.dat","rb"); 
	if (cfFile==NULL) { 
    cout << "Can't open calib.dat\n"; 
    WriteConfigFile(); 
  } else { 
    cout << "Reading calib.dat\n"; 
    for (int i=0; i<ASIZE-1; i++) fscanf(cfFile,"%lf,",&forwardMultipliers[i]);  
    fscanf(cfFile,"%lf\n",&forwardMultipliers[ASIZE]); 
    for (int i=0; i<ASIZE-1; i++) fscanf(cfFile,"%lf,",&backwardMultipliers[i]);  
    fscanf(cfFile,"%lf\n",&backwardMultipliers[ASIZE]); 
    for (int i=0; i<ASIZE-1; i++) fscanf(cfFile,"%lf,",&strafeMultipliers[i]);  
    fscanf(cfFile,"%lf\n",&strafeMultipliers[ASIZE]);  
    for (int i=0; i<ASIZE-1; i++) fscanf(cfFile,"%lf,",&turnMultipliers[i]);  
    fscanf(cfFile,"%lf\n",&turnMultipliers[ASIZE]);  
    fclose(cfFile); 
  } 
 
  for (int i=0; i<ASIZE; i++) {  
    cout << forwardMultipliers[i] << " "; 
  } 
  cout << endl; 
 
}