morerobots.java

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

/*
 * Morerobots.java
 */
 
package Domains.Morerobots;

import	EDU.gatech.cc.is.util.Vec2;
import	EDU.gatech.cc.is.abstractrobot.*;
import	EDU.gatech.cc.is.clay.*;
import  java.io.*;

import java.awt.*;
import java.net.*;
import java.awt.event.*;
import javax.swing.*;

import EDU.gatech.cc.is.communication.*;
import EDU.gatech.cc.is.simulation.SimulatedObject;

import java.util.Enumeration;



/**
 * Swarm robots FormationComm control(2008/05/09). Morerobots formation using communication
 * @author cww
 * @version $Revision: 1.0 $
 */

public class Morerobots extends ControlSystemCN150
                    //a robot that can communicate
	{
	public final static boolean DEBUG = true;
	public static boolean isRecorded = false;
	
	private NodeVec2	turret_configuration;
	private NodeVec2	steering_configuration;
	private NodeInt 	state_monitor;
	private i_FSA_ba 	STATE_MACHINE;
	int run_i;
	boolean change = true;//when the patameter"change" is true,the formation will change!
	private TransceiverHard t;
	Vec2  self_position;
	Vec2 neighbour_position;
	private int message_formationkind;//a message that respects with the kind of formation


	/**
	 * Configure the control system using Clay.
	 */
	
	public void configure()
		{
		//======
		// Set some initial hardware configurations.
		//======
	//System.out.println("0"+abstract_robot.getID(-1)+message_formationkind);
	//message_formationkind=4;
	v_Formation_r.formation_kind=4;
System.out.println(v_Formation_r.formation_kind);
		t = new TransceiverHard("localhost",abstract_robot.getID());		
	
		
		abstract_robot.setObstacleMaxRange(3.0); // don't consider things further away
	 	abstract_robot.setBaseSpeed(0.4*abstract_robot.MAX_TRANSLATION);
	 
	   
		//======
		// perceptual schemas
		//======
		
		
		//--- robot's global position
		NodeVec2
		PS_GLOBAL_POS = new v_GlobalPosition_r(abstract_robot);

		//--- obstacles
		NodeVec2Array // the sonar readings
		PS_OBS = new va_Obstacles_r(abstract_robot);

		//--- homebase 
		NodeVec2      // the place to deliver
		PS_HOMEBASE0_GLOBAL = new v_FixedPoint_(30.0,abstract_robot.getPosition(-1).y);
		
		NodeVec2      // the place to deliver
		PS_FREESPACE_GLOBAL = new v_FixedPoint_(5.0,abstract_robot.getPosition(-1).y);
		
	
		NodeVec2      // make it egocentric
		PS_HOMEBASE0 = new v_GlobalToEgo_rv(abstract_robot,
				PS_HOMEBASE0_GLOBAL);

		//======
		// Perceptual Features
		//======
		
		// close to homebase 
		NodeBoolean
		PF_CLOSE_TO_HOMEBASE0 = new b_Close_vv(0.4, PS_GLOBAL_POS,
			PS_HOMEBASE0_GLOBAL);
		
		NodeBoolean
		PF_CLOSE_TO_FREESPACE = new b_Close_vv(4, PS_GLOBAL_POS,
			PS_FREESPACE_GLOBAL);
		
		//add by yxh 082707
		
		NodeBoolean
		CHANGEFORMATION = new changeformation_test();
		
		NodeBoolean
		RETURNFORMATION = new returnformation_test();

		NodeInt
		FORMATIONKIND = new formationkind_test();
		//======
		// motor schemas
		//======
		// avoid obstacles
		NodeVec2
		MS_AVOID_OBSTACLES = new v_Avoid_va(1.5,
			abstract_robot.RADIUS + 0.1,
			PS_OBS);
			
		// swirl obstacles wrt homebase0
		NodeVec2
		MS_SWIRL_OBSTACLES_HOMEBASE0 = new v_Swirl_vav(2.0,
			abstract_robot.RADIUS + 0.22,
			PS_OBS,
			PS_HOMEBASE0);

		// go home 0
		NodeVec2
		MS_MOVE_TO_HOMEBASE0 = new v_LinearAttraction_v(0.4,0.0,PS_HOMEBASE0);	

		// noise vector
		NodeVec2
		MS_NOISE_VECTOR = new v_Noise_(5,seed);

		// swirl obstacles wrt noise
		NodeVec2
		MS_SWIRL_OBSTACLES_NOISE = new v_Swirl_vav(2.0,
			abstract_robot.RADIUS + 0.1,
			PS_OBS,
			MS_NOISE_VECTOR);
			
		// maintain line_formation
		NodeVec2
    	MS_LINE_FORMATION = new v_Formation_r(0,2.5,1,0.0,abstract_robot,2);
		
		//maintain line_formation0
		NodeVec2
		MS_LINE_FORMATION0 = new v_Formation_r(0,2.5,1,0.0,abstract_robot,1);
		
		
		//maintain column_formation
		NodeVec2
		MS_COLUMN_FORMATION = new v_Formation_r(-2.5,0,1,0.0,abstract_robot,1);
		
	    //mainatain wedge formation
	    //NodeVec2
		//MS_WEDGE_FORMATION = new v_Formation_r(2.5,0.1,1.0,0.0,abstract_robot,4);
	    
	     //for testing: maintain wedge formation 
	    NodeVec2
		MS_WEDGE_FORMATION = new v_morerobots_r(1,0,0,1,1,0,abstract_robot,2);
	   
	   
	   // maintain rank_formation  0-8,9-17,18-26   cww 080509
		NodeVec2
		MS_RANK_FORMATION = new v_morerobots_r(0,2.0,2.0,0,1,0,abstract_robot,1);
		
		//NodeVec2
		//MS_RANK_FORMATION_CHANGE = new v_morerobots_r(0,3.0,3.0,0,1,0,abstract_robot,2);
		
	   //System.out.println("1-"+abstract_robot.getID(-1)+"-"+message_formationkind);
		
		//======
		// AS_WANDER
		//======
		v_StaticWeightedSum_va 
		AS_WANDER = new v_StaticWeightedSum_va();

		AS_WANDER.weights[0]  = 1.0;
		AS_WANDER.embedded[0] = MS_AVOID_OBSTACLES;

		AS_WANDER.weights[1]  = 1.0;
		AS_WANDER.embedded[1] = MS_NOISE_VECTOR;

		AS_WANDER.weights[2]  = 1.0;
		AS_WANDER.embedded[2] = MS_SWIRL_OBSTACLES_NOISE;
	

		//======
		// AS_DELIVER0
		//======
		v_StaticWeightedSum_va
		AS_DELIVER0 = new v_StaticWeightedSum_va();

		AS_DELIVER0.weights[0]  = 2.0;//2.0
		AS_DELIVER0.embedded[0] = MS_AVOID_OBSTACLES;

		AS_DELIVER0.weights[1]  = 0.8;//0.8
		AS_DELIVER0.embedded[1] = MS_MOVE_TO_HOMEBASE0;

		AS_DELIVER0.weights[2]  = 1.5;//1.5
		AS_DELIVER0.embedded[2] = MS_SWIRL_OBSTACLES_HOMEBASE0;

		AS_DELIVER0.weights[3]  = 0.2;//0.2
		AS_DELIVER0.embedded[3] = MS_NOISE_VECTOR;
		
		AS_DELIVER0.weights[4]  = 1.5;//1.5
		AS_DELIVER0.embedded[4] = MS_RANK_FORMATION;
		
		//AS_DELIVER0.weights[5]  = 1.5;//1.5
		//AS_DELIVER0.embedded[5] = MS_LINE_FORMATION0;
			
	
	
		//=====
		// AS_CHANGE
		//=====
		v_StaticWeightedSum_va
		AS_CHANGE = new v_StaticWeightedSum_va();

		AS_CHANGE.weights[0]  = 1.2;
		AS_CHANGE.embedded[0] = MS_AVOID_OBSTACLES;

		AS_CHANGE.weights[1]  = 1.2;
		AS_CHANGE.embedded[1] = MS_MOVE_TO_HOMEBASE0;

		AS_CHANGE.weights[2]  = 1.2;
		AS_CHANGE.embedded[2] = MS_SWIRL_OBSTACLES_HOMEBASE0;

		AS_CHANGE.weights[3]  = 1.0;
		AS_CHANGE.embedded[3] = MS_NOISE_VECTOR;
		
		AS_CHANGE.weights[4]  = 1.5;//1.8
		AS_CHANGE.embedded[4] = MS_WEDGE_FORMATION;
		
		//AS_CHANGE.weights[5]  = 1.5;//1.8
		//AS_CHANGE.embedded[5] = MS_LINE_FORMATION0;
		
		//======
		// STATE_MACHINE
		//======
		STATE_MACHINE = new i_FSA_ba();

		STATE_MACHINE.state = 1;
		
		// STATE 0 WANDER
		//STATE_MACHINE.triggers[0][0]  = PF_TARGET0_VISIBLE;
		//STATE_MACHINE.follow_on[0][0] = 1; // transition to ACQUIRE0
	
		// STATE 1 DELIVER0
		STATE_MACHINE.triggers[1][0]  =CHANGEFORMATION;
		STATE_MACHINE.follow_on[1][0] = 2;// transition to CHANGE
		
		// STATE 2 CHANGE
		STATE_MACHINE.triggers[2][0]  = RETURNFORMATION;//PF_CLOSE_TO_FREESPACE;
		STATE_MACHINE.follow_on[2][0] = 1; // transition to DELIVER0
		//STATE_MACHINE.triggers[3][0]  = PF_CLOSE_TO_HOMEBASE0;
		//STATE_MACHINE.follow_on[3][0] = 1; // transition to WANDER

		state_monitor = STATE_MACHINE;
		
	System.out.println("configure:  "+changeformation_test.last_val);

		//======
		// STEERING
		//======
		v_Select_vai
		STEERING = new v_Select_vai((NodeInt)STATE_MACHINE);

		STEERING.embedded[0] = AS_WANDER;	
		STEERING.embedded[1] = AS_DELIVER0;
		STEERING.embedded[2] = AS_CHANGE;

		//======
		// TURRET
		//======
		v_Select_vai
		TURRET = new v_Select_vai((NodeInt)STATE_MACHINE);

		TURRET.embedded[0] = AS_WANDER;	
		TURRET.embedded[1] = AS_DELIVER0;
		TURRET.embedded[2] = AS_CHANGE;

		turret_configuration = TURRET;
		steering_configuration = STEERING;	
		}
		
	/**
	 * Called every timestep to allow the control system to
	 * run.
	 */
	public int takeStep()
		{
		
		Vec2	result;
		double	dresult;
		long	curr_time = abstract_robot.getTime();
		result = steering_configuration.Value(curr_time);	
		
		//change formation when a robot is locked
		double [] robot_v = new double [30];
		
		////////////////////////////////////////////////
		//added by yxh 11120107 for changing formation//
		////////////////////////////////////////////////		
		
		
		//======
		//change formation when meeting a narrow pass
		//======
			
		
		//System.out.println("how many robots? "+abstract_robot.maxID());
		
		//System.out.println("how many robots? "+abstract_robot.maxID()+"id is from "+
		//0+"to "+abstract_robot.maxID()-1);
		if ((abstract_robot.getPosition(-1).x>-22)&&(abstract_robot.getPosition(-1).x<0))
		//initial time and last time no changing formation
		{
			if (result.r>5.0)
			{
			//	a = curr_time;
				//System.out.println(a);
				//if ((curr_time-a)>0)
				if (abstract_robot.getID(-1)==4)
				{
					Message m = new StringMessage("change formation");
					t.broadcast(m);
					//message_name = getMessageID(m);
				}			
				v_Formation_r.formation_kind = 4;
				//	message_formationkind = 5;
					changeformation_test.last_val=true;
					//System.out.println("2-"+abstract_robot.getID(-1)+"-"+message_formationkind);
				
				
					
			//	changeformation_test.last_val=abstract_robot.getMessage();
			}
						
		}
		
		if (abstract_robot.getPosition(-1).x>4)
		{
			changeformation_test.last_val=false;
	    }
	    
	    robot_v[abstract_robot.getID(-1)] = abstract_robot.getPosition(-1).x;
		
		
		/*if (robot_v[0]>-8)      
		{
			Message m = new StringMessage("hello");
			//t.broadcast(m);
			Enumeration r = t.getReceiveChannel();
	 		while (r.hasMoreElements())
 	 		System.out.println(r.nextElement());
		}*/
		
		//System.out.println(t.connected());
		
		
		//======
		//change formation when passing out a narrow pass
		//======		
		if (robot_v[0]>6)		
		{
			returnformation_test.last_val = true;
		}
		//System.out.println(robot_v[abstract_robot.getID(-1)]);
		
		// STEER
		
		abstract_robot.setSteerHeading(curr_time, result.t);
		abstract_robot.setSpeed(curr_time, result.r);

		// TURRET
		result = turret_configuration.Value(curr_time);
		abstract_robot.setTurretHeading(curr_time, result.t);	

		// STATE DISPLAY
		int state = STATE_MACHINE.Value(curr_time);
		if (state == 0)
			abstract_robot.setDisplayString("wander");
		else if (state == 1)
			abstract_robot.setDisplayString("deliver");	
		else if (state == 2)
			abstract_robot.setDisplayString("change");
			
		return(CSSTAT_OK);
		
		}
	
	
	int getMessageID(Message message)
	{
		int return_value = 0;
		if(message.equals("change formation"))
		return_value = 4;
		if (message.equals("change formation1"))
		return_value = 2;
		return(return_value);
	}
}