v_morerobots_r.java

利用JAVA编写的群体机器人局部通讯完成一定得队形控制

/*
 * v_morerobots_r.java
 */

package EDU.gatech.cc.is.clay;

import java.lang.*;
import EDU.gatech.cc.is.util.Vec2;
import EDU.gatech.cc.is.util.Units;
import EDU.gatech.cc.is.abstractrobot.*;
//import EDU.gatech.cc.is.abstractrobot.MultiForageN150;
import EDU.gatech.cc.is.abstractrobot.SimpleN150;

/**
 * This node (motor schema) generates a vector that could maintain formation
 * Magnitude shivaries from 0 to 1.
 * <P>
 * This version works differently than Arkin's original
 * formulation.  In the original, a repulsion vector is computed
 * for each detected obstacle, with the result being the sum of
 * these vectors.  The impact is that several hazards grouped closely
 * together are more repulsive than a single hazard.  This causes problems
 * when each sonar return is treated as a separate hazard --- walls
 * for instance are more repulsive than a small hazard.
 * <P>
 * This version computes the direction of the repulsive vector
 * as in the original, but the returned magnitude is the largest
 * of the vectors, not the sum.
 * <P>
 * Arkin's original formulation is described in
 * "Motor Schema Based Mobile Robot
 * Navigation," <I>International Journal of Robotics Research</I>,
 * vol. 8, no 4, pp 92-112.
 * <P>
 * The source code in this module is based on "first principles"
 * (e.g. published papers) and is not derived from any previously
 * existing software.
 * <P>
 * For detailed information on how to configure behaviors, see the
 * <A HREF="../clay/docs/index.html">Clay page</A>.
 * <P>
 * <A HREF="../COPYRIGHT.html">Copyright</A>
 * (c)1997, 1998 Tucker Balch
 *
 * @author Xianghua Yin
 * @version $Revision: 1.1 $
 */
 public class v_morerobots_r extends NodeVec2
	{
	public static final boolean DEBUG = Node.DEBUG;
	private double	sphere = 1.0;//the radias of the controlled zone
	private double	safety = 0.0;//the radias of the dead zone,considered to be 0
	private double  cx0 =0.0;
	private double  cy0 = 0.0;
	private double  cx1 =0.0;
	private double  cy1 = 0.0;
	private int 	unique_id;//one's own id
	private static int     maxid;// the max id of all the robots  cww 080510
	private SimpleN150 robot;
	public  int  fk = 0;
	
	Vec2 	self_p  = new Vec2();//one's own position
	Vec2	last_val = new Vec2();
	Vec2    desire_p0 = new Vec2();
	Vec2    desire_p1 = new Vec2();
	
	long	lasttime = 0;
		
	/*---construction---*/
	public v_morerobots_r(double desire_x0,double desire_y0,
			double desire_x1,double desire_y1,double soe,double s,SimpleN150 r,int formationkind)
		{
		if (DEBUG) System.out.println("v_RankFormation_r: instantiated.");
		if ((soe < s) || (soe<0) || (s<0))
			{
			System.out.println("v_RankFormation_r: illegal parameters");
			return;         
			}
		sphere = soe;
		safety = s;
		robot  = r;
		cx0 = desire_x0;
		cy0 = desire_y0;
		cx1 = desire_x1;
		cy1 = desire_y1;
		fk=formationkind;
		}
	
		
	public Vec2 Value(long timestamp)
	{	
	
		Vec2   neighbour_before;
		Vec2   neighbour_right;	
		double tempmag;
		//self_p = robot.getNeighbourLocation( unique_id );
		self_p = robot.getPosition(timestamp);
		unique_id = robot.getID();
	    maxid=robot.maxID();
		System.out.println(maxid);	
		
	switch (fk)
	{//****************
	 //case 1  is line  formation
	 //****************
		case 1:
		   if (timestamp>0) lasttime = timestamp;
		
		   if (unique_id == 0)
			   return new Vec2(0,0);
		   else if (unique_id <= 9)
	    	{
			/*---return the robot's position at his right 
			desire_p0 is the desire position from left and right*/
			neighbour_right = robot.getNeighbourLocation( unique_id-1);
			desire_p0.sety(neighbour_right.y+cy0);
			desire_p0.setx(neighbour_right.x-cx0);
			desire_p0.sub(self_p);
			last_val = desire_p0;
		   }
		  else if ((unique_id == 10)||(unique_id == 19))
		  {
			/*---return the robot's position before it 
			desire_p1 is the desire position from fore and after*/
			neighbour_before = robot.getNeighbourLocation( unique_id-10);
			desire_p1.setx(neighbour_before.x-cx1);
			desire_p1.sety(neighbour_before.y+cy1);
			desire_p1.sub(self_p);
			last_val = desire_p1;
		   }
		  else 
		  {
			neighbour_right = robot.getNeighbourLocation( unique_id-1);
			neighbour_before = robot.getNeighbourLocation( unique_id-10);
				
			desire_p0.sety(neighbour_right.y+cy0);
			desire_p0.setx(neighbour_right.x+cx0);
			desire_p1.setx(neighbour_before.x-cx1);
			desire_p1.sety(neighbour_before.y+cy1);
			
			last_val.setx((desire_p0.x+desire_p1.x)/2.0);
			last_val.sety((desire_p0.y+desire_p1.y)/2.0);
			last_val.sub(self_p);
		 }
		break;
		//*****************
		//case  2  is rank formation
		//*****************
		case 2:
		   if (timestamp>0) lasttime = timestamp;
		
		   if (unique_id == 4)
			   return new Vec2(0,0);
		   else if (unique_id <= 3)
	    	{
			/*---return the robot's position at his right 
			desire_p0 is the desire position from left and right*/
			neighbour_before = robot.getNeighbourLocation( unique_id+1);
			desire_p0.sety(neighbour_before.y+cy0);
			desire_p0.setx(neighbour_before.x-cx0);
			desire_p0.sub(self_p);
			last_val = desire_p0;
		   }
		  else if (unique_id == 5)
		  return new Vec2(0,0);
		   else if (unique_id <= 9&&unique_id>5)
	    
	    
	    	{
			/*---return the robot's position at his right 
			desire_p0 is the desire position from left and right*/
			neighbour_before = robot.getNeighbourLocation( unique_id+1);
			desire_p0.sety(neighbour_before.y+cy0);
			desire_p0.setx(neighbour_before.x+cx0);
			desire_p0.sub(self_p);
			last_val = desire_p0;
		   }
		  else if (unique_id == 14)
		  {
			/*---return the robot's position before it 
			desire_p1 is the desire position from fore and after*/
			neighbour_right = robot.getNeighbourLocation( unique_id-10);
			desire_p1.setx(neighbour_right.x+cx1);
			desire_p1.sety(neighbour_right.y+cy1);
			desire_p1.sub(self_p);
			last_val = desire_p1;
		   }
		   else if(unique_id<=14&&unique_id<10) 
		   {
			neighbour_right = robot.getNeighbourLocation( unique_id-10);
			neighbour_before = robot.getNeighbourLocation( unique_id-1);
				
			desire_p0.sety(neighbour_right.y+cy0);
			desire_p0.setx(neighbour_right.x+cx0);
			desire_p1.setx(neighbour_before.x+cx1);
			desire_p1.sety(neighbour_before.y+cy1);
			
			last_val.setx((desire_p0.x+desire_p1.x)/2.0);
			last_val.sety((desire_p0.y+desire_p1.y)/2.0);
			last_val.sub(self_p);
		 }
		   else if (unique_id == 15)
		  {
			/*---return the robot's position before it 
			desire_p1 is the desire position from fore and after*/
			neighbour_right = robot.getNeighbourLocation( unique_id-10);
			desire_p1.setx(neighbour_right.x+cx1);
			desire_p1.sety(neighbour_right.y-cy1);
			desire_p1.sub(self_p);
			last_val = desire_p1;
		   }
		  else if(unique_id<=19&&unique_id>15) 
		  {
			neighbour_right= robot.getNeighbourLocation( unique_id+1);
			neighbour_before = robot.getNeighbourLocation( unique_id-10);
				
			desire_p0.sety(neighbour_right.y+cy0);
			desire_p0.setx(neighbour_right.x+cx0);
			desire_p1.setx(neighbour_before.x+cx1);
			desire_p1.sety(neighbour_before.y-cy1);
			
			last_val.setx((desire_p0.x+desire_p1.x)/2.0);
			last_val.sety((desire_p0.y+desire_p1.y)/2.0);
			last_val.sub(self_p);
		 }
		break;
		//*****************
		//case 3 is rank formation 
		//*****************
		case 3:
		if (timestamp>0) lasttime = timestamp;
		
		if (unique_id == 0)
			return new Vec2(0,0);
		else if (unique_id <= 6)
		{
			/*---return the robot's position at his right 
			desire_p0 is the desire position from left and right*/
			neighbour_before = robot.getNeighbourLocation( unique_id-1);
			desire_p0.sety(neighbour_before.y+cy0);
			desire_p0.setx(neighbour_before.x-cx0);
			desire_p0.sub(self_p);
			last_val = desire_p0;
		}
		else if (unique_id%7==0)
		{
			/*---return the robot's position before it 
			desire_p1 is the desire position from fore and after*/
			neighbour_right = robot.getNeighbourLocation( unique_id+7);
			desire_p1.setx(neighbour_right.x+cx1);
			desire_p1.sety(neighbour_right.y-cy1);
			desire_p1.sub(self_p);
			last_val = desire_p1;
		}
	/*	else if ((unique_id==14)||(unique_id==21))
		{
			/*---return the robot's position before it 
			desire_p1 is the desire position from fore and after*/
		/*	neighbour_right = robot.getNeighbourLocation( unique_id-7);
			desire_p1.setx(neighbour_right.x-cx1);
			desire_p1.sety(neighbour_right.y+cy1);
			desire_p1.sub(self_p);
			last_val = desire_p1;
		}*/
		else 
		{
			neighbour_right = robot.getNeighbourLocation( unique_id+7);
			neighbour_before = robot.getNeighbourLocation( unique_id-1);
				
			desire_p0.sety(neighbour_right.y-cy1);
			desire_p0.setx(neighbour_right.x+cx1);
			desire_p1.setx(neighbour_before.x-cx0);
			desire_p1.sety(neighbour_before.y+cy0);
			
			last_val.setx((desire_p0.x+desire_p1.x)/2.0);
			last_val.sety((desire_p0.y+desire_p1.y)/2.0);
			last_val.sub(self_p);
		   break;
		}	
		}	
				
		/*---right in the desired position---*/
		if(last_val.r<=safety)
			{
				tempmag=0;
			}
		
		/*---controlled zone---*/
		else if (last_val.r<sphere)
			{
				tempmag=(last_val.r- safety)/(sphere - safety);
			}
		
		/*---too far---*/
		else 
	
			{
				tempmag=Units.HUGE;
			}
	
		last_val.setr(tempmag);	
		
		return (new Vec2(last_val.x, last_val.y)); 
		}
	}