utilities.cc

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

  double incTilt1 = (currPoints.eTilt1 - currPoints.sTilt1) / currPoints.frames;
  double incTilt2 = (currPoints.eTilt2 - currPoints.sTilt2) / currPoints.frames;
  double incPan = (currPoints.ePan - currPoints.sPan) / currPoints.frames;
     // Get the tilt and pan values
  double tilt1 = currPoints.sTilt1 +  (incTilt1 * currFrame);
  double tilt2 = currPoints.sTilt2 +  (incTilt2 * currFrame);
  double pan = currPoints.sPan + (incPan * currFrame);
    // Move the head  
  Head_MoveTo(tilt1, pan, tilt2);

  currFrame++;

  if (currFrame >= currPoints.frames) {
    currFrame = 0;
    index++;
    if (index >= arraySize) {
      index = 0;
      return true;
    }  
  }
  return false;
}


// note the queue priority. this acts like a *KICK*, not a walk.
void Utilities::TurnWithBall(double turn, double stepFrequency) {
#ifdef ERS_7
  double origTurn = turn;
  if (ABS(turn) > 30) turn = turn / 1.5;
  if (origTurn >= 0) turn = MAX(turn,20);
  else origTurn = MIN(turn,-20);
  LocomotionCommand lc;
  lc.Set(LocomotionCommand::TP_SINGLEWALK,0.0,0.0,turn,0.0,stepFrequency);
  lc.odomTurn = origTurn;
  lc.frontHeight = 55.6;
  lc.frontStrideHeight = -3.2;
  lc.frontForwardOffset = 85.1;
  lc.frontSideOffset = -5.0;

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

  if (turn < 0) {
    lc.offsetWalk=true;
  }

  lcq_.Enqueue(1,lc);

 /* LocomotionCommand lc;
  lc.Set(LocomotionCommand::TP_SINGLEWALK,0.0,0.0,turn,0.0,1.6);
  lc.odomTurn = origTurn;
  lc.frontStrideHeight = -9.5;
  lc.frontForwardOffset = 90.0;
  lc.frontSideOffset = -6.36;
  lc.frontHeight = 55.2;

  lc.backStrideHeight = 42.2;
  lc.backForwardOffset = -49.3;
  lc.backSideOffset = -0.72;
  lc.backHeight = 102.7;

  lc.locusType = LocomotionCommand::L_TRAPEZOID;
  lcq_.Enqueue(1,lc);*/
  // FIXME OVERRIDING TRICK !! KEEP THAT HEAD UP !!
  
 //Head_MoveTo(-50,0,50);
#endif
#ifdef ERS_210
  LocomotionCommand lc;
  lc.Set(LocomotionCommand::TP_SINGLEWALKWITHOUTFRONT,0.0,0.0,turn,0.0,stepFrequency);
  lc.frontForwardOffset = 94.0;
  lc.frontSideOffset = -12.0;
  lc.backStrideHeight = 20.0;
  lcq_.Enqueue(1,lc);
#endif
}

// note the queue priority. this acts like a *KICK*, not a walk.
void Utilities::TurnWithBallHeadDown(double turn, double stepFrequency) {
#ifdef ERS_7
  TurnWithBall(turn,stepFrequency);
#endif
#ifdef ERS_210
  LocomotionCommand lc;
  lc.Set(LocomotionCommand::TP_SINGLEWALK,0.0,0.0,turn,0.0,stepFrequency);
  lc.frontSideOffset = 14.0;
  lc.frontForwardOffset = 80.0;
  RawCommand(lc);
#endif
}

// note the queue priority. this acts like a *KICK*, not a walk.
void Utilities::StrafeWithBall(double strafe, double stepFrequency) {
  LocomotionCommand lc;
  lc.Set(LocomotionCommand::TP_SINGLEWALK,0.0,0.0,0.0,strafe,stepFrequency);
  lc.frontForwardOffset = 70.0;
  lc.frontSideOffset = -7.0;
  RawCommand(lc);
//lcq_.Enqueue(1,lc);
}

bool Utilities::IsQueueEmpty() {
  return lcq_.IsEmpty();
}

void Utilities::ClearQueue() {
  lcq_.Clear();
}


// returns in degrees !
double Utilities::GetHeadingToGoal() {
  double currx = wo_self_->x_;
  double curry = wo_self_->y_;
  double currheading = wo_self_->heading_;

  // enemy goal coords
  double x_ = 0;
  double y_ = 220;
  if (curry>210) curry=210;

  //if (curry > 170 && currx < 30 && currx > 0) x_ = MIN(currx,20.0);
  //else if (curry > 170 && currx > -30 && currx < 0) x_ = MAX(currx,-20);

  double theta = (atan2(y_-curry, x_-currx) - (PI / 2.0)) - currheading;

  // heading to goal based on WM.
  double goalHeading = RAD_TO_DEG(theta);

  // we can see opposition goal ! Use vision instead of WM
  if (vo_oppositionGoal_ != NULL) {
    goalHeading = RAD_TO_DEG(vo_oppositionGoal_->heading_);
    if (goalHeadingLeft_ < DEG_TO_RAD(10) && goalHeadingRight_ > DEG_TO_RAD(-10)) {
      goalHeading = 0.0;
    } else {
      double temp = RAD_TO_DEG(goalHeadingLeft_)-10.0;
      if (ABS(temp) < ABS(goalHeading)) {
        goalHeading = temp;
      } 
      temp = RAD_TO_DEG(goalHeadingRight_)+10.0;
      if (ABS(temp) < ABS(goalHeading)) {
        goalHeading = temp;
      } 
    }
  }

  while (goalHeading > 180.0) goalHeading -= 360.0;
  while (goalHeading <= -180.0) goalHeading += 360.0;
  return goalHeading;
}

// allows posts !
double Utilities::GetHeadingToGoalRam() {
  double currx = wo_self_->x_;
  double curry = wo_self_->y_;
  double currheading = wo_self_->heading_;
  if (curry>210) curry=210;

  // enemy goal coords
  double x_ = 0;
  double y_ = 220;
  double theta = (atan2(y_-curry, x_-currx) - (PI / 2.0)) - currheading;
  // heading to goal based on WM.
  double goalHeading = RAD_TO_DEG(theta);

  // right post
  x_ = 20;
  y_ = 215;
  theta = (atan2(y_-curry, x_-currx) - (PI / 2.0)) - currheading;
  // heading to goal based on WM.
  double rightPostHeading = RAD_TO_DEG(theta);

  // left post
  x_ = -20;
  y_ = 215;
  theta = (atan2(y_-curry, x_-currx) - (PI / 2.0)) - currheading;
  // heading to goal based on WM.
  double leftPostHeading = RAD_TO_DEG(theta);
  

  double bestGoalHeading = leftPostHeading;
  if (ABS(rightPostHeading) < ABS(bestGoalHeading)) {
    bestGoalHeading = rightPostHeading;
  }
  if (ABS(goalHeading) < ABS(bestGoalHeading)) {
    bestGoalHeading = goalHeading;
  }
  goalHeading = bestGoalHeading;
  // we can see opposition goal ! Use vision instead of WM
  if (vo_oppositionGoal_ != NULL) {
    goalHeading = RAD_TO_DEG(vo_oppositionGoal_->heading_);
    if (goalHeadingLeft_ < DEG_TO_RAD(10) && goalHeadingRight_ > DEG_TO_RAD(-10)) {
      goalHeading = 0.0;
    } else {
      double temp = RAD_TO_DEG(goalHeadingLeft_)-10.0;
      if (ABS(temp) < ABS(goalHeading)) {
        goalHeading = temp;
      } 
      temp = RAD_TO_DEG(goalHeadingRight_)+10.0;
      if (ABS(temp) < ABS(goalHeading)) {
        goalHeading = temp;
      } 
    }
  }

  while (goalHeading > 180.0) goalHeading -= 360.0;
  while (goalHeading <= -180.0) goalHeading += 360.0;
  return goalHeading;
}


// returns in degrees !
double Utilities::GetHeadingToOwnGoal() {
  double currx = wo_self_->x_;
  double curry = wo_self_->y_;
  double currheading = wo_self_->heading_;
  // enemy goal coords
  double x_ = 0;
  double y_ = -225;
  if (curry>210) curry=210;
  if (curry<-210) curry=-210;
  double theta = (atan2(y_-curry, x_-currx) - (PI / 2.0)) - currheading;

  // heading to goal based on WM.
  double goalHeading = RAD_TO_DEG(theta);

  // we can see own goal ! Use vision instead of WM
  if (vo_ownGoal_ != NULL) {
    goalHeading = RAD_TO_DEG(vo_ownGoal_->heading_);
   // we can see opposition goal ! Use vision instead of WM.
  } else if (vo_oppositionGoal_ != NULL) {
    goalHeading = 180.0 - RAD_TO_DEG(vo_oppositionGoal_->heading_);
    //cout << "ownGoal->heading = " << RAD_TO_DEG(vo_ownGoal_->heading_) << ", goalHeading: " << goalHeading << endl << flush;
  }

  while (goalHeading > 180.0) goalHeading -= 360.0;
  while (goalHeading <= -180.0) goalHeading += 360.0;
  return goalHeading;
}

void Utilities::Head_FollowObjectHeadDown(int cx, int cy) {
#ifdef ERS_7

  HeadCommand hc;

  double currentTiltBig = MICRO_TO_RAD(sensorValues_[S_HEAD_TILT1]);
  double currentTiltSmall = MICRO_TO_RAD(sensorValues_[S_HEAD_TILT2]);
  double currentPan  = MICRO_TO_RAD(sensorValues_[S_HEAD_PAN]);

///
   double sinHeadPan = sin(currentPan);
   double cosHeadPan = cos(currentPan);
   double sinHeadTiltBig = sin(currentTiltBig);
   double cosHeadTiltBig = cos(currentTiltBig);
   double sinHeadTiltSmall = sin(currentTiltSmall);
   double cosHeadTiltSmall = cos(currentTiltSmall);

   double effectiveRoll = (asin(sinHeadPan*-1*sinHeadTiltBig)); 
///
  int xErrorOriginal = -(cx-(currentImageWidth_/2));
  int yErrorOriginal = -(cy-(currentImageHeight_/2));

  int xErrorRotated = (int) (cos(effectiveRoll)*xErrorOriginal - sin(effectiveRoll)*yErrorOriginal);
  int yErrorRotated = (int) (sin(effectiveRoll)*xErrorOriginal + cos(effectiveRoll)*yErrorOriginal);


  // Matrix row,column
  Matrix pixelErrors = Matrix(2,1);
  double radsPerPixelHoriz = FOVx/(double)currentImageWidth_;
  double radsPerPixelVert = FOVy/(double)currentImageHeight_;
  pixelErrors[0][0] = xErrorRotated*radsPerPixelHoriz;
  pixelErrors[1][0] = yErrorRotated*radsPerPixelVert;

  double smallTiltMotor = 0.0; 
  double panMotor = 0.0;
  double bigTiltMotor = 0.0; 

  double pan = MICRO_TO_DEG(currentPan);
  double desiredTiltBig = DEG_TO_RAD(-50-(ABS(pan)/2));
  double changeTiltBig = desiredTiltBig - currentTiltBig;

  Matrix Xbt = Matrix(2,1);
  Xbt[0][0] = sinHeadPan;
  Xbt[1][0] = cosHeadPan;
  Matrix Xpt = Matrix(2,2);
  Xpt[0][0] = 0;
  Xpt[1][0] = 1;
  Xpt[0][1] = 1;
  Xpt[1][1] = 0;
  Matrix XptInv = Invert22(Xpt);
  Matrix W2 = XptInv * (pixelErrors - (Xbt * changeTiltBig));

  panMotor = RAD_TO_DEG(currentPan+W2[1][0]);
  smallTiltMotor = MIN(-47,RAD_TO_DEG(currentTiltSmall+W2[0][0]));
  bigTiltMotor = RAD_TO_DEG(currentTiltBig+changeTiltBig);

//  double desiredTilt = RAD_TO_DEG(currentTilt) + 0.8*RAD_TO_DEG(atan((currentImageHeight_/2.0-newCy)/(currentImageHeight_/(2.0*tan(FOVy/2.0)))));
//  double desiredPan  = RAD_TO_DEG(currentPan)  + 0.8*RAD_TO_DEG(atan((currentImageWidth_ /2.0-newCx)/(currentImageWidth_ /(2.0*tan(FOVx/2.0)))));
  
/*  double bigTiltMotor = desiredTilt-20.0;
  double smallTiltMotor = 20.0;
  if (bigTiltMotor < -37) {  //was 37, then 33
    smallTiltMotor = desiredTilt+37.0;
    bigTiltMotor = -37.0;
  } else if (bigTiltMotor > -10) {
    smallTiltMotor = desiredTilt+10;
    bigTiltMotor = -10;
  }
*/
  hc.Set(bigTiltMotor,panMotor,smallTiltMotor,false); // FIXME interp is ON
  lcq_.SetHeadCommand(hc);
/*
  HeadCommand hc;
  double hTiltSmall = MICRO_TO_RAD(sensorValues_[S_HEAD_TILT_SMALL]);
  double hTiltBig = MICRO_TO_RAD(sensorValues_[S_HEAD_TILT_BIG]);
  double hPan  = MICRO_TO_RAD(sensorValues_[S_HEAD_PAN]);

  double x1,y1,z1;

  x1 = vo_ball_->distance_ * (currentImageWidth_/2.0-cx)/(currentImageWidth_/(2.0*tan(FOVx/2.0)));
  y1 = vo_ball_->distance_ * (currentImageHeight_/2.0-cy)/(currentImageHeight_/(2.0*tan(FOVy/2.0)));
  z1 = vo_ball_->distance_;


  double x4 = x1;
  double y4 = y1;
  double z4 = z1 + HEADLENGTH;
  double x45 = x4;
  double y45 = y4 * cos(hTiltSmall) - z4 * sin(hTiltSmall);
  double z45 = y4 * sin(hTiltSmall) + z4 * cos(hTiltSmall);
    // robot head pan - fixed in the y direction
  double x5 = x45 * cos(hPan) + z45 * sin(hPan);
  double y5 = y45;
  double z5 = -x45 * sin(hPan) + z45 * cos(hPan);
    // robot neck length and distance from ground to neck
    double x6 = x5;
    double y6 = y5 + NECKLENGTH;
    double z6 = z5;

    // rotation for head tilt - fixed in the x direction
    double x7 = x6;
    double y7 = y6 * cos(hTiltBig) - z6 * sin(hTiltBig);
    double z7 = y6 * sin(hTiltBig) + z6 * cos(hTiltBig);


    if (z7 < NECKLENGTH) {
        z7 = NECKLENGTH;
    }

    double desiredTilt = RAD_TO_DEG(atan(y7/z7)-asin(NECKLENGTH/sqrt(z7*z7 + y7*y7)));

    double x8 = x7;
    
    double z8 = z7*cos(DEG_TO_RAD(-desiredTilt))-y7*sin(DEG_TO_RAD(-desiredTilt));
    
    double desiredPan = RAD_TO_DEG(atan(x8/z8));

  double bigTiltMotor = desiredTilt-15.0;
  double smallTiltMotor = 15.0;
  if (bigTiltMotor < -40) {
    smallTiltMotor = desiredTilt+40.0;
    bigTiltMotor = -40.0;
  }

  hc.Set(bigTiltMotor,desiredPan,smallTiltMotor,false);
  lcq_.SetHeadCommand(hc);
*/

#endif
#ifdef ERS_210

  HeadCommand hc;
  double currentTilt = MICRO_TO_DEG(sensorValues_[S_HEAD_TILT]);
  double currentPan  = MICRO_TO_DEG(sensorValues_[S_HEAD_PAN]);

  double desiredTilt = currentTilt + RAD_TO_DEG(atan((currentImageHeight_/2.0-cy)/(currentImageHeight_/(2.0*tan(FOVy/2.0)))));
  double desiredPan  = currentPan  + RAD_TO_DEG(atan((currentImageWidth_ /2.0-cx)/(currentImageWidth_ /(2.0*tan(FOVx/2.0)))));
/*

  HeadCommand hc;
  double hTilt = MICRO_TO_RAD(sensorValues_[S_HEAD_TILT]);
  double hPan  = MICRO_TO_RAD(sensorValues_[S_HEAD_PAN]);


    double x1,y1,z1,x2,y2,z2,x3,y3,z3,x4,y4,z4,x5,z5;

    x1 = vo_ball_->distance_ * (currentImageWidth_/2.0-cx)/(currentImageWidth_/(2.0*tan(FOVx/2.0)));
    y1 = vo_ball_->distance_ * (currentImageHeight_/2.0-cy)/(currentImageHeight_/(2.0*tan(FOVy/2.0)));
    z1 = vo_ball_->distance_;

    x2 = x1;
    y2 = y1 + NECKLENGTH;
    z2 = z1 + HEADLENGTH;

    x3 = x2*cos((-hPan))-z2*sin((-hPan));
    y3 = y2;
    z3 = x2*sin((-hPan))+z2*cos((-hPan));

    x4 = x3;
    y4 = z3*sin((hTilt))+y3*cos((hTilt));
    z4 = z3*cos((hTilt))-y3*sin((hTilt));

    if (z4 < NECKLENGTH) {
        z4 = NECKLENGTH;
    }

    double desiredTilt = RAD_TO_DEG(atan(y4/z4)-asin(NECKLENGTH/sqrt(z4*z4 + y4*y4)));

    x5 = x4;
    
    z5 = z4*cos(DEG_TO_RAD(-desiredTilt))-y4*sin(DEG_TO_RAD(-desiredTilt));
    
    double desiredPan = RAD_TO_DEG(atan(x5/z5));

*/
  hc.Set(desiredTilt,desiredPan,0.0,false);
  lcq_.SetHeadCommand(hc);
#endif
}

double Utilities::GetClosestRobotAngle(double maxDistance) {
  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) {
      // opponent robot is too far away to care about.
      if (visionObjects_[i].distance_ > maxDistance) {
        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 -1000.0;
	
  // ok now, we only really care about the robot closest to us. after that, sif care ?
  if (visionObjects_[furthestLeftRobot].distance_ < visionObjects_[furthestRightRobot].distance_) {
    return RAD_TO_DEG(visionObjects_[furthestLeftRobot].heading_);
  }	else {
    return RAD_TO_DEG(visionObjects_[furthestRightRobot].heading_);
  }
}

bool Utilities::SpinNotSlap(int type, double oppGoalHeading) {
  if (type == 2 || ABS(oppGoalHeading) > 120 || wo_self_->y_ > 200 || (type==1 && ABS(wo_self_->x_) > 125) || (ABS(wo_self_->x_)<30 && wo_self_->y_ > 170)) return true;
  return false;
}