movementtricks.cc

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

  if (turn < -maxTurn) turn = -maxTurn;
  // end of common stuff

  // Adjusts heading all the way to target
  if (type == MTP_DYNAMIC || type == MTP_DYNAMIC_FACE_BALL || type == MTP_DYNAMIC_NO_HEADING) {

    if (type == MTP_DYNAMIC_FACE_BALL) {
      double fieldAngleToBall =(atan2(curry-wo_ball_->y_, currx-wo_ball_->x_) - (PI / 2.0));

      // Ok ... so if we are the keeper and we are basically straight at a corner beacon then 
      // we might aswell strafe a little to the wall, this will lower the chance of a ball going
      // past it
      if (isKeeper) {
          if (vo_backLeftBeacon_ != NULL && ABS(vo_backLeftBeacon_->heading_) < 40.0) strafe = 100;
          if (vo_backRightBeacon_ != NULL && ABS(vo_backRightBeacon_->heading_) < 40.0) strafe = -100;
      }

      double theta = currheading-fieldAngleToBall+PI;
//    double thetaOrig = theta;
      while (theta > PI) theta -= 2*PI;
      while (theta < -PI) theta += 2*PI;
      heading = -RAD_TO_DEG(theta);
      //cout << "Heading to ball: " << heading << endl << flush;
      turn = heading;
      //if (ABS(RAD_TO_DEG(thetaOrig)) < 180) turn*=-1;
      if (forward > -0.01) {
        if (turn > maxTurn*1.5) turn = maxTurn*1.5;
        if (turn < -maxTurn*1.5) turn = -maxTurn*1.5;
      } else {
        if (turn > maxTurn) turn = maxTurn;
        if (turn < -maxTurn) turn = -maxTurn;
      }
    } else if (type == MTP_DYNAMIC_NO_HEADING) {
      turn = 0.0;
    }
    // Slow down the step because of the strafe component
    #ifdef ERS_7
      double speed = 1.7;

      //if (ABS(strafe) > 20) speed = 1.6;

      if (ABS(turn) > 20 && (ABS(forward) > 40 || ABS(strafe) > 20)) {
        forward = forward*0.4;
        strafe = strafe*0.3;
      }
      if (forward < 0) {
        double speed = 1.6;
        LocomotionCommand lc;
        strafe = CROP(strafe,-80,80);
        forward = CROP(forward,-120,120);
        lc.Set(LocomotionCommand::TP_WALK,forward,forward,turn,strafe,speed);
        lc.locusType =  LocomotionCommand::L_TRAPEZOID;
        lc.frontHeight = 75.66;
        lc.frontStrideHeight = 10.1;
        lc.frontForwardOffset = 52.0;
        lc.frontSideOffset = -1.0;

        lc.backHeight = 70.966;
        lc.backStrideHeight = 20.337;
        lc.backForwardOffset = -55.07;
        lc.backSideOffset = 1.0;
        //if (lcq_.IsTheStrutReady())
        utils.RawCommand(lc);
        //utils.Step(LocomotionCommand::TP_WALK,forward,forward,turn,strafe,speed);
      } else {
        //if (lcq_.IsTheStrutReady())
        utils.Step(LocomotionCommand::TP_WALK,forward,forward,turn,strafe,speed);
      }
    #endif

    #ifdef ERS_210
    // Slow down the step because of the strafe component
    double speed = 1.45;
    if (ABS(strafe) > 20) speed = 1.25;

    if (ABS(turn) > 20 && (ABS(forward) > 40 || ABS(strafe) > 20)) {
      forward = forward*0.4;
      strafe = strafe*0.3;
    }
    if (forward < 0) {
      speed = 1.1;
    }
    LocomotionCommand lc;
    lc.Set(LocomotionCommand::TP_WALK,forward,forward,turn,strafe,speed);
    lc.frontHeight = 70.0;
    lc.frontStrideHeight = 5.0;
    lc.frontForwardOffset = 60.0;
    lc.frontSideOffset = 2.0;

    lc.backHeight = 100.0;
    lc.backStrideHeight = 20.0;
    lc.backForwardOffset = -47.0;
    lc.backSideOffset = 2.0;
    utils.RawCommand(lc);
    #endif

//////////////////////////////////////////////////////
  // WARNING: May not work.
  } else if (type == MTP_FORWARDS) { // Turns and faces x,y and runs at it then turns to heading
    double d = sqrt(((x-currx)*(x-currx)+(y-curry)*(y-curry)));
    double theta = (atan2(y-curry, x-currx) - (PI / 2.0)) - currheading;
//  cout << "Theta " << RAD_TO_DEG(theta) << endl << flush;
//  double thetaOrig = theta;
    while (theta > PI) theta -= 2*PI;
    while (theta < -PI) theta += 2*PI;
//  cout << "AT: " << currx << ", " << curry << ". Phi: " << RAD_TO_DEG(phi) << ", theta " << RAD_TO_DEG(theta) << endl << flush;
    double turn = RAD_TO_DEG(theta);
    double frontForward = 0;
    double backForward = 0;
    double newStrafe = 0;
  //  if (ABS(DEG_TO_RAD(thetaOrig)) < 180) turn*=-1;
    if (turn > maxTurn) turn = maxTurn;
    if (turn < -maxTurn) turn = -maxTurn; 
    double absturn = ABS(turn);
    if (absturn < 5) {
      frontForward = 0.9 * forward;
      backForward = forward;
      strafe = strafe;
    } else if (absturn < 8) {
      frontForward = 0.72 * forward;
      backForward = 0.8 * forward;
      strafe = 0.8 * strafe;
    } else if (absturn < 15) {
      frontForward = 0.6 * forward;
      backForward = 0.7 * forward;
      strafe = 0.7 * forward;
    }
    double speed = 1.8;
    if (d < 20) {  // this should be adjusted
      speed = 1.6;
    }
    utils.Step(LocomotionCommand::TP_WALK,frontForward,backForward,turn,newStrafe,speed);
  // WARNING: May not work.
  } else if (type == MTP_BACKWARDS) {
    // Code so it turns and faces away from x,y and backs towards it then turns to heading
  // WARNING: May not work.
  } else if (type == MTP_MANHATTAN) {
    if (lcq_.IsTheStrutReady() == false) return 1;
    if (ABS(origStrafe) >= ABS(origForward)) {
      forward =0;
    } else {
      strafe = 0;
    }
    double speed = 1.6;
    if (ABS(strafe > 20)) speed = 1.4;
    if (ABS(currx-x) > 15 || ABS(curry-y) > 15) {turn = 0;}
    if (forward > 0) {
      utils.Step(LocomotionCommand::TP_SINGLEWALK,forward*0.77,forward*1.15,turn,strafe,speed);
    } else {
      utils.Step(LocomotionCommand::TP_SINGLEWALK,forward,forward,turn,strafe,speed);
    }
  } 
  return 1;
}
bool MovementTricks::MoveToPoint::IsUsingHead() {
  return false; 
} 
char* MovementTricks::MoveToPoint::GetErrorMsg(int msg) {
  if (msg == 0) return "AT_POINT";
  else if (msg == 1) return "NOT_AT_POINT";
  return "MTP:NO_MESSAGE";
}
char* MovementTricks::MoveToPoint::GetName() {
  return "MoveToPoint";
}

// END MoveToPoint
////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////
// BEGIN SearchForBall

MovementTricks::SearchForBall::SearchForBall(bool _turnRight, double _degreesToTurn) {
  frameDelay = 0;
  frameDelayStart = 0;

  turnRight = _turnRight;
  degreesToTurn = _degreesToTurn;
  degreesTurned = 0.0;
  startFrame = frame_;
//headTrick = new HeadTricks::HeadPanForBall();
  headTrick = new HeadTricks::AngledSearch(!turnRight);
  headTrick->Start();
}

MovementTricks::SearchForBall::SearchForBall(double _degreesToTurn) {
  frameDelay = 0;
  frameDelayStart = 0;

  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;
  }
  /*double x = wo_ball_->x_;
  double y = wo_ball_->y_;
  double theta = (atan2(y-wo_self_->y_, x-wo_self_->x_) - (PI / 2.0)) - wo_self_->heading_;
  double ballHeading = RAD_TO_DEG(theta);

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

  turnRight = true;
  if (ballHeading > 0) {
    turnRight = false;
  } else {
    turnRight = true;
  }*/

  degreesToTurn = _degreesToTurn;
  degreesTurned = 0.0;
  startFrame = frame_;
  headTrick = new HeadTricks::AngledSearch(!turnRight);
  headTrick->Start();
}

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

int MovementTricks::SearchForBall::Continue() {
  if (frameDelay > 0) {
    if (frame_ - frameDelayStart > frameDelay) {
      if (vo_ball_ != NULL) {
        return 0;
      } else {
        return -1;
      }
    } else if (vo_ball_ != NULL) {
      utils.Head_FollowObject(vo_ball_->centreX_, vo_ball_->centreY_);
    }
    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) {
      int hC = headTrick->Continue();
      //char* hMsg = headTrick->GetErrorMsg(hC);
      // head trick completed - ie, we can see the ball. quit out.
      if (hC < 1) {
        lcq_.Clear();
        lcq_.SetEmergencyCancel(true);
        utils.Head_FollowObject(vo_ball_->centreX_, vo_ball_->centreY_);
        utils.Step(LocomotionCommand::TP_WALK,0.0,0.0,0.5*RAD_TO_DEG(vo_ball_->heading_),0.0,1.8);

        frameDelay = (int)(30.0/1.8)-2;
        frameDelayStart = frame_;
        return 2;

/*
//      lcq_.SetEmergencyCancel(true);
        strutSystem_.isWalking = false;
        strutSystem_.isKicking = false;
        strutSystem_.isInterpolating = false;

        lcq_.Clear();
        utils.Step(LocomotionCommand::TP_SINGLEWALK,0.0,0.0,RAD_TO_DEG(vo_ball_->heading_)/2.0,0.0,1.7);
// correct CX 
int rotatedCx = vo_ball_->centreX_;//utils.FindRotatedCx(vo_ball_->centreX_, vo_ball_->centreY_);
int rotatedCy = vo_ball_->centreY_;//utils.FindRotatedCy(vo_ball_->centreX_, vo_ball_->centreY_);

double radsPerPixelHoriz = FOVx/(double)currentImageWidth_;
double pixelPerRadsHoriz = (double)currentImageWidth_/FOVx;
rotatedCx-= (int)(pixelPerRadsHoriz*vo_ball_->heading_);

        utils.Head_FollowObject(rotatedCx, rotatedCy);
//      utils.Head_FollowObjectRotated(104, rotatedCy);

        frameDelay = 90;
        frameDelayStart = frame_;
        return 2;*/
      }
      // continue search..
      return 1;
    } else {
      // strut is NOT busy, and we've turned the full way.
      return -1;
    }
  }

  int hC = headTrick->Continue();

  // head trick completed - ie, we can see the ball. quit out.
  if (hC < 1) {
        lcq_.Clear();
        lcq_.SetEmergencyCancel(true);
        utils.Head_FollowObject(vo_ball_->centreX_, vo_ball_->centreY_);
        utils.Step(LocomotionCommand::TP_WALK,0.0,0.0,0.5*RAD_TO_DEG(vo_ball_->heading_),0.0,1.8);

        frameDelay = (int)(30.0/1.8)-2;
        frameDelayStart = frame_;
        return 2;
  }

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

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

  #ifdef ERS_210
  double turnAmount = 60.0;
  #endif
  #ifdef ERS_7
    double turnAmount = 62; // 70 for fast
  #endif


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

  if (turnRight) {
    turnAmount = -turnAmount;
  }
  LocomotionCommand lc;
  lc.Set(LocomotionCommand::TP_SINGLEWALK,0.0,0.0,turnAmount,0.0,1.7); // 2.1 for fast. needs odomturn
  lc.locusType = LocomotionCommand::L_ARBITRARY;
  utils.RawCommand(lc);
  return 1;
}

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