femmebotsheterog.java

一个多机器人的仿真平台

package Domains.SoccerBots.teams;

/*
 * FemmeBotsHeteroG.java
 */

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

/*
 * Author: 	M. Bernardine Dias
 * 		Robotics Institute
 * 		Carnegie Mellon University
 * Date:	10/05/1999
 *
 *
 
 */


public class FemmeBotsHeteroG  extends ControlSystemSS	{

		private long 	curr_time; 			//What time is it?
		private Vec2	me;				//Where am I in my frame?
		private Vec2	ME;				//Where am I in the global frame?
		private Vec2 	ball;				//Where is the ball?
		private Vec2	their_goal;			//Where is their goal?
		private Vec2	our_goal;			//Where is our goal?
		private Vec2[]	teammates;			//Where are my teammates?
		private Vec2[]	opponents;			//Where are my opponents?
		private Vec2	ball_to_goal;		//How far away is the ball from their goal?
		private Vec2	ball_to_home;		//How far away is the ball from our goal?
		private Vec2	teammate_near_ball;	//Who is the teammate closest 
									//   to the ball?
		private Vec2	ball_to_team;		//A vector from the ball to the teammate closest to the ball;
		private Vec2	opponent_near_ball;	//Who is the opponent closest
									//   to the ball?
		private Vec2	ball_to_opp;		//A vector from the ball to the opponent closest to the ball;
		private Vec2	closest_to_ball;		//Who is closest to the ball?
		private Vec2	just_do_it;			//What should I do?
		private boolean	kick_it;			//Should I kick the ball?
		private Vec2	nearest_teammate;		//Who is the teammate nearest to me?
		private Vec2 	nearest_opponent;		//Who is the opponent nearest to me?			
		private double	set_speed;			//What speed should I move with?
		private int		change;			//An int that helps interchange the angle at which the
									//ball is kicked to the goal
		private long	myID;				//Who am I?
		private Vec2	center;			//Where is the center of the field?
		
	/**
	Configure the Avoid control system.  This method is
	called once at initialization time.  You can use it
	to do whatever you like.
	*/
	public void Configure()
		{
			curr_time = abstract_robot.getTime();
			just_do_it = new Vec2(0,0);
		}
		
	
	/**
	Called every timestep to allow the control system to
	run.
	*/
	public int TakeStep()
		{
		/* --- First we must read our sensor outputs --- */
		//We want to assess where everything is		
		assess_situation();

		/* --- Now we can make some decisions about what to do... --- */
		//First I want to figure out who I am?
		//Based on that I can decide what to do...
		if(myID == 3)
			playCenter();
		if(myID == 2)
			playAttack();
		if(myID == 4)
			playAttack();
		if(myID == 0)
			playGoalKeeper();
		if(myID == 1)
			playDefender();
		//If I am aimed at my goal don't kick the ball		
		if ((kick_it== false) && abstract_robot.canKick(curr_time))
			just_do_it.sett(ball.t + Math.PI);

		/* --- Finally we can send commands to our actuators --- */
		//Set the heading
		abstract_robot.setSteerHeading(curr_time, just_do_it.t);

		//Set the speed
		abstract_robot.setSpeed(curr_time,set_speed);

		//Kick it if we can and should
		if (kick_it && abstract_robot.canKick(curr_time))
			abstract_robot.kick(curr_time);

		//Tell the parent we're OK
		return(CSSTAT_OK);
		}
	
	private void assess_situation()
	{

		//Get current time
		curr_time = abstract_robot.getTime();

		//This is just for conveniance
		me = new Vec2(0,0);
		
		//Who am I?
		myID = abstract_robot.getPlayerNumber(curr_time);
			
		//Initialize vector to where I am in global frame
		ME = abstract_robot.getPosition(curr_time);
		
		//Initialize center of the field
		center = new Vec2(-ME.x,-ME.y);
	
		//Initialize vector to the ball
		ball = abstract_robot.getBall(curr_time);

		//Initialize vector to opponents' goal
		their_goal = abstract_robot.getOpponentsGoal(curr_time);
		
		//Initialize vector to our goal
		our_goal = abstract_robot.getOurGoal(curr_time);

		//Get vectors to all my teammates
		teammates = abstract_robot.getTeammates(curr_time);
			
		//Get vectors to my opponents
		opponents = abstract_robot.getOpponents(curr_time);

		//Get information about who is closest to the ball
		teammate_near_ball = closest_to(ball, teammates);
		opponent_near_ball = closest_to(ball, opponents);
		
		//Initialize to kick the ball straight to the goal
		change = 0;

		//Set my speed to a default of max speed
		set_speed = 1.0;

		//Figure out vectors from the players (excluding self) closest to the ball on each team
		ball_to_team = new Vec2(teammate_near_ball.x, teammate_near_ball.y);
		ball_to_team.sub(ball);
		ball_to_opp = new Vec2(opponent_near_ball.x, opponent_near_ball.y);
		ball_to_opp.sub(ball);

		//Now easy to figure out who is closest to ball
		Vec2 temp = new Vec2(0,0);
		if(ball_to_team.r > ball_to_opp.r)
		{
			closest_to_ball = opponent_near_ball; 
			temp.setr(ball_to_opp.r);
			temp.sett(ball_to_opp.t);
		}
		else
		{
			closest_to_ball = teammate_near_ball; 
			temp.setr(ball_to_team.r);
			temp.sett(ball_to_team.t);
		}
		if(temp.r >= ball.r)
		{
			closest_to_ball = me; 
		}

		//Figure out the distance from the ball to their goal
		ball_to_goal = new Vec2(their_goal.x, their_goal.y);
		ball_to_goal.sub(ball);

		//Figure out the distance from the ball to our goal
		ball_to_home = new Vec2(our_goal.x, our_goal.y);
		ball_to_home.sub(ball);

		//Get information about who is closest to me
		nearest_teammate = closest_to(me, teammates);
		nearest_opponent = closest_to(me, opponents);

		//Initialize boolean that says if we should kick or not
		if(ball.x*their_goal.x >=0)
			kick_it = true;
		else
			kick_it = false;
	}

	private void playDefender()
	{
		if(ball_to_goal.r<(2*ball_to_home.r))
		{
			Vec2 def_pos = new Vec2(0,0);
			def_pos.setx(our_goal.x + abstract_robot.RADIUS * 3);
			def_pos.sety(our_goal.y + abstract_robot.RADIUS * 2);
			if (def_pos.r <abstract_robot.RADIUS* 2)
				set_speed = 0;
			else
			{
				just_do_it = def_pos;
				avoidcollision();
			}
		}
		else
		{
			just_do_it = Calc_block_pos();
			if(ball.r <= abstract_robot.RADIUS)
				just_do_it = their_goal;
		}
		
	}

	
	private void playGoalKeeper()
	//Defend the goal!
	{
		if( ball.x * our_goal.x > 0)
		{
			just_do_it.sett(ball.t);
			kick_it = true;
		}

		else if( (Math.abs(our_goal.x) > abstract_robot.RADIUS * 1.4) ||
			 (Math.abs(our_goal.y) > abstract_robot.RADIUS * 4.25) )

			{
				just_do_it.sett( our_goal.t);
				set_speed = 1;
				avoidcollision();
			}
		else
			{
				if( ball.y > 0)
					just_do_it.sety(7);
				else
					just_do_it.sety(-7);
				if(our_goal.x > 0)
					just_do_it.setx(1);
				else
					just_do_it.setx(-1);

				if( Math.abs( ball.y) < abstract_robot.RADIUS* 0.1)
					set_speed = 0.0;
				else
		 			set_speed = 1.0;
			}

	}
	

	private void playCenter()
	//Stick around the center and try to score 
	{
		if(closest_to_ball == me)
		{
			if(our_goal.r < abstract_robot.RADIUS)
			{
				change = 0;
				just_do_it = new Vec2(their_goal.x, their_goal.y+(abstract_robot.RADIUS*change));
			}
			else if(their_goal.r < abstract_robot.RADIUS*5)
			{
				just_do_it = new Vec2(their_goal.x, their_goal.y+(abstract_robot.RADIUS*change));
				if(change == 0)
					change = 1;
				else
					change = change * (-1);
			}
			else
				just_do_it = Calc_kick_pos();
		}
		else if(ball_to_team.r > ball.r)
		{
			just_do_it = ball;
		}
		else
		{
			if (center.r <abstract_robot.RADIUS* 2)
				set_speed = 0;
			else
			{
				just_do_it = center;
				avoidcollision();
			}
		}
		
	}
	
	private void playAttack()
	//Try to score all the time if the ball is in their half of the field
	//If the ball is on our half wait in assigned position for ball to come back to their half
	{
		if(closest_to_ball == me)
		{
			if(our_goal.r < abstract_robot.RADIUS)
			{
				change = 0;
				just_do_it = new Vec2(their_goal.x, their_goal.y+(abstract_robot.RADIUS*change));
			}
			else if(their_goal.r < abstract_robot.RADIUS*5)
			{
				just_do_it = new Vec2(their_goal.x, their_goal.y+(abstract_robot.RADIUS*change));
				if(change == 0)
					change = 1;
				else
					change = change * (-1);
			}
			else
				just_do_it = Calc_kick_pos();
		}
		else if(ball_to_goal.r <= ball_to_home.r)
		{
			just_do_it = Calc_kick_pos();
		}
		else // if ball is in our half of the field go back to pos and wait for ball
		{
			Vec2 my_pos = new Vec2(0,0);
			if(myID == 2) // go to center-north pos
			{
				my_pos.setx(center.x + abstract_robot.RADIUS*3);
				my_pos.sety(center.y + abstract_robot.RADIUS*3);
			}
			else if(myID == 4) // go to center-south pos
			{
				my_pos.setx(center.x + abstract_robot.RADIUS*3);
				my_pos.sety(center.y - abstract_robot.RADIUS*3);
			}
			if (my_pos.r <abstract_robot.RADIUS* 2)
				set_speed = 0;
			else
			{
				just_do_it = my_pos;
				avoidcollision();
			}
		}

	}


	private Vec2 closest_to(Vec2 ref_pt, Vec2[] contenders)
	//Determines which of the contenders is closest to the reference point
	{
		Vec2 closest = new Vec2(0,0);
		double minDist = 99999;
		Vec2 pt_to_contender = new Vec2(0,0);
		for(int i=0; i<contenders.length; i++)
		{
			pt_to_contender.setr(contenders[i].r);
			pt_to_contender.sett(contenders[i].t);
			pt_to_contender.sub(ref_pt);
			if(pt_to_contender.r < minDist)
			{
				closest.sett(contenders[i].t);
				closest.setr(contenders[i].r);
				minDist = pt_to_contender.r;
			}
		}
		return closest;
	}



	private Vec2 Calc_kick_pos()
	//Calculates a good position from which the ball can be kicked
	{
		Vec2 kick_pos = new Vec2(ball.x, ball.y);
		Vec2 off_goal = new Vec2(their_goal.x, their_goal.y+(abstract_robot.RADIUS*change));
		if(change == 0)
			change = 1;
		else
			change = change * (-1);
		kick_pos.sub(off_goal);
		kick_pos.setr(abstract_robot.RADIUS);
		kick_pos.add(ball);
		return kick_pos;
	}

	private Vec2 Calc_block_pos()
	//Calculates a good position from which the ball can be kicked
	{
		Vec2 block_pos = new Vec2(ball.x, ball.y);
		block_pos.add(our_goal);
		block_pos.setr(block_pos.r*0.5);
		return block_pos;
	}



	private void avoidcollision( )
	{
		// Try to avoid collisions

		if( nearest_teammate.r < abstract_robot.RADIUS*1.5 )
		{
			just_do_it.setx(-nearest_teammate.x);
			just_do_it.sety(-nearest_teammate.y);
			just_do_it.setr(1.0);
		}
		
		else if( nearest_opponent.r < abstract_robot.RADIUS*1.5 )
		{
			just_do_it.setx(-nearest_opponent.x);
			just_do_it.sety(-nearest_opponent.y);
			just_do_it.setr(1.0);
		}

	}


	}