head.cc

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

#include "TheStrut.h"

//performs head movement
void TheStrut::DoHeadLocomotion() {
  if (prevPointsSet == false) return;
/*  if (headTilt < -88.5) headTilt = -88.5;
  if (headTilt > 43) headTilt = 43;
  if (headPan < -89.6) headPan = -89.6;
  if (headPan > 89.6) headPan = 89.6;*/

//  double headTilt1 = headTilt;
//  double headTilt2 = headTilt2;

//  if (sbjCommandVector->IsReady() == false) {  
//    return;
//  }

  int regionID = FindFreeHeadRegionID();
  if (regionID != -1) {
    for (int i = 0; i < NUM_HEAD_JOINTS; i++) {
      OCommandData* jointData = headVec[regionID]->GetData(i);
      headValue[i] = (OJointCommandValue2*)jointData->value;

      OCommandInfo* info = headVec[regionID]->GetInfo(i);
      info->numFrames = maxHeadFrames;
    }
    // this is for normal head panning.. we interpolate from where we are to where we want to be, so we're as smooooth as possible.
    if (allowHeadCommandInterpolation) {
// rewritten interpolation !
      long headJointEndPoints[3];
      headJointEndPoints[0] = DEG_TO_MICRO(headTilt1);
      headJointEndPoints[1] = DEG_TO_MICRO(headPan);
      headJointEndPoints[2] = DEG_TO_MICRO(headTilt2);
      long interpRate = DEG_TO_MICRO(2.2);
      for (int i = 0; i < NUM_HEAD_JOINTS; i++) {
        long jointPosition = headJointPrevPoints[i];
        long delta = (long) (((double)headJointEndPoints[i]-(double)jointPosition)/(double)maxHeadFrames);

        if (delta > interpRate) delta = interpRate;
        if (delta < -interpRate) delta = -interpRate;

        for (int j = 0; j < maxHeadFrames; j++) {
          jointPosition+=delta;
          SetHeadJoint(&headValue[i][j].value, jointPosition, i);
        }
      }

/*
      double currTilt1 = MICRO_TO_DEG(headJointPrevPoints[0]);
      double currPan = MICRO_TO_DEG(headJointPrevPoints[1]);
      double currTilt2 = MICRO_TO_DEG(headJointPrevPoints[2]);

      double deltaTilt1 = (headTilt1-currTilt1)/(double)maxHeadFrames;
      double deltaPan = (headPan-currPan)/(double)maxHeadFrames;
      double deltaTilt2 = (headTilt2-currTilt2)/(double)maxHeadFrames;

      for (int j = 0; j < maxHeadFrames; j++) {
        currTilt1+=deltaTilt1;
        currPan+=deltaPan;
        currTilt2+=deltaTilt2;
        SetHeadJoint(&headValue[0][j].value, DEG_TO_MICRO(currTilt1), 0);
        SetHeadJoint(&headValue[1][j].value, DEG_TO_MICRO(currPan), 1);
        SetHeadJoint(&headValue[2][j].value, DEG_TO_MICRO(currTilt2), 2);
      }*/
    } else {
      // snap head to follow ball !
      for (int j = 0; j < maxHeadFrames; j++) {
        SetHeadJoint(&headValue[0][j].value, DEG_TO_MICRO(headTilt1), 0);
        SetHeadJoint(&headValue[1][j].value, DEG_TO_MICRO(headPan), 1);
        SetHeadJoint(&headValue[2][j].value, DEG_TO_MICRO(headTilt2), 2);
      }
/*
      double currTilt1 = MICRO_TO_DEG(headJointPrevPoints[0]);
      double currPan = MICRO_TO_DEG(headJointPrevPoints[1]);
      double currTilt2 = MICRO_TO_DEG(headJointPrevPoints[2]);

      double deltaTilt1 = (headTilt1-currTilt1)/(double)maxHeadFrames;
      double deltaPan = (headPan-currPan)/(double)maxHeadFrames;
      double deltaTilt2 = (headTilt2-currTilt2)/(double)maxHeadFrames;

      for (int j = 0; j < maxHeadFrames; j++) {
        currTilt1+=deltaTilt1;
        currPan+=deltaPan;
        currTilt2+=deltaTilt2;
        SetHeadJoint(&headValue[0][j].value, DEG_TO_MICRO(currTilt1), 0);
        SetHeadJoint(&headValue[1][j].value, DEG_TO_MICRO(currPan), 1);
        SetHeadJoint(&headValue[2][j].value, DEG_TO_MICRO(currTilt2), 2);
      }
*/
    }
    sbjCommandVector->SetData(cmdHeadRegion[regionID]);
  }
}