movementtricks.cc

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

    // check if ball on opposition wall
    if (wo_ball_->y_ > 165) {
      double h = RAD_TO_DEG(wo_self_->heading_);
      double minHeading = 50;
      double maxHeading = 115;
      // check if we're on the left side of the opposition goal (using our normal coords) and kind of facing away from it, with the ball on our right
      if (wo_self_->x_ < 0 && wo_ball_->x_ < 0 && h < maxHeading && h > minHeading && wallOnRight_) {
        walkLeftAroundBall=true;
      }
      minHeading = -115;
      maxHeading = -50;
      // check if we're on the right side of the opposition goal and kind of facing away from 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;
      }
    }
  }
  if (walkRightAroundBall && !walkLeftAroundBall) {
    // we want to step right a bit to try and get around the ball
    nLC.motionType = LocomotionCommand::TP_SINGLEWALK;
    nLC.stepFrequency = 1.45;
    nLC.frontStrideLength = 20.0;
    nLC.backStrideLength = 40.0;
    #ifdef ERS_210
    nLC.strafe = (-80.0); // most important !
    #endif
    #ifdef ERS_7
    nLC.stepFrequency = 2.1;
    nLC.strafe = (-140.0);
    #endif
    nLC.turn = 10.0;
    //if (nLC.turn > 10.0) nLC.turn = 8.0;
    lcq_.Clear();
    utils.RawCommand(nLC);
//  utils.RawCommand(nLC);
//  utils.RawCommand(nLC);
    return true;
  } else if (walkLeftAroundBall && !walkRightAroundBall) {
    // we want to step left a bit to try and get around the ball
    nLC.motionType = LocomotionCommand::TP_SINGLEWALK;
    nLC.stepFrequency = 1.45;
    nLC.frontStrideLength = 20.0;
    nLC.backStrideLength = 40.0;
    #ifdef ERS_210
    nLC.strafe = (80.0); // most important !
    #endif
    #ifdef ERS_7
    nLC.stepFrequency = 2.1;
    nLC.strafe = (140.0);
    #endif
    nLC.turn = -10.0;
    //if (nLC.turn < -10.0) nLC.turn = -10.0;

    lcq_.Clear();
    utils.RawCommand(nLC);
//  utils.RawCommand(nLC);
//  utils.RawCommand(nLC);
    return true;
  }
  return false;
}

double MovementTricks::ChaseBall::VeerAroundBallHeadingModifier(long headTilt, double ballDistance) {  
  double veerStrafe = 0.0;
  double maxStrafe = 30.0;
  const double goalX = 0;
  const double goalY = 215;
  
  return 0.0;
  double deltaHeadingGoal = Normalise_PI(atan2(goalY-wo_ball_->y_, goalX-wo_ball_->x_) - (PI / 2.0) - (wo_self_->heading_));
 
//  double deltaHeadingGoal90 = Normalise_PI(deltaHeadingGoal - PI/2);
//  double deltaHeadingGoal90n= Normalise_PI(deltaHeadingGoal + PI/2);
  double deltaHeading=deltaHeadingGoal;
//  if (ABS(deltaHeadingGoal90) < ABS(deltaHeading)) deltaHeading=deltaHeadingGoal90;
//  if (ABS(deltaHeadingGoal90n) < ABS(deltaHeading)) deltaHeading=deltaHeadingGoal90n;

  deltaHeading=CROP(RAD_TO_DEG(deltaHeading),-maxStrafe,maxStrafe);
  veerStrafe = -1*deltaHeading;
  ballDistance=CROP(ballDistance,0.0,200.0);
  veerStrafe = (MAX(log10(ballDistance/2)-1.0,0))*veerStrafe;

  if ( ABS(RAD_TO_DEG(deltaHeadingGoal)) > 130 && ABS(wo_ball_->x_) > 5) {
      veerStrafe = 0.75;
      if (wo_ball_->x_ < 0 ) veerStrafe = ABS(veerStrafe);
      else veerStrafe = -ABS(veerStrafe);
  }
  
  if (!getAroundBall || (ABS(wo_self_->x_) > 100) || ballDistance < 35 || ABS(veerStrafe) < 10) veerStrafe = 0.0;

  return veerStrafe;
}

double MovementTricks::ChaseBall::PushBallDownWallHeadingModifier() {
  // check if ball is close to side and we're facing the RIGHT way
  if (wo_self_->y_ > 170) return 0.0;
  if (ABS(wo_ball_->x_) > 100 && ABS(wo_self_->heading_) < DEG_TO_RAD(40)) {
    if (wo_ball_->x_ > 0 && wallOnRight_ && !wallOnLeft_) {
      pushBallRightWall = true;
      pushBallLeftWall = false;
      lastPushBallDetect = frame_;
    } else if (wo_ball_->x_ < 0 && wallOnLeft_ && !wallOnRight_) {
      pushBallLeftWall = true;
      pushBallRightWall = false;
      lastPushBallDetect = frame_;
    }
  }
  if (frame_ - lastPushBallDetect > 15) {
    pushBallLeftWall = false;
    pushBallRightWall = false;
  }
  if (pushBallLeftWall) {
    //cout << "Ram left wall" << endl << flush;
    return RamBall(2);
  } else if (pushBallRightWall) {
    //cout << "Ram right wall" << endl << flush;
    return RamBall(1);
  }
  return 0.0;
}

double MovementTricks::ChaseBall::RamBall(int side) {
  double ballx = sin(vo_ball_->heading_) * ABS(vo_ball_->distance_);
  double bally = cos(vo_ball_->heading_) * ABS(vo_ball_->distance_);
  double ofs = 6.0;
  double target_x = 0.0;
  if (side == 0) {
    if (ballx < 0) {
      // right paw
      target_x = ballx+ofs;
    } else if (ballx >= 0) {
      // left paw
      target_x = ballx-ofs;
    }
  } else if (side == 1) { // RIGHT paw
    target_x = ballx+ofs;
  } else if (side == 2) { // LEFT paw
    target_x = ballx-ofs;
  }
    
  double target_y = bally - 4.2;
  double target_d = sqrt(target_x * target_x + target_y * target_y);
  double target_h = asin(target_x / target_d);

/////////
    
  double headingModifier = target_h-vo_ball_->heading_;
  if (headingModifier > DEG_TO_RAD(30.0)) headingModifier = DEG_TO_RAD(30.0);
  if (headingModifier < -DEG_TO_RAD(30.0)) headingModifier = -DEG_TO_RAD(30.0);
  return headingModifier;
}

bool MovementTricks::ChaseBall::CheckStealthDog(double* leftObstacle, double* rightObstacle) {
  double ballDistance = ABS(vo_ball_->distance_);
  // ball too close => no need
  if (ballDistance < 40.0) return false;

/*
  // FIXME these are fucked, -CM
  bool nearWallRight = (wo_self_->x_ > 100) && (wo_self_->heading_ > DEG_TO_RAD(50) && wo_self_->heading_ < DEG_TO_RAD(130));
  bool nearWallLeft  = (wo_self_->x_ < -100) && (wo_self_->heading_ < DEG_TO_RAD(-50) && wo_self_->heading_ > DEG_TO_RAD(-130));
  bool nearWallBack =    (wo_self_->y_ < -170) && (wo_self_->heading_ < DEG_TO_RAD(-135) || wo_self_->heading_ > DEG_TO_RAD(135.0));
	bool nearWallForward = (wo_self_->y_ > 170) && (wo_self_->heading_ < DEG_TO_RAD(45) || wo_self_->heading_ > DEG_TO_RAD(-45));
  if (nearWallRight || nearWallLeft || nearWallBack || nearWallForward) return false;
*/

  int furthestLeftRobot = -1;
  int furthestRightRobot = -1;
  for (int i = 0; i < numVisionObjects_; i++) {
    if (visionObjects_[i].type_ == VisionObject::OT_ROBOT_RED || visionObjects_[i].type_ == VisionObject::OT_ROBOT_BLUE) {
      // ball is closer (or almost closer) than this robot, so we don't care about stealth dog
      // we can do better than this ... FIXME
      if (ballDistance-15.0 < visionObjects_[i].distance_) {
        continue;
      }
      // opponent robot is too far away to care about.
      if (visionObjects_[i].distance_ > 75) {
        continue;
      }
      if (furthestLeftRobot == -1 || (visionObjects_[i].heading_ > visionObjects_[furthestLeftRobot].heading_)) furthestLeftRobot = i;
      if (furthestRightRobot == -1 || (visionObjects_[i].heading_ < visionObjects_[furthestRightRobot].heading_)) furthestRightRobot = i;
    }
  }
  // no robots in the way ! screw you stealth dog.
  if (furthestLeftRobot == -1 || furthestRightRobot == -1) return false;
	
  // ok now, we only really care about the robot closest to us. after that, sif care ?
  if (visionObjects_[furthestLeftRobot].distance_ < visionObjects_[furthestRightRobot].distance_) {
    *leftObstacle = visionObjects_[furthestLeftRobot].heading_;
    *rightObstacle = visionObjects_[furthestLeftRobot].heading_;
  }	else {
    *leftObstacle = visionObjects_[furthestRightRobot].heading_;
    *rightObstacle = visionObjects_[furthestRightRobot].heading_;
  }
	return true;
}

double MovementTricks::ChaseBall::StealthDog() {
  double leftObstacle, rightObstacle;

  if (CheckStealthDog(&leftObstacle, &rightObstacle)) {
    double averageObstacle = (leftObstacle + rightObstacle)/2.0;
    double desiredStealthHeading=0.0;
    
    //cout << "Stealth - " << vo_ball_->heading_ << " " <<  averageObstacle;
    if (vo_ball_->heading_ < averageObstacle) {
      desiredStealthHeading = DEG_TO_RAD(-55.0);
    } else {
      desiredStealthHeading = DEG_TO_RAD(55.0);
    }
    double veerStrafe = 0.0;
    double maxStrafe = 50.0;
    double deltaHeading = Normalise_PI(desiredStealthHeading);
    deltaHeading=CROP(RAD_TO_DEG(deltaHeading),-maxStrafe,maxStrafe);
    veerStrafe = deltaHeading;
    //cout << " " << desiredStealthHeading << " " << veerStrafe << endl << flush;
    return veerStrafe;
  } 
  previousStealthDirection = 0;
  return 0.0;
}

bool MovementTricks::ChaseBall::IsUsingHead() {
  return true;
}
char* MovementTricks::ChaseBall::GetErrorMsg(int msg) {
  if (msg == 2) return "CLOSE_SEARCH";
  else if (msg == 1) return "CHASING_BALL";
  else if (msg == 0) return "BALL_UNDER_CHIN";
  else if (msg == -1) return "LOST_BALL";
  else if (msg == -2) return "BLOCKED";
  return "CB:NO_MESSAGE";
}
char* MovementTricks::ChaseBall::GetName() {
  return "ChaseBall";
}

// END CHASEBALL
////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////
// BEGIN MoveToPoint

MovementTricks::MoveToPoint::MoveToPoint(double _x, double _y, double _h, int _t) { // Heading in degrees
  x = _x;
  y = _y;
  heading = _h;
  type = _t;

  canExit = false; // This is important .. most of our old dude code DOES not handle exits 
  xRange = 10;
  yRange = 10;
  hRange = 10;
  if (type == MTP_MANHATTAN) lcq_.Clear();
  isKeeper = false;
}

MovementTricks::MoveToPoint::MoveToPoint(double _x, double _y, double _h, int _t, bool _e, double _xR, double _yR, double _hR) { // Heading in degrees
  x = _x;
  y = _y;
  heading = _h;
  type = _t;
  canExit = _e;
  xRange = _xR;  
  yRange = _yR;
  hRange = _hR;
  if (type == MTP_MANHATTAN) lcq_.Clear();
  isKeeper = false;
}

MovementTricks::MoveToPoint::~MoveToPoint() {
  lcq_.SetEmergencyCancel(true);
  lcq_.Clear();
}

int MovementTricks::MoveToPoint::Start() { 
  return Continue();
}
int MovementTricks::MoveToPoint::Abort() {
  return 0;
}
int MovementTricks::MoveToPoint::Continue() {
  // common set up stuff.
  double currx = wo_self_->x_;
  double curry = wo_self_->y_;
  double currheading = wo_self_->heading_;

  // If we are allowed to exit then see if we are close enough to our home point
  if (canExit && (ABS(currx - x) < xRange) &&  (ABS(curry - y) < yRange) && (ABS(RAD_TO_DEG(currheading) - heading) < hRange)) {
    return 0;
  }

  double maxForward = configuration_.GetAsDouble("GTP_MaxForward");
  double maxStrafe = configuration_.GetAsDouble("GTP_MaxStrafe");
  double maxTurn = configuration_.GetAsDouble("GTP_MaxTurn");

  // These next variables are common accross most MTP types
  double strafe = 0.0;
  double forward = 0.0;
  double turn = 0.0;
  
  double d = sqrt(((x-currx)*(x-currx)+(y-curry)*(y-curry)));
  double phi = 0.0;
  if ((y-curry)==0.0) phi = PI/2.0;
  else phi = atan2(currx-x,y-curry);
  
  double theta = currheading-phi;
 // double thetaOrig = theta;
  while (theta > PI) theta -= 2*PI;
  while (theta < -PI) theta += 2*PI;

  strafe = -d*sin(theta)*3;
  forward = d*cos(theta)*5;

  #ifdef ERS_7
  forward = d*cos(theta)*10;
  strafe = -d*sin(theta)*10;
  #endif

  double origForward = forward;
  double origStrafe = strafe;

  if (forward > maxForward) forward = maxForward;
  if (strafe > maxStrafe) strafe = maxStrafe;
  if (forward < -maxForward) forward = -maxForward;
  if (strafe < -maxStrafe) strafe = -maxStrafe;

/*
//this is circle limiting code by CM. not tested, but should be cool.
  double radius = sqrt(forward*forward + strafe*strafe);
  double maxRadius = 150;
  if (radius > maxRadius) {
    // doing too much. we have to limit, and we have to do it in a semi-intelligent way.
    double strafeComponent = strafe/radius;
    double forwardComponent = forward/radius;
    strafe = strafeComponent*maxRadius;
    forward = forwardComponent*maxRadius;
  }
*/

  turn = -1 * (RAD_TO_DEG(currheading) - heading);
  //if (ABS(RAD_TO_DEG(thetaOrig)) < 180) turn*=-1;

  if (turn > maxTurn) turn = maxTurn;