thestrut.cc

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

    bodyJointEndPoints[5] = RAD_TO_MICRO(ang3);

    // back right limb
    CalculateComponents_A(&f, &s, &h, timeParameter+timeParameterOffset, BR_numPoints, BR_locusPoints, -backSideComponent + backTurnComponent*cosineTheta5, backSideComponent+backTurnComponent*cosineTheta5, backStrideHeight, backForwardOffset, backSideOffset, backHeight);
    CalculateAngles_B(f, s, h, &ang1, &ang2, &ang3);
    bodyJointEndPoints[6] = RAD_TO_MICRO(ang2);
    bodyJointEndPoints[7] = RAD_TO_MICRO(ang1);
    bodyJointEndPoints[8] = RAD_TO_MICRO(ang3);

    // back left limb
    CalculateComponents_A(&f, &s, &h, timeParameter+timeParameterOffset+0.5, BL_numPoints, BL_locusPoints, backSideComponent-backTurnComponent*cosineTheta5, backSideComponent+backTurnComponent*cosineTheta5, backStrideHeight, backForwardOffset, backSideOffset, backHeight);
    CalculateAngles_B(f, s, h, &ang1, &ang2, &ang3);
    bodyJointEndPoints[9] = RAD_TO_MICRO(ang2);
    bodyJointEndPoints[10] = RAD_TO_MICRO(ang1);
    bodyJointEndPoints[11] = RAD_TO_MICRO(ang3);

    if (ShouldInterpolate(false)) {
//    cout << "Interpolation ON" << endl << flush;
      isWalking = false;
      isKicking = false;
      isInterpolating = true;
      interpolationRate = DEG_TO_MICRO(2.0);
      interpolateHead = false;
      memcpy(&nextLocomotionCommand, tslc, sizeof(LocomotionCommand));
      return;
    
    } else {
//    cout << "Interpolation OFF" << endl << flush;
      isWalking = true;
      isKicking = false;
      isInterpolating = false;
      return;
    }
  }
*/
  if (motionType > LocomotionCommand::TP_DONOTHING) {
    isWalking = false;
    isInterpolating = false;
    isKicking = true;
    kickIndex = -1;
    kickFrame = 0.0;
    kickUsesHead = tslc->allowHeadUse;


    for (unsigned int k = 0; k < kicks.size(); k++) {
      if (kicks[k].id == motionType) {
        if (motionType != LocomotionCommand::TP_GETUP) {
          lastKickType_ = motionType;
          lastKickFrame_ = frame_;
        }

        kickIndex = k;
        kickName = kicks[k].name;
    
        if (enableInterpolation) {
          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]);
          }
          if (ShouldInterpolate(kickUsesHead)) {
            double temp = (double)(kicks[kickIndex].frames[0][15]);
            if (temp > 64.0) {
              temp = 64.0;
            }
            interpolationRate = DEG_TO_MICRO( (65.0-temp)/32.0 );
            // if we're told to go FAST, don't interpolate !
            if (temp >= 1.0)  {
              isInterpolating = true;
              isKicking = false;
              interpolateHead = kickUsesHead;
              // store the kick command that we were going to do. we will reinterpret it again once interpolation is complete !
              memcpy(&nextLocomotionCommand, tslc, sizeof(LocomotionCommand));
            }
          }
        }
      }
    }
    if (kickIndex == -1) {
      cout << "TheStrut: Critical error. No motiontype " << motionType << " found in kick file." << endl << flush;
      isKicking = false;
    }
    mouthPosition_ = -3.1; // Close the mouth for all kicks.

  } else if (motionType == -1) {
    isWalking = false;
    isKicking = false;
    isInterpolating = false;
  } else {
    isWalking = true;
    isKicking = false;
    isInterpolating = false;
  }
}

void TheStrut::DoShakeYourBooty() {
  static double wagParameter=0;
  static double wagDirection=0;

  int otherRegionID = FindFreeOtherRegionID();
  if (otherRegionID != -1) {
#ifdef ERS_7
    for (unsigned int i = 0; i < NUM_OTHER_JOINTS; i++) {
      OCommandData* jointData = otherVec[otherRegionID]->GetData(i);
      otherValue[i] = (OJointCommandValue2*)jointData->value;
      for (unsigned int j = 0; j < ocommandMAX_FRAMES; j++) {
        otherValue[i][j].value = 0;
      }
    }

    for (unsigned int j=0; j<ocommandMAX_FRAMES; j++) {
      if (lcq_.IsTailWagging()) {
        if (wagDirection == 0) wagParameter+=0.007;
        if (wagDirection == 1) wagParameter-=0.007;
        if (wagParameter > 0.2) wagDirection=1;
        if (wagParameter < -0.2) wagDirection=0;
      }
      otherValue[0][j].value = DEG_TO_MICRO(mouthPosition_);
#ifdef TAIL_ENABLED
      otherValue[1][j].value = DEG_TO_MICRO(20);
      otherValue[2][j].value = DEG_TO_MICRO(wagParameter*160.0);
#endif
    }
#endif

#ifdef ERS_210
    for (unsigned int i = 0; i < NUM_OTHER_JOINTS; i++) {
      OCommandData* jointData = otherVec[otherRegionID]->GetData(i);
      otherValue[i] = (OJointCommandValue2*)jointData->value;
      for (unsigned int j = 0; j < ocommandMAX_FRAMES; j++) {
        otherValue[i][j].value = 0;
      }
    }

    for (unsigned int j=0; j<ocommandMAX_FRAMES; j++) {
      if (lcq_.IsTailWagging()) {
        if (wagDirection == 0) wagParameter+=0.007;
        if (wagDirection == 1) wagParameter-=0.007;
        if (wagParameter > 0.15) wagDirection=1;
        if (wagParameter < -0.15) wagDirection=0;
      }
      otherValue[0][j].value = DEG_TO_MICRO(mouthPosition_);
//    otherValue[1][j].value = DEG_TO_MICRO(0);
//    otherValue[2][j].value = DEG_TO_MICRO(0);
      otherValue[1][j].value = DEG_TO_MICRO(wagParameter*160.0);
      otherValue[2][j].value = DEG_TO_MICRO(20);
    }
#endif
    sbjCommandVector->SetData(cmdOtherRegion[otherRegionID]);
  }
}

void TheStrut::DoDefaultStance() {
  frontStrideLength = 0;
  backStrideLength = 0;

  frontForwardComponent=0.0;
  backForwardComponent=0.0;
  frontSideComponent = 0.0;
  backSideComponent = 0.0;
  frontTurnComponent = 0.0;
  backTurnComponent = 0.0;

  allowHeadCommandInterpolation = true;

  frontStrideHeight = LocomotionCommand::defaultFrontStrideHeight;
  frontForwardOffset = LocomotionCommand::defaultFrontForwardOffset;
  frontSideOffset = LocomotionCommand::defaultFrontSideOffset;
  frontHeight = LocomotionCommand::defaultFrontHeight;

  backStrideHeight = LocomotionCommand::defaultBackStrideHeight;
  backForwardOffset = LocomotionCommand::defaultBackForwardOffset;
  backSideOffset = LocomotionCommand::defaultBackSideOffset;
  backHeight = LocomotionCommand::defaultBackHeight;

  stepFrequency = 1.6;
  swaySpeed = 0.002;
  swayMax = 0.15;
  timeParameter = 0.0;

  int regionID = FindFreeBodyRegionID();

  if (regionID != -1) {
    double ang1;
    double ang2;
    double ang3;
    for (unsigned int i = 0; i < NUM_BODY_JOINTS; i++) {
      OCommandData* jointData = bodyVec[regionID]->GetData(i);
      bodyValue[i] = (OJointCommandValue2*)jointData->value;
      for (unsigned int j = 0; j < ocommandMAX_FRAMES; j++) {
        bodyValue[i][j].value = 0;
      }
      OCommandInfo* jointInfo = bodyVec[regionID]->GetInfo(i);
      jointInfo->numFrames = ocommandMAX_FRAMES;
    }


    double s; // sideways
    double h; // up
    double f; // forwards

    for (unsigned int j = 0; j < ocommandMAX_FRAMES; j++) {
      if (swayDirection == 1) {
         swayParameter = swayParameter+swaySpeed;
        if (swayParameter > swayMax) {
          swayDirection = 0;
        }
      } else {
        swayParameter = swayParameter-swaySpeed;
        if (swayParameter < -swayMax) {
          swayDirection = 1;
        }
      }

      // front right limb
      CalculateComponents_TZ(&f, &s, &h, 0, 0, 0, 0, frontStrideHeight, frontForwardOffset, frontSideOffset, frontHeight);   
      CalculateAngles_F(f, s, h, &ang1, &ang2, &ang3);
      SetBodyJoint(&bodyValue[0][j].value, RAD_TO_MICRO(ang2), 0);
      SetBodyJoint(&bodyValue[1][j].value, RAD_TO_MICRO(ang1/*+swayParameter*/), 1);
      SetBodyJoint(&bodyValue[2][j].value, RAD_TO_MICRO(ang3), 2);

      // front left limb
      CalculateComponents_TZ(&f, &s, &h, 0, 0, 0, 0, frontStrideHeight, frontForwardOffset, frontSideOffset, frontHeight);   
      CalculateAngles_F(f, s, h, &ang1, &ang2, &ang3);
      SetBodyJoint(&bodyValue[3][j].value, RAD_TO_MICRO(ang2), 3);
      SetBodyJoint(&bodyValue[4][j].value, RAD_TO_MICRO(ang1/*-swayParameter*/), 4);
      SetBodyJoint(&bodyValue[5][j].value, RAD_TO_MICRO(ang3), 5);


      // back right limb
      CalculateComponents_TZ(&f, &s, &h, 0, 0, 0, 0, backStrideHeight, backForwardOffset, backSideOffset, backHeight);
      CalculateAngles_B(f, s, h, &ang1, &ang2, &ang3);
      SetBodyJoint(&bodyValue[6][j].value, RAD_TO_MICRO(ang2), 6);
      SetBodyJoint(&bodyValue[7][j].value, RAD_TO_MICRO(ang1/*+swayParameter*/), 7);
      SetBodyJoint(&bodyValue[8][j].value, RAD_TO_MICRO(ang3), 8);

      // back left limb
      CalculateComponents_TZ(&f, &s, &h, 0, 0, 0, 0, backStrideHeight, backForwardOffset, backSideOffset, backHeight);      CalculateAngles_B(f, s, h, &ang1, &ang2, &ang3);
      SetBodyJoint(&bodyValue[9][j].value, RAD_TO_MICRO(ang2), 9);
      SetBodyJoint(&bodyValue[10][j].value, RAD_TO_MICRO(ang1/*-swayParameter*/), 10);
      SetBodyJoint(&bodyValue[11][j].value, RAD_TO_MICRO(ang3), 11);
    }
    inDefaultStance = true;
    sbjCommandVector->SetData(cmdBodyRegion[regionID]);
  }
}

// keep joints in current position !
void TheStrut::DoNothing() {
  int bodyRegionID = FindFreeBodyRegionID();
  if (bodyRegionID != -1) {
    for (int i = 0; i < NUM_BODY_JOINTS; i++) {
      OCommandData* jointData = bodyVec[bodyRegionID]->GetData(i);
      bodyValue[i] = (OJointCommandValue2*)jointData->value;
      OCommandInfo* jointInfo = bodyVec[bodyRegionID]->GetInfo(i);
      jointInfo->numFrames = maxDoNothingFrames;

      for (int j = 0; j < maxDoNothingFrames; j++) {
        SetBodyJoint(&bodyValue[i][j].value, bodyJointPrevPoints[i],i);
      }
    }
    sbjCommandVector->SetData(cmdBodyRegion[bodyRegionID]);
  }
}

int TheStrut::FindFreeBodyRegionID() {
  for (int i = 0; i < NUM_BUFFERS; i++) {
    if (cmdBodyRegion[i]->NumberOfReference() == 1) return i;
  }
  return -1;
}

int TheStrut::FindFreeHeadRegionID() {
  for (int i = 0; i < NUM_BUFFERS; i++) {
    if (cmdHeadRegion[i]->NumberOfReference() == 1) return i;
  }
  return -1;
}

int TheStrut::FindFreeOtherRegionID() {
  for (int i = 0; i < NUM_BUFFERS; i++) {
    if (cmdOtherRegion[i]->NumberOfReference() == 1) return i;
  }
  return -1;
}

int TheStrut::FindFreeLedRegionID() {
  for (int i = 0; i < NUM_BUFFERS; i++) {
    if (cmdLedRegion[i]->NumberOfReference() == 1) return i;
  }
  return -1;
}

void TheStrut::SetBodyJoint(long* dest, long val, int num) {
  *dest = val;
  bodyJointPrevPoints[num] = val;
}

void TheStrut::SetHeadJoint(long* dest, long val, int num) {
  *dest = val;
  headJointPrevPoints[num] = val;
}

void TheStrut::SetRobotLeds() {
#ifdef ERS_7
  if (jointsInitialised == false) {
    return;
  }

  bool turnAllOn = false;
/*
  if (frame_ % 1800 == 0) turnAllOn = true;
  else if (frame_ % 1801 == 0) turnAllOn = true;
  else if (frame_ % 1802 == 0) turnAllOn = true;
  else if (frame_ % 1803 == 0) turnAllOn = true;
  else if (frame_ % 1804 == 0) turnAllOn = true;
*/

  int ledRegionID = FindFreeLedRegionID();
  if (ledRegionID != -1) {
    for (int i = 0; i < NUM_LEDS+NUM_LEDS_2; i++) {
      OCommandData* data = ledVec[ledRegionID]->GetData(i);
      if (i < 6) {
        ledValue[i] = (OLEDCommandValue2*)data->value;
        ledValue[i][0].led = oledOFF;
        if (ledOn_[i] || turnAllOn) {
          ledValue[i][0].led = oledON;
        }
        ledValue[i][0].period = 4;
      } else {
        int index = i-6;
        ledValue2[index] = (OLEDCommandValue3*)data->value;
        ledValue2[index][0].intensity = 0;

        if (ledOn2_[i] || turnAllOn) {
          ledValue2[index][0].intensity = 255;
        }
        ledValue2[index][0].mode = oled3_MODE_B;
        ledValue2[index][0].period = 4;
      }
    }

    // COLOR
    ledValue2[BACK_FRONT_LIGHT_COLOR][0].intensity = 0;
    ledValue2[BACK_FRONT_LIGHT_WHITE][0].intensity = 0;
    ledValue2[BACK_REAR_LIGHT_COLOR][0].intensity = 0;
    ledValue2[BACK_REAR_LIGHT_WHITE][0].intensity = 0;

    if (robotState_.GetTeam() == RobotState::RT_RED || turnAllOn) { // RED
      ledValue2[BACK_REAR_LIGHT_COLOR][0].intensity = 255;
      ledValue2[BACK_REAR_LIGHT_WHITE][0].intensity = 0;
    }
    if (robotState_.GetTeam() == RobotState::RT_BLUE || turnAllOn) { // BLUE
      ledValue2[BACK_FRONT_LIGHT_COLOR][0].intensity = 255;
      ledValue2[BACK_FRONT_LIGHT_WHITE][0].intensity = 0;
    }

    // KICK OFF
    if ((robotState_.GetKickOff() == RobotState::KO_OWN && (robotState_.GetState() == RobotState::ST_INITIAL || robotState_.GetState() == RobotState::ST_READY)) || turnAllOn) { // own
      ledValue2[BACK_MIDDLE_LIGHT_COLOR][0].intensity = 255;
      ledValue2[BACK_MIDDLE_LIGHT_WHITE][0].intensity = 0;
    } else { // opposition
      ledValue2[BACK_MIDDLE_LIGHT_COLOR][0].intensity = 0;
      ledValue2[BACK_MIDDLE_LIGHT_WHITE][0].intensity = 0;
    }

    sbjCommandVector->SetData(cmdLedRegion[ledRegionID]);
  }
#endif

#ifdef ERS_210
  if (jointsInitialised == false) {
    return;
  }
  int ledRegionID = FindFreeLedRegionID();

  if (ledRegionID != -1) {
    for (int i = 0; i < NUM_LEDS; i++) {
      OCommandData* data = ledVec[ledRegionID]->GetData(i);
      ledValue[i] = (OLEDCommandValue2*)data->value;

      ledValue[i][0].led = oledOFF;
      if (ledOn_[i]) {
        ledValue[i][0].led = oledON;
      }

      ledValue[i][0].period = 1;
    }

    // override normal LED settings to indicate ready/playing and team.
    if (robotState_.GetState() == RobotState::ST_PLAYING) {
      ledValue[HEAD_LED_TOP][0].led = oledON;
    }
    if (robotState_.GetTeam() == RobotState::RT_RED) {
      ledValue[TAIL_LED_RED][0].led = oledON;
      ledValue[TAIL_LED_BLUE][0].led = oledOFF;
    } else if (robotState_.GetTeam() == RobotState::RT_BLUE) {
      ledValue[TAIL_LED_RED][0].led = oledOFF;
      ledValue[TAIL_LED_BLUE][0].led = oledON;
    }
    sbjCommandVector->SetData(cmdLedRegion[ledRegionID]);
  }
#endif
}