kechze.java

一个多机器人的仿真平台

package Domains.SoccerBots.teams;

/*
 * Kechze.java
 *
 * KEnt, CHris, and ZEllyn's JavaSoccer Team
 * kent@cc, jurney@cc, zellyn@cc
 * cs4324 - Spring 1998
 * Chris Atkeson's Class
 *
 * Assignment One
 *
 *
 *
 * Based on BrianTeam.java by Brian McNamara, although the code itself was
 *  not cut and pasted - it was written from scratch.
 *
 */

import EDU.gatech.cc.is.util.Vec2;
import EDU.gatech.cc.is.abstractrobot.*;
import java.io.*;

public class Kechze extends ControlSystemSS
{
  // The current time
  long curr_time;

  // Position of ball, vector to ball, vectors to goals, position of robot
  Vec2 ball, EgoBall, ourgoal, theirgoal, me;

  // Arrays of team-members
  Vec2 Teammates[], Opponents[];

  // Useful constants
  static final double PI = Math.PI, PI2 = 2*Math.PI;
  static final double GOAL_WIDTH = .4, BALL_RADIUS = .02;
  // goal a little smaller to be safe
  static final double ROBOT_RADIUS = .06;

  // By default, robots move at full speed
  static final double DEFAULT_SPEED = 1.0;

  // How close robots must get to a target spot before stopping
  static final double CLOSE_TO_SPOT = 0.01;

  // How close ball must be to goal for goalie to act "aggressively"
  static final double CLOSE_TO_GOAL = 0.4;

  // How many milliseconds ahead the goalie predicts the ball's position
  static final long GOALIE_LOOK_AHEAD = (long)200.0;

  // How many milliseconds ahead players predict the ball's position when
  //  simply running in the direction of the ball (bottom-out default
  //  behavior)
  static final long DEFAULT_LOOK_AHEAD = (long)600.0;

 // How far back from the center robots wait for the ball to re-spot
  static final double CENTER_OFFSET = 0.1;

  // How far the ball must be from the center for respotting-wait to happen
  static final double CENTER_RADIUS = 1.3;

  long last_time; // what the time was last step
  Vec2 last_ball; // where the ball was last step

  long steps;
  double ratio;

 /*
 Configure the control system.
 */
  public void Configure()
  {
    last_time = (long)0.0; //zjh
    last_ball = new Vec2(0.0,0.0); //zjh

    steps = 0;
    ratio = 0.0;
  }

  /**
 Called every timestep to allow the control system to run.
 */
  public int TakeStep()
  {
    // the eventual movement command is placed here
    Vec2 result = new Vec2(0,DEFAULT_SPEED);

    // get the current time for timestamps
    curr_time = abstract_robot.getTime();

    me = abstract_robot.getPosition(curr_time);

    boolean KickBall = false;

    /*--- Get some sensor data ---*/
    // get vector to the ball
    EgoBall = abstract_robot.getBall(curr_time);
    ball = new Vec2(EgoBall.x, EgoBall.y);
    ball.add(me);

    // get vector to our and their goal
    ourgoal = abstract_robot.getOurGoal(curr_time);
    theirgoal = abstract_robot.getOpponentsGoal(curr_time);

    // get a list of the positions of our teammates
    Teammates = abstract_robot.getTeammates(curr_time);
    Opponents = abstract_robot.getOpponents(curr_time);

    if(HaveBall(me))
      {
        abstract_robot.setDisplayString("Have Ball");
        if (ClearShot(me) && theirgoal.r < .35)
          {
            //System.out.print("*ClearShot  ");
            abstract_robot.setDisplayString("Shooting");
            KickBall = true;
            //    result.sett(TopGoalPost().t); //theirgoal;
            result.sett(EgoBall.t); //theirgoal;
          }
        else if(ClosestToMyGoal(me))
          {
            //System.out.print("*Closest to goal ");
            abstract_robot.setDisplayString("Clearing");
            KickBall = true;
            result.sett(EgoBall.t);
          }
        else
          {
            //System.out.print(abstract_robot.getPlayerNumber(curr_time));
            //System.out.print("*Closest to ball  ");
            result.sett(GoBehindBall());
            //System.out.println(result.t + "  ");
            //System.out.println("Else* " + result.t);
          }
      }
    else
      {
        //System.out.print("*Dont have ball  ");
        if (ClosestToBall(me))
          {
            //System.out.print(abstract_robot.getPlayerNumber(curr_time));
            //System.out.print("*Closest to ball  ");
            abstract_robot.setDisplayString("Going to Ball");
            result.sett(GoBehindBall());
            //System.out.println(result.t + "  ");
          }
        //-dedicated goalie now  else if(ClosestToMyGoal(me)) {result =
	//                                                     goalieFunc();}
        else

          if (ClosestToCenter(me) && (ball.r>CENTER_RADIUS))
            {
              Vec2 ourwait;
              abstract_robot.setDisplayString("Center");
              if (ourgoal.x > 0)
                {
                  ourwait = new Vec2(CENTER_OFFSET,0);
                }
              else
                {
                  ourwait = new Vec2(-CENTER_OFFSET,0);
                }

              ourwait.sub(me);
              result = easeToRelativeSpot(ourwait, (EastTeam(me)?PI:0.0));

            }

          else {
            //System.out.print("*else  ");
            //PUT code here somewhere
            result = ClosestOpponent();
            if( result.r < ROBOT_RADIUS * 2)
              {
                abstract_robot.setDisplayString("Avoiding Opp.");
                result.sett(NormalizePI(result.t + PI));
              }
            else
              {
                result = ClosestTeammate();
                if( result.r < ROBOT_RADIUS * 4)
                  {
                    abstract_robot.setDisplayString("Avoiding Teammate");
                    result.sett(NormalizePI(result.t + PI));
                  }
                else
                  {
                    abstract_robot.setDisplayString("Find Ball");
                    result = predictEgoBall(curr_time+DEFAULT_LOOK_AHEAD);
//zjh
                  }
              }
            result.setr(DEFAULT_SPEED);
          }
      }
    //System.out.println("");System.out.println("");
    if (abstract_robot.getPlayerNumber(curr_time)==0)
      {
        abstract_robot.setDisplayString("Goalie");
        result = goalieFunc();
        //    System.out.println("Goalie: " + result);
      }

    steps++;
    if (steps>100) steps=100;
    ratio = ratio * (1.0-1.0/steps) + 1.0/steps * (haveGoalie()?1.0:0.0);
    //  System.out.println(ratio);
    if ((abstract_robot.getPlayerNumber(curr_time)==4) && (ratio>0.7))
      {
        abstract_robot.setDisplayString("Block Goalie");
        result = blockFunc();
      }

    /*--- Send commands to actuators ---*/
    // set the heading
    abstract_robot.setSteerHeading(curr_time, result.t);
    // set speed at maximum
    abstract_robot.setSpeed(curr_time, result.r);

  // kick it if we can & desired
    if (abstract_robot.canKick(curr_time) && KickBall)
      abstract_robot.kick(curr_time);

    last_time = curr_time;  //zjh
    last_ball = new Vec2(ball.x, ball.y); //zjh

    // tell the parent we're OK
    return(CSSTAT_OK);
  }

  /* GoBehindBall
   *  Get behind the ball, and try to turn it towards their goal
   *  Described more fully in the html documentation
   */
  double GoBehindBall()
       // which theta to goto to go behind the ball (me->ball->goal)
  {
    Vec2 result;
    result=EgoBall;
    //  if(CloseToBall(me) && !HaveBall(me))
    {
      Vec2 kickspot = new Vec2(EgoBall.x, EgoBall.y);
      kickspot.sub(theirgoal);
      kickspot.sety(kickspot.y+ROBOT_RADIUS*2);
      //   kickspot.sub(BottomGoalPost());
      Vec2 B = new Vec2(kickspot.x, kickspot.y);
      Vec2 C = new Vec2(kickspot.x, kickspot.y);
      Vec2 D = new Vec2(kickspot.x, kickspot.y);
      kickspot.setr(ROBOT_RADIUS);// + BALL_RADIUS);
      B.setr(ROBOT_RADIUS * 2);
      C.setr(ROBOT_RADIUS * 2);
      D.setr(ROBOT_RADIUS * 1.5);

      B.sett(B.t - PI / 2); C.sett(C.t + PI / 2);
      kickspot.add(EgoBall);
      B.add(EgoBall); C.add(EgoBall);
      D.add(EgoBall);
      // figure out wich quadrant w/ respect to ball

      int quad = Quadrant(EgoBall.t - theirgoal.t);
      if ((quad == 1) || (quad == 4))
        {
          if (Math.abs(EgoBall.t - theirgoal.t) < PI/6)
            {
              result = kickspot;
              result.setr(1.0);
            }
          else
            {
              result = D;
              //     result.setr(1.0);
            }
        }
      else if (quad == 3)
        {result = B; result.setr(1.0);}
      else // quad = 2
        {result = C; result.setr(1.0);}
    }
    return result.t;
  }

  /* Vec2 toward
   *  Given two vectors, return a vector pointing from the first to the
	second
   */
  static Vec2 toward(Vec2 a, Vec2 b)
  {
    Vec2 temp = new Vec2(b.x, b.y);
    temp.sub(a);
    return temp;
  }

  /* ClosestOpponent
   *  Return the position of my closest opponent
   */
  Vec2 ClosestOpponent()
  {
    Vec2 closest = new Vec2( 99999, 0);
    for (int i=0; i< Opponents.length; i++)
      {
        if (Opponents[i].r < closest.r)
          closest = Opponents[i];
      }
    return closest;
  }

  /* ClosestTeammate
   *  Return the position of my closest teammate
   */
  Vec2 ClosestTeammate()
  {
    Vec2 closest = new Vec2( 99999, 0);
    for (int i=0; i< Teammates.length; i++)
      {
        if (Teammates[i].r < closest.r)
          closest = Teammates[i];
      }
    return closest;
  }

  /* haveGoalie
   *  Return true if there is an opponent player in the area where a goalie
   *  would probably be -- ie., guess if they have a dedicated goalie
   */
  boolean haveGoalie()
  {
    double x,y;
    Vec2 dist;
    for (int i=0; i< Opponents.length; i++)
      {
        x = Opponents[i].x; y = Opponents[i].y;
        if ((Math.abs(x-theirgoal.x) < 3 * ROBOT_RADIUS))

 // we commented this out, because the y-value of Opponent position seems
 // to be messed up - perhaps this is a bug in the JavaBots system?

          //       (y < TopGoalPost().y+2*ROBOT_RADIUS) &&
          //       (y > BottomGoalPost().y-2*ROBOT_RADIUS))
          return true;
      }
    return false;
  }

  /* CloseToBall
   *  Return true if the given robot is close to the ball
   */
  boolean CloseToBall(Vec2 robot)
  {
    Vec2 temp = new Vec2(robot.x, robot.y);
    temp.sub(ball);
    return !(temp.r > ROBOT_RADIUS + BALL_RADIUS + .02);
    // Robot radius + how close
  }