traction.cc

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

#include "Traction.h"
#include "../Common/Common.h"
#include "../Globals.h"
#include <math.h>

// The state of this code is unknown.
Traction::Traction() {
  isTraining = false;

  isTraining = false;
  isTrainingContinue = false;
  isTractionControl = false;
  
  lastTime = 0;

  for (int t = 0; t < TP_PRECISION; t++) {
    for (int j = 0; j < BACK_STRIDE_LENGTH_PRECISION; j++) {
      for (int k = 0; k < TURN_PRECISION; k++) {
        for (int l = 0; l < STRAFE_PRECISION; l++) {
          for (int m = 0; m < NUM_TRACTION_JOINTS; m++) {
              errors[m]=0;
              items[t][j][k][l][m].count=0;
              items[t][j][k][l][m].total=0.0;
              items[t][j][k][l][m].sqrTotal=0.0;
          }
        }
      }
    }
  }
}

void Traction::LoadParameters() {
   configuration_.ParseFile("/ms/open-r/mw/data/traction.cfg");
  //isTractionLogging = configuration_.GetAsBool("TractionLogging");
  isTraining = configuration_.GetAsBool("TractionLearning");
  isTrainingContinue = configuration_.GetAsBool("TractionLearningContinue");
  isTractionControl = configuration_.GetAsBool("TractionControl");
  bool tractionSaveNow = configuration_.GetAsBool("TractionSaveNow");

  if (isTrainingContinue) {
    if (isTraining) {
      cout << "Traction: ERROR! TractionLearning and TractionLearningContinue are mutually exclusive." << endl << flush;
      cout << "Traction: TractionLearning has been disabled." << endl << flush;
    }
    isTraining=true;
    char* filename = configuration_.GetAsString("TractionDataFile");
    printf("Traction: Loading traction data %s for continued learning.\n", filename);
    FILE* f = fopen(filename,"rb");
    if (f == NULL) {
      printf("Traction: Unable to open file %s.\n",filename);
    } else {
      fread(items,1,sizeof(Data)*NUM_TRACTION_JOINTS*TP_PRECISION*BACK_STRIDE_LENGTH_PRECISION*TURN_PRECISION*STRAFE_PRECISION,f);
      fclose(f);
    }
  }

  if (isTraining && isTractionControl) {
    isTractionControl=false;
    cout << "Traction: ERROR! TractionLearning and TractionControl are mutually exclusive." << endl << flush;
    cout << "Traction: TractionControl has been disabled." << endl << flush;
  }
  if (isTraining && tractionSaveNow) {
    tractionSaveNow=false;
    cout << "Traction: ERROR! TractionLearning and TractionSaveNow are mutually exclusive." << endl << flush;
    cout << "Traction: TractionSaveNow has been disabled." << endl << flush;
  }
  if (isTractionControl) {
    char* filename = configuration_.GetAsString("TractionDataFile");
    printf("Traction: Loading traction data %s for control.\n", filename);
    FILE* f = fopen(filename,"rb");
    if (f == NULL) {
      printf("Traction: Unable to open file %s.\n",filename);
    } else {
      fread(items,1,sizeof(Data)*NUM_TRACTION_JOINTS*TP_PRECISION*BACK_STRIDE_LENGTH_PRECISION*TURN_PRECISION*STRAFE_PRECISION,f);
      fclose(f);
    }
  }
  if (tractionSaveNow) {
    char* filename = configuration_.GetAsString("TractionDataFile");
    printf("Traction: Writing traction information to %s.\n",filename);
    FILE* f = fopen(filename,"wb");
    if (f != NULL) { 
      fwrite(items,1,sizeof(Data)*NUM_TRACTION_JOINTS*TP_PRECISION*BACK_STRIDE_LENGTH_PRECISION*TURN_PRECISION*STRAFE_PRECISION,f);
      fclose(f);
    } else {
      cout << "Traction: Unable to open file " << filename << " for writing." << endl << flush;
    }
  }
}

void Traction::DoTractionSensing(double stepFrequency, double timeParameter, double backStrideLength, double turn, double strafe) {

    if (timeParameter < lastTime) {
//     cout << type << endl;
       for (int i=0; i<NUM_TRACTION_JOINTS; i++)
          errors[i]=0;
       type = 0;
    }

    long sensorJointPoints[12];
    sensorJointPoints[0] = sensorValues_[S_RF_ROTATOR];
    sensorJointPoints[1] = sensorValues_[S_RF_ABDUCTOR];
    sensorJointPoints[2] = sensorValues_[S_RF_KNEE];
    sensorJointPoints[3] = sensorValues_[S_LF_ROTATOR];
    sensorJointPoints[4] = sensorValues_[S_LF_ABDUCTOR];
    sensorJointPoints[5] = sensorValues_[S_LF_KNEE];

    sensorJointPoints[6] = sensorValues_[S_RR_ROTATOR];
    sensorJointPoints[7] = sensorValues_[S_RR_ABDUCTOR];
    sensorJointPoints[8] = sensorValues_[S_RR_KNEE];
    sensorJointPoints[9] = sensorValues_[S_LR_ROTATOR];
    sensorJointPoints[10] = sensorValues_[S_LR_ABDUCTOR];
    sensorJointPoints[11] = sensorValues_[S_LR_KNEE];

    double jVal[NUM_TRACTION_JOINTS]; 
    jVal[0] = sensorJointPoints[0];
    jVal[1] = sensorJointPoints[1];

    jVal[2] = sensorJointPoints[3];
    jVal[3] = sensorJointPoints[4];

    jVal[4] = sensorJointPoints[6];
    jVal[5] = sensorJointPoints[7];

    jVal[6] = sensorJointPoints[9];
    jVal[7] = sensorJointPoints[10];

    // only allow for standard speed walk..
    if (stepFrequency > 0 && (isTractionControl || isTraining)) {
      int tp = (int)(timeParameter*(double)TP_PRECISION);
      int bsl = (int)(((backStrideLength+100.0)/200.0) * BACK_STRIDE_LENGTH_PRECISION);
      int t = (int)(((turn+25.0)/50.0) * TURN_PRECISION);
      int s = (int)(((strafe+80)/160.0) * STRAFE_PRECISION);
      bool inBounds = true;
      if (s >= STRAFE_PRECISION || s < 0) {
        cout << "Traction: Critical error in traction control. Strafe value out of bounds." << endl << flush;
        inBounds = false;
      }
      if (t >= TURN_PRECISION || t < 0) {
        cout << "Traction: Critical error in traction control. Turn value out of bounds." << endl << flush;
        inBounds = false;
      }
      if (bsl >= BACK_STRIDE_LENGTH_PRECISION || bsl < 0) {
        cout << "Traction: Critical error in traction control. BackStrideLength value out of bounds." << endl << flush;
        inBounds = false;
      }

      if (isTraining && inBounds) {
        for (int i=0; i<NUM_TRACTION_JOINTS; i++) {
          items[tp][bsl][t][s][i].count++;
          items[tp][bsl][t][s][i].total += jVal[i];
          items[tp][bsl][t][s][i].sqrTotal += (jVal[i]*jVal[i]);
        }
      } else if (isTractionControl && (items[tp][bsl][t][s][0].count > 2) && inBounds) {
        // calculate std devation & average for these parameters
        double stdev[NUM_TRACTION_JOINTS];
        double avg[NUM_TRACTION_JOINTS];
        for (int i=0; i<NUM_TRACTION_JOINTS; i++) {
          stdev[i]=sqrt((items[tp][bsl][t][s][i].sqrTotal/(items[tp][bsl][t][s][i].count-1)) - (((items[tp][bsl][t][s][i].total)*(items[tp][bsl][t][s][i].total))/((items[tp][bsl][t][s][i].count-1)*items[tp][bsl][t][s][i].count)));
          avg[i]=items[tp][bsl][t][s][i].total/items[tp][bsl][t][s][i].count;
          if ( (jVal[i] > (avg[i]+2*stdev[i])) || (jVal[i] < (avg[i]-2*stdev[i])) ) errors[i]++;
        }
        // Detect forwards collision
        if (backStrideLength >20 && ABS(turn) < 9 && ABS(strafe) < 50 && timeParameter > 0.2 && timeParameter < 0.8) {
          if (errors[0] > 2 || errors[2] > 2 || errors[4] > 2 || errors[6] > 2) type = 1;
        }
        // Detect turns collision
        if (backStrideLength < 20 && ABS(turn) > 9 && ABS(strafe) < 50 && timeParameter > 0.2 && timeParameter < 0.8) {
          if (turn > 0 && (errors[5] > 2 || errors[7] > 1)) type = 2;
          else if (turn < 0 && (errors[5] > 2 || errors[7] > 2)) type = 3;     
        }     
      } 
    }
    lastTime = timeParameter;
}