v_wedgeformation_r.java

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

/*
 * v_WedgeFormation_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_WedgeFormation_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 SimpleN150 robot;
	
	
	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_WedgeFormation_r(double desire_x,double desire_y,double soe,double s,SimpleN150 r)//parameter???
		{
		if (DEBUG) System.out.println("v_WedgeFormation_r: instantiated.");
		if ((soe < s) || (soe<0) || (s<0))
			{
			System.out.println("v_WedgeFormation_r: illegal parameters");
			return;         
			}
		sphere = soe;
		safety = s;
		robot  = r;
		cx = desire_x;
		cy = desire_y;
		
		}
	
		
	public Vec2 Value(long timestamp)
	{	
	
		Vec2   neighbour;	
		double tempmag;
		//self_p = robot.getNeighbourLocation( unique_id );
		self_p = robot.getPosition(timestamp);
		unique_id = robot.getID();
	    double a = 0;
		
		
		if (timestamp>0) lasttime = timestamp;
		
		//there are 9 robots totally,id is from 0to 8,no.0robor is  
 				//in the bottom of the formation
 		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;
		
				
		/*---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)); 
		}
	}