v_formation_r.java

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

/*
 * v_Formation_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_Formation_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  cx =0.0;
	private double  cy = 0.0;
	private int 	unique_id;//one's own id
	private static int     maxid;// the max id of all the robots
	private SimpleN150 robot;
	public  int  fk = 0;
	public static int formation_kind;
	
	Vec2 	self_p  = new Vec2();//one's own position
	Vec2	last_val = new Vec2();
	Vec2    desire_p = new Vec2();
	
	long	lasttime = 0;
		
	/*---construction---*/
	public v_Formation_r(double desire_x,double desire_y,double soe,double s,SimpleN150 r,int formation_kind)
		{
		if (DEBUG) System.out.println("v_Formation_r: instantiated.");
		if ((soe < s) || (soe<0) || (s<0))
			{
			System.out.println("v_Formation_r: illegal parameters");
			return;         
			}
		sphere = soe;
		safety = s;
		robot  = r;
		cx = desire_x;
		cy = desire_y;
		fk = formation_kind;
		
		}
	
		
	public Vec2 Value(long timestamp)
	{	
	
		Vec2   neighbour;	
		double tempmag;
		//self_p = robot.getNeighbourLocation( unique_id );
		self_p = robot.getPosition(timestamp);
		unique_id = robot.getID();
	    maxid=robot.maxID();
	
	    double a = 0;
		
		switch (fk)
		{
			//===========
			//case 1 and case 2 are line formation
			//===========
		case 1:
		    
		    if (timestamp>0) lasttime = timestamp;
		
			if (unique_id == maxid)
			return new Vec2(0,0);
			else
				neighbour = robot.getNeighbourLocation( unique_id+1);
		
		
			desire_p.sety(neighbour.y-cy);
			desire_p.setx(neighbour.x-cx);
			
			desire_p.sub(self_p);
		
			last_val=desire_p;
		
		break;	
			
		case 2:
			
			if (unique_id == 0)
			return new Vec2(0,0);
			else
				neighbour = robot.getNeighbourLocation( unique_id-1);
		
			desire_p.sety(neighbour.y+cy);
			desire_p.setx(neighbour.x+cx);
		
			desire_p.sub(self_p);
		
			last_val=desire_p;
			
		break;
		
		
			//===========
			//case 3 is diamand formation
			//===========
		case 3:
			
			if (unique_id == maxid/2)
			return new Vec2(0,0);
			//else if (unique_id == 2||unique_id == 1)
			else if (((((maxid+1)/4)-1)<=unique_id)&&(unique_id<(((maxid+1)/2-1))))
			{
				neighbour = robot.getNeighbourLocation( unique_id+1);
				desire_p.sety(neighbour.y-cy);
				desire_p.setx(neighbour.x-cx);
			}
			else if  (unique_id == 4||unique_id == 5)
			//else if  (unique_id == 4||unique_id == 5)
			{
				neighbour = robot.getNeighbourLocation( unique_id-1);
				desire_p.sety(neighbour.y+cy);
				desire_p.setx(neighbour.x-cx);
			}
			else if  (unique_id == 6||unique_id == 7)
			{
				neighbour = robot.getNeighbourLocation( unique_id-1);
				desire_p.sety(neighbour.y-cy);
				desire_p.setx(neighbour.x-cx);
			}
			else 
			{
				neighbour = robot.getNeighbourLocation( unique_id+1);
				desire_p.sety(neighbour.y+cy);
				desire_p.setx(neighbour.x-cx);
			}
		
			desire_p.sub(self_p);
			
			last_val=desire_p;
	
		break;	
		
			//===========
			//case 4 is wedge formation
			//===========
		case 4:
		
			if (unique_id == maxid/2)
			return new Vec2(0,0);
			else if(unique_id < maxid/2)
			{
			neighbour = robot.getNeighbourLocation( unique_id +1 );
			desire_p.setx(neighbour.x-cx);
			desire_p.sety(neighbour.y-cy);
			}
			else
			{
			neighbour = robot.getNeighbourLocation( unique_id -1 );
			desire_p.setx(neighbour.x-cx);
			desire_p.sety(neighbour.y+cy);
			}
			
			desire_p.sub(self_p);
		
			last_val=desire_p;	
			
		break;		
			//===========
			//case 5 is wedge0 formation
			//===========
		case 5:
		
			if (unique_id == maxid/2)
			return new Vec2(0,0);
			else if(unique_id < maxid/2)
			{
			neighbour = robot.getNeighbourLocation( unique_id +1 );
			desire_p.setx(neighbour.x+cx);
			desire_p.sety(neighbour.y-cy);
			}
			else
			{
			neighbour = robot.getNeighbourLocation( unique_id -1 );
			desire_p.setx(neighbour.x+cx);
			desire_p.sety(neighbour.y+cy);
			}
			
			desire_p.sub(self_p);
		
			last_val=desire_p;	
			
		break;	
		
			//*****************
		//case  6  is wedge formation
		//*****************
		
		case 6:
		    if(unique_id<9)
			{if (unique_id == 4)
			return new Vec2(0,0);
			else if(unique_id < 4)
			{
			neighbour = robot.getNeighbourLocation( unique_id +1 );
			desire_p.setx(neighbour.x-cx);
			desire_p.sety(neighbour.y-cy);
			}
			else
			{
			neighbour = robot.getNeighbourLocation( unique_id -1 );
			desire_p.setx(neighbour.x-cx);
			desire_p.sety(neighbour.y+cy);
			}
			
			desire_p.sub(self_p);
		
			last_val=desire_p;
			}
			else if(unique_id<18)	
			{if (unique_id == 13)
			return new Vec2(0,0);
			else if(unique_id <13)
			{
			neighbour = robot.getNeighbourLocation( unique_id +1 );
			desire_p.setx(neighbour.x-cx);
			desire_p.sety(neighbour.y-cy);
			}
			else
			{
			neighbour = robot.getNeighbourLocation( unique_id -1 );
			desire_p.setx(neighbour.x-cx);
			desire_p.sety(neighbour.y+cy);
			}
			
			desire_p.sub(self_p);
		
			last_val=desire_p;
			}
			else 	
			{if (unique_id == 22)
			return new Vec2(0,0);
			else if(unique_id <22)
			{
			neighbour = robot.getNeighbourLocation( unique_id +1 );
			desire_p.setx(neighbour.x-cx);
			desire_p.sety(neighbour.y-cy);
			}
			else
			{
			neighbour = robot.getNeighbourLocation( unique_id -1 );
			desire_p.setx(neighbour.x-cx);
			desire_p.sety(neighbour.y+cy);
			}
			
			desire_p.sub(self_p);
		
			last_val=desire_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)); 
	}
}