headtricks.cc

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

// Bag of head tricks
#include <math.h>

#include "HeadTricks.h"
#include "BasicTricks.h"
#include "../Common/Common.h"
#include "../Globals.h"


HeadTricks::LookAt::LookAt(double t, double p, double r, int nF) {
  tilt = t;
  pan = p;
  roll = r;
  numFrames = nF;
  currentFrame = 0;
}
int HeadTricks::LookAt::Start() {
  utils.Head_MoveTo(tilt,pan,roll);
  return 1;
}
int HeadTricks::LookAt::Abort() {
  return 0;
}
int HeadTricks::LookAt::Continue() {
  if (vo_ball_ != NULL) {
    return -1;
  }
  utils.Head_MoveTo(tilt,pan,roll);

  currentFrame++;
  if (currentFrame > numFrames) {
    return 0;
  }
  return 1;  
}
bool HeadTricks::LookAt::IsUsingHead() {
  return true;
} 
char* HeadTricks::LookAt::GetErrorMsg(int msg) {
  if (msg == -1) return "SEE_BALL";
  else if (msg == 0) return "NO_BALL";
  else if (msg == 1) return "DOING";
  return "LA:NO_MESSAGE";
}
char* HeadTricks::LookAt::GetName() {
  return "LookAt";
}


// INCOMPLETE
HeadTricks::FixateOnBeacon::FixateOnBeacon(int _framesAllowed) {
  currentFrame = 0;
  framesAllowed = _framesAllowed;

  double smallestAngle = 10000.0;
  double smallestActual = 10000.0;
  double smallestDistance = 0.0;
  int smallestIndex = -1;
  for (int i = VisionObject::OT_BEACON_YP; i <= VisionObject::OT_BEACON_PB; i++) {
    double theta = MICRO_TO_RAD(sensorValues_[S_HEAD_PAN]) + wo_self_->heading_ - (atan2(wo_self_->y_-beaconCoords[i][1], wo_self_->x_-beaconCoords[i][0]) - (PI / 2.0)) + PI;
    while (theta > PI) theta -= 2*PI;
    while (theta < -PI) theta += 2*PI;

    double temp = (theta-MICRO_TO_RAD(sensorValues_[S_HEAD_PAN]));
    while (temp > PI) temp -= 2*PI;
    while (temp < -PI) temp += 2*PI;

    if (ABS(temp) > DEG_TO_RAD(85.0)) continue;
    if (ABS(theta) < ABS(smallestAngle)) {
      smallestAngle = theta;
      smallestActual = temp;
      smallestIndex = i;
      smallestDistance = sqrt(pow((wo_self_->y_-beaconCoords[i][1]),2.0)+pow((wo_self_->x_-beaconCoords[i][0]),2.0));
    }
  }

  if (smallestIndex == -1) return;
  targetBeacon = smallestIndex;
  targetAngle = RAD_TO_DEG(smallestActual);
  targetTilt = 15.0;

  cout << "target tilt: " << RAD_TO_DEG(atan2(30.0-18.0,smallestDistance)) << endl << flush;
  targetTilt = RAD_TO_DEG(atan2(30.0-18.0,smallestDistance));


//cout << "targetangle: " << targetAngle << endl << flush;
}

int HeadTricks::FixateOnBeacon::Start() { 
  return 1;
}
int HeadTricks::FixateOnBeacon::Abort() {
  return 0;
}
int HeadTricks::FixateOnBeacon::Continue() {
  currentFrame++;
  if (currentFrame > framesAllowed || targetBeacon == -1) {
    return 0;
  }
  double dTilt = targetTilt+20.0;


#ifdef ERS_210
  utils.Head_MoveTo(dTilt,-targetAngle,0.0);
#endif
#ifdef ERS_7
  double bigTiltMotor = dTilt-15.0;
  double smallTiltMotor = 15.0;
  if (bigTiltMotor < -40) {
    smallTiltMotor = dTilt+40.0;
    bigTiltMotor = -40.0;
  } else if (bigTiltMotor > -5) {
    smallTiltMotor = dTilt+5;
    bigTiltMotor = -5;
  }
  utils.Head_MoveTo(bigTiltMotor,-targetAngle,smallTiltMotor);
#endif
  return 1; 
}
bool HeadTricks::FixateOnBeacon::IsUsingHead() {
  return true;
} 
char* HeadTricks::FixateOnBeacon::GetErrorMsg(int msg) {
  return "FOB:NO_MESSAGE";
}
char* HeadTricks::FixateOnBeacon::GetName() {
  return "FixateOnBeacon";
}


////////////////////////////////////////////////////////////////
// BEGIN HEADPANFORBALL

HeadTricks::AngledSearch::AngledSearch(bool _panLeft) {
  panLeft = _panLeft;
}
int HeadTricks::AngledSearch::Start() { 
  if (vo_ball_ != NULL) return 0;
#ifdef ERS_7
  if (panLeft) {
    utils.Head_MoveTo(-20.0,50.0,15.0);
  } else {
    utils.Head_MoveTo(-20.0,-50.0,15.0);
  }
#endif
#ifdef ERS_210
  if (panLeft) {
    utils.Head_MoveTo(-18.0,65.0,0.0);
  } else {
    utils.Head_MoveTo(-18.0,-65.0,0.0);
  }
#endif
  return 1;
}
int HeadTricks::AngledSearch::Abort() {
  return 0;
}
int HeadTricks::AngledSearch::Continue() {
  if (vo_ball_ != NULL) return 0;
#ifdef ERS_7
  if (panLeft) {
    utils.Head_MoveTo(-20.0,50.0,15.0);
  } else {
    utils.Head_MoveTo(-20.0,-50.0,15.0);
  }
#endif
#ifdef ERS_210
  if (panLeft) {
    utils.Head_MoveTo(-18.0,65.0,0.0);
  } else {
    utils.Head_MoveTo(-18.0,-65.0,0.0);
  }
#endif
  return 1;  
}
bool HeadTricks::AngledSearch::IsUsingHead() {
  return true;
} 
char* HeadTricks::AngledSearch::GetErrorMsg(int msg) {
  if (msg == 0) return "SEE_BALL";
  else if (msg == 1) return "PANNING";
  return "AS:NO_MESSAGE";
}
char* HeadTricks::AngledSearch::GetName() {
  return "AngledSearch";
}

// END HEADPANFORBALL
////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////
// BEGIN FOLLOWBALLWITHHEADSTICKY

HeadTricks::FollowBallWithHeadSticky::FollowBallWithHeadSticky(int _stickiness) {
  underChinAngle = configuration_.GetAsDouble("UnderChinAngle");
  stickiness = _stickiness;
  stickinessCount = 0 ;

  lastElevation = 0;
  lastHeading = 0;
  lastPan = 0;
  lastTiltBig = 0;
  lastTiltSmall = 0;

  lastBallCx = currentImageWidth_/2;
  lastBallCy = currentImageWidth_/2;
}
int HeadTricks::FollowBallWithHeadSticky::Start() { 

  if (sensorValues_[S_HEAD_PAN] > 0) panDirection  = 1;
  else panDirection  = -1;
  return Continue();
}
int HeadTricks::FollowBallWithHeadSticky::Abort() {
  return 0;
}
int HeadTricks::FollowBallWithHeadSticky::Continue() {
  if (vo_ball_ == NULL) {
    if (stickinessCount >= stickiness) {
      return -1;
    }
    // keep head still for just a split second before pulling it down ...
    if (stickinessCount < 2) {
      stickinessCount++;
      return 2;
    }
    
    HeadCommand hc = lcq_.GetHeadCommand();
    LocomotionCommand currentLocomotionCommand = lcq_.GetCurrentLocomotionCommand();
//    cout << "S: " << stickinessCount << ", turn: " << currentLocomotionCommand.turn << endl << flush;

    #ifdef ERS_7
    double pan = hc.targetPan_;
    double tilt1 = hc.targetTilt1_;
    double tilt = hc.targetTilt2_;
    
    int cx = lastBallCx;
    int cy = lastBallCy;

    double effectiveRoll = (asin(sin(lastPan)*sin(lastTiltBig))) -(asin(sin(lastPan)*sin(lastTiltSmall)));
/// double effectiveRoll = (asin(sin(lastPan)*sin(lastTilt)));

    int nCx = cx-(currentImageWidth_/2);
    int nCy = -(cy-(currentImageHeight_/2));
    
    int newCx = (int) (cos(effectiveRoll)*nCx - sin(effectiveRoll)*nCy);
    int newCy = (int) (sin(effectiveRoll)*nCx + cos(effectiveRoll)*nCy);

    newCy = -newCy;
    newCy+=currentImageHeight_/2;
    newCx+=currentImageWidth_/2;
    
  
    if (lastElevation >= DEG_TO_RAD(7)) {
      if (newCx > 3*currentImageWidth_/4) {
        tilt += 1.0;
        if (tilt > 15) tilt = 15.0;
        pan -= 2.5;
      } else if (newCx < currentImageWidth_/4) {
        tilt += 1.0;
        if (tilt > 15) tilt = 15.0;
        pan += 2.5;
      } else {
        tilt = hc.targetTilt2_ - 0.9;
      }
    } else { 
      //double change = ((double)stickinessCount)/5.0;
      //tilt = hc.targetTilt2_ - change;
      tilt1 = hc.targetTilt1_ - 1.0;
      tilt = hc.targetTilt2_ - 3.0;
      
      //if  (stickinessCount < ((double)stickiness / 3.0)) {
      //  pan = pan + 2*panDirection;
      //} else pan = pan - panDirection;
      //pan = CROP(pan,-((double)stickiness),((double)stickiness));
      if (sensorValues_[S_HEAD_TILT_BIG]+sensorValues_[S_HEAD_TILT_SMALL] < DEG_TO_MICRO(-37)) pan = 10;  
      //pan=0;
      //cout << frame_ << " " << pan << " " << flush << endl;
    }
    //if (tilt < underChinAngle-10) return -1;
    utils.Head_MoveTo(tilt1,pan,tilt);
    #endif
    stickinessCount++;
//    if (lastElevation < DEG_TO_RAD(-15))  utils.Step(LocomotionCommand::TP_WALK,10.0,10.0,0.0,0.0,1.6);
    return 2;
  }