balltricks.cc

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

char* BallTricks::TurnWithBallUntilGoal::GetName() {
  return "TurnWithBallUntilGoal";
}

// END TurnWithBallUntilGoal



//////////////////////////////////////////////////////////////
// BEGIN TurnWithBall
BallTricks::TurnWithBall::TurnWithBall(double _normalAmount) {
  turnAmount = _normalAmount;
  curTurnAmount = 0.0;
}
int BallTricks::TurnWithBall::Start() {
  if (mouthPosition_ < - 2.5) { //closed
    utils.Kick(LocomotionCommand::TP_GRAB,true);
  }  
  return Continue();
}
int BallTricks::TurnWithBall::Abort() {
  // get out ASAP!!
  // have to dump the queue otherwise we may keep stepping.
  lcq_.Clear();
  return 0;
}
int BallTricks::TurnWithBall::Continue() {
  // 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())) {
    utils.TurnWithBall(turnAmount,1.6);
    return -1;
  }
  // waiting for thestrut to finish turning
  return 1;
}
bool BallTricks::TurnWithBall::IsUsingHead() {
  return false;
}
char* BallTricks::TurnWithBall::GetErrorMsg(int msg) {
  if (msg == 1) return "IN_PROGRESS";
  else if (msg == -1) return "DONE";
  return "TWB:NO_MESSAGE";
}
char* BallTricks::TurnWithBall::GetName() {
  return "TurnWithBall";
}
// END TurnWithBall
////////////////////////////////////////////////////////////////

// BEGIN TurnWithBallHeadDown

//WARNING ! Only call with TurnAmounts which are multiples of 45
BallTricks::TurnWithBallHeadDown::TurnWithBallHeadDown(double _turnAmount) {
  turnAmount = _turnAmount;
  curTurnAmount = 0.0;
}
int BallTricks::TurnWithBallHeadDown::Start() {
  return Continue();
}
int BallTricks::TurnWithBallHeadDown::Abort() {
  return 0;
}
int BallTricks::TurnWithBallHeadDown::Continue() {
  // check if queue is empty and the strut is idle
  if ((lcq_.IsTheStrutReady() == true) || (lcq_.IsWalkOnly())) {
    // queue empty, strut idle. have we turned ?
    if (ABS(curTurnAmount-turnAmount) <= 1) {
      // yes. quit out of this trick successfully 1!
      return -1;
    }
    // haven't stepped yet .. so do it !
    double ta = 0.0;
    while (ABS(curTurnAmount-turnAmount) > 1) {
      ta=turnAmount-curTurnAmount;
      if (ta > 65) ta = 65.0;
      if (ta < -65) ta = -65.0;
      curTurnAmount+=ta;
      // queue steps 
      #ifdef ERS_210
      utils.Head_MoveTo(-53,0,0);
      #endif
      #ifdef ERS_7
      utils.Head_MoveTo(-70,0,30);
      #endif
      utils.TurnWithBallHeadDown(ta,1.6);
    }
    // trick still in progress...
    return 1;
  }
  // waiting for thestrut to finish turning
  return 1;
}
bool BallTricks::TurnWithBallHeadDown::IsUsingHead() {
  return true;
}
char* BallTricks::TurnWithBallHeadDown::GetErrorMsg(int msg) {
  if (msg == 1) return "IN_PROGRESS";
  if (msg == -1) return "DONE_TURNWITHBALLHEADDOWN_WITH_BALL";
  return "SWB:NO_MESSAGE";
}
char* BallTricks::TurnWithBallHeadDown::GetName() {
  return "TurnWithBallHeadDown";
}
// END TurnWithBallHeadDown
////////////////////////////////////////////////////////////////

// BEGIN StrafeWithBall

BallTricks::StrafeWithBall::StrafeWithBall(double _strafeAmount) {
  strafeAmount = _strafeAmount;
  curStrafeAmount = 0.0;
}
int BallTricks::StrafeWithBall::Start() {
  return Continue();
}
int BallTricks::StrafeWithBall::Abort() {
  return 0;
}
int BallTricks::StrafeWithBall::Continue() {
  // check if queue is empty and the strut is idle
  if ((lcq_.IsTheStrutReady() == true) || (lcq_.IsWalkOnly())) {
    // queue empty, strut idle. have we turned ?
    if (ABS(curStrafeAmount-strafeAmount) <= 1) {
      // yes. quit out of this trick successfully 1!
      return -1;
    }
    // haven't stepped yet .. so do it !
    double sa = 0.0;
    while (ABS(curStrafeAmount-strafeAmount) > 1) {
      sa=strafeAmount-curStrafeAmount;
      if (sa > 120.0) sa = 120.0;
      if (sa < -120.0) sa = -120.0;
      curStrafeAmount+=sa;
      // queue steps
      utils.Head_MoveTo(-53,0,0);
      utils.StrafeWithBall(sa,1.3);
    }
    // trick still in progress...
    return 1;
  }
  // waiting for thestrut to finish turning
  return 1;
}
bool BallTricks::StrafeWithBall::IsUsingHead() {
  return true;
}
char* BallTricks::StrafeWithBall::GetErrorMsg(int msg) {
  if (msg == 1) return "IN_PROGRESS";
  if (msg == -1) return "DONE_STRAFE_WITH_BALL";
  return "SWB:NO_MESSAGE";
}
char* BallTricks::StrafeWithBall::GetName() {
  return "StrafeWithBall";
}
// END StrafeWithBall
////////////////////////////////////////////////////////////////

// BEGIN TurnKick

BallTricks::TurnKick::TurnKick(double _turnAmount,bool _hard, bool _didGrab) {
  turnAmount = _turnAmount;
  hard = _hard;
  didGrab = _didGrab;
  headTrick = NULL;
  frames = 0;
}

BallTricks::TurnKick::~TurnKick() {
  if (headTrick != NULL) delete headTrick;
}

int BallTricks::TurnKick::Start() {
  return Continue();
}
int BallTricks::TurnKick::Abort() {
  return 0;
}
int BallTricks::TurnKick::Continue() {
  mouthPosition_ = -3.1;
  if (didGrab) {
    int hC=0;
    if (headTrick == NULL) {
      headTrick = new HeadTricks::HoldThenMove(8.0,0.0,0.0,0.0,15.0);
      hC=headTrick->Start();
    } else hC=headTrick->Continue();

    if (hC < 1 && frames != 1000) {
      if (hard) utils.Step(LocomotionCommand::TP_SINGLEWALK,160,160,0,0,1.8);
    else
      utils.Step(LocomotionCommand::TP_SINGLEWALK,120.0,120.0,0.0,0.0,1.8);
      utils.Head_MoveTo(-19,turnAmount*0.2,15);
      if (turnAmount < 0) lastKickType_ = LocomotionCommand::TP_TURN_RIGHT;
      else lastKickType_ = LocomotionCommand::TP_TURN_LEFT;
      lastKickFrame_ = frame_;
      frames=1000;
      return 1;
    }

    if (frames==1000) {
      if (lcq_.IsTheStrutReady()) { cout << "**Spin Kick of size "  << turnAmount << endl << flush; return 0; } // trick has finished
      else return 1;
    }
    LocomotionCommand lc;
    lc.Set(LocomotionCommand::TP_WALK,30.0,20.0,turnAmount*0.7,0.0,1.7);
    lc.odomTurn = turnAmount;
    lc.frontHeight = 55.6;
    lc.frontStrideHeight = -3.2;
    lc.frontForwardOffset = 85.1;
    lc.frontSideOffset = 9.0;

    lc.backHeight = 110.4;
    lc.backStrideHeight = 25.1;
    lc.backForwardOffset = -45.1;
    lc.backSideOffset = 2.6;

    utils.RawCommand(lc);
    frames++;
    return 1;
  } else { // no grab
    int hC=0;
    if (headTrick == NULL) {
      headTrick = new HeadTricks::HoldThenMove(4.0,0.0,0.0,0.0,20.0);
      hC=headTrick->Start();
    } else hC=headTrick->Continue();

    if (hC < 1 && frames != 1000) {
      if (hard) utils.Step(LocomotionCommand::TP_SINGLEWALK,160,160,0,0,1.8);
    else
      utils.Step(LocomotionCommand::TP_SINGLEWALK,120.0,120.0,0.0,0.0,1.8);
      utils.Head_MoveTo(-19,turnAmount*0.2,15);
      if (turnAmount < 0) lastKickType_ = LocomotionCommand::TP_TURN_RIGHT;
      else lastKickType_ = LocomotionCommand::TP_TURN_LEFT;
      lastKickFrame_ = frame_;
      frames=1000;
      return 1;
    }

    if (frames==1000) {
      if (lcq_.IsTheStrutReady()) { cout << "**Spin Kick of size "  << turnAmount << endl << flush; return 0; } // trick has finished
      else return 1;
    }
    LocomotionCommand lc;
    double speed = 1.5 + frames/40; // changed from 1.2 on plane to lisbon
    lc.Set(LocomotionCommand::TP_WALKTURNKICK,30.0,20.0,turnAmount*0.5,0.0,speed); 
    lc.odomTurn = turnAmount;
    lc.frontHeight = 55.6;
    lc.frontStrideHeight = -3.2;
    lc.frontForwardOffset = 45.1;
    lc.frontSideOffset = 9.0;

    lc.backHeight = 110.4;
    lc.backStrideHeight = 25.1;
    lc.backForwardOffset = -45.1;
    lc.backSideOffset = 2.6;

    utils.RawCommand(lc);
    frames++;
    return 1;
  }
  return 1;
}

bool BallTricks::TurnKick::IsUsingHead() {
  return true;
}
char* BallTricks::TurnKick::GetErrorMsg(int msg) {
  if (msg == 1) return "IN_PROGRESS";
  if (msg == 0) return "DONE_TURNKICK";
  return "TK:NO_MESSAGE";
}
char* BallTricks::TurnKick::GetName() {
  return "TurnKick";
}
// END TurnKick
////////////////////////////////////////////////////////////////
// BEGIN GrabTrick
BallTricks::GrabTrick::GrabTrick() {
  frames = 0;
}

BallTricks::GrabTrick::~GrabTrick() {
}

int BallTricks::GrabTrick::Start() {
  startPan = MICRO_TO_DEG(sensorValues_[S_HEAD_PAN]);
  LocomotionCommand lc;
  
 // if (vo_ball_ == NULL) 
    lc.Set(LocomotionCommand::TP_SINGLEWALK,100.0,110.0,0.0,0.0,1.72);
//  else
//    lc.Set(LocomotionCommand::TP_SINGLEWALK,ABS(vo_ball_->distance_)*cos(vo_ball_->heading_)*6,ABS(vo_ball_->distance_)*cos(vo_ball_->heading_)*7,ABS(vo_ball_->distance_)*sin(vo_ball_->heading_)*2.0,0.0,1.72);
  lc.frontForwardOffset=70.0;
  lc.frontSideOffset=5.0;
  utils.RawCommand(lc);
  return Continue();
}

int BallTricks::GrabTrick::Abort() {
  return 0;
}

int BallTricks::GrabTrick::Continue() {
 mouthPosition_ = 3;
   
  int dir = 1;
  if (startPan > 0) dir = -1;
  
  double value1 = 14500;
  double value2 = 16000;
  if (BOTID_ == 2) {
    value1 = 13500;
    value2 = 15000;
  }

  if (sensorValues_[S_BODY_PSD] > value1 && frames < 5) frames = 8;
  if (sensorValues_[S_BODY_PSD] > value2 && frames < 10) frames = 12;
  if (frames < 5) {
    utils.Head_MoveTo(-10,MICRO_TO_DEG(sensorValues_[S_HEAD_PAN])/5.0,10);
    frames++;
  } else {
    utils.Head_MoveTo(-30-frames,MICRO_TO_DEG(sensorValues_[S_HEAD_PAN])/5.0,30);
    mouthPosition_=-40;
    frames++;
  }
  if (frames > 20) {
    mouthPosition_ = 3;
    if (vo_ball_ != NULL) {
       lcq_.SetEmergencyCancel(true);
       lcq_.Clear();
       return -10000;
    }
    //cout << "Chin = " << sensorValues_[S_MOUTH] << " " << sensorValues_[S_HEAD_TILT_BIG] << " " << sensorValues_[S_HEAD_TILT_SMALL] << " " << ball << flush << endl;
   //if (sensorValues_[S_HEAD_TILT_SMALL] < (872664-10000)) {1000000
    if (sensorValues_[S_BODY_PSD] < 140000) {
   //if (sensorValues_[S_MOUTH] < -80000) {
     lcq_.SetEmergencyCancel(true);
     lcq_.Clear();
   //  cout << "GRAB FAILED" << flush << endl;
     return -10000;
    } else { 
      lcq_.SetEmergencyCancel(true);
      lcq_.Clear();
   //   cout << "GRAB PASSED" << flush << endl;
      return 0;
    }
  }
  
  return 1;
}

bool BallTricks::GrabTrick::IsUsingHead() {
  return true;
}
char* BallTricks::GrabTrick::GetErrorMsg(int msg) {
  if (msg == 1) return "IN_PROGRESS";
  if (msg == 0) return "GRAB_DONE";
  if (msg == -10000) return "GRAB_FAILED";
  return "TK:NO_MESSAGE";
}
char* BallTricks::GrabTrick::GetName() {
  return "GrabTrick";
}
// END GrabTrick
////////////////////////////////////////////////////////////////

// BEGIN SlapTrick
BallTricks::SlapTrick::SlapTrick(int _kickType) {
  kickType_ = _kickType;
  frames = 0;
  far = 0;
  grab = false;
}

BallTricks::SlapTrick::SlapTrick(int _kickType, bool _grab) {
  kickType_ = _kickType;
  frames = 0;
  far = 0;
  grab = _grab;
}


BallTricks::SlapTrick::~SlapTrick() {
  if (headTrick != NULL) delete headTrick;
  if (currentTrick != NULL) delete currentTrick;
}

int BallTricks::SlapTrick::Start() {
  if (!grab) {
    headTrick = new HeadTricks::FollowBallWithHeadSticky(7);
    headTrick->Start();
  } else utils.Head_MoveTo(0.0,0.0,0.0);
  
  if (grab) currentTrick = new BasicTricks::Kick(kickType_,true);
  currentTrick = new BasicTricks::Kick(kickType_,false);
  currentTrick->Start();
  return currentTrick->Continue();
}
int BallTricks::SlapTrick::Abort() {
  return 0;
}
int BallTricks::SlapTrick::Continue() {
  frames++;
  if (frames > 25) return 0;
  if (grab) return 1;
  int hC = headTrick->Continue();
  char* hMsg = headTrick->GetErrorMsg(hC);
  if ((strcmp(hMsg,"LOST_BALL_TEMP") == 0 )) {
    return 1;    
  } else if (hC < 1 && frames < 15) { // head trick ended... ie, we lost the ball !
    lcq_.SetEmergencyCancel(true);
    lcq_.Clear();
    return -1000;
  }
  int cC = currentTrick->Continue();
  if (cC < 1) {
    delete currentTrick;
    currentTrick = new BasicTricks::NullBody();
    currentTrick->Start();
  }
 
  if (vo_ball_==NULL) return 1;
  double tempX = ABS(vo_ball_->distance_) * sin(vo_ball_->heading_);
  double tempY = ABS(vo_ball_->distance_) * cos(vo_ball_->heading_); 

  if (tempY > (20 + 0.2*frames)  || ABS(tempX) > (10 + 0.2*frames)/* && !ballPrediction_.isMovingAway_*/) { 
    far++;
  } else far--;
  far = MAX(far,0);
  if (far > 2 && frames < 15) {
    lcq_.SetEmergencyCancel(true);
    lcq_.Clear();
    return -1000;
  }  
  return 1;
}

bool BallTricks::SlapTrick::IsUsingHead() {
  return true;
}
char* BallTricks::SlapTrick::GetErrorMsg(int msg) {
  if (msg == 1) return "IN_PROGRESS";
  if (msg == 0) return "SLAP_DONE";
  if (msg == -10000) return "SLAP_FAILED";
  return "ST:NO_MESSAGE";
}
char* BallTricks::SlapTrick::GetName() {
  return "SlapTrick";
}
// END SlapTrick
////////////////////////////////////////////////////////////////