thestrut.cc

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

#include <OPENR/core_macro.h>
#include "TheStrut.h"
#include "../Common/Common.h"
#include "../Common/LocomotionCommand.h"
#include "../Common/HeadCommand.h"
#include "../Globals.h"
#include <math.h>

TheStrut::TheStrut() {
  mouthPosition_ = -3.1;
  for (int i = 0; i < NUM_LEDS; i++) {
    ledOn_[i] = false;
  }
  for (int i = 0; i < NUM_LEDS_2; i++) {
    ledOn2_[i] = false;
  }

  lastKickType_ = 0;
  lastKickFrame_ = -1000;

  // we don't yet have current joint positions.
  prevPointsSet = false;

  //initialize high level variables
  jointsInitialised = false;
  jointsReady = false;
  headJointsInitialised = false;
  inDefaultStance = false;
  isWalking = false;
  isKicking = false;

  isDebugStall = false;

  locusType = LocomotionCommand::L_TRAPEZOID;
  locusFunction = &TheStrut::CalculateComponents_TZ;



  // set up arbitrary walk locus.
  origFrontLocusPoints_ = new double*[NUM_LOCUS_POINTS];
  origBackLocusPoints_ = new double*[NUM_LOCUS_POINTS];
  for (int i = 0; i < NUM_LOCUS_POINTS; i++) {
    origFrontLocusPoints_[i] = new double[2];
    origBackLocusPoints_[i] = new double[2];
  }

  // intermediate non scaled/interpolated locus points
  FL_origLocusPoints = new double*[NUM_LOCUS_POINTS];
  FR_origLocusPoints = new double*[NUM_LOCUS_POINTS];
  BL_origLocusPoints = new double*[NUM_LOCUS_POINTS];
  BR_origLocusPoints = new double*[NUM_LOCUS_POINTS];
  for (int i = 0; i < NUM_LOCUS_POINTS; i++) {
    FL_origLocusPoints[i] = new double[2];
    FR_origLocusPoints[i] = new double[2];
    BL_origLocusPoints[i] = new double[2];
    BR_origLocusPoints[i] = new double[2];
  }

  // final locus points - these have been scaled and interpolated.
  // only used internally
  FR_locusPoints = new double*[NUM_LOCUS_POINTS_INTERNAL];
  FL_locusPoints = new double*[NUM_LOCUS_POINTS_INTERNAL];
  BR_locusPoints = new double*[NUM_LOCUS_POINTS_INTERNAL];
  BL_locusPoints = new double*[NUM_LOCUS_POINTS_INTERNAL];
  for (int i = 0; i < NUM_LOCUS_POINTS_INTERNAL; i++) {
    FR_locusPoints[i] = new double[2];
    FL_locusPoints[i] = new double[2];
    BR_locusPoints[i] = new double[2];
    BL_locusPoints[i] = new double[2];
  }

/*

  origLocusPoints_[0][0] = 0;
  origLocusPoints_[0][1] = 13;

  origLocusPoints_[1][0] = -25;
  origLocusPoints_[1][1] = 13;

  origLocusPoints_[2][0] = -38;
  origLocusPoints_[2][1] = 0;

  origLocusPoints_[3][0] = -45;
  origLocusPoints_[3][1] = -13;

  origLocusPoints_[4][0] = -20;
  origLocusPoints_[4][1] = -13;
 
  origLocusPoints_[5][0] = 20;
  origLocusPoints_[5][1] = -13;
 
  origLocusPoints_[6][0] = 45;
  origLocusPoints_[6][1] = -13;

  origLocusPoints_[7][0] = 38;
  origLocusPoints_[7][1] = 0;

  origLocusPoints_[8][0] = 25;
  origLocusPoints_[8][1] = 13;

  origLocusPoints_[9][0] = 0;
  origLocusPoints_[9][1] = 13;
*/


  origFrontLocusPoints_[0][0] = 0.065037;
  origFrontLocusPoints_[0][1] = 14.197699;

  origFrontLocusPoints_[1][0] = -21.215409;
  origFrontLocusPoints_[1][1] = 12.639057;

  origFrontLocusPoints_[2][0] = -38.436484;
  origFrontLocusPoints_[2][1] = -0.238459;

  origFrontLocusPoints_[3][0] = -44.611586;
  origFrontLocusPoints_[3][1] = -14.968670;

  origFrontLocusPoints_[4][0] = -18.971555;
  origFrontLocusPoints_[4][1] = -13.434177;
 
  origFrontLocusPoints_[5][0] = 18.215626;
  origFrontLocusPoints_[5][1] = -11.448770;
 
  origFrontLocusPoints_[6][0] = 44.717295;
  origFrontLocusPoints_[6][1] = -15.105657;

  origFrontLocusPoints_[7][0] = 35.584208;
  origFrontLocusPoints_[7][1] = 0.344644;

  origFrontLocusPoints_[8][0] = 24.680340;
  origFrontLocusPoints_[8][1] = 15.835739;

  origFrontLocusPoints_[9][0] = 0.065037;
  origFrontLocusPoints_[9][1] = 14.197699;


  origBackLocusPoints_[0][0] = -2.112054;
  origBackLocusPoints_[0][1] = 12.532211;

  origBackLocusPoints_[1][0] = -23.995352;
  origBackLocusPoints_[1][1] = 11.365820;

  origBackLocusPoints_[2][0] = -39.370477;
  origBackLocusPoints_[2][1] = -0.573397;

  origBackLocusPoints_[3][0] = -45.594032;
  origBackLocusPoints_[3][1] = -16.551079;

  origBackLocusPoints_[4][0] = -19.719138;
  origBackLocusPoints_[4][1] = -15.320306;
 
  origBackLocusPoints_[5][0] = 20.369987;
  origBackLocusPoints_[5][1] = -12.751072;
 
  origBackLocusPoints_[6][0] = 45.521750;
  origBackLocusPoints_[6][1] = -12.886851;

  origBackLocusPoints_[7][0] = 37.382107;
  origBackLocusPoints_[7][1] = -0.623826;

  origBackLocusPoints_[8][0] = 25.259007;
  origBackLocusPoints_[8][1] = 11.137847;

  origBackLocusPoints_[9][0] = -2.112054;
  origBackLocusPoints_[9][1] = 12.532211;



  // interpolation parameters
  isInterpolating = false;
  interpolationError = DEG_TO_MICRO(2);
  interpolateHead = false;

  //initialize continuously variable parameters
  swayDirection = 0;
  swayParameter = 0;
  timeParameter = 0.0;

  maxWalkFrames = ocommandMAX_FRAMES;
  maxKickFrames = ocommandMAX_FRAMES;
  maxHeadFrames = ocommandMAX_FRAMES;
  maxInterFrames = ocommandMAX_FRAMES;
  maxDoNothingFrames = ocommandMAX_FRAMES;
}

void TheStrut::LoadParameters(char* parameterFile, char* odometryFile) {
  configuration_.ParseFile(parameterFile);
  configuration_.ParseFile(odometryFile);


  // odometry file
  turnMultiplier = configuration_.GetAsDouble("TurnMultiplier");
  turnMultiplierWithoutFront = configuration_.GetAsDouble("TurnMultiplierWithoutFront");
  forwardOdometryMultiplier = configuration_.GetAsDouble("ForwardOdometryMultiplier");

  forwardMultiplier = configuration_.GetAsDouble("ForwardMultiplier");
  backMultiplier = configuration_.GetAsDouble("BackMultiplier");
  strafeMultiplier = configuration_.GetAsDouble("StrafeMultiplier");

  // parameter file
  char* locusTypeString = configuration_.GetAsString("LocusType");

  int tempLocusType = LocomotionCommand::L_TRAPEZOID;

  if (strcmp(locusTypeString,"TRAPEZOID") == 0) {
    tempLocusType = LocomotionCommand::L_TRAPEZOID;
  } else if (strcmp(locusTypeString,"ELLIPSE") == 0) {
    tempLocusType = LocomotionCommand::L_ELLIPSE;
  } else if (strcmp(locusTypeString,"RECTANGLE") == 0) {
    tempLocusType = LocomotionCommand::L_RECTANGLE;
  } else if (strcmp(locusTypeString,"ARBITRARY") == 0) {
    tempLocusType = LocomotionCommand::L_ARBITRARY;
  } else {
    tempLocusType = LocomotionCommand::L_TRAPEZOID;
  }

  LocomotionCommand::SetDefaults(configuration_.GetAsDouble("DefaultFrontHeight"), configuration_.GetAsDouble("DefaultFrontStrideHeight"), configuration_.GetAsDouble("DefaultFrontForwardOffset"),configuration_.GetAsDouble("DefaultFrontSideOffset"), configuration_.GetAsDouble("DefaultBackHeight"), configuration_.GetAsDouble("DefaultBackStrideHeight"), configuration_.GetAsDouble("DefaultBackForwardOffset"), configuration_.GetAsDouble("DefaultBackSideOffset"), tempLocusType);

  fallOverBound1 = configuration_.GetAsDouble("FallOverBound1");
  fallOverBound2 = configuration_.GetAsDouble("FallOverBound2");
  fallOverBound3 = configuration_.GetAsDouble("FallOverBound3");

  maxWalkFrames = configuration_.GetAsInt("MaxWalkFrames");
  maxKickFrames = configuration_.GetAsInt("MaxKickFrames");
  maxHeadFrames = configuration_.GetAsInt("MaxHeadFrames");
  maxInterFrames = configuration_.GetAsInt("MaxInterFrames");
  maxDoNothingFrames = configuration_.GetAsInt("MaxDoNothingFrames");

  isDebugStall = configuration_.GetAsBool("DebugStall");

  verticalStrokeTimeMultiplier = configuration_.GetAsDouble("VerticalStrokeTimeMultiplier");
  if (verticalStrokeTimeMultiplier < 0.1) verticalStrokeTimeMultiplier = 1.0;

  XMLParseKicksFile(configuration_.GetAsString("KicksFile"));

  // reinit a couple of variables
  isWalking=false; isKicking=false; isInterpolating=false; inDefaultStance=false; timeParameter=0.0;

  // Reload traction files
  traction.LoadParameters();
}

// check if we've fallen over. if so, forces us into a get up kick
void TheStrut::CheckIfFallen() {
  double accelerationZ = sensorValues_[S_ACCEL_FOR];
  double accelerationY = sensorValues_[S_ACCEL_SIDE];
  double accelerationX = sensorValues_[S_ACCEL_Z];

  // if we don't know where our legs are, we can't interpolate. have to wait till we can !
  if (prevPointsSet == false) return;

  // if we're already trying to get up, there's little point in messing around here !
  if ( (isKicking || isInterpolating) && (motionType == LocomotionCommand::TP_GETUP || motionType == LocomotionCommand::TP_GETUP_FRONT || motionType == LocomotionCommand::TP_GETUP_LEFT || motionType == LocomotionCommand::TP_GETUP_RIGHT || motionType == LocomotionCommand::TP_GETUP_BACK)) {
    return;
  }
  
  // detect fall over stuff
  double phi = atan2(sqrt((accelerationZ*accelerationZ)+(accelerationY*accelerationY)), -accelerationX);
  bool fallen = false;
  if (phi > fallOverBound1) {
    double theta = atan2(accelerationY, accelerationZ);
    if (ABS(theta) < fallOverBound2) {
      fallen = true; 
      motionType=LocomotionCommand::TP_GETUP_FRONT;
    }
    if (ABS(theta) < fallOverBound3) {
      fallen=true;
      if (theta < 0) {
        motionType=LocomotionCommand::TP_GETUP_LEFT;
      } else {
        motionType=LocomotionCommand::TP_GETUP_RIGHT;
      }
    } else {
      fallen=true;
      motionType=LocomotionCommand::TP_GETUP_BACK;
    }
  }

  // queue get up kick
  if (fallen) {
    cout << "TheStrut: Detected a fall. Getting up !" << endl << flush;
    timeParameter = 0.0;
    for (unsigned int k = 0; k < kicks.size(); k++) {
      if (kicks[k].id == motionType) {
        LocomotionCommand tslc;
        tslc.SetKick(motionType,true);
        isKicking=true;
        isWalking=false;
        isInterpolating=false;
        kickIndex = k;
        for (int i = 0; i < NUM_HEAD_JOINTS; i++) {
          headJointEndPoints[i] = DEG_TO_MICRO(kicks[kickIndex].frames[0][i]);
        }
        for (int i = 0; i < NUM_BODY_JOINTS; i++) {
          bodyJointEndPoints[i] = DEG_TO_MICRO(kicks[kickIndex].frames[0][i+3]);
        }
        kickUsesHead = true;
        if (ShouldInterpolate(kickUsesHead)) {
          isInterpolating = true;
          isKicking = false;
          interpolateHead = kickUsesHead;
          interpolationRate = DEG_TO_MICRO(1.5);
          // store the kick command that we were going to do. we will reinterpret it again once interpolation is complete !
          memcpy(&nextLocomotionCommand, &tslc, sizeof(LocomotionCommand));
        }
      }
    }
  }
}

// some set of joints are ready for new commands to be sent
void TheStrut::IncJointsReady() {
  if (jointsInitialised == false) {
    InitialiseJoints();
  } else if (jointsReady == false && jointsInitialised == true) {
    SetJointGain();
    jointsReady = true;
  } else if (prevPointsSet) {
    CheckIfFallen();
    DoShakeYourBooty();

    // Check for over current
    #ifdef ERS_7
    OStatus powerStatusResult;
    OPowerStatus currentStatus;
    OServiceEntry entry;
    //powerStatusResult = OPENR::ObservePowerStatus(currentStatus,entry);
    powerStatusResult = OPENR::GetPowerStatus(&currentStatus);
    if(currentStatus.robotStatus & orsbBATTERY_OVER_CURRENT) {
        cout << "** NUBot  :: BATTERY_OVER_CURRENT = " << currentStatus.current << endl << flush;
    }
    if (currentStatus.current < -3500) { 
      //cout << " **** WARNING - BATTERY CURRENT IS HIGH ****\n" << flush;
      ledOn_[2] = true; // red on top of head
      ledOn_[3] = false;
      ledOn_[4] = false;
     // sbjCommandVector->NotifyObservers();
     // return;
    } else if (currentStatus.current < -2500) { 
      ledOn_[4] = true; // blue on top of head
      ledOn_[2] = false;
      ledOn_[3] = false;
      
    } else if (currentStatus.current < -1500) { 
      ledOn_[3] = true; // green on top of head
      ledOn_[2] = false;
      ledOn_[4] = false;
    } else {
      ledOn_[2] = false; // no light on top of head
      ledOn_[3] = false;
      ledOn_[4] = false;
      
    }
    // -----------------
    #endif


    if (robotState_.GetState() == RobotState::ST_INITIAL || robotState_.GetState() == RobotState::ST_SET || robotState_.GetState() == RobotState::ST_PENALIZED) {
      if (isInterpolating) {
        DoInterpolate();
      } else if (isKicking) {
        DoKick();
      } else {
        DoDefaultStance();
      }
    } else if (robotState_.GetState() == RobotState::ST_PLAYING || robotState_.GetState() == RobotState::ST_READY) { // playing !
      if (lcq_.IsEmergencyCancel()) {
        isWalking = false;
        isKicking = false;
        isInterpolating = false;
      }
      if (FindFreeBodyRegionID() != -1 || (isKicking==true && FindFreeHeadRegionID() != -1)) {
        // we can only really get a new loco command when we're walking.. (or doing nothing)
        // note the special case to prevent us getting a new loco command when we're doing TP::SINGLEWALK
        if ( (isKicking == false 
              && isInterpolating == false 
              && ((motionType != LocomotionCommand::TP_SINGLEWALK && motionType != LocomotionCommand::TP_SINGLEWALKWITHOUTFRONT && motionType != LocomotionCommand::TP_SINGLEWALKTURNKICK ) || !isWalking))
              && GetNextLocomotionCommand() == false) {
          DoNothing();
          lcq_.SetTheStrutIdle(true);
        } else {
          lcq_.SetTheStrutIdle(false);
          if (isWalking) {
            DoWalk();
          } else if (isInterpolating) {
            DoInterpolate();
          } else if (isKicking) {
            DoKick();
          }
        }
      }
    }

    GetNextHeadCommand();
    // if we're kicking or interpolating, don't mess with the head !
    if ((isKicking == false || kickUsesHead == false) && (isInterpolating == false || interpolateHead == false)) {
      DoHeadLocomotion();
    }
  }

  sbjCommandVector->NotifyObservers();

}

void TheStrut::IncRobotState() {
  if (robotState_.GetState() == RobotState::ST_INITIAL || robotState_.GetState() == RobotState::ST_PENALIZED || robotState_.GetState() == RobotState::ST_SET) {
    timeParameter = 0.0;