movementtricks.cc

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

// Bag of primarily movement tricks

#include <math.h>

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

////////////////////////////////////////////////////////////////
// BEGIN CHASEBALL

void MovementTricks::ChaseBall::Init(bool gab, int csd) {
  underChinAngle = configuration_.GetAsDouble("UnderChinAngle");
  maxWalkAroundDistance = configuration_.GetAsDouble("MaxWalkAroundDistance");
  getAroundBall = gab;
  chaseMode = csd;

  pushBallLeftWall = false;
  pushBallRightWall = false;
  lastPushBallDetect = 0;

  previousStealthDirection = 0;
  previousVeerDirection = 0;
}

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

int MovementTricks::ChaseBall::Start() { 
  headTrick = new HeadTricks::FollowBallWithHeadSticky(10);
  headTrick->Start();
  timeBallUnderChin = 0;
     
  return Continue();
}
int MovementTricks::ChaseBall::Abort() {
  return 0;
}

int MovementTricks::ChaseBall::Continue() {
  #ifdef ERS_210
    return Continue210();
  #endif
  #ifdef ERS_7
    return Continue7();
  #endif
}


void MovementTricks::ChaseBall::SetChaseMode(int m) {
#ifdef ERS_7
  // see if the ball is under the slap trigger
  if ((robotState_.GetState() != RobotState::ST_PLAYING || (robotState_.GetKickOff() == RobotState::KO_OWN && (frame_-lastReadyFrame_) < 90 && BOTID_ != 1)) ) {    
    chaseMode = m;
  }
  if (CheckUnderChin7(true) || CheckUnderChin7(false) || vo_ball_ == NULL) {
    return;
  }
  chaseMode = m;
#endif
#ifdef ERS_210
  long headPan = sensorValues_[S_HEAD_PAN];
  long headTilt = sensorValues_[S_HEAD_TILT];
  if (vo_ball_==NULL) return;
  double ballElevation = RAD_TO_MICRO(vo_ball_->elevation_); 
  if ((ballElevation <= DEG_TO_MICRO(underChinAngle+0) && (ABS(headPan) <= (90.0 - RAD_TO_DEG(atan(frontLength/sideLength))))) return;
  chaseMode = m;
#endif
}


bool MovementTricks::ChaseBall::CheckUnderChin7(bool slapping) {
/////////
  if (vo_ball_ == NULL) return false;

  double chinAngle = underChinAngle;
  long headPan = sensorValues_[S_HEAD_PAN];
  long headTilt = sensorValues_[S_HEAD_TILT1]+sensorValues_[S_HEAD_TILT2];
  
  double ballElevation = vo_ball_->elevation_;
    double fieldAngleToBall =Normalise_PI((atan2(wo_ball_->y_-wo_self_->y_, wo_ball_->x_-wo_self_->x_) - (PI / 2.0)) - wo_self_->heading_);
  double d = sqrt(((wo_ball_->x_-wo_self_->x_)*(wo_ball_->x_-wo_self_->x_)+(wo_ball_->y_-wo_self_->y_)*(wo_ball_->y_-wo_self_->y_)));

  double tempX = ABS(d) * sin(fieldAngleToBall);
  double tempY = ABS(d) * cos(fieldAngleToBall);
  /*double tempX = ABS(vo_ball_->distance_) * sin(vo_ball_->heading_);
  double tempY = ABS(vo_ball_->distance_) * cos(vo_ball_->heading_); 
  if (ballPrediction_.DoPrediction(3)) {
    tempX = ballPrediction_.predictedBall_.x;
    tempY = ballPrediction_.predictedBall_.y;
  }
 */
  if (slapping) {
//   if (headTilt <= DEG_TO_MICRO(chinAngle) && ABS(headPan) <= DEG_TO_MICRO(14)) { 
      if (!utils.SpinNotSlap(1,utils.GetHeadingToGoal())) { // i.e slap
        if (tempY < 15 && ABS(tempX) < 4/* && !ballPrediction_.isMovingAway_*/) { 
          return true;
        }
      } else {
        if (headTilt <= DEG_TO_MICRO(chinAngle) && ballElevation <= DEG_TO_RAD(chinAngle) && ABS(tempX) < 2.0/*vo_ball_->heading_ <= DEG_TO_RAD(15) && ABS(headPan) <= DEG_TO_MICRO(8)*/) {
      //  if (tempY < 13 && ballElevation <= DEG_TO_RAD(-37.0) && ABS(tempX) < 2/* && !ballPrediction_.isMovingAway_*/) {   
          return true;
        }
      } 
      return false;
 //   }
    return false;
  }
  //long ballElevationTop = RAD_TO_MICRO(topBallElevation_); 
/*  if (headTilt <= DEG_TO_MICRO(chinAngle) && ABS(headPan) <= DEG_TO_MICRO(12) && (ballElevationTop <= DEG_TO_MICRO(chinAngle-1) || sensorValues_[S_BODY_PSD] > 17000)) { // 17000 means something is very close to our chest
      return true;
  }*/
    double chinModifier = 0.0;
    if (lcq_.Front().frontStrideLength > 120.0) chinModifier = 0.0;
    
    //if (headTilt <= DEG_TO_MICRO(chinAngle) && ballElevation <= DEG_TO_RAD(chinAngle-2) && ABS(tempX) < 2.0/*vo_ball_->heading_ <= DEG_TO_RAD(15) && ABS(headPan) <= DEG_TO_MICRO(8)*/) {
    if (tempY < 18 && ABS(tempX) < 2.0 && headTilt <= DEG_TO_MICRO(-30) || (headTilt <= DEG_TO_MICRO(-30) && ABS(lastBallDistance_) < 30 && sensorValues_[S_BODY_PSD] > 140000) /* && !ballPrediction_.isMovingAway_*/) {   
      return true;
    }
    return false;
}


int MovementTricks::ChaseBall::Continue7() {
#ifdef ERS_7
//  cout << frame_ << ", bd: " << vo_ball_->distance_ << endl << flush;
  ledOn2_[10] = false; // Indicates stealth dog
  int hC = headTrick->Continue();
  char* hMsg = headTrick->GetErrorMsg(hC);
  
  // this is fucked !!!
  if ((robotState_.GetState() != RobotState::ST_PLAYING || (robotState_.GetKickOff() == RobotState::KO_OWN && (frame_-lastReadyFrame_) < 90 && BOTID_ != 1)) ) {    
    chaseMode = CB_RAM;
  }

  // If we are searching close don't fool with current step!
  if ((strcmp(hMsg,"LOST_BALL_TEMP") == 0 )) {
    LocomotionCommand lc = lcq_.Front();
    /*if ((chaseMode == CB_RAM && lastBallDistance_ < 40) || lastBallDistance_ > 50.0) utils.Step(LocomotionCommand::TP_WALK,lc.frontStrideLength*0.95,lc.backStrideLength*0.95,lc.turn*0.95,lc.strafe*0.95,lc.stepFrequency);
    else utils.Step(LocomotionCommand::TP_WALK,lc.frontStrideLength/2,lc.backStrideLength/2,lc.turn/2,lc.strafe/2,lc.stepFrequency);
    */
    utils.Step(LocomotionCommand::TP_WALK,lc.frontStrideLength*0.95,lc.backStrideLength*0.95,lc.turn*0.95,lc.strafe*0.95,lc.stepFrequency);
    return 2;    
  } else if (hC < 1) { // head trick ended... ie, we lost the ball !
        // make sure we fuck off any walk steps hanging around in the queue.
    lcq_.Clear();
    return -1;
  }
  
//  long headPan = sensorValues_[S_HEAD_PAN];
  long headTilt = sensorValues_[S_HEAD_TILT1]+sensorValues_[S_HEAD_TILT2];

  bool underChin = false;
  // check ball under chin !
//  double ballElevation = RAD_TO_MICRO(vo_ball_->elevation_);
  if (CheckUnderChin7(chaseMode == CB_SLAP)) {
    if (timeBallUnderChin > 0 || (chaseMode == CB_GRAB)) { 
      timeBallUnderChin = 0;
      return 0;
    } else {
      timeBallUnderChin++;
      underChin = true;
      if (chaseMode == CB_SLAP) utils.Step(LocomotionCommand::TP_WALK,10.0,10.0,0,0,1.7);
      //else utils.Step(LocomotionCommand::TP_WALK,140.0,150.0,0,0,1.82);
    }
  } else { // ball isn't under chin. reset counter.
    timeBallUnderChin--;
    if (timeBallUnderChin < 0) timeBallUnderChin = 0;
  }

  // when close, lock onto the middle of the ball
  // what the ? if (ABS(ballElevation) <= DEG_TO_MICRO(25) && ballElevation < DEG_TO_MICRO(8)) {
  //if (ballElevation <= DEG_TO_MICRO(25)) {
  //  aimAtMiddleOfBall_ = true;
  //}
  bool allowSlowDown = true;

//  double maxFrontStrideLength = 180.97;
//  double maxBackStrideLength = 200.55;
 
 double maxFrontStrideLength = 180.97;
  double maxBackStrideLength = 200.55;
 
 
  double maxStrafe = 30.0;
  
  double ballDistance = ABS(vo_ball_->distance_);
  double ballHeading = vo_ball_->heading_;

  // reset our normal locomotioncommand variable and setting parameters again
  nLC.Reset();
  nLC.motionType = LocomotionCommand::TP_WALK;
  nLC.stepFrequency = 1.82; // set robot step speed
 
   // handle veer around ball, stealth dog and push ball down wall
  double headingModifier = PushBallDownWallHeadingModifier();
  double veerStrafe = 0.0;
  double stealthStrafeModifier = StealthDog();
  if (ABS(RAD_TO_DEG(headingModifier)) >= 0.01) { // if push down wall, we don't do slowdown.
    allowSlowDown = false;
  } else if (ABS(stealthStrafeModifier) >= 0.01) { // doing stealth dog ?
    veerStrafe = stealthStrafeModifier;
    //allowSlowDown = false; // no need for this rubbish .. ball is miles away etc.
    ledOn2_[10] = true;
  } else if (chaseMode == CB_RAM) { // no push down wall. are we ramming the ball ?
   //allowSlowDown=false;
   headingModifier = RamBall(0); // 0 = any paw will do
  } else if (ABS(veerStrafe) < 0.01)  { // we may wish to veer around the ball some if we're not ramming etc.
    veerStrafe = VeerAroundBallHeadingModifier(headTilt,ballDistance);
    if (ABS(veerStrafe) > 0.01) ledOn2_[7] = true; 
  }

  if (chaseMode == CB_SLAP) { ledOn2_[18] = true; ledOn2_[19] = true; } // Indicates chase slap
  if (chaseMode == CB_RAM || allowSlowDown == false) ledOn2_[13] = true; // Indicates chase ram

  //double change = 0.0;
  //if (MICRO_TO_DEG(headTilt) < -15) change = MIN(4.0,ABS(MICRO_TO_DEG(headTilt)+20)/2);
//  nLC.frontForwardOffset+=2*change; //ffo+(MICRO_TO_DEG(HeadTilt)+60)/10.0;
  //nLC.frontSideOffset+=change; //fso+(MICRO_TO_DEG(HeadTilt)+60)/10.0;
  double frontLength = maxFrontStrideLength / 20.0 + nLC.frontForwardOffset/10 + 3.0; // 
  //double sideLength = nLC.frontSideOffset/10+4.0;

   // set up some useful variables for 'normal chase'
  ballHeading += headingModifier;
  ballHeading = Normalise_PI(ballHeading);
  double newBallDistance = ballDistance;
 
  if (/*chaseMode == CB_SLAP &&*/ ballDistance > 30) { 
    newBallDistance = sqrt((ballDistance*ballDistance)+(frontLength*frontLength)-(2*ballDistance*frontLength*cos(vo_ball_->heading_)));
    if (newBallDistance > 18) ballHeading = Normalise_PI(asin((sin(ballHeading)*ballDistance)/newBallDistance));
    else newBallDistance = ballDistance;
  }

  double front=0.0;
  double back=0.0;
  double strafe=0.0;
  double turn=0.0;
  double turnAngleDeg=18; //was 30 .. please check me out in the morning as I might be wrong !!!
 
  ballDistance = MAX(6,ballDistance-8); 
  if (RAD_TO_DEG(vo_ball_->elevation_) < underChinAngle) ballDistance = MIN(8,ballDistance);
  if (ballDistance > 25) maxStrafe = 10.0;

  bool ramNoTurn = false;
  if (chaseMode == CB_RAM || !allowSlowDown) {
    double tX = ballDistance * sin(ballHeading);
    if (RAD_TO_DEG(ballHeading) > turnAngleDeg && tX < 7.0) ramNoTurn=true; 
  }
  
  double newBallHeading = ballHeading;
  double newBallHeadingDeg = RAD_TO_DEG(newBallHeading);
  double newAbsBallHeadingDeg = ABS(newBallHeadingDeg);
   //cout << vo_ball_->distance_ << " " << RAD_TO_DEG(vo_ball_->heading_) << " " << RAD_TO_DEG(vo_ball_->elevation_) << " " << tempX << " " << tempY << endl << flush; 
  if (newAbsBallHeadingDeg < turnAngleDeg || ramNoTurn) {
    front = CROP(newBallDistance*10.0*cos(newBallHeading),30,maxFrontStrideLength);
    back = CROP(newBallDistance*10.0*cos(newBallHeading),30,maxBackStrideLength);

    if (chaseMode == CB_RAM || !allowSlowDown) {
      front = maxFrontStrideLength;
      back = maxBackStrideLength;
    }

    double strafeMult = 10.0;
    if (newBallDistance < 30) strafeMult = 10.0;
    if (ABS(veerStrafe) > 0.01) strafe = veerStrafe;
    else strafe = CROP(strafeMult*newBallDistance*sin(newBallHeading),-maxStrafe,maxStrafe);
    double maximumTurn = turnAngleDeg - sqrt(back*back/100.0 + strafe*strafe/100.0); // 28 was 18
    double absMaximumTurn = ABS(maximumTurn) * 0.6;
    if (ABS(turn) > 2) turn = MIN(newAbsBallHeadingDeg, absMaximumTurn); 
    else turn = 0.0;
  } else {  
    //front = ABS(CROP(newBallDistance*10.0,5,100)*cos(newBallHeading));
    //back = ABS(CROP(newBallDistance*10.0,5,100)*cos(newBallHeading));
    front = CROP(newBallDistance*4.0*cos(newBallHeading),5,maxFrontStrideLength);
    back = CROP(newBallDistance*4.0*cos(newBallHeading),5,maxBackStrideLength);

    double mult = 0.5;
    if (newAbsBallHeadingDeg > 70 || (newBallDistance > 80 && newAbsBallHeadingDeg > 40)) mult = 0.7;
    turn = MIN(newAbsBallHeadingDeg, 60) * mult;; 
    strafe = CROP(10*sin(newBallHeading),-25,25);
	}	

  
  nLC.frontStrideLength = front;
  nLC.backStrideLength = back;
  nLC.strafe = strafe;
  nLC.turn = turn;
	// set turn direction - negative value means turn clockwise (RIGHT)
	if (ballHeading < 0) {
    nLC.turn = -nLC.turn;
  }


  // handle ball on wall while going backwards special case
  if (GetAroundBallOnWall()) {
    return 1;
  }

  utils.RawCommand(nLC); 
#endif
  return 1;
}

bool MovementTricks::ChaseBall::GetAroundBallOnWall() {
  // new walk around ball code... should be able to get it off the back walls as well.
  bool walkLeftAroundBall = false;
  bool walkRightAroundBall = false;
  double ballDistance = ABS(vo_ball_->distance_);

  if (strutSystem_.motionType == LocomotionCommand::TP_SINGLEWALK) return false;


  if (ballDistance < maxWalkAroundDistance && ballDistance > 20 && ballDistance > 0) {
    // make sure we're not already walking around the ball
    // FIXME this is total shit. should be done in a special trick ..

    // check if ball is close to side and we're facing wrong way
    if (ABS(wo_ball_->x_) > 65 && ABS(wo_self_->heading_) > DEG_TO_RAD(110)) {
      if (wo_ball_->x_ < 0 && wallOnRight_ && RAD_TO_DEG(vo_ball_->heading_) < 40.0) {
        walkLeftAroundBall=true;
      }
      if (wo_ball_->x_ > 0 && wallOnLeft_ && RAD_TO_DEG(vo_ball_->heading_) > -40.0) {
        walkRightAroundBall=true;
      }
    }
    // check if ball is on home wall
    if (wo_ball_->y_ < -165) {
      double h = RAD_TO_DEG(wo_self_->heading_);
      double minHeading = -130;
      double maxHeading = -80;
      // check if we're on the left side of our own goal and kind of facing towards it, with wall to the right of the ball
      if (wo_self_->x_ < 0 && wo_ball_->x_ < 0 && h < maxHeading && h > minHeading && wallOnRight_) {
        walkLeftAroundBall=true;
      }
      minHeading = 80;
      maxHeading = 130;
      // check if we're on the right side of our own goal and kind of facing towards it, with wall to the left of the ball
      if (wo_self_->x_ > 0 && wo_ball_->x_ > 0 && h < maxHeading && h > minHeading && wallOnLeft_) {
        walkRightAroundBall=true;
      }
    }