movementtricks.cc

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

char* MovementTricks::SearchForBall::GetErrorMsg(int msg) {
  if (msg == 0) return "SEE_BALL";
  else if (msg == -1) return "LOST_BALL";
  else if (msg == 1) return "SEARCHING";
  return "SFB:NO_MESSAGE";
}
char* MovementTricks::SearchForBall::GetName() {
  return "SearchForBall";
}
// END SearchForBall
////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////
// BEGIN CheckLocalisation
MovementTricks::CheckLocalisation::CheckLocalisation(double _degreesToTurn, int _turnIncrement) {
  double x = 0;
  double y = 215;
  double theta = (atan2(y-wo_self_->y_, x-wo_self_->x_) - (PI / 2.0)) - wo_self_->heading_;
  double goalHeading = RAD_TO_DEG(theta);

  while (goalHeading >= 180) goalHeading -= 360.0;
  while (goalHeading < -180) goalHeading += 360.0;

  turnRight = true;
  if (goalHeading > 0) {
    turnRight = false;
  } else {
    turnRight = true;
  }
  degreesToTurn = _degreesToTurn;
  degreesTurned = 0.0;
  startFrame = frame_;
  headTrick = new HeadTricks::HeadPan(85,-85,3,50);
  startedHead=false;
  utils.Step(LocomotionCommand::TP_WALK,0.0,0.0,0.0,0.0,1.82);
  turnIncrement = _turnIncrement;
}

MovementTricks::CheckLocalisation::~CheckLocalisation() {
  delete headTrick;
}
int MovementTricks::CheckLocalisation::Start() {
  return Continue();
}
int MovementTricks::CheckLocalisation::Abort() {
  return 0;
}

int MovementTricks::CheckLocalisation::Continue() {
  
  if ( (frame_-startFrame) >= 6) {
    if (startedHead == false) {
      headTrick->Start();
      startedHead=true;
    }
  } else {
    return 1;
  }

  // turned the full way, can't see ball. get out !
  if (degreesTurned >= degreesToTurn) {
    // strut is busy- probably still turning. we can't exit yet !
    if (lcq_.IsTheStrutReady() == false) {
      headTrick->Continue();
      // continue search..
      return 1;
    } else {
      // strut is NOT busy, and we've turned the full way.
      return 0;
    }
  }

  headTrick->Continue();

  if ( (frame_-startFrame) < 6) return 1;

  // strut is still stepping. don't queue !
  if ((lcq_.IsTheStrutReady() == false) && (degreesTurned != 0.0)) {
    return 1;
  }

  double turnAmount = turnIncrement;
  if ((degreesTurned+turnAmount) > degreesToTurn) {
    turnAmount = degreesToTurn-degreesTurned;
  }
  degreesTurned = degreesTurned+turnAmount;

  if (turnRight) {
    turnAmount = -turnAmount;
  }
  utils.Step(LocomotionCommand::TP_SINGLEWALK,0.0,0.0,turnAmount,0.0,1.6);
  return 1;
}

bool MovementTricks::CheckLocalisation::IsUsingHead() {
  return true;
}
char* MovementTricks::CheckLocalisation::GetErrorMsg(int msg) {
  if (msg == 0) return "DONE";
  else if (msg == 1) return "DOING_SPIN";
  return "CL:NO_MESSAGE";
}
char* MovementTricks::CheckLocalisation::GetName() {
  return "CheckLocalisation";
}
// END CheckLocalisation
////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////
// BEGIN CHASEBEACON

MovementTricks::ChaseBeacon::ChaseBeacon(VisionObject** _targetBeaconReference) {
  targetBeaconReference = _targetBeaconReference;

  stepFreq = 1.6;
  maxForward = 110.0;
  fsh = LocomotionCommand::defaultFrontStrideHeight;
  ffo = LocomotionCommand::defaultFrontForwardOffset;
  fso = LocomotionCommand::defaultFrontSideOffset;
  fh = LocomotionCommand::defaultFrontHeight;
  bsh = LocomotionCommand::defaultBackStrideHeight;
  bfo = LocomotionCommand::defaultBackForwardOffset;
  bso = LocomotionCommand::defaultBackSideOffset;
  bh = LocomotionCommand::defaultBackHeight;
}

MovementTricks::ChaseBeacon::~ChaseBeacon() {
  delete headTrick;
}

int MovementTricks::ChaseBeacon::Start() { 
  headTrick = new HeadTricks::FollowBeaconWithHeadSticky(10,targetBeaconReference);  // GP beacon
  headTrick->Start();
  timeObjectInRange = 0;
  return Continue();
}

void MovementTricks::ChaseBeacon::Set(double stepFreq_, double maxForward_,double maxBack_, double fsh_, double ffo_, double fso_, double fh_, double bsh_, double bfo_, double bso_, double bh_) {
  stepFreq = stepFreq_;
  maxForward = maxForward_;
  maxBack = maxBack_;
  fsh = fsh_;
  ffo = ffo_;
  fso = fso_;
  fh = fh_;
  bsh = bsh_;
  bfo = bfo_;
  bso = bso_;
  bh = bh_;
}

int MovementTricks::ChaseBeacon::Abort() {
  return 0;
}

int MovementTricks::ChaseBeacon::Continue() {
  VisionObject* targetBeacon = *targetBeaconReference;

  int hC = headTrick->Continue();
  char* hMsg = headTrick->GetErrorMsg(hC);
  
  // If we are searching close don't step!
  if ((strcmp(hMsg,"LOST_BEACON_TEMP") == 0 )) {
    return 2;    
  }

  // head trick ended... ie, we lost the ball !
  if (hC < 1) {
    LocomotionCommand lc;
    lc.Set(LocomotionCommand::TP_WALK,0.1,0.1,0.1,0.0,1.0);
    utils.RawCommand(lc);
    return -1;
  }

  if (targetBeacon == NULL) {
    LocomotionCommand lc;
    lc.Set(LocomotionCommand::TP_WALK,0.1,0.1,0.1,0.0,1.0);
    utils.RawCommand(lc);
    return 1;
  }

  if (targetBeacon->distance_ < 40) return 0;

  double forward= 0;
  double back = 0;
  double turn = 0;
  double strafe = 0;

  double beaconHeading = RAD_TO_DEG(targetBeacon->heading_);
  double beaconDistance = ABS(targetBeacon->distance_);
  double absBeaconHeading = ABS(beaconHeading);

	if (absBeaconHeading > 40) {
    forward = MIN(beaconDistance*10.0,20.0)*cos(targetBeacon->heading_);
    back = MIN(beaconDistance*10.0,20.0)*cos(targetBeacon->heading_);
    strafe = MIN(beaconDistance*10.0,20.0)*sin(targetBeacon->heading_);
    turn = MIN(absBeaconHeading, 30.0);
	}	else {
    forward = MIN(beaconDistance*5.0,maxForward)*cos(targetBeacon->heading_);
    back = MIN(beaconDistance*5.0,maxBack)*cos(targetBeacon->heading_);
    strafe = MIN(beaconDistance*10.0,15.0)*sin(targetBeacon->heading_);
    double maximumTurn = 25.0 - sqrt(forward*forward/100.0 + strafe*strafe/100.0); // 28 was 18
    double absMaximumTurn = ABS(maximumTurn);
    turn = MIN(absBeaconHeading, absMaximumTurn);
	}

	// set turn direction - negative value means turn clockwise (RIGHT)
	if (beaconHeading < 0) {
    turn = -turn;
  }

  // can't use a normal step here... have to bypass the trick AND part of the utils abstraction layer
  // in order to set the front side offset how we want it.
  LocomotionCommand lc;
  lc.Set(LocomotionCommand::TP_WALK,forward,back,turn,strafe,stepFreq);
  
  lc.frontStrideHeight = fsh;
  lc.frontForwardOffset = ffo;
  lc.frontSideOffset = fso;
  lc.frontHeight = fh;

  lc.backStrideHeight = bsh;
  lc.backForwardOffset = bfo;
  lc.backSideOffset = bso;
  lc.backHeight = bh;
  
  utils.RawCommand(lc);
  return 1;
}

bool MovementTricks::ChaseBeacon::IsUsingHead() {
  return true;
}

char* MovementTricks::ChaseBeacon::GetErrorMsg(int msg) {
  if (msg == 2) return "CLOSE_SEARCH";
  else if (msg == 1) return "CHASING_BEACON";
  else if (msg == 0) return "BEACON_IN_RANGE";
  else if (msg == -1) return "LOST_BEACON";
  return "CB:NO_MESSAGE";
}
char* MovementTricks::ChaseBeacon::GetName() {
  return "ChaseBeacon";
}

// END CHASEBEACON
////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////
// BEGIN FaceBeacon
MovementTricks::FaceBeacon::FaceBeacon(VisionObject** _targetBeaconReference, bool turnRight_) {
  targetBeaconReference = _targetBeaconReference;
  turnRight = turnRight_;
  doneNull = false;
  if (turnRight) headTrick = new HeadTricks::HeadPan(-50,-5,10,30);
  else headTrick = new HeadTricks::HeadPan(50,5,10,30);
  headTrick->Start();
  utils.Step(LocomotionCommand::TP_WALK,0.0,0.0,0.0,0.0,2.5);
}

MovementTricks::FaceBeacon::~FaceBeacon() {
  delete headTrick;
}
int MovementTricks::FaceBeacon::Start() {
  return Continue();
}
int MovementTricks::FaceBeacon::Abort() {
  return 0;
}

int MovementTricks::FaceBeacon::Continue() {
  VisionObject* targetBeacon = *targetBeaconReference;

// strut is still stepping. don't queue !
  if (doneNull) {
     if (lcq_.IsTheStrutReady() == false) return 1;
     return 0;
  }

  int hC = headTrick->Continue();
  //char* hMsg = headTrick->GetErrorMsg(hC);
  if (hC >= 1 && strcmp("HeadPan",headTrick->GetName())==0) {
    if (targetBeacon !=NULL) {
      delete headTrick;
      headTrick = new HeadTricks::FollowBeaconWithHeadSticky(10,targetBeaconReference);
      headTrick->Start();
    }
  } else if (hC < 1) {
    delete headTrick;
    if (turnRight) headTrick = new HeadTricks::HeadPan(-50,-5,10,30);
    else headTrick = new HeadTricks::HeadPan(50,5,10,30);
    headTrick->Start();
  }

  double turnAmount = 15.0;
  if (hC == 1 && strcmp("FollowBeaconWithHeadSticky",headTrick->GetName())==0 && ABS(MICRO_TO_DEG(sensorValues_[S_HEAD_PAN]))<50) {
    if (ABS(MICRO_TO_DEG(sensorValues_[S_HEAD_PAN]))<7) {
      utils.Step(LocomotionCommand::TP_SINGLEWALK,0.0,0.0,0,0.0,1.4); 
      doneNull = true;
      return 1;
    }  else turnAmount = 10.0;
  } else {   
    turnAmount = 15.0;
  }
  
  //b (turnRight && turnAmount == 17) turnAmount = 22; //hack
  if (turnRight) turnAmount = -1*turnAmount;

  LocomotionCommand lc;
  lc.Set(LocomotionCommand::TP_WALK,0.0,0.0,turnAmount,0.0,1.4);
  
  lc.frontStrideHeight = -9.5;
  lc.frontForwardOffset = 60.0;
  lc.frontSideOffset = 3.36;
  lc.frontHeight = 64.2;

  lc.backStrideHeight = 42.2;
  lc.backForwardOffset = -49.3;
  lc.backSideOffset = -0.72;
  lc.backHeight = 102.7;

  lc.locusType = LocomotionCommand::L_TRAPEZOID;
  
  utils.RawCommand(lc);

  //utils.Step(LocomotionCommand::TP_WALK,0.0,0.0,turnAmount,0.0,1.4);
  return 1;
}

bool MovementTricks::FaceBeacon::IsUsingHead() {
  return true;
}
char* MovementTricks::FaceBeacon::GetErrorMsg(int msg) {
  if (msg == 0) return "DONE";
  else if (msg == 1) return "DOING_SPIN";
  return "FB:NO_MESSAGE";
}
char* MovementTricks::FaceBeacon::GetName() {
  return "FaceBeacon";
}

// END FACEBEACON
////////////////////////////////////////////////////////////////