balltricks.cc

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

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

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

////////////////////////////////////////////////////////////////
// BEGIN WaitForKick
BallTricks::WaitForKick::WaitForKick() {
}
int BallTricks::WaitForKick::Start() {
  return Continue();
}
int BallTricks::WaitForKick::Abort() {
  return 0;
}
int BallTricks::WaitForKick::Continue() {
  if (lcq_.IsTheStrutReady() == false) return 1;

  if (vo_ball_ != NULL) {
    return -2;
  }
  return -1;
}
bool BallTricks::WaitForKick::IsUsingHead() {
  return true;
}
char* BallTricks::WaitForKick::GetErrorMsg(int msg) {
  if (msg == -1) return "NO_BALL_AFTER_KICK";
  else if (msg == -2) return "SEE_BALL_AFTER_KICK";
  else if (msg == 1) return "KICKING";
  return "WFK:NO_MESSAGE";
}
char* BallTricks::WaitForKick::GetName() {
  return "WaitForKick";
}
// END WaitForKick
////////////////////////////////////////////////////////////////
// BEGIN WaitForGrab
// same as above, but gives a specific error when we can see the ball once the grab has finished
BallTricks::WaitForGrab::WaitForGrab() {
}
int BallTricks::WaitForGrab::Start() {
  return Continue();
}
int BallTricks::WaitForGrab::Abort() {
  return 0;
}
int BallTricks::WaitForGrab::Continue() {
  if (lcq_.IsTheStrutReady() == false) return 1;

  if (vo_ball_ != NULL) return -2;
  return -1;
}
bool BallTricks::WaitForGrab::IsUsingHead() {
  return true;
}
char* BallTricks::WaitForGrab::GetErrorMsg(int msg) {
  if (msg == -1) return "GRAB_SUCCESS";
  else if (msg == -2) return "GRAB_FAIL";
  else if (msg == 1) return "GRABBING";
  return "WFG:NO_MESSAGE";
}
char* BallTricks::WaitForGrab::GetName() {
  return "WaitForGrab";
}
// END WaitForGrab
////////////////////////////////////////////////////////////////
// START CheckForBall

BallTricks::CheckForBall::CheckForBall() {
  cout << "BallTricks::CheckForBall is retarded. Please don't use it!" << endl << flush;
  sentGrabTest = false;
  sentGrabComplete = false;
  currentTrick = new BasicTricks::NullBody();
}
BallTricks::CheckForBall::~CheckForBall() {
  delete currentTrick;
}
int BallTricks::CheckForBall::Start() {
  return Continue();
}
int BallTricks::CheckForBall::Abort() {
  return 0;
}
int BallTricks::CheckForBall::Continue() {
  if (!sentGrabTest) {
    sentGrabTest=true;
    utils.Kick(LocomotionCommand::TP_GRAB_TEST, false);
    delete currentTrick;
    currentTrick = new BallTricks::WaitForKick();
    currentTrick->Start();
    return 2;
  }
  if (!sentGrabComplete) {
    int cC = currentTrick->Continue();
    if (cC < 1) {
      // CHECK IF GRAB SUCCEEDED
//    if (lcq_.GetGrabSuccess()) {
        // grab success
        sentGrabComplete=true;
        utils.Kick(LocomotionCommand::TP_POST_GRAB_POSITION,false);
        delete currentTrick;
        currentTrick = new BallTricks::WaitForKick();
        currentTrick->Start();
        return 3;
//    } else {
//      // grab fail
//      return 0;
//    }
    }
    return 1;
  }
  // wait for post_grab to finish..
  int cC = currentTrick->Continue();
  if (cC < 1) {
    return -1;
  }
  return 1;
}
bool BallTricks::CheckForBall::IsUsingHead() {
  return false;
}
char* BallTricks::CheckForBall::GetErrorMsg(int msg) {
  if (msg == 3) return "GRAB_SUCCESS";
  else if (msg == 2) return "SENDING_GRAB_TEST";
  else if (msg == 1) return "WAITING_FOR_TEST";
  else if (msg == 0) return "GRAB_FAIL";
  else if (msg == -1) return "CFB_DONE";
  return "CFB:NO_MESSAGE";
}
char* BallTricks::CheckForBall::GetName() {
  return "CheckForBall";
}

// END CheckForBall
//////////////////////////////////////////////////////////////
// BEGIN TurnWithBallUntilGoal
/*
BallTricks::TurnWithBallUntilGoal::TurnWithBallUntilGoal(double _normalAmount, double _maxAmount) {
  if (_maxAmount > 0) isTurningLeft = true;
  else isTurningLeft = false;

  maxAmount = _maxAmount;
  turnAmount = _normalAmount;
  curTurnAmount = 0.0;
  sawGoal = false;
}
int BallTricks::TurnWithBallUntilGoal::Start() {
  return Continue();
}
int BallTricks::TurnWithBallUntilGoal::Abort() {
  // get out ASAP!!
  // have to dump the queue otherwise we may keep stepping.
  lcq_.Clear();
  return 0;
}
int BallTricks::TurnWithBallUntilGoal::Continue() {
  // can we see goal?
  if (vo_oppositionGoal_ != NULL) {
    sawGoal=true;
    // are we lined up with goal?
    if (ABS(RAD_TO_DEG(vo_oppositionGoal_->heading_)) < 10.0) {
      out << "TurnWithBallUntilGoal: Goal is lined up ! Aborting." << endl << flush;
      // abort turn and get the hell out of here. it's time to shoot!!
      Abort();
      return -1;
    } else { // not lined up with goal.
      utils.Head_FollowObject(vo_oppositionGoal_->centreX_, vo_oppositionGoal_->centreY_);
      // we're allowed to turn further
      if (ABS(curTurnAmount) < ABS(maxAmount)) {
        // try to make turnAmount match how far we have to turn.
        turnAmount = curTurnAmount+RAD_TO_DEG(vo_oppositionGoal_->heading_);
        // limit turn amount to our maximum.
        if ((turnAmount > 0) && (turnAmount > maxAmount)) turnAmount = maxAmount;
        if ((turnAmount < 0) && (turnAmount < maxAmount)) turnAmount = maxAmount;
      }
    }
  } else {
  // can't see the goal. move head so we're looking just in front of where we're turning
    double currentPan = MICRO_TO_DEG(sensorValues_[S_HEAD_PAN]);
    double pan = 0.0;
    if (isTurningLeft == false) {
      pan = currentPan - 10.0;
    } else {
      pan = currentPan + 10.0;
    }
    if (pan > 45.0) pan = 45.0;
    if (pan < -45.0) pan = -45.0;
    utils.Head_MoveTo(7.5,pan,0.0);
    sawGoal=false;
  }
  // check if queue is empty and the strut is idle.. OR that we're only walking (so we can start a turnwithball w/o a grab)
  if ( (lcq_.IsTheStrutReady() == true) || (lcq_.IsWalkOnly())) {
    // queue empty, strut idle. have we completely turned ?
    if (ABS(curTurnAmount-turnAmount) <= 1) {
      // yep. get out!
      if (sawGoal) {
        return -1;
      } else {
        return -2;
      }
    }
    // haven't turned yet .. so do it !
    double ta = 0.0;
    if (ABS(curTurnAmount-turnAmount) > 1) {
      ta=turnAmount-curTurnAmount;
      if (ta > 25.0) ta = 25.0;
      if (ta < -25.0) ta = -25.0;
      curTurnAmount+=ta;
      // queue turn
      utils.TurnWithBall(ta,1.3);
    }
    // trick still in progress...
    return 1;
  }
  // waiting for thestrut to finish turning
  return 1;
}
bool BallTricks::TurnWithBallUntilGoal::IsUsingHead() {
  return true;
}
char* BallTricks::TurnWithBallUntilGoal::GetErrorMsg(int msg) {
  if (msg == 1) return "IN_PROGRESS";
  else if (msg == -1) return "SAW_GOAL";
  else if (msg == -2) return "NO_GOAL";
  return "TWBUG:NO_MESSAGE";
}
char* BallTricks::TurnWithBallUntilGoal::GetName() {
  return "TurnWithBallUntilGoal";
}
// END TurnWithBallUntilGoal
*/


BallTricks::TurnWithBallUntilGoal::TurnWithBallUntilGoal(double _normalAmount, double _maxAmount) {
  if (_maxAmount > 0) isTurningLeft = true;
  else isTurningLeft = false;
  startFrame = frame_;
  maxAmount = _maxAmount;
  turnAmount = _normalAmount;
  absCurTurnAmount = 0.0;
  sawGoal = false;
  numStepsTaken = 0;
  maxSteps = 2;
  headUpDelay = 0;
  quitAngle = 10;
}

BallTricks::TurnWithBallUntilGoal::TurnWithBallUntilGoal(int _headUpDelay, double _normalAmount, double _maxAmount) {
  if (_maxAmount > 0) isTurningLeft = true;
  else isTurningLeft = false;
  startFrame = frame_;
  maxAmount = _maxAmount;
  turnAmount = _normalAmount;
  absCurTurnAmount = 0.0;
  sawGoal = false;
  numStepsTaken = 0;
  maxSteps = 2;
  quitAngle = 10;
  headUpDelay = _headUpDelay;
}


BallTricks::TurnWithBallUntilGoal::TurnWithBallUntilGoal(double _normalAmount, double _maxAmount, int _maxSteps) {
  if (_maxAmount > 0) isTurningLeft = true;
  else isTurningLeft = false;
  startFrame = frame_;
  maxAmount = _maxAmount;
  turnAmount = _normalAmount;
  absCurTurnAmount = 0.0;
  sawGoal = false;
  numStepsTaken = 0;
  maxSteps = _maxSteps;
  headUpDelay = 0;
  quitAngle = 10;
}

BallTricks::TurnWithBallUntilGoal::TurnWithBallUntilGoal(double _normalAmount, double _maxAmount, int _maxSteps, double _quitAngle) {
  if (_maxAmount > 0) isTurningLeft = true;
  else isTurningLeft = false;
  startFrame = frame_;
  maxAmount = _maxAmount;
  turnAmount = _normalAmount;
  absCurTurnAmount = 0.0;
  sawGoal = false;
  numStepsTaken = 0;
  maxSteps = _maxSteps;
  headUpDelay = 0;
  quitAngle = _quitAngle;
}
BallTricks::TurnWithBallUntilGoal::TurnWithBallUntilGoal(int _headUpDelay, double _normalAmount, double _maxAmount, int _maxSteps) {
  if (_maxAmount > 0) isTurningLeft = true;
  else isTurningLeft = false;
  startFrame = frame_;
  maxAmount = _maxAmount;
  turnAmount = _normalAmount;
  absCurTurnAmount = 0.0;
  sawGoal = false;
  numStepsTaken = 0;
  maxSteps = _maxSteps;
  headUpDelay = _headUpDelay;
  quitAngle = 10;
}

int BallTricks::TurnWithBallUntilGoal::Start() {
  return 1;
}
int BallTricks::TurnWithBallUntilGoal::Abort() {
  // get out ASAP!!
  // have to dump the queue otherwise we may keep stepping.
  lcq_.Clear();
  lcq_.SetEmergencyCancel(true);
  return 0;
}
int BallTricks::TurnWithBallUntilGoal::Continue() {
  
  // can we see goal?
  if (vo_oppositionGoal_ != NULL) {
    sawGoal=true;
    // are we lined up with goal?
    if (ABS(RAD_TO_DEG(vo_oppositionGoal_->heading_)) < quitAngle || ((RAD_TO_DEG(goalHeadingLeft_) < quitAngle) && (RAD_TO_DEG(goalHeadingRight_) > quitAngle))) {
      cout << "TurnWithBallUntilGoal: Goal lined up. heading: " << RAD_TO_DEG(vo_oppositionGoal_->heading_) << " gHL: " << RAD_TO_DEG(goalHeadingLeft_) << " gHR: " << RAD_TO_DEG(goalHeadingRight_) << endl << flush;
      // abort turn and get the hell out of here. it's time to shoot!!
      Abort();
      return -1;
    } else { // not lined up with goal.
      if (vo_oppositionGoal_->topBlob_->minX > currentImageWidth_/2 || vo_oppositionGoal_->topBlob_->maxX < currentImageWidth_/2) {
        int closestX = vo_oppositionGoal_->topBlob_->maxX;
        if ( ABS(currentImageWidth_/2 - vo_oppositionGoal_->topBlob_->minX) < ABS(currentImageWidth_/2 - vo_oppositionGoal_->topBlob_->maxX)) {
          closestX = vo_oppositionGoal_->topBlob_->minX;
        }
#ifdef ERS_210
        utils.Head_FollowObject(closestX, vo_oppositionGoal_->centreY_);
#endif
#ifdef ERS_7
        utils.Head_FollowObjectHeadDown(closestX, vo_oppositionGoal_->centreY_);
        double pan = MICRO_TO_DEG(sensorValues_[S_HEAD_PAN]);
        mouthPosition_ = -55.0+ABS(pan); // Open the mouth
#endif
      }
      // we're allowed to turn further
      if (absCurTurnAmount < ABS(maxAmount)) {
        // try to make turnAmount match how far we have to turn. this actually allows us to change direction mid turn, if we see the goal
//      turnAmount = RAD_TO_DEG(vo_oppositionGoal_->heading_);
        double goalHeading = RAD_TO_DEG(vo_oppositionGoal_->heading_);
        if (goalHeadingLeft_ < DEG_TO_RAD(quitAngle) && goalHeadingRight_ > DEG_TO_RAD(-quitAngle)) {
          goalHeading = 0.0;
        } else {
          double temp = RAD_TO_DEG(goalHeadingLeft_)-quitAngle;
          if (ABS(temp) < ABS(goalHeading)) {
            goalHeading = temp;
          }
          temp = RAD_TO_DEG(goalHeadingRight_)+quitAngle;
          if (ABS(temp) < ABS(goalHeading)) {
            goalHeading = temp;
          }
        }

        turnAmount = goalHeading;
        // limit turn amount to our maximum.
        // Commented out by MQ on 11th June, It takes the same time to turn the larger ammount so why do we bother cropping it ?
      //  if ((turnAmount >= 0) && (absCurTurnAmount+turnAmount > maxAmount)) turnAmount = maxAmount-absCurTurnAmount;
      //  if ((turnAmount < 0) &&  (-absCurTurnAmount+turnAmount < maxAmount)) turnAmount = maxAmount+absCurTurnAmount;
      }
    }
  } else {
  // can't see the goal. move head so we're looking just in front of where we're turning
    double currentPan = MICRO_TO_DEG(sensorValues_[S_HEAD_PAN]);
    double pan = 0.0;
#ifdef ERS_210
    if (isTurningLeft == false) {
      pan = currentPan - 10.0;
    } else {
      pan = currentPan + 10.0;
    }
    if (pan > 45.0) pan = 45.0;
    if (pan < -45.0) pan = -45.0;
    if (frame_ - startFrame >= headUpDelay) {
    utils.Head_MoveTo(7.5,pan,0.0);
#endif
#ifdef ERS_7
    if (isTurningLeft == false) {
      if (strutSystem_.timeParameter > 0.7) pan = currentPan + 5.0;
      else pan = currentPan - 5.0;
      pan = CROP(pan,-40.0,0.0);
    } else {
      if (strutSystem_.timeParameter > 0.7) pan = currentPan - 5.0;
      else pan = currentPan + 5.0;
      pan = CROP(pan,0.0,40.0);
    }
    if (pan > 30.0) pan = 30.0;
    if (pan < -30.0) pan = -30.0;
    if (frame_ - startFrame >= headUpDelay) {
    utils.Head_MoveTo(-50-(ABS(pan)/2),pan,50.0);
    mouthPosition_ = -55.0+ABS(pan); // Open the mouth
    }
  #endif
    sawGoal=false;
  }
  // check if queue is empty and the strut is idle.. OR that we're only walking (so we can start a turnwithball w/o a grab)
  if ( (lcq_.IsTheStrutReady() == true) || (lcq_.IsWalkOnly())) {
    if (absCurTurnAmount > 0) {
      if (vo_ball_ != NULL) {
        return -3;
      }
    }
    // queue empty, strut idle. have we completely turned ?
    // I added this || ABS(absCurTurnAmount)>=ABS(maxAmount) .. is it required ??
    if (ABS(turnAmount) <= 15 || ABS(absCurTurnAmount)>=ABS(maxAmount) || numStepsTaken == maxSteps) {
      // yep. GTFO !
      if (sawGoal) {
        return -1;
      } else {
        return -2;
      }
    }
    // haven't fully turned yet .. so do it !
    // Outdated comment -> Set max turn to 30 not 45 -- testing this .. may be too small we need to see how far he can turn without slipping (also maybe test a min turn)
    double ta = turnAmount;
    if (ta > 90.0) ta = 90.0;  //was 45 
    if (ta < -90.0) ta = -90.0;
    absCurTurnAmount+=ABS(ta);
    turnAmount-=ta;
    // queue turn
    numStepsTaken++;
    #ifdef ERS_7
    utils.TurnWithBall(ta,1.6);
    #endif
    #ifdef ERS_210
    utils.TurnWithBall(ta,1.5);
    #endif
    // trick still in progress...
    return 1;
  }
  // waiting for thestrut to finish turning
  return 1;
}
bool BallTricks::TurnWithBallUntilGoal::IsUsingHead() {
  return true;
}
char* BallTricks::TurnWithBallUntilGoal::GetErrorMsg(int msg) {
  if (msg == 1) return "IN_PROGRESS";
  else if (msg == -1) return "SAW_GOAL";
  else if (msg == -2) return "NO_GOAL";
  else if (msg == -3) return "GRAB_FAIL";
  return "TWBUG:NO_MESSAGE";
}