v_socialpotential_r.java

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

/*
 * v_SocialPotential_va.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.MultiForageN150;

/**
 * This node (motor schema) generates a vector away from the
 * items detected by its embedded perceptual schema.
 * Magnitude varies 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 yym
 * @version $Revision: 1.0 $
 */

public class v_SocialPotential_r extends NodeVec2
{	
	
	
	// prameter setting
	private double      c_1 = 1250.0;
	private double      c_2 = 250.0;
	private double    lou_1 = 2.0;
	private double    lou_2 = 1.0;
	private double     zone = 0.5;
	private double  balance = 5.0; 
	private double threshold= 3.5; 
	
	private MultiForageN150 robot;	
	
	/**
	Instantiate a v_SocialPotential_va schema.	
	*/
	public v_SocialPotential_r( MultiForageN150 r)
	{
		robot = r;		
	}

	Vec2[]  neighbours;
	Vec2	last_val = new Vec2();
	
	/**
	Return a Vec2 representing the direction to go away from
	the detected hazards.
	@param timestamp long, only get new information if timestamp > than last call
                or timestamp == -1.
	@return the movement vector.
	*/
	public Vec2 Value(long timestamp)
	{
		double	tempmag = 0;	
		
		/*--- reset output ---*/
		last_val.setr(0);			
		neighbours=robot.getNeighbour();


		/*--- consider each neighbour ---*/
		for(int i = 0; i<neighbours.length; i++)
		{
		
			/*--- calculate social potential ---*/
			
			
			if (Math.abs(neighbours[i].r-balance) > zone)	
				tempmag = -(c_1/Math.pow(neighbours[i].r,lou_1))+c_2/Math.pow(neighbours[i].r,lou_2);
			else
			{
				tempmag = 0;				
			}			
				
			/*--- set the social vector ---*/
			neighbours[i].setr(tempmag);
		
			/*--- add it to the sum ---*/			
			last_val.add(neighbours[i]);
		}
		
		// Test if the force is big enough.
		if (last_val.r < threshold)
			last_val.setr(0);		
			
		return (new Vec2(last_val.x, last_val.y));
	}
}