calibrater.cc

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

        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;

}