headtricks.cc

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

  // can see ball 
  lastBallCx = vo_ball_->centreX_;
  lastBallCy = vo_ball_->centreY_;

  lastElevation = vo_ball_->elevation_;
  lastHeading = vo_ball_->heading_;
#ifdef ERS_7
  lastTiltBig = MICRO_TO_RAD(sensorValues_[S_HEAD_TILT_BIG]);
  lastTiltSmall = MICRO_TO_RAD(sensorValues_[S_HEAD_TILT_SMALL]);
#endif
#ifdef ERS_210
  lastTiltBig = MICRO_TO_RAD(sensorValues_[S_HEAD_TILT]);
  lastTiltSmall = 0;
#endif
  lastPan  = MICRO_TO_RAD(sensorValues_[S_HEAD_PAN]);

  stickinessCount=0;
  utils.Head_FollowObject(vo_ball_->centreX_,vo_ball_->centreY_);
  return 1;
}
bool HeadTricks::FollowBallWithHeadSticky::IsUsingHead() {
  return true;
}
char* HeadTricks::FollowBallWithHeadSticky::GetErrorMsg(int msg) {
  if (msg == -1) return "LOST_BALL";
  else if (msg == 1) return "SEE_BALL";
  else if (msg == 2) return "LOST_BALL_TEMP";
  return "FBWH:NO_MESSAGE";
}
char* HeadTricks::FollowBallWithHeadSticky::GetName() {
  return "FollowBallWithHeadSticky";
}

// END FOLLOWBALLWITHHEADSTICKY
////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////
// BEGIN FOLLOWBEACONWITHHEADSTICKY

HeadTricks::FollowBeaconWithHeadSticky::FollowBeaconWithHeadSticky(int _stickiness, VisionObject** tb) {
  stickiness = _stickiness;
  stickinessCount = 0;
  targetBeaconReference = tb;
//  targetGP = targetGP_;
//  if (targetGP) targetBeacon = vo_greenPinkBeacon_;
//  else targetBeacon = vo_pinkGreenBeacon_;
}
int HeadTricks::FollowBeaconWithHeadSticky::Start() { 
  return Continue();
}
int HeadTricks::FollowBeaconWithHeadSticky::Abort() {
  return 0;
}
int HeadTricks::FollowBeaconWithHeadSticky::Continue() {
  //if (targetGP) targetBeacon = vo_greenPinkBeacon_;
  //else targetBeacon = vo_pinkGreenBeacon_;
  VisionObject* targetBeacon =  *targetBeaconReference;

  if (targetBeacon == NULL) {
    if (stickinessCount >= stickiness) {
      return -1;
    }

    HeadCommand hc = lcq_.GetHeadCommand();
    double pan = hc.targetPan_;
    double tilt = hc.targetTilt2_ + 0.3;
    utils.Head_MoveTo(hc.targetTilt1_,pan,tilt);

    stickinessCount++;
    return 2;
  } 
  stickinessCount=0;
  utils.Head_FollowObject(targetBeacon->centreX_,targetBeacon->centreY_);
  return 1;

}
bool HeadTricks::FollowBeaconWithHeadSticky::IsUsingHead() {
  return true;
}
char* HeadTricks::FollowBeaconWithHeadSticky::GetErrorMsg(int msg) {
  if (msg == -1) return "LOST_BEACON";
  else if (msg == 1) return "SEE_BEACON";
  else if (msg == 2) return "LOST_BEACON_TEMP";
  return "FBEWH:NO_MESSAGE";
}
char* HeadTricks::FollowBeaconWithHeadSticky::GetName() {
  return "FollowBeaconWithHeadSticky";
}

// END FOLLOWBEACONWITHHEADSTICKY
////////////////////////////////////////////////////////////////

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

HeadTricks::HeadPanForBall::HeadPanForBall(int k) {
  numFramesPerSweep = k;
}
HeadTricks::HeadPanForBall::HeadPanForBall() {
//cout << "HeadPanForBall constructor" << endl << flush;
  numFramesPerSweep = 70;
}
int HeadTricks::HeadPanForBall::Start() { 
//cout << "Start()" << endl << flush;

  if (vo_ball_ != NULL) return 0;

  utils.Head_PanForBall(true,false,numFramesPerSweep);
  if (lcq_.head.movingLeft_) return 2;
  else return 1;
}
int HeadTricks::HeadPanForBall::Abort() {
  return 0;
}
int HeadTricks::HeadPanForBall::Continue() {
  if (vo_ball_ != NULL) return 0;
//cout << "Continue()" << endl << flush;
  
  utils.Head_PanForBall(false,false,numFramesPerSweep);
  if (lcq_.head.movingLeft_) return 2;
  else return 1;
}
bool HeadTricks::HeadPanForBall::IsUsingHead() {
  return true;
} 
char* HeadTricks::HeadPanForBall::GetErrorMsg(int msg) {
  if (msg == 0) return "SEE_BALL";
  else if (msg == 1) return "PAN_RIGHT";
  else if (msg == 2) return "PAN_LEFT";
  return "HPFB:NO_MESSAGE";
}
char* HeadTricks::HeadPanForBall::GetName() {
  return "HeadPanForBall";
}

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

////////////////////////////////////////////////////////////////
// BEGIN HEADPAN

HeadTricks::HeadPan::HeadPan(double l, double r, double t, double s) {
  left = l;
  right = r;
  tilt = t;
  speed = s;
}
int HeadTricks::HeadPan::Start() {
  utils.Head_Pan(left,right,tilt,speed,true);
  if (lcq_.head.movingLeft_) return 2;
  else return 1;
}
int HeadTricks::HeadPan::Abort() {
  return 0;
}
int HeadTricks::HeadPan::Continue() {
  utils.Head_Pan(left,right,tilt,speed,false);
  if (lcq_.head.movingLeft_) return 2;
  else return 1;
}
bool HeadTricks::HeadPan::IsUsingHead() {
  return true;
} 
char* HeadTricks::HeadPan::GetErrorMsg(int msg) {
  if (msg == 1) return "PAN_RIGHT";
  else if (msg == 2) return "PAN_LEFT";
  return "HPFB:NO_MESSAGE";
}
char* HeadTricks::HeadPan::GetName() {
  return "HeadPan";
}
// END HEADPAN
////////////////////////////////////////////////////////////////

// BEGIN HoldThenMove
HeadTricks::HoldThenMove::HoldThenMove(double holdTime_, double t1_, double t2_, double p_, double framesToMove_) {
  holdTime = holdTime_;
  t1 = t1_;
  t2 = t2_;
  p = p_;
  speed = framesToMove_;
  
  //calculate the steps required in each move
  double currTilt1 = MICRO_TO_DEG(sensorValues_[S_HEAD_TILT_BIG]);
  double currTilt2 = MICRO_TO_DEG(sensorValues_[S_HEAD_TILT_SMALL]);
  double currPan = MICRO_TO_DEG(sensorValues_[S_HEAD_PAN]);

  t1step = (t1 - currTilt1) / speed;
  t2step = (t2 - currTilt2) / speed;
  pstep = (p - currPan) / speed;
}

int HeadTricks::HoldThenMove::Start() { 
  return Continue();
}
int HeadTricks::HoldThenMove::Abort() {
  return 0;
}
int HeadTricks::HoldThenMove::Continue() {

  if (holdTime > 0) {
    holdTime --;
    return 1; // holding
  }
  
  if (speed < 1) return 0; // done
  speed --;

 // double currTilt1 = MICRO_TO_DEG(sensorValues_[S_HEAD_TILT_BIG]);
 // double currTilt2 = MICRO_TO_DEG(sensorValues_[S_HEAD_TILT_SMALL]);
 // double currPan = MICRO_TO_DEG(sensorValues_[S_HEAD_PAN]);

/*double pan = currPan+pstep;
  double tilt = currTilt1+t1step;
  double tilt2 = currTilt2+t2step;
*/
  //utils.Head_MoveTo(tilt,pan,tilt2);
  utils.Head_MoveTo(t1,p,t2);
  return 2;
}

bool HeadTricks::HoldThenMove::IsUsingHead() {
  return true;
}
char* HeadTricks::HoldThenMove::GetErrorMsg(int msg) {
  if (msg == 0) return "DONE";
  else if (msg == 1) return "HOLDING";
  else if (msg == 2) return "MOVING";
  return "HTM:NO_MESSAGE";
}
char* HeadTricks::HoldThenMove::GetName() {
  return "HoldThenMove";
}

// END HOLDTHENMOVE
////////////////////////////////////////////////////////////////

HeadTricks::QuickPan::QuickPan(int _numFrames, bool _panLeft) {
  currentFrame = 0;
  numFrames = _numFrames;
  halfNumFrames = _numFrames/2;
  panLeft = _panLeft;

  currentTilt1 = MICRO_TO_DEG(sensorValues_[S_HEAD_TILT_BIG]);
  currentTilt2 = MICRO_TO_DEG(sensorValues_[S_HEAD_TILT_SMALL]);
  currentPan = MICRO_TO_DEG(sensorValues_[S_HEAD_PAN]);

  double destTilt1 = 0.0;
  double destTilt2 = 0.0;
  double destPan = -95.0;

  if (panLeft) destPan = -1*destPan;

  interpolationRateTilt1 = (destTilt1-currentTilt1)/halfNumFrames;
  interpolationRateTilt2 = (destTilt2-currentTilt2)/halfNumFrames;
  interpolationRatePan = (destPan-currentPan)/halfNumFrames;
}
int HeadTricks::QuickPan::Start() {
  return Continue();
}
int HeadTricks::QuickPan::Abort() {
  return 0;
}
int HeadTricks::QuickPan::Continue() {
  currentFrame++;
  if (currentFrame >= numFrames) return 0;
  if (currentFrame <= halfNumFrames) utils.Head_MoveTo(currentTilt1+interpolationRateTilt1*currentFrame, currentPan+interpolationRatePan*currentFrame, currentTilt2+interpolationRateTilt2*currentFrame);
  else utils.Head_MoveTo(currentTilt1+interpolationRateTilt1*(numFrames-currentFrame), currentPan+interpolationRatePan*(numFrames-currentFrame), currentTilt2+interpolationRateTilt2*(numFrames-currentFrame));

  return 1;  
}
bool HeadTricks::QuickPan::IsUsingHead() {
  return true;
} 
char* HeadTricks::QuickPan::GetErrorMsg(int msg) {
  if (msg == 0) return "FINISHED";
  else if (msg == 1) return "DOING";
  return "QuickPan:NO_MESSAGE";
}
char* HeadTricks::QuickPan::GetName() {
  return "QuickPan";
}